Not your usual PID Help

Orange_L · January 14, 2024, 7:07am

So our team recently made a PID, but it’s really janky and has a bit of stuff we added in. The 3 main issues right now with it are

When I set the total error = 0, it make the wheels stop abruptly and only happens when I am using the integral term.
When I increase the derivative term, for long distances, the error is less, but for short distances, the error is more. The D term basically scales the error if it increases or decreases. For example, if I want the robot to go forward 12 inches, and then 36 inches, at a kD value of like 0.5, at 12 inch distance, the robot always has steady state error, while at 36 inch distance, it has no steady state error.
When there is only a proportional term, the robot doesnt oscillate; it just stops the moment the code detects the error is 0. Even if I have a high or low kP value, the wheels just instantly stop once it reaches the desired position.

Here’s our code:

double t = 0.00;

const double wheelRad = 5.7/2;
const double wheelCir = wheelRad * 2 * 3.14159;

bool enableDrivePID = true;
bool resetSensors = false;

double inchesTarget = 0;
double rotationTarget = 0;

//lateral
double kP = 0.06; //0.085, 0.4, 0.07, 0.085   
double kI = 0.000; //0.0, 0.001, 0, 0    less than 0.0010
double kD = 0.0; //0.0, 0.35, 0.3, 0.45

double error;
double prevError = 0.0;
double totalError = 0.0;
double derivative;

//rotation -> t to represent variables
double tKP = 0.15;
double tKI = 0.0;
double tKD = 0.0;

double tError = 0;
double tPrevError = 0.0;
double tTotalError = 0.0;
double tDerivative = 0;

bool directionLeft = true;

//motor power
double leftPower = 0;
double rightPower = 0;

timer PIDTimeout = timer();

int drivePID()
{
  while(enableDrivePID)
  {
    if(resetSensors)
    {
      resetSensors = false;
      leftFront.resetPosition();
      leftBack.resetPosition();
      leftTop.resetPosition();
      rightFront.resetPosition();
      rightBack.resetPosition();
      rightTop.resetPosition();
      turnSensor.resetHeading();
      turnSensor.setHeading(1, degrees);
      if(directionLeft) {
        turnSensor.setHeading(359, degrees);
      }
    }

    /*  general */

    // collect sensor values
    double leftDegrees = (leftFront.position(degrees) + leftBack.position(degrees) + leftTop.position(degrees)) / 3;
    double rightDegrees = (rightFront.position(degrees) + rightBack.position(degrees) + rightTop.position(degrees)) / 3;

    double rotationDegrees = turnSensor.heading(degrees);
    

    /*  lateral */
    // calculations for degrees needed to move for lateral
    double lateralDeg = inchesTarget * 360 / wheelCir;

    // lateral position
    double currentPos = (leftDegrees + rightDegrees) / 2;
    if(lateralDeg != 0)
    {
      // proportional
      error =  lateralDeg - currentPos;

      // integral + anti-windup
      totalError += error;

      if(fabs(error/360*wheelCir) < 0.25 || fabs(error/360*wheelCir) > 100) {
        totalError = 0;
      }

      // derivative
      derivative = error - prevError;

    }
    else
    {
      error = 0;
      totalError = 0;
      derivative = 0;
    }

    // power
    double lateralPower = (error * kP) + (totalError * kI) + (derivative * kD);
    

    /*  turning */    

    /*  general */


    if(rotationTarget != 0 && !directionLeft)
    {  
       // proportional
      tError = rotationTarget + 1 - rotationDegrees;

      // integral
      tTotalError += tError;

      // derivative
      tDerivative = tError - tPrevError;

      double rotationPower = (tError * tKP) + (tTotalError * tKI) + (tDerivative * tKD);

      leftPower = lateralPower + rotationPower;
      rightPower = lateralPower - rotationPower;

    }
    else if(rotationTarget != 0 && directionLeft)
    {
      // proportional
      tError = -rotationDegrees + (rotationTarget);

      // integral
      tTotalError += tError;

      // derivative
      tDerivative = tError - tPrevError;

      double rotationPower = (tError * tKP) + (tTotalError * tKI) + (tDerivative * tKD);

      leftPower = lateralPower + rotationPower;
      rightPower = lateralPower - rotationPower;
    } 
    else
    {
      tError = 0;
      tPrevError = 0;
      tTotalError = 0;
      tDerivative = 0;
      leftPower = lateralPower;
      rightPower = lateralPower;
    }

    double friction = 0.94;

    // set motor power
    leftFront.spin(forward, leftPower*friction, volt);
    leftBack.spin(forward, leftPower*friction, volt);
    leftTop.spin(forward, leftPower*friction, volt);
    rightFront.spin(forward, rightPower, volt);
    rightBack.spin(forward, rightPower, volt);
    rightTop.spin(forward, rightPower, volt);

    prevError = error;
    tPrevError = tError;

    wait(10, msec);
  }
  return 1;
}

