Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
Last updated: 20 Apr 2021
Affordable and flexible platform to ease prototyping using a STM32F401RET6 microcontroller.
Weird Ticker Behavior Inside Class
Topic last updated 11 Oct 2014, by Abraham Howell.
1 reply
During program development I simply created my entire program within a single file, "main.cpp". However, my ultimate goal is to create a class library. This library is being used to control a simple 2-wheeled robot that is being controlled by a Nucleo F401RE board & custom motor shield.
My single file program works great. I am using a Ticker that interrupts every 205 microseconds in order to calculate motor velocity by using encoder feedback. I also use this event to update the tone on a piezo buzzer.
After converting my program into a class library I discovered that the Ticker would interrupt several times upon startup, but then would stop working. Not sure if I am doing something wrong?
<<code title=main.cpp>>
#include "mbed.h"
#include "Apeiros.h"
Apeiros apeiros(SERIAL_TX, SERIAL_RX);
int main() {
// Play initialize buzzer tones. //
apeiros.SetBuzzerTone(1);
wait_ms(250);
apeiros.SetBuzzerTone(10);
wait_ms(250);
apeiros.SetBuzzerTone(1);
wait_ms(250);
apeiros.SetBuzzerTone(0);
while(1) {
if (apeiros.isSerialDataAvail()) apeiros.ParseUartData();
}
}
<</code>>
<<code title=Apeiros.h>>
#ifndef APEIROS_H
#define APEIROS_H
#include "mbed.h"
#define Dwh 2.621 // wheel diameter
#define PI 3.14159 // PI
#define Cwh (Dwh * PI) // wheel circumference
#define TSCALE 10 // convert to 10ths of an inch
#define INVTICK 4883 // this is 1 / 204.8 us, to avoid using floating point math
#define NCLKS 128 // number of encoder clocks per wheel rotation
#define Ktps ((Cwh * TSCALE * INVTICK) / NCLKS)
#define MaxMotorSpeed 255
#define MIN_GRIPPER_PULSE 1000
#define MAX_GRIPPER_PULSE 2300
#define MAX_BUZZER_PWM 100
class Apeiros : public Serial{
public:
Apeiros(PinName tx, PinName rx);
bool isSerialDataAvail(void);
void ParseUartData(void);
void SetBuzzerTone(int buzzerTone);
void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
void SetGripperPosition(int pulseWidth_us);
private:
AnalogIn ad_0;
AnalogIn ad_1;
AnalogIn ad_2;
AnalogIn ad_3;
AnalogIn ad_4;
AnalogIn ad_5;
DigitalOut buzzerOut;
DigitalOut SN_3A;
DigitalOut SN_4A;
DigitalOut SN_2A;
DigitalOut SN_1A;
DigitalIn leftFrontIR;
DigitalIn centerFrontIR;
DigitalIn rightFrontIR;
DigitalIn leftEncoderDirPin;
DigitalIn rightEncoderDirPin;
InterruptIn leftEncoderClk;
InterruptIn rightEncoderClk;
PwmOut rightMotorPWM;
PwmOut leftMotorPWM;
PwmOut gripperPWM;
Ticker motorControl;
volatile char rxBuff[16];
volatile char rxBuffIndx;
volatile char tmpRxBuff[16];
volatile bool uartDataRdy;
volatile int motorUpdateTickCount;
volatile int motionLoopCount;
volatile int leftEncoderCount;
volatile int rightEncoderCount;
volatile bool leftEncoderDir;
volatile bool rightEncoderDir;
volatile int leftMotorOffset;
volatile int rightMotorOffset;
volatile unsigned short analogIndex;
volatile float adSensors[6];
volatile char encoderArrayIndex_L, encoderArrayIndex_R;
volatile int encoderPeriodArray_L[4], encoderPeriodArray_R[4];
volatile int encoderPeriod_L, encoderPeriod_R, encoderPeriodSum_L, encoderPeriodSum_R;
volatile int prevT3Count_L, prevT3Count_R;
volatile bool encoderUpdated_L, encoderUpdated_R;
volatile int encoderSpeed_L, encoderSpeed_R;
// Buzzer Variables //
volatile int buzzerPeriod;
volatile int buzzerDuty;
volatile int buzzerTick;
volatile bool enableBuzzer;
void getUartData(void);
void motorControlISR(void);
void leftEncoderTick(void);
void rightEncoderTick(void);
void ParseMotorCommand(void);
void ParseBuzzerCommand(void);
void ParseGripperCommand(void);
void CalculateWheelSpeed(void);
};
#endif
<</code>>
<<code title=Apeiros.cpp>>
#include "Apeiros.h"
#include "mbed.h"
//------------------------------------------------------------------------
// Constructor for Apeiros Class.
//------------------------------------------------------------------------
Apeiros::Apeiros(PinName tx, PinName rx) : Serial(tx, rx), ad_0(PA_0), ad_1(PA_1),ad_2(PA_4),ad_3(PB_0),ad_4(PC_1),ad_5(PC_0),buzzerOut(PA_5),
SN_3A(PB_5),SN_4A(PC_7),SN_2A(PB_3),SN_1A(PA_8),leftFrontIR(PC_15),centerFrontIR(PC_14),rightFrontIR(PC_13),leftEncoderDirPin(PA_6),
rightEncoderDirPin(PA_7),leftEncoderClk(PH_1),rightEncoderClk(PH_0),rightMotorPWM(PB_10),leftMotorPWM(PB_4),gripperPWM(PB_6)
{
// Set direction of PWM dir pins to be low so robot is halted. //
SN_3A = 0;
SN_4A = 0;
SN_2A = 0;
SN_1A = 0;
// Configure Left & Right Motor PWMs. //
rightMotorPWM.period_us(255);
rightMotorPWM.pulsewidth_us(0);
leftMotorPWM.period_us(255);
leftMotorPWM.pulsewidth_us(0);
// Configure Gripper PWM. //
gripperPWM.period_ms(20);
SetGripperPosition(2300);
// Configure Wheel Encoder Inputs & ISRs. //
leftEncoderDirPin.mode(PullUp);
rightEncoderDirPin.mode(PullUp);
leftEncoderClk.mode(PullDown);
rightEncoderClk.mode(PullUp);
leftEncoderClk.rise(this, &Apeiros::leftEncoderTick);
rightEncoderClk.fall(this, &Apeiros::rightEncoderTick);
motorControl.attach_us(this, &Apeiros::motorControlISR, 205);
Serial::attach(this, &Apeiros::getUartData,Serial::RxIrq);
baud(115200);
printf("\r\n");
printf("Hello, my name is Apeiros!\r\n");
rxBuffIndx = 0;
uartDataRdy = false;
motorUpdateTickCount = 0;
motionLoopCount = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
leftEncoderDir = 1;
rightEncoderDir = 1;
leftMotorOffset = 120;
rightMotorOffset = 115;
analogIndex = 0;
buzzerPeriod = 2;
buzzerDuty = buzzerPeriod/2;
buzzerTick = 0;
enableBuzzer = false;
}
//------------------------------------------------------------------------
// Serial Interrupt Service Routine (ISR) for RawSerial.
//------------------------------------------------------------------------
void Apeiros::getUartData()
{
if (!uartDataRdy)
{
rxBuff[rxBuffIndx] = getc();
if (rxBuff[rxBuffIndx] == 13)
{
for (rxBuffIndx = 0; rxBuffIndx < 16; rxBuffIndx ++) tmpRxBuff[rxBuffIndx] = rxBuff[rxBuffIndx];
rxBuffIndx = 0;
uartDataRdy = true;
}
else
{
rxBuffIndx++;
if (rxBuffIndx > 15)
{
rxBuffIndx = 0;
for (int indx=0;indx<16;indx++) rxBuff[indx] = 0;
}
}
}
}
//------------------------------------------------------------------------
// Motor Control Interrupt Service Routine (ISR).
//------------------------------------------------------------------------
void Apeiros::motorControlISR()
{
motorUpdateTickCount++;
motionLoopCount++;
if (motionLoopCount > 250)
{
CalculateWheelSpeed();
motionLoopCount = 0;
}
// Update analog conversions. //
switch (analogIndex)
{
case 0:
adSensors[0] = ad_0.read();
break;
case 1:
adSensors[1] = ad_1.read();
break;
case 2:
adSensors[2] = ad_2.read();
break;
case 3:
adSensors[3] = ad_3.read();
break;
case 4:
adSensors[4] = ad_4.read();
break;
case 5:
adSensors[5] = ad_5.read();
break;
}
analogIndex++;
if (analogIndex > 5) analogIndex = 0;
// Update buzzer tone as needed. //
if (enableBuzzer)
{
buzzerTick++;
if (buzzerTick > buzzerPeriod)
{
buzzerTick = 0;
buzzerOut = 1;
}
else if (buzzerTick > buzzerDuty)
{
buzzerOut = 0;
}
}
}
//------------------------------------------------------------------------
// Interrupt Service Routine (ISR) for Left Wheel Encoder Transitions.
//------------------------------------------------------------------------
void Apeiros::leftEncoderTick()
{
leftEncoderDir = leftEncoderDirPin.read();
if (!leftEncoderDir) leftEncoderCount++;
else leftEncoderCount--;
// Remove the oldest value from the sum.
encoderPeriodSum_L -= encoderPeriodArray_L[encoderArrayIndex_L];
// Store the newest value, and add it to the sum.
encoderPeriodSum_L += encoderPeriodArray_L[encoderArrayIndex_L] = motorUpdateTickCount - prevT3Count_L;
// Calculate a new average period.
encoderPeriod_L = encoderPeriodSum_L/4;
// Move to the next array position.
encoderArrayIndex_L++;
if (encoderArrayIndex_L > 3) encoderArrayIndex_L = 0;
// Store the current timer3TickCount for next time.
prevT3Count_L = motorUpdateTickCount;
// Set encoder as updated.
encoderUpdated_L = true;
}
//------------------------------------------------------------------------
// Interrupt Service Routine (ISR) for Right Wheel Encoder Transitions.
//------------------------------------------------------------------------
void Apeiros::rightEncoderTick()
{
rightEncoderDir = rightEncoderDirPin.read();
if (!rightEncoderDir) rightEncoderCount--;
else rightEncoderCount++;
// Remove the oldest value from the sum.
encoderPeriodSum_R -= encoderPeriodArray_R[encoderArrayIndex_R];
// Store the newest value, and add it to the sum.
encoderPeriodSum_R += encoderPeriodArray_R[encoderArrayIndex_R] = motorUpdateTickCount - prevT3Count_R;
// Calculate a new average period.
encoderPeriod_R = encoderPeriodSum_R/4;
// Move to the next array position.
encoderArrayIndex_R++;
if (encoderArrayIndex_R > 3) encoderArrayIndex_R = 0;
// Store the current timer3TickCount for next time.
prevT3Count_R = motorUpdateTickCount;
// Set encoder as updated.
encoderUpdated_R = true;
}
//------------------------------------------------------------------------
// Function to return status of UART data.
//------------------------------------------------------------------------
bool Apeiros::isSerialDataAvail()
{
if (uartDataRdy) return true;
else return false;
}
//------------------------------------------------------------------------
// Function to parse UART data from Rx buffer for SDIO1 (PA9 & PA10).
//------------------------------------------------------------------------
void Apeiros::ParseUartData()
{
switch (tmpRxBuff[0]){
case 'A':
printf("Sensors = (%0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f)\r\n", adSensors[0], adSensors[1], adSensors[2], adSensors[3], adSensors[4], adSensors[5]);
break;
case 'B':
ParseBuzzerCommand();
printf("b\r\n");
break;
case 'D':
printf("Sensors = (%d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read());
break;
case 'E':
printf("Encoder Ticks = %d, %d\r\n",leftEncoderCount, rightEncoderCount);
break;
case 'G':
ParseGripperCommand();
printf("g\r\n");
break;
case 'H':
SetMotorSpeed(0, 0);
printf("h\r\n");
break;
case 'M':
ParseMotorCommand();
printf("ma\r\n");
break;
case 'V':
printf("Motor Velocity = %d, %d\r\n",encoderSpeed_L, encoderSpeed_R);
break;
case 'Y':
gripperPWM.pulsewidth_us(2000);
printf("ya\r\n");
break;
case 'Z':
gripperPWM.pulsewidth_us(1000);
printf("za\r\n");
break;
default:
printf("Cmd unrecognized!\r\n");
break;
}
for (int buffIndx = 0; buffIndx < 16; buffIndx++) tmpRxBuff[buffIndx] = 0;
uartDataRdy = false;
}
//------------------------------------------------------------------------
// Function to set tone of buzzer.
//------------------------------------------------------------------------
void Apeiros::SetBuzzerTone(int buzzerTone)
{
if (buzzerTone > 0)
{
if (buzzerTone > MAX_BUZZER_PWM) buzzerTone = MAX_BUZZER_PWM;
buzzerTick = 1;
buzzerPeriod = buzzerTone;
buzzerDuty = buzzerPeriod/2;
enableBuzzer = true;
}
else
{
enableBuzzer = false;
buzzerOut = 0;
}
}
//------------------------------------------------------------------------
// Calculate Wheel Speeds
//
// Speed in 0.1"/sec = (Cwh / NCLKS) * TSCALE / (TICK * PER) = Ktps / PER
// where Cwh = 8.12" wheel circumference, NCLKS = 128 (32 stripe disk),
// TSCALE = 10 (to convert inches to 0.1"), TICK = 205us per timer2 tick,
// and PER is the measured period in timer 2 ticks
// Ktps = 3098
//------------------------------------------------------------------------
void Apeiros::CalculateWheelSpeed()
{
// If the wheel is spinning so slow that we don't have current reading.
if (!encoderUpdated_R) encoderPeriod_R = motorUpdateTickCount - prevT3Count_R; // Calculate period since last update.
else encoderUpdated_R = false; // Otherwise use it.
// Converts from 205us ticks per edge to multiples of 0.1 inches per second.
encoderSpeed_R = (signed short)(Ktps / encoderPeriod_R);
// Check direction of wheel rotation & adjust as needed. */
if (!rightEncoderDir) encoderSpeed_R = -encoderSpeed_R;
if (!encoderUpdated_L) encoderPeriod_L = motorUpdateTickCount - prevT3Count_L;
else encoderUpdated_L = false;
// Converts from 205us ticks per edge to multiples of 0.1 inches per second
encoderSpeed_L = (signed short)(Ktps / encoderPeriod_L);
// Check direction of wheel rotation & adjust as needed. */
if (leftEncoderDir) encoderSpeed_L = -encoderSpeed_L;
}
//------------------------------------------------------------------------
// Function to set left & right motor speed and direction.
//------------------------------------------------------------------------
void Apeiros::SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed)
{
int tmpLeftSpeed = 0;
int tmpRightSpeed = 0;
if (rightMotorSpeed < 0)
{
SN_2A = 0;
SN_1A = 1;
} else {
SN_2A = 1;
SN_1A = 0; }
if (leftMotorSpeed < 0)
{
SN_3A = 0;
SN_4A = 1;
} else {
SN_3A = 1;
SN_4A = 0; }
if (leftMotorSpeed != 0) tmpLeftSpeed = abs(leftMotorSpeed) + leftMotorOffset;
else
{
tmpLeftSpeed = 0;
SN_3A = SN_4A = 0;
}
if (rightMotorSpeed != 0) tmpRightSpeed = abs(rightMotorSpeed) + rightMotorOffset;
else
{
tmpRightSpeed = 0;
SN_1A = SN_2A = 0;
}
if (tmpLeftSpeed > MaxMotorSpeed) tmpLeftSpeed = MaxMotorSpeed;
if (tmpRightSpeed > MaxMotorSpeed) tmpRightSpeed = MaxMotorSpeed;
leftMotorPWM.pulsewidth_us(tmpLeftSpeed);
rightMotorPWM.pulsewidth_us(tmpRightSpeed);
}
//------------------------------------------------------------------------
// Function to parse motor commands from UART data.
//------------------------------------------------------------------------
void Apeiros::ParseMotorCommand()
{
unsigned int commaPos, endPos, index1, index2;
signed short leftWheel, rightWheel;
char charBuff[5];
bool foundNegative = false;
commaPos = 0;
endPos = 0;
// Find comma position and return char.
for (index1=2;index1<16;index1++)
{
if (tmpRxBuff[index1] == ',') commaPos = index1;
else if (tmpRxBuff[index1] == 13)
{
endPos = index1;
break;
}
}
// Parse out left motor data.
for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
index2 = 0;
for (index1=2;index1<commaPos;index1++)
{
if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
else foundNegative = true;
index2++;
}
leftWheel = atol(charBuff);
if (foundNegative) leftWheel = -leftWheel;
foundNegative = false;
// Parse out right motor data.
for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
index2 = 0;
for (index1=commaPos+1;index1<endPos;index1++)
{
if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
else foundNegative = true;
index2++;
}
rightWheel = atol(charBuff);
if (foundNegative) rightWheel = -rightWheel;
//stdio1.printf("Left Motor = %d\r\n", leftWheel);
//stdio1.printf("Right Motor = %d\r\n", rightWheel);
SetMotorSpeed(leftWheel, rightWheel);
}
//------------------------------------------------------------------------
// Function to parse buzzer tone commands from UART data.
//------------------------------------------------------------------------
void Apeiros::ParseBuzzerCommand()
{
unsigned int index1, index2, endPos;
unsigned int buzzerTonePWM;
char charBuff[3];
index1 = index2 = endPos = 0;
buzzerTonePWM = 1;
for (index1=0;index1<3;index1++) charBuff[index1] = ' ';
// Find position of return char.
for (index1=1;index1<16;index1++)
{
if (tmpRxBuff[index1] == 13)
{
endPos = index1;
break;
}
}
index2 = 0;
for (index1=1;index1<endPos;index1++)
{
charBuff[index2] = tmpRxBuff[index1];
index2++;
}
buzzerTonePWM = atol(charBuff);
SetBuzzerTone(buzzerTonePWM);
}
//------------------------------------------------------------------------
// Function to parse gripper servo commands from UART data.
//------------------------------------------------------------------------
void Apeiros::ParseGripperCommand()
{
unsigned int index1, index2, endPos;
unsigned int gripperPosition;
char charBuff[4];
index1 = index2 = endPos = 0;
gripperPosition = 2000;
for (index1=0;index1<4;index1++) charBuff[index1] = ' ';
// Find position of return char.
for (index1=1;index1<16;index1++)
{
if (tmpRxBuff[index1] == 13)
{
endPos = index1;
break;
}
}
index2 = 0;
for (index1=1;index1<endPos;index1++)
{
charBuff[index2] = tmpRxBuff[index1];
index2++;
}
gripperPosition = atol(charBuff);
SetGripperPosition(gripperPosition);
}
//------------------------------------------------------------------------
// Function to set gripper servo position using supplied pulsewidth[us].
//------------------------------------------------------------------------
void Apeiros::SetGripperPosition(int pulseWidth_us)
{
if (pulseWidth_us > MAX_GRIPPER_PULSE) pulseWidth_us = MAX_GRIPPER_PULSE;
if (pulseWidth_us < MIN_GRIPPER_PULSE) pulseWidth_us = MIN_GRIPPER_PULSE;
gripperPWM.pulsewidth_us(pulseWidth_us);
}
<</code>>
Resolved my issue with the ticker by simply attaching the ticker in a begin method that is called before using my Apeiros class. Now the ticker interrupts as expected.
main.cpp
#include "mbed.h" #include "Apeiros.h" Apeiros apeiros(SERIAL_TX, SERIAL_RX); int main() { apeiros.Begin(); while(1) { if (apeiros.isSerialDataAvail()) apeiros.ParseUartData(); } }
Apeiros.cpp
//------------------------------------------------------------------------ // Function to Begin. //------------------------------------------------------------------------ void Apeiros::Begin() { motorControl.attach_us(this, &Apeiros::motorControlISR, 205); printf("\r\n"); printf("Hello, my name is Apeiros!\r\n"); // Play initialize buzzer tones. // SetBuzzerTone(1); wait_ms(250); SetBuzzerTone(10); wait_ms(250); SetBuzzerTone(1); wait_ms(250); SetBuzzerTone(0); }
Regards, Abe
Resolved my issue with the ticker by simply attaching the ticker in a begin method that is called before using my Apeiros class. Now the ticker interrupts as expected.
<<code title=main.cpp>>
#include "mbed.h"
#include "Apeiros.h"
Apeiros apeiros(SERIAL_TX, SERIAL_RX);
int main() {
apeiros.Begin();
while(1) {
if (apeiros.isSerialDataAvail()) apeiros.ParseUartData();
}
}
<</code>>
<<code title=Apeiros.cpp>>
//------------------------------------------------------------------------
// Function to Begin.
//------------------------------------------------------------------------
void Apeiros::Begin()
{
motorControl.attach_us(this, &Apeiros::motorControlISR, 205);
printf("\r\n");
printf("Hello, my name is Apeiros!\r\n");
// Play initialize buzzer tones. //
SetBuzzerTone(1);
wait_ms(250);
SetBuzzerTone(10);
wait_ms(250);
SetBuzzerTone(1);
wait_ms(250);
SetBuzzerTone(0);
}
<</code>>
Regards, Abe
During program development I simply created my entire program within a single file, "main.cpp". However, my ultimate goal is to create a class library. This library is being used to control a simple 2-wheeled robot that is being controlled by a Nucleo F401RE board & custom motor shield.
My single file program works great. I am using a Ticker that interrupts every 205 microseconds in order to calculate motor velocity by using encoder feedback. I also use this event to update the tone on a piezo buzzer.
After converting my program into a class library I discovered that the Ticker would interrupt several times upon startup, but then would stop working. Not sure if I am doing something wrong?
main.cpp
Apeiros.h
Apeiros.cpp