Encoder - drive straight PID

VEX V5 General Discussion Technical Discussion
1

//Motor power
float lFwd;
float rFwd;

I used the code from prior to drive straight … but after going forward for sometime … the Rmotors are continuosly running while the left motors stop properly. I’m sure I’m missing something here. Thoughts ?

//PID Variables
float kpL = 0.25;
float kpR = 0.25;
float kiL = 0.00005;
float kiR = 0.00005;
float kdL = 0.1;
float kdR = 0.1;
float errorL = 0.1; //initialization
float previousErrorL;
float integralL;
float derivativeL;
float errorR = 0.1; //initialization
float previousErrorR;
float integralR;
float derivativeR;

void SetDriveMotors()
{
motor[rightbmMotor] = rFwd;
motor[rightfrontMotor] = rFwd;
motor[leftbmMotor] = lFwd;
motor[leftfrontMotor] = lFwd;
}

void ResetSensors()
{
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
}

void drStraight(int target){
ResetSensors();
integralL = 0;
previousErrorL = target;
integralR = 0;
previousErrorR = target;

while(errorL > 0){
errorL = target - SensorValue[leftEncoder];
errorR = target - SensorValue[rightEncoder];
integralL = integralL + errorL;
integralR = integralR + errorR;

if(abs(errorL) < 5 || errorL > 500){
integralL = 0;
}
if(abs(errorR) < 5 || errorR > 500){
integralR = 0;
}

derivativeL = errorL-previousErrorL;
derivativeR = errorR-previousErrorR;
previousErrorL = errorL;
previousErrorR = errorR;

lFwd = kpLerrorL + kiLintegralL + kdLderivativeL;
rFwd = kpRerrorR + kiRintegralR + kdRderivativeR;

if((abs(SensorValue[leftEncoder]) > abs(SensorValue[rightEncoder]+ 5)) && errorL > 50)
{
lFwd = lFwd - (lFwd/abs(lFwd))*10;
}
if((abs(SensorValue[leftEncoder]) < abs(SensorValue[rightEncoder] - 5)) && errorR > 50)
{
rFwd = rFwd - (rFwd/abs(rFwd))*10;
}
SetDriveMotors();
wait1Msec(20);
}
}

2

I believe it could be that in your ‘drStraight’ function the while statement is checking if ‘errorL’ is greater than 0, so once it isn’t, the while statement exits and ‘rFwd’ can no longer be updated. That would mean that right motors are going to be stuck running at whatever the last value was.

3

Trying to put it in a code snippet :


void drStraight(int target)
{
 ResetSensors();
 integralL = 0;
 previousErrorL = target;
 integralR = 0;
 previousErrorR = target;

 while(errorR > 0){
  errorL = target - SensorValue[leftEncoder];
  errorR = target - SensorValue[rightEncoder];
  integralL = integralL + errorL;
  integralR = integralR + errorR;

  if(abs(errorL) < 5 || errorL > 500){
    integralL = 0;
  }
  if(abs(errorR) < 5 || errorR > 500){
    integralR = 0;
  }

  derivativeL = errorL-previousErrorL;
  derivativeR = errorR-previousErrorR;
  previousErrorL = errorL;
  previousErrorR = errorR;

  lFwd = kpL*errorL + kiL*integralL + kdL*derivativeL;
  rFwd = kpR*errorR + kiR*integralR + kdR*derivativeR;

  if((abs(SensorValue[leftEncoder]) > abs(SensorValue[rightEncoder]+ 5)) && errorL > 50)
  {
    lFwd = lFwd - (lFwd/abs(lFwd))*10;
  }
  if((abs(SensorValue[leftEncoder]) < abs(SensorValue[rightEncoder] - 5)) && errorR > 50)
  {
    rFwd = rFwd - (rFwd/abs(rFwd))*10;
  }
  SetDriveMotors();
  wait1Msec(20);
 }
}
4

@Cameron - I guess I can make both motors - 0 after the while loop, but still don’t understand why rightMotors keep spinning.

5

Any hints on this please ?

6

@jpearman Can you please help on why one side keeps spinning in infinite loop ?