code is here:
int DrivePID(int setpoint){
LF.setPosition(0, degrees);
LB.setPosition(0, degrees);
RF.setPosition(0, degrees);
RB.setPosition(0, degrees);
float error = 1;
float integral = 0;
float derivative = 0;
float preverror = 0;
while (error > 0.05 || error < 0.05){
//basic values for P I and D
//create values for them
error = setpoint - ((LF.position(degrees) + LB.position(degrees) + RF.position(degrees) + RB.position(degrees))/4);
preverror = error;
if(200 > error && -200 > error){
integral += error;
}
derivative = error - preverror;
//regulating values for power
float kP = 0.05;
float kI = 0.05;
float kD = 0.05;
//integral regulator
//motor power
float velo = (error*kP) + (integral*kI) + (derivative*kD);
if(velo > 1){
velo = 1;
}
if(velo < -1){
velo = -1;
}
//run motors
LF.spin(forward, 12*velo, volt); //sigma sigma on the wall
LB.spin(forward, 12*velo, volt);
RF.spin(forward, 12*velo, volt);
RB.spin(forward, 12*velo, volt);
}
if(error <=0.05 && error >= -0.05) {
LF.stop();
LB.stop();
RF.stop();
RB.stop();
}
Brain.Screen.print("done");
wait(20, msec);
return 0;
}
int TurnPID(int setpoint){
LF.setPosition(0, degrees);
LB.setPosition(0, degrees);
RF.setPosition(0, degrees);
RB.setPosition(0, degrees);
float error = 1;
float integral = 0;
float derivative = 0;
float preverror = 0;
while (error > 0.1 || error < -0.1){
//basic values for P I and D
float initial = Inertial.rotation(degrees);
//create values for them
error = setpoint - initial;
preverror = error;
if(10 > error && -10 < error){
integral += error;
}
derivative = error - preverror;
//regulating values for power
float kP = 0.02;
float kI = 0.8;
float kD = 0.1;
//integral regulator
//motor power
float velo = (error*kP) + (integral*kI) + (derivative*kD);
if(velo > 1){
velo = 1;
}
if(velo < -1){
velo = -1;
}
//run motors
LF.spin(forward, -12*velo, volt); //sigma sigma on the wall
LB.spin(forward, -12*velo, volt);
RF.spin(forward, 12*velo, volt);
RB.spin(forward, 12*velo, volt);
}
if (error <= 0.05 && error >= -0.05){
LF.stop();
LB.stop();
RF.stop();
RB.stop();
}
Brain.Screen.print("done");
wait(20, msec);
return 0;
}
It’s on a 4 motor, 4 wheel drive testing bot (not our actual comp bot) and the drivePID hasn’t had tuning yet. When we run this code, the turn works perfectly, but it never goes on to the next thing.
Our auton is
TurnPID(90);
wait(20, msec);
DrivePID(400);
it does the turn perfectly, but never moves on to the drive, and vice versa when you change the order. I’ve had my coach, more experienced programmers from my school, and more look at this and I’ve been thinking about it for hours, I’ve tried every change I can think of.
Also, sometimes it will start spinning and not stop after a slight hesitation after a perfect turn, rather than completely stopping - but it never does exit the loop as it does not print the “done”.
The PID is self-made and self-tuned, though if you are curious I used Ascend and Orange Depot and George Gillard’s descriptions/guides for help in developing it.