Using the MsTimer2 library efficiently


My project is to achieve dead reckoning using rotary encoders...
What I can successfully do with my robot so far :

  • Recognize the direction of the wheels and increment / decrement encoder ticks accordingly using interrupts;
  • Calculate the absolute (x,y,theta) coordinates of my robot instantly;
  • Using MsTimer2 library, I execute the dead reckoning function with a 5ms period;
    Problem is, when I tried adding motor driving, to all this to make my robot go to a certain set of coordinates...the motors don't run :astonished:
    Here's my code...
#include <MsTimer2.h>


// Encoder and interrupt pins and variables
int encoderPinB[2] = {25,22}; // Encoder B pin
long encoderTicks[2];
int odoLPin = 4; // interrupt 4 for digital Pin 19 
int odoRPin = 5; // interrupt 5 for digital Pin 18

// Constants
#define COEF_Dist 11.0184191 // 1 cm -> 11.0184191 encoder ticks
#define COEF_ROT 4.352083   //  1°    -> 4.35 encoder ticks 
#define PI 3.14159265
#define CONV_RD PI/(180*COEF_ROT) // angle conversion from ticks to radian

//  Global variables
long dist,last_dist ; // dist : distance covered at instant tn ; last_dist : distance covered at instant tn-1
long orient, last_orient, orient_init; // orient : current angle at instant tn; last_orient : last angle at instant tn-1
float x, y; // (x,y) coordinates in ticks
int x_cm, y_cm; // (x,y) coordinates in cm

// Motor Driving variables
 #define TOPSPEED 200
 #define FORWARD_SPEED 150
 #define TURN_SPEED 60
 // Left motor 
 int dir1PinA = 13;
 int dir2PinA = 12;
 int speedPinA = 10;
 // Right motor 
 int dir1PinB = 11;
 int dir2PinB = 8;
 int speedPinB = 9;
 int oam_speed[2]={0}; //oam_speed[0] = left wheel ; oam_speed[1] = right wheel

 // Dead reckoning variables
 int delta_d, delta_orient; // difference in dist ; difference in angle
 long orient_mean; // mean of current and last angle
 float orient_mean_radian, delta_orient_radian;  // radian converted 
 float K=1, dx,dy; // differential x ; differential y

// Initial Conditions
void init_dead_reckoning (float x0, float y0, long teta0)
  orient_init = teta0 ;
  last_orient = teta0 ;
  x = x0 ;
  y = y0 ;
  dist = 0 ;
  last_dist=0 ;

// Update function executed every 5 ms
void update()
  dead_reckoning () ; // Coordinates calculation

  while(orient_mean_radian<0){orient_mean_radian+=2*PI;} // Keeping angle in a [0, 2*pi] interval

// Dead reckoning
void dead_reckoning (void)
  dist = ((encoderTicks[1]+encoderTicks[0])/2); // distance covered at instant tn
  orient = orient_init +(encoderTicks[1]-encoderTicks[0]); 
  delta_d = dist - last_dist; 
  delta_orient = orient - last_orient; 
  orient_mean = (orient +last_orient) / 2;  
  delta_orient_radian = CONV_RD*delta_orient; 
  orient_mean_radian = CONV_RD*orient_mean; 
  x= x + dx; // Values in ticks
  y= y + dy;
  x_cm=(int)(x/COEF_Dist); // This calculus can be done outside the dead_reckoning function
  last_orient = orient ; // Refreshing angle value
  last_dist = dist ; // Refreshing distance value
