PID Never Stops Moving

Programming Support VEXcode V5 Tech Support
#1

This is the first time I’m programming PID. I’ve run into a problem where my robot will move forwards indefinitely and never stop. I’m trying to implement rotational sensors into it instead of motors, which may be the reason it’s running into problems, but I just wanted some advice. Thank you in advance! Below is the code:

#include "vex.h"

using namespace vex;

competition Competition;

///////////////////////////////////////////////////////////////////////////////////
//PREAUTON CODE
///////////////////////////////////////////////////////////////////////////////////
void pre_auton(void) {
  vexcodeInit();
  DrivetrainInertial.calibrate();
  
  //inertial calibration
  while(DrivetrainInertial.isCalibrating()) {
    wait(2500, msec);
  }
  //rotational calibration
  trackingLeft.setPosition(0, degrees);
  trackingLeft.setPosition(0, degrees);
}
///////////////////////////////////////////////////////////////////////////////////

//Lateral PID settings
double kP = 0.1; //Potential (Pid)
double kI = 0.0; //Integral (pId)
double kD = 0.0; //Derivative (piD)

//Turning PID settings
double turnkP = 0.065; //Potential turn
double turnkI = 0.1; //Integral turn
double turnkD = 0.5; //Derivative turn

int maxTurnIntegral = 300; //These cap the integrals
int maxIntegral = 300;
int integralBound = 3;

//Autonomous Settings
int desiredValue = 100;
int desiredTurnValue = 0;

int error; //SensorValue - DesiredValue : Position -> speed -> acceleration
int prevError = 0; //Position 20 ms ago
int derivative; //error - prevError : speed
int totalError = 0; //totalError = totalError + error

int turnError; //SensorValue - DesiredValue : Position -> speed -> acceleration
int turnPrevError = 0; //Position 20 ms ago
int turnDerivative; //error - prevError : speed
int turnTotalError = 0; //totalError = totalError + error

bool resetDrivetrainsensors = false;

//Variables modifed for use
bool enableDrivePID = true;

double signum_c(double x){
  if (x > 0.0) return 1.0;
  if (x < 0.0) return -1.0;
  return x;
}

