6 Motor Drive PID - Tower PID

Programming Support VEXcode Pro V5 Text Tech Support
#1

Hey everybody, I just made a few PID’s for my robot and I was wondering if there were any blatant issues with them that I need to fix or if there are some improvements to make. I am running the arm off a PID as well, but if that turns our to be overkill then I may just use waitUntil(potentiometer = whatever);
//Drive PID Function
// Positional Tuners
double latKp = 2;
double rotKp = 2;

//Integral Tuners
double latKi = 0.005;
double rotKi = 0.005;

//Derivative Tuners
double latKd = 0.1;
double rotKd = 0.1;

//Motor Speed Variables
double latSpeed;
double rotSpeed;

//Potential Variables
double latError;
double rotError;

//Previous Potential Variables
double latPrevError = 0;
double rotPrevError = 0;

//Integral Variables
double latIntegral = 0;
double rotIntegral = 0;

//Derivative Variables
double latDerivative;
double rotDerivative;

//Manual Toggle
bool LateralPID_ON = false;
bool RotationalPID_ON = false;

//Reset Rotational Sensors
void resetSensors(){
RightSensor.setPosition(0,degrees);
LeftSensor.setPosition(0,degrees);
}

//Lateral PID
void LateralPID(int distance_cm){
resetSensors();
while (LateralPID_ON){
//Print Error Values
Brain.Screen.printAt(10,50,“Lateral Error %6.2f”, latError);
Brain.Screen.printAt(10,50,“Rotational Error %6.2f”, rotError);

//Get Left and Right Values in cm
double LeftValue = (LeftSensor.position(degrees)/360) * 8.255;
double RightValue = (RightSensor.position(degrees)/360) * 8.255;

//Get Error Values
latError = distance_cm - ((LeftValue - RightValue)/2);
rotError = LeftValue - RightValue;

//Get Integral Values with Boundaries
if ( (sqrt(latError * latError)) < 2 && latError != 0){
  latIntegral = latIntegral + latError;
}
else{
  latIntegral = 0;
}

if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
  rotIntegral = rotIntegral + rotError;
}
else{
  rotIntegral = 0;
}

//Get Derivative Value
latDerivative = latError - latPrevError;
rotDerivative = rotError - rotPrevError;

//Update Previous Error
latPrevError = latError;
rotPrevError = rotError;

//Set Motor Speeds
latSpeed = (latError * latKp) + (latIntegral * latKi) + (latDerivative * latKd);
rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);

//Spin Motor
LeftBottom.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
LeftTop.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
RightBottom.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);
RightTop.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);

//Provide Exist Condition
if (latError < 1 && latError > -1){
  LeftBottom.stop();
  LeftTop.stop();
  RightBottom.stop();
  RightTop.stop();
  break;
}

//wait 20 msec
vex::task::sleep(20);

}
}

void Rotational_PID (int bot_angle){
while (RotationalPID_ON){
//Print Rotational Error
Brain.Screen.printAt(10,50,“Rotational Error %6.2f”, rotError);

//Get Sensor Values
double LeftValue = LeftSensor.position(degrees);
double RightValue = RightSensor.position(degrees);

//Establish Rotational Error
rotError = bot_angle - (LeftValue - RightValue);

//Get Rotational Integral with Boundaries
if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
  rotIntegral = rotIntegral + rotError;
}
else{
  rotIntegral = 0;
}

//Get Rotational Derivative
rotDerivative = rotError - rotPrevError;

//Update Rotational Previous Error
rotPrevError = rotError;

//Set Rotational Motor Speed
rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);

//Spin Motors
LeftBottom.spin(reverse, (rotSpeed), voltageUnits::volt);
LeftTop.spin(reverse, (rotSpeed), voltageUnits::volt);
RightBottom.spin(forward, (rotSpeed), voltageUnits::volt);
RightTop.spin(forward, (rotSpeed), voltageUnits::volt);

//Provide Exist Condition
if (rotError < 1 && rotError > -1){
  LeftBottom.stop();
  LeftTop.stop();
  RightBottom.stop();
  RightTop.stop();
  break;
}

//Wait 20 msec
vex::task::sleep(20);

}

}

//Arm PID Function

//Positional Tuner
double ArmKp = 2;

//Integral Tuner
double ArmKi = 0.005;

//Derivative Tuner
double ArmKd = 0.1;

//Arm Speed
double ArmSpeed;

//Potential Variable
int ArmError;

//Previous Potential Variable
int PrevArmError = 0;

//Integral Variable
int ArmIntegral = 0;

//Derivative Variable
int ArmDerivative;

//Manual Toggle
bool ArmPID_ON = false;

