Go Down

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

#### amine244

##### May 15, 2011, 11:34 pmLast 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
Here's my code...

Code: [Select]
`#include <MsTimer2.h>/////////////////////////////////////////////////////////////////////////////VARIABLES///////////////////////////////////////////////////////////////////////////////// Encoder and interrupt pins and variablesint encoderPinB[2] = {25,22}; // Encoder B pinlong 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 variableslong dist,last_dist ; // dist : distance covered at instant tn ; last_dist : distance covered at instant tn-1long orient, last_orient, orient_init; // orient : current angle at instant tn; last_orient : last angle at instant tn-1float x, y; // (x,y) coordinates in ticksint 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 Conditionsvoid 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 msvoid 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 reckoningvoid 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 INPUTpinMode(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 loopvoid 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
Thank you^^

#### rbtying

#1
##### May 16, 2011, 06:21 am
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

#2
##### May 16, 2011, 08:51 am
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...

#### rbtying

#3
##### May 16, 2011, 09:42 am
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

#4
##### May 16, 2011, 10:33 am
I tried, nothing changed....
And then I tried not using the MsTimer function, the motors worked...
So there's clearly some kind of conflict between Ms timer and the motor commands in the main loop...

#### rbtying

#5
##### May 16, 2011, 03:46 pm
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

#6
##### May 16, 2011, 07:32 pm
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

#7
##### May 16, 2011, 10:10 pm
The basic method is like this:

Code: [Select]
`#define SAMPLE_TIME 5 // 5 msunsigned long previousTime; // keep track of previous loop timevoid 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