PID loop not working for rotational movement

Hello! Our team has a competition coming up soon and we are coding a PID loop for our autonomous. I followed this tutorial to get a base code down. I then modified that code for our teams 4-motor drive. I also used the inertial sensor value as the ‘target value’ for the turning portion of the PID. After tuning the constants I was able to get the Lateral movement PID to work. I am now happy with how that portion is working. However, I am struggling to get the turning movement to behave properly.

The turning PID has done several things. It has either speed up, as it turned around and got closer to the target value, or the robot would just spin at a constant speed, or it would move very slowly and eventually stop but it overshoot the target. I have been unable to determine exactly what causes each of these behaviors and it seems semi-random. However, I know the constants have something to do with it.

I try to change the turnKp constant but I have not gotten a satisfactory fix. When I decrease the constant the robot moves very slowly and eventually stops while overshooting the target or i have noticed that when I increase the constant the robot spins continuously. I have double checked my code multiple times and I have been unsuccessful in figuring out what I need to fix.

I attached my code to this post.

//Settings __________________________
double Kp = 0.22;
double Ki = 0.0;
double Kd = 0.2;

double turnKp = 0.2;
double turnKi = 0.0;
double turnKd = 0.0;

int integralBound = 3;
// auton settings 
int desiredValue = 0;
int desiredDistance = 0;
int desiredTurnValue = 0;

int error; //Current sensor value - desired value : Position
int prevError = 0; // Error 20ms ago 
int derivitive; //error - prevError : Speed
int totalError = 0; //total error over time 

int turnError; //Current sensor value - desired value : Position
int turnPrevError = 0; // Error 20ms ago 
int turnDerivitive; //error - prevError : Speed
int turnTotalError = 0; //total error over time 

bool enableSetSensor = true;
bool enableDPID = true; 

int drivetrainPID() {
  Brain.Screen.print("PID ran");
  Brain.Screen.newLine();
  while(enableDPID){
    
   while (enableSetSensor){ //re-setting all of the sensors when enableSetSensor is true
      enableSetSensor = false;
      Brain.Screen.print("set sensor ran");
      Brain.Screen.newLine();
      Inertial.setRotation(0, degrees); 
      RightB.setPosition(0, degrees);
      RightF.setPosition(0, degrees);
      LeftB.setPosition(0, degrees);
      LeftF.setPosition(0, degrees);
    }
    desiredValue = desiredDistance / .0349; //converting from inches to degrees to get the value to input to the motor
    //Brain.Screen.print(desiredValue);
    //Brain.Screen.newLine();
    //getting the values from the internal encoders 
    int RightBPos = RightB.position(degrees);
    int RightFPos = RightF.position(degrees);
    int LeftBPos = LeftB.position(degrees);
    int LeftFPos = LeftF.position(degrees);

//Get average position of the motors

//////////////////////////////////////////////////////////////////////
//Lateral PID
/////////////////////////////////////////////////////////////////////

int averagePosition = (RightBPos + RightFPos +LeftBPos + LeftFPos)/4 ;

//Potential
error = desiredValue - averagePosition;
//Brain.Screen.print(error);
//Brain.Screen.newLine();
//derivative
derivitive = error - prevError;

//Integral
    totalError += error;
  /*
    if (error == 0){
      totalError = 0;
    }

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

double lateralmotorPower = (error*Kp + derivitive*Kd + totalError*Ki)/12;

//////////////////////////////////////////////////
//Turning PID 
////////////////////////////////////////////

int inertialReading = Inertial.rotation(degrees); //setting the variable to the current position in relation to calibration

Brain.Screen.print(desiredTurnValue);
Brain.Screen.newLine();
Brain.Screen.print(turnError);
Brain.Screen.newLine();

//Potential
turnError = desiredTurnValue - inertialReading;

//derivative
turnDerivitive = turnError - turnPrevError;

//Integral
turnTotalError += turnError;

//preventing integral from becoming to high
/*
if (turnerror == 0){
  turntotalError = 0;
}

if(abs(error) < integralBound){
turntotalError+=turnerror; 
}  else {
turntotalError = 0; 
}
*/
double turnMotorPower = (turnError*turnKp + turnDerivitive*turnKd + turnTotalError*turnKi)/12;

//--------------Setting Motor Velocity -------------------//
LeftB.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
LeftF.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
RightB.spin(forward,lateralmotorPower + turnMotorPower, voltageUnits::volt);
RightF.spin(forward, lateralmotorPower + turnMotorPower, voltageUnits::volt);

//Setting the previous errors to the current errors, so the next time the code runs through the prevErrors are 20ms behind
prevError = error; 
turnPrevError = turnError;
vex::task::sleep(20);
  }
  return 1;
  }

During the autonomous section this is what I call:

  vex::task drivePIDtask(drivetrainPID);
  desiredTurnValue = 360;
  vex::task::sleep(1000);

A couple of notes regarding my code:

-I commented out the integral barriers, one because I don’t fully understand how they work, or what they do and I don’t feel an immediate need to use them as integral was not necessary for my drive PID portion to work properly. Therefore I am really using a PD loop.

-I was trying to use the print statements to help me debug the code, but it did not help.

Does anyone have any suggestions on how to fix this?

1 Like

I don’t know why the code copied in funnily. I am going to try and fix it.