void runPID(double lateral, double turning, double ti, bool direction)
{

  resetSensors = true;
  directionLeft = direction;
  inchesTarget = lateral;

  if(directionLeft) 
  {
    turning = 360 - turning;
  }
  rotationTarget = turning;
  wait(ti, seconds);

  inchesTarget = 0;
  rotationTarget = 0;
  turnSensor.resetHeading();

}

I’m testing the code in the main function so that I don’t have to keep downloading the code, so I can just change each of the constants. Right now, I’m only testing lateral movement, but maybe the turning movement affects it somehow which is causing the issues? Or maybe I need a threshold somewhere where it limits the top motor speed? (if the error is really large, the robot will start off really fast, and because of issue 3, it also stops really fast)

Lastly, some additional info is we have a timeout so that the PID will stop correcting after a certain amount of time. but we’re trying to make it so the PID will stop once it reaches the desired position and stays there for a certain amount of time(so if a robot bumps us, the PID should recorrect half the time). For the rotation part, since we used heading, we had a bunch of issues with turning left specifically, so we worked out a weird way that fixed it, which is why there’s some weird stuff there. We also have some friction on the right side of our drive, which is why we are multiplying the leftPower by a number less than 1.

Thanks.

ps. ik this is a lot lol

iseau395 · January 15, 2024, 3:37am

tldr of most of below: You might just need to spend more time tuning your PID constants. The method I find most effective is this:

Set all the constants equal to 0 except kP, which is set to some arbitrary number in the ballpark of where it should be
Increase (or decrease if it was set too high initially) kP until the robot starts overshooting and driving backwards
Increase kD until the oscillating stops
Repeat steps 2 and 3 until step 3 doesn’t work anymore, then go back to the last version that worked

Usually kI isn’t necessary for the conditions in vex, it’s usually only necessary if the robot gets affected by some outside unpredictable force which causes long-term error, which doesn’t usually happen.

This is probably because when you were testing this the integral term was providing the majority of the power and p and d weren’t doing a lot, so when it got reset the power went down a lot

If the PID is tuned well enough this effect should be pretty small. Your kD could just be too big compared to your kP. One thing you could also try is to make your kD change based on your target distance, but I don’t recommend doing this unless you have to.

I don’t see anything which could be causing this in your code, your kP is probably just too low. How high is the highest you set it?

wdym by this? If you make a change to any part of the code you’re going to need to redownload

Theoretically it shouldn’t be messing it up but you can test with all the turn PID constants set to 0 to stop it from doing anything

Instead of just waiting a certain amount of time you can make a loop which repeatedly checks if the error is less then a certain threshold (make sure to account for negative error), then stops the loop when it is. If you want it to stay in the threshold for a certain amount of time you can have it increase a counter variable if it’s in the threshold then exit the loop if the counter gets high enough. Here’s some pseudocode:

var counter = 0
while counter < 3 {
    if abs(error) < 0.25 {
        counter += 1
    } else {
        counter = 0
    }

   wait(10 milliseconds)
}

Max_Waldman · January 15, 2024, 3:37am

It looks like you are resetting the total error at the wrong times (you want to only reset it when the absolute value of the error is greater than 0.25 and when the error changes signs), and you are never resetting your turn total error, both of which (especially the first) will cause issues with your I term. I am not sure about your third problem, as your implementation of P looks correct, but for the second problem, make sure that you are resetting all of your errors/total errors when you change the target distance. I hope this helps!

Orange_L · January 18, 2024, 5:41am