int drivePID(){
  //resent sensors
  if(resetDrivetrainsensors) {
    resetDrivetrainsensors = false;
    trackingLeft.resetPosition();
    trackingRight.resetPosition();
    //leftMotorA.setPosition(0, degrees);
    //rightMotorA.setPosition(0, degrees);
  }

  while(enableDrivePID){

  //Get positions of both motors
  //int leftTrackingPosition = leftMotorA.position(degrees);
  //int rightTrackingPosition = rightMotorA.position(degrees);
  double leftTrackingPosition = trackingLeft.position(turns);
  double rightTrackingPosition = trackingRight.position(turns);

  //////////////////////////////////////////////////////////////////////////////////
  //LATERAL PID
  //////////////////////////////////////////////////////////////////////////////////
  //Get the average position of both motors
  int averagePosition = (leftTrackingPosition + rightTrackingPosition)/2;

  //Potential: fix is proportionate to the error//
  error = desiredValue - averagePosition;

  //Derivative: controls speed of motors to match each other//
  derivative = error - prevError;

  //Integral// Velocity -> Position -> Absement (position * time)
  if(abs(error) < integralBound) {
    totalError += error;
  } else {
    totalError = 0;
  }

  //this caps the integral
  totalError = abs(totalError) > maxIntegral ? signum_c(totalError) * maxIntegral : totalError;

  double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
  //////////////////////////////////////////////////////////////////////////////////

  

  //////////////////////////////////////////////////////////////////////////////////
  //TURNING PID
  //////////////////////////////////////////////////////////////////////////////////
  int turnDifference = leftTrackingPosition - rightTrackingPosition;

  //Potential: fix is proportionate to the error//
  turnError = desiredTurnValue - turnDifference;

  //Derivative: controls speed of motors to match each other//
  turnDerivative = turnError - turnPrevError;

  //Integral// Velocity -> Position -> Absement (position * time)
  if (abs(error) < integralBound) {
    turnTotalError += turnError;
  } else {
    turnTotalError = 0;
  }

  //this caps integral
  turnTotalError = abs(turnTotalError) > maxIntegral ? signum_c(turnTotalError) * maxIntegral : turnTotalError;

  double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
  ///////////////////////////////////////////////////////////////////////////////////


  //trackingLeft.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
  //trackingRight.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
  LeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
  RightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
  
  prevError = error;
  turnPrevError = turnError;

  //prints values on brain
  Brain.Screen.print(desiredValue);
  Brain.Screen.newLine();
  Brain.Screen.print(leftTrackingPosition);
  Brain.Screen.newLine();
  Brain.Screen.print(rightTrackingPosition);
  Brain.Screen.newLine();
  Brain.Screen.print(error);
  Brain.Screen.newLine();
  Brain.Screen.print(derivative);
  Brain.Screen.newLine();
  Brain.Screen.print(lateralMotorPower);
  Brain.Screen.newLine();
  Brain.Screen.print(turnMotorPower);
  Brain.Screen.newLine();
  Brain.Screen.newLine();

  vex::task::sleep(20);
  }

return 1;

}
///////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////
//AUTONOMOUS CODE
///////////////////////////////////////////////////////////////////////////////////
void autonomous(void) {
  vex::task mainFunction(drivePID);

  resetDrivetrainsensors = true;
  desiredValue = 100;
  desiredTurnValue = 100;
  vex::task::sleep(100);
}
///////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////
//USER CONTROL CODE
///////////////////////////////////////////////////////////////////////////////////
void usercontrol(void) {
  while (1) {

    wait(20, msec);
  }
}

//
// main will set up the competition functions and callbacks
//
int main() {
  // set up callbacks for autonomous and driver control periods
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // run the pre-autonomous function
  pre_auton();

  // prevent main from exiting with an infinite loop
  while (true) {
    wait(100, msec);
  }
}
///////////////////////////////////////////////////////////////////////////////////
#2

I recommend you simplify your PID and turn it into a function like this.

2 Likes
#3

Thanks! You explained in the thread that you would need to make a separate PID for driving a distance. Would I be able to create that code based on the one given?

#4

Yes, it is ideal that you have separate PID for turning and driving straight. It will work the same but you will need to adjust the kp, ki, and kd. You can use the search bar to find some methods to tune PIDs

3 Likes
#5

This is also where the concept of “Don’t Repeat Yourself” in coding comes into play. Rather than literally writing 2 PID functions that are copy-pasta except for the kp, ki, and kd terms, write one function or class that implements the PID calculation but can be parameterized to handle different tuning values.

1 Like
#6

The 2 PID functions would be more different than just different kp, ki, and kd values. A PID drive function would use the internal motor encoder (or a rotation sensor) and control both sides of the drivebase equally. A PID turn function would use an IMU (gyro sensor) and control the difference in speed between both sides of the drivebase.

#7

Those are examples of where the PID gets the value to compare against the target. You are right in that they should also be parameterized.

This is a trite example that is somewhat meaningless, but which is better organized:

int doubleIMUReading() {
  return 2 * imu.heading();
}

int doubleLeftRotationSensor() {
  return 2 * rotationLeft.value();
}

int doubleRightRotationSensor() {
  return 2 * rotationRight.value();
}

Versus:

int doubleValue(int v) {
  return 2 * v;
}

// ....
doubleValue(imu.heading());
doubleValue(rotationLeft.value());
doubleValue(rotationRight.value());

The second example demonstrates (inartfully) how one would “Don’t Repeat Yourself” as well as “Separation of Concerns”. The act of “getting a value” is separate from the act of “doubling the value” in this case, wheres in the first example, you have 3 separate functions that both get a value and then double it.

1 Like