//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);
}
}