Sorry I took so long to respond. But I did test all your solutions and they worked for turning, but sadly still not for forward and backward movement. Turning worked perfectly, and I followed your steps and it works 100% of the time now(thx!), but the lateral movement still stops the moment it reaches the desired position.

I completely simplified everything down in a new code and the entire code is here:

competition Competition;
controller Controller1;
motor leftFront = motor (PORT1, true);
motor leftBack = motor (PORT2, true);
motor leftTop = motor (PORT3);
motor rightFront = motor (PORT8, true);
motor rightBack = motor (PORT9);
motor rightTop = motor (PORT10);

double t = 0.00;
double wheelRad = 5.7/2;
double wheelCir = wheelRad * 2 * 3.14159;
bool enableDrivePID = true;
bool resetSensors = false;
double inchesTarget = 0.0;

//lateral
double kP = 0.1;
double kD = 0.0; 

double error = 0.0;
double prevError = 0.0;
double derivative = 0.0;

double leftPower = 0.0;
double rightPower = 0.0;

timer PIDTimeout = timer();

int drivePID()
{
  while(enableDrivePID)
  {
    if(resetSensors)
    {
      resetSensors = false;
      leftFront.resetPosition();
      leftBack.resetPosition();
      leftTop.resetPosition();
      rightFront.resetPosition();
      rightBack.resetPosition();
      rightTop.resetPosition();
      error = 0.0;
      derivative = 0.0;
    }

    double leftDegrees = (leftFront.position(degrees) + leftBack.position(degrees) + leftTop.position(degrees)) / 3;
    double rightDegrees = (rightFront.position(degrees) + rightBack.position(degrees) + rightTop.position(degrees)) / 3;
    double lateralDeg = inchesTarget*19.5; //*360/wheelCir;
    double currentPos = (leftDegrees + rightDegrees) / 2;

    if(lateralDeg != 0)
    {
      error =  lateralDeg - currentPos;
      derivative = error - prevError;
    }
    else
    {
      error = 0;
      derivative = 0;
    }

    double lateralPower = (error * kP) + (derivative * kD);

    leftPower = lateralPower;
    rightPower = lateralPower;

    leftFront.spin(forward, leftPower, volt);
    leftBack.spin(forward, leftPower, volt);
    leftTop.spin(forward, leftPower, volt);
    rightFront.spin(forward, rightPower, volt);
    rightBack.spin(forward, rightPower, volt);
    rightTop.spin(forward, rightPower, volt);

    prevError = error;
    wait(10, msec);
  }
  return 1;
}

void runPID(double lateral, double turning, double ti, bool direction)
{
  resetSensors = true;
  inchesTarget = lateral;
  wait(ti, seconds);
  inchesTarget = 0;
}

//-----------------------------------------------------------------------------------------------------------------------------------------------------------//
//-----------------------------------------------------------------------------------------------------------------------------------------------------------//
//-----------------------------------------------------------------------------------------------------------------------------------------------------------//

int shownumbers() {
  while(true) {
    Controller1.Screen.clearScreen();
    Controller1.Screen.setCursor(1, 1);
    Controller1.Screen.print("%.3f", kP);
    Controller1.Screen.setCursor(2, 1);
    Controller1.Screen.print("%.3f", kD);
    Controller1.Screen.setCursor(3, 1);
    Controller1.Screen.print("%.3f", error);
    wait(100, msec);
  }
  return 1;
}

void preAutonomous(void) {
  leftFront.setBrake(brakeType::coast);
  leftBack.setBrake(brakeType::coast);
  leftTop.setBrake(brakeType::coast);
  rightFront.setBrake(brakeType::coast);
  rightBack.setBrake(brakeType::coast);
  rightTop.setBrake(brakeType::coast);
}

void autonomous(void) {
  vex::task numbers = vex::task (shownumbers);
  vex::task PIDDrive = vex::task (drivePID);
  //runPID(lateral, turn, timeout, directionLeft);



}

