How to code PID for vex iq robots

Programming Support
1

Our team has been trying to implement a PID loop for our robots drivertrain for driving straight and turning. I am not sure how in supposed to tune PID constants, and how in supposed to set the power to the motor after the calculations. Can anybody help with this ?

1 Like
2

Hi Trishul, welcome to the forum.

Your best tool on the forum is the search tool. If you stick “pid function in python” into the search bar you will get some decent answers.

Doing the same in Google also gets you results. Adding simple to the search will get you to this:

Here’s a plain-English description of what the P, I, and D in PID control theory stand for:

  • P: the further you are from where you want to be, the harder you should try to get there.
  • I: the longer you haven’t been where you want to be, the harder you should try to get there.
  • D: if you’re quickly getting close to where you want to be, slow down.

In PID control, each of these methods of correction would have its influence be scaled by a parameter that you can tune to optimize the control.

  • Controlling with P alone can lead to oscillations around your set point. D helps counteract that by slowing down as you get close to the set point so you don’t overshoot.
  • Controlling with P alone can lead to equilibrium at some point that is close to but not quite at the set point, without enough of a correcting force to actually hit the set point. I corrects for that.

Lots of people use just P and I since the robots are small, slow and the distances are not that big.

3 Likes
3

I see most people using the voltage units to set the calculated power to the motor with the spin command. Motor.spin( Forward, PID value, voltage units::volts) but this isn’t working in my program. So when I do it with normal pct velocity units, in not sure in what range my p I and d variables have to be.

1 Like
4 
//Settings
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;

double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;


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

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

bool resetDriveSensors = false;

//Variables modified for use
bool enableDrivePID = true;

int desiredValue = 0;
int desiredTurnValue = 0;

int drivePID(){
  while(enableDrivePID){

    if(resetDriveSensors) {
      resetDriveSensors = false;

      LeftDriveSmart.setPosition(0, degrees);
      RightDriveSmart.setPosition(0, degrees);


    }
    //Get the position of both motors
    int leftMotorPosition = LeftDriveSmart.position(degrees);
    int rightMotorPosition = RightDriveSmart.position(degrees);

    //////////////////////////////////////////////////
    //Lateral movement PID
    /////////////////////////////////////////////////
    //Get average of two motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    error = averagePosition - desiredValue;
    
    //Derivative
    derivative = error - prevError;

    // Velocity -> Position -> Absement
    totalError += error;

    double lateralMotorPower = turnError * turnkP + turnDerivative *turnkD + turnTotalError *turnkI;


    //////////////////////////////////////////
    //Turn PID
    /////////////////////////////////////////
    int turnDifference = leftMotorPosition - rightMotorPosition;

    turnError = turnDifference - desiredTurnValue;
    
    //Derivative
    turnDerivative = turnError - turnPrevError;

    // Velocity -> Position -> Absement
    turnTotalError += turnError;

    double turnMotorPower = error * kP + derivative *kD + totalError *kI;

    



    LeftDriveSmart.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightDriveSmart.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    if(error <= 10) {
      
    }
    prevError = error;
    vex::task::sleep(20);

  }
  return 1;
}
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  // Begin project code

  vex::task autonomousDrivePID(drivePID);
  desiredValue = 300;
  desiredTurnValue = 600;

  
}

This is my code that I used from Connors tutorial video. I was confused on how to apply this for my vex iq autonomous. I wanted to have almost like a drive function that I could call that would trigger the PID, then, it would run until it gets really close to the target. But, now it seems like I just have to set values to a variable and stop the motors after a specific amount of time. Any help to change the code would be appreciated.

5

I’m also a little confused on how we are allowed to pass voltage units through the spin function, because in the documentation it says you can use a valid veloctiyUnit or percent.

6

IQ motors do not support direct voltage control

3 Likes
7

So do I have to tune my constants differently because it is a percent ? Should I divide motor power by a value to make the constant values more reasonable to tune ?

1 Like