Edit: For some reason the formatting got messed up, and I don’t know how to fix it, but the code is all in one chunk now.

Hmm I can’t see anything wrong in it although it is hard to read

Thank you for taking a look. I don’t know how to properly format the code in the forum, if you have any suggestions on how to make the code more readable I would be happy to edit my post to implement that.

[code] ... [/code]

It will not let me edit my previous post, so I will just re-post my code here:

This is the function for the PID:


//Settings __________________________
double Kp = 0.22;
double Ki = 0.0;
double Kd = 0.2;

double turnKp = 0.2;
double turnKi = 0.0;
double turnKd = 0.0;

int integralBound = 3;
// auton settings 
int desiredValue = 0;
int desiredDistance = 0;
int desiredTurnValue = 0;

int error; //Current sensor value - desired value : Position
int prevError = 0; // Error 20ms ago 
int derivitive; //error - prevError : Speed
int totalError = 0; //total error over time 

int turnError; //Current sensor value - desired value : Position
int turnPrevError = 0; // Error 20ms ago 
int turnDerivitive; //error - prevError : Speed
int turnTotalError = 0; //total error over time 

bool enableSetSensor = true;
bool enableDPID = true; 

int drivetrainPID() {
  Brain.Screen.print("PID ran");
  Brain.Screen.newLine();
  while(enableDPID){
    
    while (enableSetSensor){ //re-setting all of the sensors when enableSetSensor is true
      enableSetSensor = false;
      Brain.Screen.print("set sensor ran");
      Brain.Screen.newLine();
      Inertial.setRotation(0, degrees); 
      RightB.setPosition(0, degrees);
      RightF.setPosition(0, degrees);
      LeftB.setPosition(0, degrees);
      LeftF.setPosition(0, degrees);
    }
    desiredValue = desiredDistance / .0349; //converting from inches to degrees to get the value to input to the motor
    //Brain.Screen.print(desiredValue);
    //Brain.Screen.newLine();
    //getting the values from the internal encoders 
    int RightBPos = RightB.position(degrees);
    int RightFPos = RightF.position(degrees);
    int LeftBPos = LeftB.position(degrees);
    int LeftFPos = LeftF.position(degrees);
   
    //Get average position of the motors
    
    //////////////////////////////////////////////////////////////////////
    //Lateral PID
    /////////////////////////////////////////////////////////////////////

    int averagePosition = (RightBPos + RightFPos +LeftBPos + LeftFPos)/4 ;

    //Potential
    error = desiredValue - averagePosition;
    //Brain.Screen.print(error);
    //Brain.Screen.newLine();
    //derivative
    derivitive = error - prevError;

    //Integral
    totalError += error;
  /*
    if (error == 0){
      totalError = 0;
    }

     if(abs(error) < integralBound){
    totalError+=error; 
    }  else {
    totalError = 0; 
    }
    */
    
    double lateralmotorPower = (error*Kp + derivitive*Kd + totalError*Ki)/12;

    //////////////////////////////////////////////////
    //Turning PID 
    ////////////////////////////////////////////

    int inertialReading = Inertial.rotation(degrees); //setting the variable to the current position in relation to calibration
    
    Brain.Screen.print(desiredTurnValue);
    Brain.Screen.newLine();
    Brain.Screen.print(turnError);
    Brain.Screen.newLine();

    //Potential
    turnError = desiredTurnValue - inertialReading;
    
    //derivative
    turnDerivitive = turnError - turnPrevError;

    //Integral
    turnTotalError += turnError;

    //preventing integral from becoming to high
    /*
    if (turnerror == 0){
      turntotalError = 0;
    }

    if(abs(error) < integralBound){
    turntotalError+=turnerror; 
    }  else {
    turntotalError = 0; 
    }
    */
    double turnMotorPower = (turnError*turnKp + turnDerivitive*turnKd + turnTotalError*turnKi)/12;

    //--------------Setting Motor Velocity -------------------//
    LeftB.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
    LeftF.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
    RightB.spin(forward,lateralmotorPower + turnMotorPower, voltageUnits::volt);
    RightF.spin(forward, lateralmotorPower + turnMotorPower, voltageUnits::volt);

    //Setting the previous errors to the current errors, so the next time the code runs through the prevErrors are 20ms behind
    prevError = error; 
    turnPrevError = turnError;
    vex::task::sleep(20);
  }
  return 1;
  }

This is what I call during autonomous:

vex::task drivePIDtask(drivetrainPID);
  
  desiredTurnValue = 360;
  vex::task::sleep(1000);
1 Like

I think it’s easier just to make the pid loop a function instead of a task

To be clear, a task just runs multiple functions in parallel. It is easier to manage a program without threading though, that is true. Unless you’re running multiple separate subsystems or algorithms in the background when your drive PID is running, I see no need for it having its own task.

1 Like

That is good to know. I am trying to modify my code to be similar to what was posted here. When I was modifying it I kept the task. However, I will get rid of it in my code as I will not be running any other algorithms in parallel, and as you mentioned it will be simpler without it.

1 Like

I might have to make a “PID Tutorial” though I really don’t like that word. At least, if enough people care, I’ll make a guide on a generic PID controller so you don’t have to declare 100 variables each time you want something to use PID.

1 Like

That would be very helpful. you seem like a very skilled person when it comes to programming, so that would be wonderful. Thank you!