PID won’t stop driving

So i have made a PID for my drive train but it never stops driving. I tried only making it go .1 but it still drove across the whole field and i have no idea how to fix it. Please help

You should be a bit more specific by providing units and a more detailed explanation of your problem. Maybe even include your code in this style ––> Preformatted text<––

Anyways, from how it sounds, you’re settling condition and possibly even your error calculations are off. The error might be inverted making the robot think it’s approaching the target when in reality it’s going in the opposite direction. This will cause the robot to move forever in the opposite direction. To fix it, you need to invert the power output sign or invert the error sign. Whatever suits your direction metrics (ex: forward is positive)

double kP = .01;
double kI = 0.001;
double kD = 0.0;
double turnKP = 0.0;
double turnKI = 0.0;
double turnKD = 0.0;

//autonomous settings
int desiredValue = 0;
int desiredTurnValue = 0;

int error;//sensor value - desired value = position
int prevError = 0;// position 20 msec ago
int derivative;//error - preverror = speed
int totalError = 0;// totalError = totalError + error

int turnError;//sensor value - desired value = position
int turnPrevError = 0;// position 20 msec ago
int turnDerivative;//error - preverror = speed
int turnTotalError = 0;// totalError = totalError + error

bool resetDriveSensors = false;

//variables madified for use
bool enableDrivePID = true;

int DrivePID(){
  if(enableDrivePID) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
    //get the position of all motors
    int frontLeftMotorPosition = FLDrive.position(degrees);
    int frontRightMotorPosition = FRDrive.position(degrees);
    int backLeftMotorPosition = BLDrive.position(degrees);
    int backRightMotorPositon = BRDrive.position(degrees);

    //lateral movement PID
    //get the average of the 4 motors
    int averagePosition = (frontLeftMotorPosition + frontRightMotorPosition + backLeftMotorPosition + backRightMotorPositon)/2;

    error = averagePosition - desiredValue;

    derivative = error - prevError;

    totalError += error;

    double lateralMotorpower = (error * kP + derivative * kD + totalError * kI);

    //turning movement PID
    //get the average of the 4 motors
    int turnDifference = frontLeftMotorPosition - frontRightMotorPosition + backLeftMotorPosition - backRightMotorPositon;

    turnError = averagePosition - desiredValue;

    turnDerivative = turnError - turnPrevError;

    turnTotalError += turnError;

    double turnMotorpower = (turnError * turnKP + turnDerivative * turnKD + turnTotalError * turnKI);

    FLDrive.spin(forward, lateralMotorpower + turnMotorpower, voltageUnits::volt);
    FRDrive.spin(forward, lateralMotorpower - turnMotorpower, voltageUnits::volt);
    BLDrive.spin(forward, lateralMotorpower + turnMotorpower, voltageUnits::volt);
    BRDrive.spin(forward, lateralMotorpower - turnMotorpower, voltageUnits::volt);


    prevError = error;
    turnPrevError = turnError;

  return 1;

void autonomous(void) {
  vex::task Drive(DrivePID);

  resetDriveSensors = true;
  desiredValue = 500, true;


I think I see the problem right here. Your error is inverted. Instead you should do desiredValue - averagePosition.

Your code is also very cluttered and redundant at parts. Just a word of advice, try to declutter your code as much as possible. Also, if I’m not mistaken, this code looks like it came from @Connor’s tutorial. I’m not sure what the current behavior of the robot is in comparison to what you want the robot to do. So again, you’re gonna need to be more specific if you want to debug the problem


Shouldn’t averagePosition be sumOfPositions/4 instead of /2?

Also, I think, you wanted to have turnError=turnDifference/2.

Finally, you need to have some some limits for the totalErrors (see integral windup) and an exit condition for breaking out of while() loop.

Take a look at this topic for some useful info:

1 Like