void userControl(void) {
  vex::task numbers = vex::task (shownumbers);
  vex::task PIDDrive = vex::task (drivePID);

  while (true) {

    if(Controller1.ButtonDown.pressing()) {
      runPID(12, 0, 5, false);
    }

    if(Controller1.ButtonB.pressing()) {
      runPID(-36, 0, 5, false);  
    }

    if(Controller1.ButtonLeft.pressing()) {
      kP +=  0.005;
      wait(10, msec);
    }

    if(Controller1.ButtonY.pressing()) {
      kP -= 0.005;
      wait(10, msec);
    }

    if(Controller1.ButtonRight.pressing()) {
      kD += 0.1;
      wait(10, msec);
    }

    if(Controller1.ButtonA.pressing()) {
      kD -= 0.1;
      wait(10, msec);
    }

    wait(20, msec);
  }
}

int main() {
  competition Competition;
  Competition.autonomous(autonomous);
  Competition.drivercontrol(userControl);
  preAutonomous();
  while (true) {
    wait(100, msec);
  }
}

Could you take a look at this because the robot still stops instantly even if I have a high kP?

Max_Waldman · January 18, 2024, 5:12pm

It looks like you are logging some values like error to the controller to debug. Is the error going down like it should? I would also check if the error goes negative, or if it is set to 0 somewhere, which may make your robot abruptly stop. Also, maybe try logging prevErr as well and seeing if it is correct. I hope this helps!

iseau395 · January 18, 2024, 5:49pm

I’m not sure what you mean when you say it stops when it reaches the desired position. Is it driving a fast speed the whole time then instantly going down to 0 speed at the end and coasting, or is it slowing down then stopping at the right point? I’m assuming it’s the first one because the second one doesn’t seem like a problem.

I read through your code a couple times and I can’t see anything obvious which would cause this other then your wait in the runPID function being too short, but I have a couple of ideas of potential problems. What happens to the error variable when it passes 0, does it go negative or do something else? You can also maybe try switching all the motor.position(degrees) functions to motor.rotation(degrees), I think one of them doesn’t output numbers bigger then 360 so that could be causing problems, but I don’t know which one it is.

Other then that I’m out of ideas, you can try to debug by printing a bunch of variables like lateralPower and currentPos on the brain or in the terminal and seeing which ones aren’t what they should be.

Orange_L · January 18, 2024, 8:18pm

I just uploaded the video

Orange_L · January 18, 2024, 8:19pm

The robot drives fast the entire time and when the robot gets to the desired position, the speed goes to 0 instantly and doesn’t coast at all. I have a video but idk how to share it. But your rotation idea may work tho…

iseau395 · January 19, 2024, 5:43pm

It’s braking like that because the robot instantly switches from trying to go fast forward to going fast backwards, which is caused by kP being way too high. There should be a sweet spot between braking hard like that and going super slow, which is where you want kP to be.

Orange_L · January 19, 2024, 11:42pm

But the issue is, is that when I set the kp lower, then for short distances there isn’t enough power to mitigate the steady state error. As I said, I’m trying to make the robot overshoot and oscillate so then I can find the right kd value. But since I’ve tried everything including rotation and it doesn’t work, I really don’t know what to do.

iseau395 · January 20, 2024, 2:39am

There should be a value in between which works, maybe the way you are tuning it with the controller isn’t changing it by a small enough value so you can’t get it precise enough. What could be more productive though then just trying to find the magic number is adding a slew rate to the code, so if the output of the PID changes by too much then you limit it, making the movement a lot smoother

Slew rate:

// Get the sign of a number, returns -1 is the number is negative,
// 1 if positive, and 0 if it's 0
double sign(double number) {
    if (number != 0) {
        return number / fabs(number);
    } else {
        return 0;
    }
}

// ...

double power = // PID output

double powerChange = power - previousPower;

// If the absolute value of the change in power is more then the max...
if (fabs(powerChange) > maxChange) {
    // ...only change it by the max amount, in the same direction as the power change
    power = previousPower + maxChange * sign(powerChange);
}
previousPower = power;

// spin motors

this does add another tunable value (max_change) but it shouldn’t be too hard to tune, just set it to however fast you want the robot to speed up/slow down at the most

1082X · January 31, 2024, 11:52pm

My question is, have you tried setting both kI and kD to zero and tried tuning kP until it oscillates smoothly? My guess is your kP is quite a bit too high, but lowering it makes it weird because you’re still combining it with the other values.

system · January 30, 2025, 11:52pm

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.