Breaking out of PID loop

Programming Support VEXcode Pro V5 Text Tech Support
#1

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);
}
#2

There is variable in your code that exists explicitly for that reason:
Set enableDrivePID = false in driver control or whever you need the code to stop. (Note: Stop, not pause. You will have to re-run the task if you want it to continue again)

#3

What happens with your function is it checks the condition of the while loop finding it to be true, executes the code, then checks the while loop again finding it to be true, and the cycle repeats indefinitely. You want the condition of your while loop to be whether you have reached desiredValue, or a small range of values with desiredValue in the middle. This way, when your robot reaches desiredValue (or within your range), the PID loop will exit.

On a side note, this is not a PID loop, it is a PD loop since you don’t use the I term. If this is just because it is still in development, that’s fine, but if this is intentional, you can take out any references to totalError, as they do nothing.

You also want to change your error by subtracting your encoder value from desiredValue instead of the other way around, or you will have a negative error before you reach the target and a positive error after you pass it with the way you have it now.

6 Likes
#4

yeah i tried following ur video. if i wanted to drive a certain distance and then drive another distance after would i have to pause the PID and resume it each time between functions

#5

oh ok thank you so much!

#6

@dsibal, a lot of PID questions are covered here: VEXCode PID Tutorial
Please, make sure to read that topic if you haven’t done it already.

Also, once you change your PID loop exit condition per @Anomalocaris suggestion and get it working, you may want to look at this thread for more advanced code examples and discussion: Exiting a PID loop

3 Likes