VEXCode PID Tutorial

Programming Support VEXcode Pro V5 Text Tech Support
21

@anon4126930 @mvas
I have tried to implement your code into a PID control loop where the Left Motor tries to stay at the same height as the right one. I also changed the encoder readings to raw data units rather than degrees. Could you please tell me if there’s anything wrong with my code before I start trying to tune my PID constants?

Lift PID function and variables

///Settings
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
int maxIntegral = 300;
int integralBound = 20; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

//Autonomous Settings
int desiredValue = 0;

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

bool resetLiftSensors = false;

//Variables modified for use
bool enableLiftPID = 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 liftPID(){ 
  while(enableLiftPID){
    if (resetLiftSensors) {
      resetLiftSensors = false;
      LiftMotorLeft.setPosition(0,degrees);
      LiftMotorRight.setPosition(0,degrees);
    }

    //Get the position of both motors
    int leftMotorPosition = LiftMotorLeft.position(rotationUnits::raw);
    int rightMotorPosition = LiftMotorRight.position(rotationUnits::raw);

    ///////////////////////////////////////////
    //Lateral movement PID
    /////////////////////////////////////////////////////////////////////

    //Potential
    error = rightMotorPosition - leftMotorPosition;

    //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 + totalError * kI;
    /////////////////////////////////////////////////////////////////////

    LiftMotorLeft.spin(forward, lateralMotorPower, voltageUnits::volt);

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

  }

  return 1;
}

Running the function as a separate task (I put this at the beginning of autonomous):

vex::task liftEquilibrium(liftPID);

Thanks a lot for the guidance (having only started out with cpp about a month ago, this guide and your help is proving to be very useful despite having previous programming knowledge).