PID cmd_drive can't drive straight?

[gnormhurst]

Am I missing something?

I've been thinking about the PID system and scripting system provided in Navigation_FRC2005_01_08, and I think that there is a fundamental problem with the way the PID system is being used by cmd_drive() in robot.c. cmd_drive sets the left and right target positions WAY out in front of the robot, and then just lets the PID system try to get there. The last state of cmd_drive() sets these positions:

    case COMPLETE:
        {
            set_pos(LEFT, counts);
            set_pos(RIGHT, counts);
         [...]
        }

I think that the PID system immediately sees a huge position error and sets both motors to the maximum speed allowed by the MAX_ and MIN_PWM values in pid.h.

With the PWM values limited (clipped), the feedback loop cannot make any changes to the motor's speed. So the "loop gain" has gone to zero. And a loop gain of zero is another way of saying that the feedback loop is open! If there are differences in the left and right motor characterstics, the robot will not go straight, and the PID system is powerless to do anything about it, because its outputs are clipped.

At the very end, when the encoders have nearly reached their targets, the PID system kicks in (becomes linear and therefore closed loop). And the drive distance is indeed accurately met. But this distance is around the arc of a circle, not necessarily on a straight line.

I think that to maintain control, the PID system must never be put in a position where it must clip or limit its values. I have implemented a version of cmd_drive() that does not simply set a target position once and sit back to let the PID system max out, but rather continually increments the target positions a little at a time. (The amount of the increment is the desired velocity.) The PID system "chases" a moving target, and is able to stay linear the whole time.

Here is my sDrive() ("script drive") routine, so you can see how the target position is continually incremented. The calling form is

{ CMD_DRIVE, <distance>, <velocity>, <tolerance> },

Note that this script command does not "COMPLETE" until the target is reached.

unsigned char sDrive(void)
{
  static int state = START;
  unsigned char  rc = 0;

  long int leftToGo, rightToGo;
  long int left,right;
  static long int leftTarget, rightTarget;
  static long int distance;
  static int velocity;
  static long int tol;
  long int leftError, rightError;
  static unsigned char stabilityCount;

  // update PID loops
  //
  pid( LEFT );
  pid( RIGHT );
  
  switch (state)
  {
  case START:
    distance = scriptList[scriptPointer].parm_1;
    velocity = scriptList[scriptPointer].parm_2;
    tol      = scriptList[scriptPointer].parm_3;
    
    stabilityCount = 0;
    
    leftTarget  = get_pos( LEFT  ) + distance;
    rightTarget = get_pos( RIGHT ) + distance;
    
      indent();
      printf("drive <%d> <%d> <%d>: IN_PROGRESS\n",
        (int)distance, (int)velocity, (int)tol );
      state = IN_PROGRESS;
      rc = 0;
      break;

  case IN_PROGRESS:
  
      // increment the PID's desired position
      //
      change_pos( LEFT,  velocity );
      change_pos( RIGHT, velocity );

      // where are we?
      //
      left  = get_pos( LEFT  );
      right = get_pos( RIGHT ); 
      
      // calculate how far we have yet to go
      //
      leftToGo  =  leftTarget - left;
      rightToGo = rightTarget - right;

      // if we are within the amount that we increment each time (velocity)
      // then we are close enough
      //        
      if ( ABS( leftToGo  ) <= ABS( velocity ) 
        || ABS( rightToGo ) <= ABS( velocity ) )
      {
        // force it to be the exact desired position
        //
        set_pos( LEFT, leftTarget );
        set_pos( RIGHT, rightTarget );
        
        // now let the PID system stabilize
        //
        state = STABILIZING;
        indent();
        printf("drive: STABILIZING\n");

      }
      rc = 0;
      break;
      
  case STABILIZING:
      leftError  = leftTarget  - get_pos( LEFT  );
      rightError = rightTarget - get_pos( RIGHT );

      if ( (ABS( leftError  )) < tol  
        && (ABS( rightError )) < tol ) 
      {
          stabilityCount++;
      }  
      else
      {
          stabilityCount = 0;  // need a series all in a row!
      }
        
      if ( stabilityCount > 10 )
      {
        state = COMPLETE;
        indent();
        printf("drive: COMPLETE\n");

      }
      rc = 0;
      break;

  case COMPLETE:
      stopMotors();
      state = START;
      rc = 1;
      break;

  }
  return rc;
}

Without any integration (#define KI_P (0)) the robot turns until enough difference has been accumulated to compensate for the motor differences, and then it will maintain a straight course, but at an angle to the desired path. It doesn't turn back, so the heading will be off.

With some integration (I expect -- I haven't tried it yet) it will turn back to eventually be parallel to the desired path. To get back on the path, I think one would need to additionally integrate the "off path" error and apply it. But that additional integration would require very careful tuning to make it stable. I intend to try this in the coming days.

Comments?

[Don Reid]

Yes, we saw something similar. Our solution was to set the velocity for the startup, is several steps to ramp up the speed, then when the distance is close, switch to position.

It is common for PID implementations to inhibit accumulating an integral of the error when the output is clamped to a limit (high or low). You can put in code like:

         // Only integrate if not driving at a limit
         if ((motor_info[motor].pwm < (MAX_PWM-PWM_ZERO)) &&
             (motor_info[motor].pwm > (MIN_PWM-PWM_ZERO))){
             motor_info[motor].vel_error_i += motor_info[motor].vel_error;
             }

[gnormhurst]

Originally Posted by Don Reid

Yes, we saw something similar. Our solution was to set the velocity for the startup, is several steps to ramp up the speed, then when the distance is close, switch to position.

As long as your outputs never limit, then that solution avoids the "open loop" problem.

Originally Posted by Don Reid

It is common for PID implementations to inhibit accumulating an integral of the error when the output is clamped to a limit (high or low). You can put in code like:

         // Only integrate if not driving at a limit
         if ((motor_info[motor].pwm < (MAX_PWM-PWM_ZERO)) &&
             (motor_info[motor].pwm > (MIN_PWM-PWM_ZERO))){
             motor_info[motor].vel_error_i += motor_info[motor].vel_error;
             }

Interesting idea. It seems, though, that once the outputs are up against the stops (limits), stopping further integration won't necessarily get it back off the stops. And the loop has no control if anything is clipping.

I didn't get any time today to experiment with integration for the PID position mode. I notice that the default code has the integrator turned off (KI_P is zero):

#define KP_P (50)
#define KI_P (0)
#define KD_P (10)
#define DIV_P (50)

I also noticed that he is dividing the (apparently unused) integrator by the "pid_time" (counts since invoking CMD_DRIVE).

    motor_info[motor].pwm = (KP_P * motor_info[motor].pos_error) + 
                           ((KI_P * motor_info[motor].pos_error_i)/pid_time) + 
                            (KD_P * motor_info[motor].pos_error_d);

I guess this was an attempt to stabilize the loop before he gave up and set KI_P to zero.

Has anyone tried using integration with the position mode?

-norm

[Don Reid]

I comented on the use of "/ pid_time" earlier. Someone suggested that this may be a way to deal with wind up, but I think it just makes the itegral correction ineffective after the beginning. It was pointed out that the original code always calls set_pid_stop() which zeros pid_time.

We have removed this dividing by pid_time, and used the conditional integration I posted above. This stops integration while the system is limited. But leaves it working to correct errors later after the system gets close. There will still be
some accumulated integral value that has to be removed by going beyong the
the set point, but much less than otherwise.

[the_undefined]

Hey gnormhurst,

I just found the exact same problem looking at the cmd_drive command, thanks for your solution : ) ...

Bye

Felix