Zumo Drive Heading PID Controller

I have been working on a drive heading function with PID motor speed controller for my Zumobot. I have had minimal success getting this control well. Has anyone been able to get this to work?

I want the robot to read the current heading and drive in the direction for a specified amount of time.
avgHeading(int) just averages a specified number of heading readings to act as a bit of a filter.

I have a 10 ms delay in the control loop.

Any thoughts or help would be much appreciated.

void driveHeading(int speed, int time)
{
  float k_proportional = 2.5;
  float k_derivative = 30;
  int t_init = millis();
  float init_heading = avgHeading(10);
    Serial.print("Initial heading");
    Serial.print("\t");
    Serial.print(init_heading);
    Serial.println();
  float error = 0;
  float lastError = 0;
  int rightSpeed = speed;
  int leftSpeed = speed;

  while (time >(millis() - t_init))
      {
      error = avgHeading(10)-init_heading;
      int adjSpeed = k_proportional * error + k_derivative * (error - lastError);
      lastError = error;
      rightSpeed +=adjSpeed;
      if (rightSpeed > 400) rightSpeed=400;
      if (rightSpeed < 0) rightSpeed=0;
      motors.setSpeeds(leftSpeed,rightSpeed);
      delay(10);

 
       }
  }

I’ve done something similar with PID and I found the best way to debug it is to log everything to the serial output so you can see what is going on.

I also log to CSV format so I can create graphs in a spread sheet. For example, here is code I’ve used:


//----------------------------------------
//
//----------------------------------------

void resetPID()
{
  pid_total_lticks = 0;
  pid_total_rticks = 0;
  pid_lastErr = 0;
  pid_sumErrs = 0;
  adjustLMotor = adjustRMotor = 0;
  updateMotors();
  clear_ticks();
  pid_time = 0;
  #if CSV_OUTPUT && PID_INFO
  Serial.print("PID Reset: System Bias = ");
  Serial.println(SYSTEM_BIAS);
  Serial.println("Time, Interval, Left Ticks, Right Ticks, Error, Sum Erros, Adjust, Left, Adjust Right");
  #endif
}

//----------------------------------------
//
//----------------------------------------

void driveStraight()
{
  static int16_t lticks = 0, rticks = 0;
  static uint16_t ms = 0;
  int16_t dlticks, drticks, diff;
  int32_t delta;
  uint16_t dms;
  
  get_ticks_since_last( &dlticks, &drticks, &dms);

  lticks += dlticks;
  rticks += drticks;
  pid_total_lticks += dlticks;
  pid_total_rticks += drticks;
  ms += dms;
  pid_time += dms;

  if ( ms > 200 )
  {
    int16_t rdir = rticks < 0 ? -1 : 1;
    int16_t ldir = lticks < 0 ? -1 : 1;

    // make the values positive
    lticks *= ldir;
    rticks *= rdir;

    int16_t bias = (rticks*SYSTEM_BIAS)/10000L;
    diff = ((lticks  - rticks + bias )*100L)/ms;

    // we want the difference to be 0

    // track the integral 
    pid_sumErrs += diff;

    // get the differential
    delta = (int32_t) (diff - pid_lastErr);

    int16_t P = (int16_t) ((Kp*((int32_t)diff) + Ki*((int32_t)pid_sumErrs) + (Kd*delta))/1000L);

    pid_lastErr = diff;

    // a positive error means the left motor is 
    // turning more than the right so adjust 
    // each motor accordingly
    int16_t adjust = (P>>1);
    adjustLMotor -= adjust*ldir;
    adjustRMotor += adjust*rdir;

    // Put a limit on the total adjustment in case PID gets out of control
    constrain( adjustLMotor, -MAX_ADJUSTMENT, MAX_ADJUSTMENT);
    constrain( adjustRMotor, -MAX_ADJUSTMENT, MAX_ADJUSTMENT);

    #if PID_INFO
      #if CSV_OUTPUT
        Serial.print(pid_time); Serial.print(", ");
        Serial.print(ms); Serial.print(", ");
        Serial.print(lticks); Serial.print(", ");
        Serial.print(rticks); Serial.print(", ");
        Serial.print(diff); Serial.print(", ");
        Serial.print(pid_sumErrs); Serial.print(", ");
        Serial.print(adjustLMotor); Serial.print(", ");
        Serial.print(adjustRMotor);
        Serial.println();
    #else
        Serial.print("DIFF = ");
        Serial.print(diff);
        Serial.print(" ERR = ");
        Serial.print(pid_sumErrs);
        Serial.print(" ADJ = (");
        Serial.print(adjustLMotor);
        Serial.print(", ");
        Serial.print(adjustRMotor);
        Serial.println(")");
      #endif
    #endif
    updateMotors();
    lticks = 0;
    rticks = 0;
    ms = 0;
  }
}

Pretty simple.

You can see some of the graphs I was able to produced here : wp.me/p493sy-dQ
Was very helpful for figuring out my bugs.

Even if your PID control is working properly I think there might be an issue with the compass. I’ve only played with the Zumo compass a little but I noticed that the readings are greatly effected by the indoor environment. I would create a sketch that just continually prints the heading and manually move the bot in straight lines to see if the heading stays constant.

BTW, I recommend getting a bluetooth module if you don’t already have one. It’s great not having to tether your laptop to the bot when trying to collect data. You can get them for around $10 on amazon.

While the compass headings are likely to be a problem, your timing functions won’t work as you plan. millis() returns an unsigned long integer and all operations on the output should also involve unsigned long integers.Example: int t_init = millis(); Anything to do with t_init will overflow quickly.

Jim,

Thanks for the help. You should see me trying to collect data without a bluetooth module. I carry the tethered tablet behind the bot. Not ideal at all. I stripped almost all of the serial print code out before posting.

I do see some large large jumps in the compass readings which makes the control difficult. I thought it may have been me move the bot with the tether.

I need to go bluetooth so I know it isn’t the tether causing the issues. I will pick up a Bluetooth adapter. That will save a ton of time because I won’t have to wait for the com port to sync every time I plug in the bot.

I don’t seem to have any issues with the timing function so far. How would you suggest I improve them?

Change t_init to “unsigned long” instead of “int”. The current code will cause a problem after about 30 seconds so you may not have noticed anything yet.

Also I’d think about removing the derivative component from the PID calc unless you feel it is essential. The derivative can exasperate any noise in the readings. I’ve personally haven’t found the derivative useful for any of the PID I’ve done so far with the Zumo.

If you’ve been collecting data then it should tell you what is happening. Sometimes it helps to graph it.

Can you post a typical session?

To check whether the motors are interfering with the compass readings, try suspending the Zumo so that the tracks can spin freely but it can’t move. Collect compass headings for various motor speeds, right alone, left alone, etc. and check for deviations.

Also, perhaps the compass needs to be recalibrated to correct for gain and offset errors. With motors off, check the raw compass readings as you manually rotate the Zumo in a full circle.

What Bluetooth adapter are you using on the Zumu robot? I didn’t see any that had a Type B male plug?

I’m using this one: http://www.amazon.com/gp/product/B0093XAV4U

I added a perfboard platform:

I show how I did this here: http://wp.me/p493sy-be