void resetRotations(){
right_back.resetPosition();
rightGroup.resetPosition();
}
void drivePID(double distance, double degrees){
double kP = 0.1;
double kI = 0.01;
double kD = 0.001;
double Err;
int totalErr = 0;
int It = 0;
int pErr = 0;
int dt = 20;
int complete = true;
while(complete){
//Proportional
Err = ((leftGroup.position(deg) + rightGroup.position(deg))/2) - distance;
double P = kP - Err;
//Integral
It += (kI * totalErr * dt);
//Derivtive
double D = kD * (Err - pErr)/dt;
//Moves robot laterally
double output = P + It + D;
leftGroup.spin(fwd, output, pct);
rightGroup.spin(fwd, output, pct);
//prepares to rerun loop
totalErr += Err;
pErr = Err;
//conditional end
if(fabs(Err) < 0.05){
complete = false;
resetRotations();
}
vex::task::sleep(dt);
}
}
PID Controller Explained • PID Explained
The first equation for Derivative is correct, D = kD x (Err – pErr) / dt