//Arm PID
void ArmPID (int Arm_Angle){
while(ArmPID_ON){
//Print Arm Error
Brain.Screen.printAt(10,50,“Arm Error %6.2f”, ArmError);

//Get Sensor Value
int Arm_Position = TowerSensor.angle(degrees);

//Get Potential Value
ArmError = Arm_Angle - Arm_Position;

//Get Integral Value with Contraints
if (abs(ArmError) < 5 && ArmError != 0 ){
  ArmIntegral = ArmIntegral + ArmError;
}
else{
  ArmIntegral = 0;
}

//Get Derivative Value
ArmDerivative = ArmError - PrevArmError;

//Update Previous Error
PrevArmError = ArmError;

//Set Motor Speed
ArmSpeed = (ArmError * ArmKp) + (ArmIntegral * ArmKi) + (ArmDerivative * ArmKd);

//Spin Arm
Tower.spin(forward, ArmSpeed, voltageUnits::volt);

//Provide Exit Condition
if(ArmError < 1 && ArmError > -1){
  Tower.stop();
  break;
}

//Wait 20 msec
vex::task::sleep(20);

}
}
For reference, lat means lateral, rot means rotational.

1 Like
#2

Please surround your code with
[code]
Code here
[/code]

It is very difficult to read your code how it is right now. Surrounding it with code blocks tells the forum to format the text as a snippet of code.

#3

Oh. Thanks, I didn’t know that. So more like

//Drive PID Function
// Positional Tuners
double latKp = 2;
double rotKp = 2;

//Integral Tuners
double latKi = 0.005;
double rotKi = 0.005;

//Derivative Tuners
double latKd = 0.1;
double rotKd = 0.1;

//Motor Speed Variables
double latSpeed;
double rotSpeed;

//Potential Variables
double latError;
double rotError;

//Previous Potential Variables
double latPrevError = 0;
double rotPrevError = 0;

//Integral Variables
double latIntegral = 0;
double rotIntegral = 0;

//Derivative Variables
double latDerivative;
double rotDerivative;

//Manual Toggle
bool LateralPID_ON = false;
bool RotationalPID_ON = false;

//Reset Rotational Sensors
void resetSensors(){
  RightSensor.setPosition(0,degrees);
  LeftSensor.setPosition(0,degrees);
}

//Lateral PID
void LateralPID(int distance_cm){
  resetSensors();
  while (LateralPID_ON){
    //Print Error Values
    Brain.Screen.printAt(10,50,"Lateral Error %6.2f", latError);
    Brain.Screen.printAt(10,50,"Rotational Error %6.2f", rotError);

    //Get Left and Right Values in cm
    double LeftValue = (LeftSensor.position(degrees)/360) * 8.255;
    double RightValue = (RightSensor.position(degrees)/360) * 8.255;

    //Get Error Values
    latError = distance_cm - ((LeftValue - RightValue)/2);
    rotError = LeftValue - RightValue;

    //Get Integral Values with Boundaries
    if ( (sqrt(latError * latError)) < 2 && latError != 0){
      latIntegral = latIntegral + latError;
    }
    else{
      latIntegral = 0;
    }

    if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
      rotIntegral = rotIntegral + rotError;
    }
    else{
      rotIntegral = 0;
    }

    //Get Derivative Value
    latDerivative = latError - latPrevError;
    rotDerivative = rotError - rotPrevError;

    //Update Previous Error
    latPrevError = latError;
    rotPrevError = rotError;

    //Set Motor Speeds
    latSpeed = (latError * latKp) + (latIntegral * latKi) + (latDerivative * latKd);
    rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);

    //Spin Motor
    LeftBottom.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
    LeftTop.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
    RightBottom.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);
    RightTop.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);

    //Provide Exist Condition
    if (latError < 1 && latError > -1){
      LeftBottom.stop();
      LeftTop.stop();
      RightBottom.stop();
      RightTop.stop();
      break;
    }

    //wait 20 msec
    vex::task::sleep(20);
  }
}

void Rotational_PID (int bot_angle){
  while (RotationalPID_ON){
    //Print Rotational Error
    Brain.Screen.printAt(10,50,"Rotational Error %6.2f", rotError);

    //Get Sensor Values
    double LeftValue = LeftSensor.position(degrees);
    double RightValue = RightSensor.position(degrees);

    //Establish Rotational Error
    rotError = bot_angle - (LeftValue - RightValue);

    //Get Rotational Integral with Boundaries
    if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
      rotIntegral = rotIntegral + rotError;
    }
    else{
      rotIntegral = 0;
    }

    //Get Rotational Derivative
    rotDerivative = rotError - rotPrevError;

    //Update Rotational Previous Error
    rotPrevError = rotError;

    //Set Rotational Motor Speed
    rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);

    //Spin Motors
    LeftBottom.spin(reverse, (rotSpeed), voltageUnits::volt);
    LeftTop.spin(reverse, (rotSpeed), voltageUnits::volt);
    RightBottom.spin(forward, (rotSpeed), voltageUnits::volt);
    RightTop.spin(forward, (rotSpeed), voltageUnits::volt);

    //Provide Exist Condition
    if (rotError < 1 && rotError > -1){
      LeftBottom.stop();
      LeftTop.stop();
      RightBottom.stop();
      RightTop.stop();
      break;
    }

    //Wait 20 msec
    vex::task::sleep(20);
  }

}

