PID Control Loop

Programming Support VEXcode Pro V5 Text Tech Support
1

Please help point out any issues in my PID control loop because it is not working properly.

//Settings
double KP = 2.0;
double KI = 0.04;
double KD = 0.0;
double turnKP = 2.0;
double turnKI = 0.04;
double turnKD = 0.0;

//Autonomous Settings
int desiredValue = 200;
int desiredturValue = 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;

int drivePID(){

  while(enabledrivePID){
    
   if (resetDriveSensors) {
    resetDriveSensors = false;
    leftfront.setPosition(0,degrees);
    leftback.setPosition(0,degrees);
    rightfront.setPosition(0,degrees);
    rightback.setPosition(0,degrees);


   }

    //Motor positions
    int leftfrontPosition = leftfront.position(degrees);
    int leftbackPosition = leftback.position(degrees);
    int rightfrontPosition = rightfront.position(degrees);
    int rightbackPosition = rightback.position(degrees);
    
    ////////////////////////////////////////////////////
    //Later Movement PID
    ///////////////////////////////////////////////////////////////////
    //Average of the motor positions
    int averagePosition = (leftfrontPosition + leftbackPosition + rightfrontPosition + rightbackPosition)/4;
    
    //Potential
    error = averagePosition + desiredturValue;
    
    //Derivative
    derivative = error - prevError;

    //Integral

    double lateralMotorPower = (error * KP + derivative * KD) / 12.0;

    ////////////////////////////////////////////////////
    //Turning Movement PID
    ///////////////////////////////////////////////////////////////////
    int turnDifference = leftfrontPosition + leftbackPosition - rightfrontPosition + rightbackPosition;
    
    //Potential
    turnerror = turnDifference + desiredValue;
    
    //Derivative
    turnderivative = turnerror - turnprevError;

    //Integral

    double turnMotorPower = (turnerror * turnKP + turnderivative * turnKD) / 360.0;
   /////////////////////////////////////////////////////////////////

  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);
  
  /*leftfront.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
  leftback.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
  rightfront.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
  rightback.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees);*/

    //code
    prevError = error;
    turnprevError = turnerror;
    vex::task::sleep(15);

  }

  return 1;
}
2

What isn’t working about it?

Can you show the code you use to use the PID?

Also,

Your code isn’t set up to turn properly, you need to make one set of motors (either left or right depending on how it is built) to -turn motor power

Error should be average position minus desired position I believe, I don’t have my code in front of me right now though

And, if you can I would suggest using an inertial sensor for your turn PID, because that would be much more accurate.

1 Like
3

Also, comments are // not / /

4

It’s just inaccurate and has an unsettling amount of oscillation. Also how do I use an inertial sensor for my turn PID? I would immensely appreciate it if you informed me

5

Thank you for pointing that put, I hadn’t noticed.

6

You might need to calibrate the PID more to make it more accurate and have less oscillation. There are some topics already created if you need help with PID calibration so you can just use the search bar if you don’t know

Basically do the drive PID but with the inertial sensors rotation instead of average position (I think you might need to change a sign or two in the PD calculations but I could be wrong). Once again there are a couple topics already on the forum about this, so I would suggest doing some research on the forums for more help

3 Likes
7

Sounds good, I highly appreciate your help! Wish me luck at worlds!

8

Good luck, hope to see you there!