Go Down

Topic: Using the MsTimer2 library efficiently (Read 3795 times) previous topic - next topic

amine244

May 15, 2011, 11:34 pm Last Edit: May 16, 2011, 02:50 am by amine Reason: 1
Hello,

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 :smiley-eek:
Here's my code...

Code: [Select]
#include <MsTimer2.h>

///////////////////////////////////////////////////////
//////////////////////VARIABLES////////////////////////
///////////////////////////////////////////////////////

// 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

///////////////////////////////////////////////////////
//////////////////////FUNCTIONS////////////////////////
///////////////////////////////////////////////////////

// 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
 while(orient_mean_radian>2*PI){orient_mean_radian-=2*PI;}
}

// 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;
 
 dx=K*delta_d*cos(orient_mean_radian);
 dy=K*delta_d*sin(orient_mean_radian);
 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
 y_cm=(int)(y/COEF_Dist);
 last_orient = orient ; // Refreshing angle value
 last_dist = dist ; // Refreshing distance value
}
// Left encoder Ticks
void incL(){  
 if (digitalRead(encoderPinB[0]) == HIGH){  
    encoderTicks[0]++;
       
 } else {  
    encoderTicks[0]--;
 }  
}  

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

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 (){
 init_dead_reckoning(0,0,0);
 // Serial communication
 Serial.begin(38400);
 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
 MsTimer2::start();
}

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

 analogWrite(speedPinA,0);
 analogWrite(speedPinB,0);

  /* Serial.print(encoderTicks[0]);
   Serial.print("|");
   Serial.print(encoderTicks[1]);
   Serial.print("\tx = ");
   Serial.print(x_cm);
   Serial.print("| y = ");
   Serial.print(y_cm);
   Serial.print("\tTheta = ");
   Serial.println(orient_mean_radian*180/PI);*/
}


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



rbtying

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). 

amine244

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... :smiley-eek-blue:

rbtying

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.

amine244

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

rbtying

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.

amine244

Yeah it works... when I put the dead_reckoning() function in loop...
But I'm afraid of timing problems...so 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??

rbtying

The basic method is like this:

Code: [Select]

#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.

Go Up