//Arm PID Function 

//Positional Tuner
double ArmKp = 2;

//Integral Tuner
double ArmKi = 0.005;

//Derivative Tuner
double ArmKd = 0.1;

//Arm Speed
double ArmSpeed;

//Potential Variable
int ArmError;

//Previous Potential Variable
int PrevArmError = 0;

//Integral Variable
int ArmIntegral = 0;

//Derivative Variable
int ArmDerivative;

//Manual Toggle
bool ArmPID_ON = false;

//Arm PID
void ArmPID (int Arm_Angle){
  while(ArmPID_ON){
    //Print Arm Error
    Brain.Screen.printAt(10,50,"Arm Error %6.2f", ArmError);

    //Get Sensor Value
    int Arm_Position = TowerSensor.angle(degrees);

    //Get Potential Value
    ArmError = Arm_Angle - Arm_Position;

    //Get Integral Value with Contraints
    if (abs(ArmError) < 5 && ArmError != 0 ){
      ArmIntegral = ArmIntegral + ArmError;
    }
    else{
      ArmIntegral = 0;
    }

    //Get Derivative Value
    ArmDerivative = ArmError - PrevArmError;

    //Update Previous Error
    PrevArmError = ArmError;

    //Set Motor Speed
    ArmSpeed = (ArmError * ArmKp) + (ArmIntegral * ArmKi) + (ArmDerivative * ArmKd);

    //Spin Arm
    Tower.spin(forward, ArmSpeed, voltageUnits::volt);

    //Provide Exit Condition
    if(ArmError < 1 && ArmError > -1){
      Tower.stop();
      break;
    }

    //Wait 20 msec
    vex::task::sleep(20);
  }
}

Ignore the tuning values

1 Like
#4

Your code looks good based upon my initial impressions. Is something not working correctly?

By the way, running PID on arm is a really good idea for any arbitrary position, but if you have a hardstop (like for the down position) it is a good idea to just run a regular P loop at that one instance to prevent integral windup against the hardstop. If you don’t use a regular P loop against a hardstop, it will cause the motor to burn out by eventually going 100% down.

1 Like
#5

u forgot a semicolon

#6

Would it be possible to point out where it is?

1 Like
#7

Ok, so for one, I don’t see that semi-colon error. Two, nothing has gone wrong yet as I haven’t tested, but this is my first PID in this particular style of incorporating the desired value into the function. Plus, my previous attempts haven’t gone particularly well. And three, for the p loop on the arm, could I just take the ArmKi and ArmKd values and set them to 0?

#8

Yes, you can do that. Setting a PID constant to 0 will just disable that component.

2 Likes
#9

Yep! That’s all that you need to do. For the down position you change the potentially damaging constants to 0 and then for up you change the constants back.

1 Like
#10

Ok, one last question, when I tune this particular PID, should I be using fairly small desired values or large ones or does it matter?

#11

You should be testing a multitude of desired value cases both small and large. From that you should be monitoring the error and seeing how fast it goes to 0 (exception is the hardstop which should have the desired beyond the hardstop with only kP active)

3 Likes
#12

Hi I’m quite new to PIDs and this one seems to be close to the one I have. I want to know how to integrate the lateral & rotational PIDs with auton and also if this PID is using 6 total motors and if I could replace 2 with shaft encoders so it’s a 4 motor drivetrain

#13

So, the way I do it is lateralPID(whatever value I wanna hit);
since there is a break in the PID, it should move onto the next line of code after it hits the desired value, but you will need to tune it accurately for it to consistently hit the value you want.

#14

And you could replace two of the motors with shaft encoders, but I would recommend making tracking wheels. They should give you more consistent results than a shaft encoder placed in the drive train.

#15

In my experience, if you use brakeMode::hold, you can just use the waitUntil for a fourbar.

#16

Good to know, tuning that was probably gonna take too much time anyways, so thanks for the recommendation!

#17

So, during tuning I found an issue, when I was getting the average of the two sensor values, I subtracted. I fixed it, but if anyone wants to use this code as a template to follow, make sure to change that.

#18

Could I switch the distance from centimeters to inches?

#19

Yes, but that has more to do with the conversion from rotations of the tracking wheel to lateral motion traveled. So, so long as you understand the math for that, you can convert it to whatever you want. I would recommend changing from int to double for the distance though.

1 Like