First of all I know there’s a bunch of forum posts and resources on PID loops, but I wasn’t able to find a solution to my problem nor could I solve this problem after many days of trying…so sorry in advance. This is my first time programming a PID so any help would be great.
I am having trouble with calling another function in autonomous after using the PID loop for my drive. Is there a way to turn off and on the PID loop or isolate it into a function that only performs when called. I tried isolating the PID loop in a separate function and while loop but I am still unable to call it again as if it is stuck in the first function.
////////////////////////////////////////////////////////////////////////////
//
// AUTONOMOUS FUNCTIONS
//
////////////////////////////////////////////////////////////////////////////
double kP = 0.2;
double kD = 0.1;
int error;
int totalError = 0;
int prevError = 0;
int drivePower;
int derivative;
bool enableDrivePID = true;
void resetDriveSensors(){
EncoderA.setPosition(0, degrees);
}
void drivePID(int desiredValue){
resetDriveSensors();
while(enableDrivePID){
error = EncoderA.position(degrees) - desiredValue;
totalError += error;
derivative = error - prevError;
drivePower = (kP * error) + (kD * derivative);
LFrontDrive.spin(reverse, drivePower, pct);
LBackDrive.spin(reverse, drivePower, pct);
RFrontDrive.spin(reverse, drivePower, pct);
RBackDrive.spin(reverse, drivePower, pct);
prevError = error;
vex::task::sleep(20);
}
}
void driveStop()
{
LFrontDrive.stop();
LBackDrive.stop();
RFrontDrive.stop();
RBackDrive.stop();
}
int displayEncoder()
{
while(1){
Brain.Screen.setCursor(3, 20);
Brain.Screen.print("%f", EncoderA.position(degrees));
}
return 1;
}
////////////////////////////////////////////////////////////////////////////
//
//
//
//
// AUTONOMOUS
//
//
//
//
////////////////////////////////////////////////////////////////////////////
void autonomous(void) {
vexcodeInit();
vex::task displayEncoderCall(displayEncoder);
drivePID(400);
wait(1, sec);
drivePID(200);
vex::task::sleep(20);
}