I am currently trying to program my first PID Controller. I finished programming all of the components of the controller, but when I run it, it will continuous run without stopping. After putting checks in, the PID controller exits the loop but the motors still run. I then tried to manually stop the motors but that resulted in the motors never starting.
float proportionalControl(float error){
const float K_P = .2;
float proportional = K_P * error;
if(fabs(proportional) > 12){
proportional = 12;
}
return proportional;
}
float integralControl(float buffer){
const float K_I = .2;
float integral = 0;
if(buffer != 0){
integral = K_I * buffer;
}
if(integral > 12){
integral = 12;
}
else if(integral < -12){
integral = -12;
}
return integral;
}
float derivativeControl(float error, float lastError){
return error - lastError;
}
void pidControlForward(float distance) {
//to calculate error, one needs a set point and a process variable
//process variable tells the distance traveled
//SET_POINT tells me the distance i need to travel (is a constant)
//distance will be in inches, so I need to translate it into meters.
leftDrive.resetRotation();
rightDrive.resetRotation();
const float SET_POINT = distance;
float processVariable = 0;
//integral needs a buffer to store all past error.
float buffer = 0;
float error = SET_POINT - processVariable;
const float wheelCircumference = 12.59843;
float lastError = 0;
Brain.Screen.print("Pid Control begins! Inital Error: %f", error);
while(error > 0.01){
Brain.Screen.print("Entered loop!");
if(processVariable / SET_POINT >= .7){
buffer += error;
}
float voltageApplied = proportionalControl(error) + integralControl(buffer) + derivativeControl(error, lastError);
if(voltageApplied > 12){
voltageApplied = 12;
}
else if(voltageApplied < -12){
voltageApplied = -12;
}
leftDrive.spin(fwd, voltageApplied, voltageUnits::volt);
rightDrive.spin(fwd, voltageApplied, voltageUnits::volt);
processVariable += leftDrive.rotation(rev) * wheelCircumference;
lastError = error;
error -= processVariable;
}
Brain.Screen.clearScreen();
Brain.Screen.print("Error: %f, pv: ", error, processVariable);
//Stop motors
}