Revision history [back]

I think your problem is your delaying 500 millisecond on your port functions. So your code is stuck in the messageCb function and doesn't have a chance to spin until the callback finishing, thus losing sync.

Also your Arduino loop is at 20 hz instead of 50 hz.

I think your problem is your delaying 500 millisecond on your port functions. So your code is stuck in the messageCb function and doesn't have a chance to spin until the callback finishing, thus losing sync.

Also your Arduino loop is at 20 hz instead of 50 hz.

int motorEnabledFlag = 0;  
unsigned long cutoffMillis;

//...
//...


void loop()
{
  nh.spinOnce();

  if(motorEnabledFlag)
  {
     if(millis()  >= cutoffMillis) 
     { //500 milliseconds has passed
       analogWrite(PWM_R, 0);
       analogWrite(PWM_L, 0);
       motorEnabledFlag = 0;
     }   
  }
  delay(50);//Might be more effiecent to just delay 1 and control the frequency
  //loop in the publisher
}

void forwardPort (int rs, int ls)
{
  digitalWrite(DIR_R, LOW);
  digitalWrite(DIR_L, LOW);
  analogWrite(PWM_R, rs);
  analogWrite(PWM_L, ls);
  motorEnabledFlag = 1;
  cutoffMillis = millis() + 500;//Probably should use a ros Time function
  //delay(T1); -- If T1 = 500, this causes a 0.5 second delay where 
  //nothing else happens so you can't run a publisher at 10 hz.  
}

//...
//...

I think your problem is your delaying 500 millisecond on your port functions. So your code is stuck in the messageCb function and doesn't have a chance to spin until the callback finishing, thus losing sync.

Also your Arduino loop is at 20 hz instead of 50 hz.

Updated:

You have to get rid of the 500 millisecond delays (0.5 seconds), to run something at 10,20 or 50 hz. The Uno is single core, so your messageCb has to be executed and finished in < 100 millisecond to run something at 10 hz. So ros can get back to looking for published message. If 0.5 second goes by before ros can start looking for published message it will be unable to keep up at 10 hz.

Since I don't know all your details or exactly what your trying to do, there might be a better way of implementing this. Here's an idea for removing the long delays, there might be some additional bookkeeping needed in you messageCb function.

int motorEnabledFlag = 0;  
unsigned long cutoffMillis;

//...
//...


void loop()
{
  nh.spinOnce();

  if(motorEnabledFlag)
  {
     if(millis()  >= cutoffMillis) 
     { //500 milliseconds has passed
       analogWrite(PWM_R, 0);
       analogWrite(PWM_L, 0);
       motorEnabledFlag = 0;
     }   
  }
  delay(50);//Might be more effiecent to just delay 1 and control the frequency
  //loop in the publisher
}

void forwardPort (int rs, int ls)
{
  digitalWrite(DIR_R, LOW);
  digitalWrite(DIR_L, LOW);
  analogWrite(PWM_R, rs);
  analogWrite(PWM_L, ls);
  motorEnabledFlag = 1;
  cutoffMillis = millis() + 500;//Probably should use a ros Time function
  //delay(T1); -- If T1 = 500, this causes a 0.5 second delay where 
  //nothing else happens so you can't run a publisher at 10 hz.  
}

//...
//...