PID loop threshold

[Vexellent]

Hi , I had a question on how to use thresholds, and timestops in my PID loop so that I can tune it properly and get an autonomous working

void pre_auton(void) {
// Initializing Robot Configuration.
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
double kP = -0.05;
double kI = -0.0;
double kD = -0.07;
double turnkP = -0.5;
double turnkI = -0.0;
double turnkD = -0.5;
int maxTurnIntegral = 300; // These cap the integrals
int maxIntegral = 300;
int integralBound = 3; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

//Autonomous Settings
int desiredValue = 0;
int desiredTurnValue = 0;

int error; //SensorValue - DesiredValue : Position
int prevError = 0; //Position 20 miliseconds ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error

int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0; //Position 20 miliseconds ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use
bool enableDrivePID = true;

//Pasted from a C++ resource
double signnum_c(double x) {
  if (x > 0.0) return 1.0;
  if (x < 0.0) return -1.0;
  return x;
}

int drivePID(){
  
  while(enableDrivePID){

    if (resetDriveSensors) {
      resetDriveSensors = false;
      Leftfront.setPosition(0,degrees);
      Rightfront.setPosition(0,degrees);
      Leftback.setPosition(0,degrees);
      Rightback.setPosition(0,degrees);
    }

    //Get the position of four motors
    int leftfrontMotorPosition = Leftfront.position(degrees);
    int rightfrontMotorPosition = Rightfront.position(degrees);
    int rightbackMotorPosition = Rightback.position(degrees);
    int leftbackMotorPosition = Leftback.position(degrees);
    ///////////////////////////////////////////
    //Lateral movement PID
    /////////////////////////////////////////////////////////////////////
    //Get average of the four motors
    int averagePosition = (leftfrontMotorPosition + rightfrontMotorPosition + leftbackMotorPosition + rightbackMotorPosition)/4;

    //Potential
    error = averagePosition - desiredValue;

    //Derivative
    derivative = error - prevError;

    //Integral
    if(abs(error) < integralBound){
    totalError+=error; 
    }  else {
    totalError = 0; 
    }
    //totalError += error;

    //This would cap the integral
    totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;

    double lateralMotorPower = error * kP + derivative * kD ;
    /////////////////////////////////////////////////////////////////////


    ///////////////////////////////////////////
    //Turning movement PID
    /////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    int turnDifference = leftfrontMotorPosition && leftbackMotorPosition - rightbackMotorPosition && rightfrontMotorPosition;

    //Potential
    turnError = turnDifference - desiredTurnValue;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    if(abs(error) < integralBound){
    turnTotalError+=turnError; 
    }  else {
    turnTotalError = 0; 
    }
    //turnTotalError += turnError;

    //This would cap the integral
    turnTotalError = abs(turnTotalError) > maxIntegral ? signnum_c(turnTotalError) * maxIntegral : turnTotalError;

     double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
    /////////////////////////////////////////////////////////////////////

    Leftfront.spin(forward, lateralMotorPower + turnMotorPower , voltageUnits::volt);
    Leftback.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    Rightfront.spin(forward, lateralMotorPower - turnMotorPower , voltageUnits::volt);
    Rightback.spin(forward, lateralMotorPower - turnMotorPower , voltageUnits::volt);

    prevError = error;
    turnPrevError = turnError;
    vex::task::sleep(20);

  }

  return 1;
}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

  vex::task billWiTheScienceFi(drivePID);

  resetDriveSensors = true;
  
  
  desiredValue = 1000;
vex::task::sleep(1000);

  resetDriveSensors = true;
  desiredValue = -1000;

}

[AperatureLabs]

So I recommend to my students to create a function (specifically for use in auton) which is in the format

void drivePID(desired val, max speed, timeout, threshold).  //or turnPID(...)

For the timer, I will let someone else handle it, but you would reset a timer when the function was called and check it in your loop to compare it to the arg from the function.

For final threshold…

if (abs(error-derivative)>threshold) {Do da pid;}
else {stop da motors, hit the brakes!!!;}

Hope that helps a bit.