I have self-made (with tutorial help) drive and turn PIDs, and while after lots of testing and tweaking and tuning the Turn PID works like a charm, there is an issue with the drive PID.
void DrivePID(float setpoint) {
LF.setRotation(0, degrees);
LM.setRotation(0, degrees);
LB.setRotation(0, degrees);
RF.setRotation(0, degrees);
RM.setRotation(0, degrees);
RB.setRotation(0, degrees);
float error = setpoint * (360/(3.25*pi));
float integral = 0;
float derivative = 0;
float preverror = 0;
float velo = 0;
float prevelo = 0;
float kP = 0.1;
float kI = 0.0;
float kD = 0.0;
while(fabs(error) >= 1) {
error = setpoint * (360/(3.25*pi)) - ((LF.position(degrees) + LM.position(degrees) + LB.position(degrees) + RF.position(degrees) + RM.position(degrees) + RB.position(degrees))/6);
derivative = error - preverror;
preverror = error;
if(fabs(error) <= 10) {
integral += error;
}
else if(error <= 1 && error >= -1){
integral = 0;
}
velo = (error*kP) + (integral*kI) + (derivative*kD);
prevelo = velo;
LF.spin(forward, 12*velo, volt);
LM.spin(forward, 12*velo, volt);
LB.spin(forward, 12*velo, volt);
RF.spin(forward, 12*velo, volt);
RM.spin(forward, 12*velo, volt);
RB.spin(forward, 12*velo, volt);
wait(20, msec);
}
LF.stop();
LM.stop();
LB.stop();
RF.stop();
RM.stop();
RB.stop();
Brain.Screen.print("done");
wait(20, msec);
}
I will ask it to go 24 inches (“DrivePID(24)”) or roughly the length of a tile, and it does not go near there, stopping around roughly 17 inches and doing its little corrections with P. These kP, kI, and kD numbers are obviously not final, as I cannot tune it if it doesn’t work in the first place. I’ve looked at it extensively and can’t find anything wrong, and the degrees-to-inches math should be right as we are using 3.25 inch wheels.