PID Competition Help

VEX V5 General Discussion General Forum
1

Atm our bot is using a pid loop that seems to work fine outside of the competition template. However, when we put it into the comp template it drives forward to the requested distance, then suddenly drives backwards forever. Here is the function.

void driveDist(float dist, int time) {
SensorValue(right) = 0;
SensorValue(left) = 0;
SensorValue(gyro) = 0;

float integralDRIVE = 0;
float errorDRIVE = 0;
int integRangeDRIVE = 50;
float prevErrorDRIVE = 0;
float kPDRIVE = 2.17684;
float kIDRIVE = 0.003056;
float kDdrive = 0.4852;
float derivDRIVE = 0;
float speedDRIVE = 0;
float timerDRIVE = 0;

int timeMaxDRIVE = time;
while (1==1) {
	errorDRIVE = dist + (SensorValue(left));
	if (errorDRIVE == 0) {
		integralDRIVE = 0;
	}
	if (abs(errorDRIVE) > integRangeDRIVE) {
		integralDRIVE = 0;
	}
	integralDRIVE += errorDRIVE*kIDRIVE;
	derivDRIVE = errorDRIVE - prevErrorDRIVE;
	speedDRIVE = errorDRIVE*kPDRIVE + integralDRIVE*kIDRIVE + derivDRIVE*kDdrive;
	errorD = errorDRIVE; 
	motor[ChassisL] = speedDRIVE;
	motor[ChassisR] = speedDRIVE;
	motor[ChassisRR] = speedDRIVE;
	motor[ChassisLL] = speedDRIVE;
	motor[ChassisLLL] = speedDRIVE;
	motor[ChassisRRR] = speedDRIVE;
	prevErrorDRIVE = errorDRIVE;
	timerDRIVE += 30;
	if (timerDRIVE >= timeMaxDRIVE) {
		break;
	}
	wait1Msec(20);
}

}
Any help is appreciated as we have a competition the next day!

2

@Entropy170 the reason it drives back is, probably, because you overshoot and when you exit the loop the motors are still set to negative direction (that would be necessary to drive back a little).

The simplest solution is to set all motor power to zero explicitly after you break out of the loop.

Also, if you don’t drive very fast (i.e. all turbos on 4" wheels or faster) you, probably, don’t need I and D components. Simple P+C controller should be enough, where C is a constant base power - the motor power that is slightly less that what you would need to move your robot.

For example, when we need to program our robot for slow moving autonomous we would do


int C = 15;
int maxCycles = 5 * 1000 / 20; // 5 sec
while(--maxCycles)
{
    int errorL = targetPosition - SensorValue(left);
    int errorR = targetPosition - SensorValue(right);
    if( abs(errorL) < 5 && abs(errorR) < 5 ) break;
    motor[LL] = errorL * kP + C * sign(errorL);
    motor[RR] = errorR * kP + C * sign(errorR);
    delay(20);
}
motor[LL]=motor[RR]=0;

“D” component is usually very noisy with small intervals (20 msec) you would need to measure it over longer period of time as you get closer to your destination.

“I” component would be necessary to estimate if you are carrying variable load (or no load). If you don’t have such uncertainty, just pick good value for C instead. For example, pick 15 if your robot would always start moving at 19 but will not move at 16 even with fresh battery.

If you have some time left after you get it to work, you may want to scale output power depending on the battery voltage, otherwise try to do it for the next competition.

3

@technik3k Thank you so much!! We will definitely let you know how it works out! :smiley:

4

That is a fantastic description of “I” control versus adding a constant!