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?