PID Tuning For Zumo

Now that the Zumo bot encoders and motors are in working order it’s time to tune the PID controller for driving in straight lines.

Testing the Encoders

Need to first ensure the encoders are accurate by measuring how many ticks each wheel reports when turned 10 revolutions. Theoretically they should show 12000 ticks each.

Let’s see:

Getting counts of around 12035 to 12045 ticks which means each revolution is about 1204 ticks, give or take one. The left and right wheels also seem very close to each other, to at least 1 or 2 ticks per revolution. I think we can safely say the encoders are functioning as well as can be expected.

No PID Control

With out any PID correction the bot has a severe drift to the left, as this video demonstrates:

The new Bluetooth module makes collecting realtime data from the bot a cinch.

With the data I made a graph of the error between left and right wheels, i.e. the difference in their tick count (which should be zero for a straight line), as well as the total sum of errors over time. The x-axis is in milliseconds.

Zumo - No PID - 1

Clearly there is an imbalance.

The PID Controller

Time to turn on the PID controller and tuned it to get the errors way down.

For reference here is the code:


//----------------------------------------
// Config
//----------------------------------------

#define Kp            300L
#define Ki            600L
#define Kd            0L

//----------------------------------------
// Data
//----------------------------------------

int16_t pid_lastErr;
int16_t pid_sumErrs;
uint16_t pid_time;

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

void resetPID()
{
  pid_lastErr = 0;
  pid_sumErrs = 0;
  adjustLMotor = adjustRMotor = 0;
  updateMotors();
  clear_ticks();
  pid_time = 0;
}

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

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;
  ms += dms;
  pid_time += dms;

  if ( ms > 100 )
  {
    // we assume wheels are turning in the same direction
    int16_t dir = ( lticks < 0 || rticks < 0) ? -1 : 1;

    // make the values positive
    lticks *= dir;
    rticks *= dir;

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

    // 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/2)*dir;

    adjustLMotor -= adjust;
    adjustRMotor += adjust;
    updateMotors();

    lticks = 0;
    rticks = 0;
    ms = 0;
  }
}

And the data is perfect:

Zumo - PID - 2

It looks a mess but the reality is the error never gets above 9 ticks at any time. That’s a maximum difference of 0.75% between left and right tracks.

However this synchronicity did not result in straight driving.

I’ve scratched my head a lot over this. Had the same kind of problem with the last bot. However, in this case, the drift from center line is consistent run after run, indicating there is some inherent bias in the system. Probably an alignment issue I’m guessing, but whatever it is, it’s consistent.

We could counter this drift by scaling up the right wheel’s tick count. This in effect would cause the PID controller favor the left motor with more power.

And adding the biasing code worked out really well. Ended up using a compensation on the right motor of 1.85%.

This is the code modification:


:
#define SYSTEM_BIAS   -185L      // 1.85%
:
:
    int16_t bias = (rticks*SYSTEM_BIAS)/10000L;
    diff = ((lticks  - rticks + bias )*100L)/ms;
:
:

And this was the result:

And the data:

Zumo - PID - Bias - 3

Probably not the end of the story but a good result so far.

About these ads

2 comments

  1. […] also know, again from previous experience, that even when each track is turning the same number of ticks, the bot’s path is not […]

  2. […] One of the simplest tests for systematic errors is to get the bot to drive in a straight line. I’ve already posted about doing this in “PID Tuning for Zumo” […]

Leave a Reply

Please log in using one of these methods to post your comment:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Follow

Get every new post delivered to your Inbox.