// Left encoder Ticks 
void incL(){   
  if (digitalRead(encoderPinB[0]) == HIGH){  
  } else {  

// Left encoder Ticks   
void incR(){  
  if (digitalRead(encoderPinB[1]) == LOW){  
  } else {  

void  init_encoders(){
pinMode(encoderPinB[0], INPUT); // Encoder pin B INPUT
pinMode(encoderPinB[1], INPUT); // Encoder pin B INPUT

void init_motors(){
  pinMode(dir1PinA, OUTPUT);
  pinMode(dir2PinA, OUTPUT);
  pinMode(speedPinA, OUTPUT);
  pinMode(dir1PinB, OUTPUT);
  pinMode(dir2PinB, OUTPUT);
  pinMode(speedPinB, OUTPUT);

// Setup and main loop

void setup (){
  // Serial communication
  init_motors(); // Motor Pin intialization
  init_encoders(); //Encoders Pin initialization
  // attach the odometer interrupt
  attachInterrupt(odoLPin, incL, RISING); //18
  attachInterrupt(odoRPin, incR, RISING); //19
  // Timer2 to execute update function
  MsTimer2::set(5, update); // 5ms period

void loop(){
  //  Execute a full revolution then stop
  while(orient_mean_radian*180/PI < 360){
  digitalWrite(dir1PinA, LOW);
  digitalWrite(dir2PinA, HIGH);
  digitalWrite(dir1PinB, HIGH);
  digitalWrite(dir2PinB, LOW);


   /* Serial.print(encoderTicks[0]);
    Serial.print("\tx = ");
    Serial.print("| y = ");
    Serial.print("\tTheta = ");

Why aren't the motors working? (I tried them seperately they're running just fine)
I'm clueless
PLease share your insights
Thank you^^

How are your motors not running? Please describe what should be happening and what is actually happening.

Also, be aware that while you are in the MsTimer2 ISR, the encoder interrupts will NOT trigger (interrupts are disabled in ISRs, they're supposed to be really short, after all).

For the interrupts there's no problem, since I'm using an Arduino Mega, MsTImer3 and my encoders are using each a different interrupt. Actually, when I upload this code, when I push the robot manually forward, backwards, and if I turn it left and right, I can see on the monitor that my dead reckoning works. I see the x,y and theta coordinates changing, and when I make the robot do a full turn on itself, the theta value goes from 0 to 360°, so I think encoders, and the dead reckoning function are all working just fine. But as you see on the loop, I'm telling it to turn on itself whil (theta<360) but motors simply don't run. So I don't understand...
MS timers happens outside the loop, every 5ms, encoders ticks also increment and decrement outside the loop, so what's left in loop is the Serial prints and the motor commands....the serials are working, the motors Not... :fearful:

Try making all of the variables that change in interrupts into volatiles (you're supposed to, anyways) - the compiler isn't expecting them to change in your main loop, and may be optimizing them out.

I tried, nothing changed....
And then I tried not using the MsTimer function, the motors worked... :astonished:
So there's clearly some kind of conflict between Ms timer and the motor commands in the main loop...

That's entirely possible - try switching to using a non-interrupt-driven routine (this is probably better for your code, too).

Instead of having a hard-coded time constant, you'll want to use (millis() - previousLoopTime) * 0.001 - that way it'll adjust for when the spacing isn't exactly equal to your interval.

I think what may be happening is that your motor control code is using Timer2 for PWM, and you're also using Timer2 for interrupts - thus ensuring that it only works on one of the two cases.

Yeah it works... when I put the dead_reckoning() function in loop...
But I'm afraid of timing could you explain to me the bit about (millis() - previousLoopTime) * 0.001 ?
How can I use it to adjust the sample time for my fuction??

The basic method is like this:

#define SAMPLE_TIME 5 // 5 ms
unsigned long previousTime; // keep track of previous loop time

void loop() {
  unsigned long cTime = millis();
  if (cTime - previousTime >= SAMPLE_TIME) {
    double dt = (cTime - previousTime) * 0.001; // sampling time, in seconds
    previousTime = cTime; // keep track of previous loop time
    // deadreckoning stuff

dt will equal 0.005 in the ideal case (5ms), but if there is an interrupt, or something blocks execution for too long, more than SAMPLE_TIME milliseconds will pass - thus why you do cTime - previousTime, which will tell you exactly how many milliseconds have passed since you last executed the loop contents.