PID Spinning Forever

Programming Support VEXcode Pro V5 Text Tech Support
#1

My PID code has just started spinning forever and I have no idea why… I was tuning it at practice earlier today and it was working just fine and I brought the robot home to finish tuning it when all of a sudden it just started driving into walls. I believe it is an issue with the while loop and/or the error calculation, because I had the brain print the error out after 1sec of running and it was the same as the value I put into the function. My PID code is:

(the 1 as kP was just a test value - I set everything else to 0 after the issue started to occur and it still happened)

double kP = 1;
double kI = 0.0;
double kD = 0.0;

double TurnkP = 0.0;
double TurnkI = 0.0;
double TurnkD = 0.0;

float bettererror;      
float previousError = 0;
float derivative;     
float totalError = 0; 

float TurnBettererror;       
float TurnPreviousError = 0; 
float TurnDerivative;     
float TurnTotalError = 0; 

bool resetDriveSensors = false;

bool enableDrivePID = false;
bool enableTurnPID = false;

double desValue;
double desTurnValue;

double latmotorPower = 0;
double averagePos = 0;
int fwdPID(){
  while (enableDrivePID) {

    if (resetDriveSensors) {
      resetDriveSensors = false;

      BL.setPosition(0, deg);
      RM.setPosition(0, deg);
      BR.setPosition(0, deg);

      FR.setPosition(0, deg);
      RM.setPosition(0, deg);
      FL.setPosition(0, deg);
    }

    float BLpos = BL.position(deg);
    float RMpos = RM.position(deg);
    float BRpos = BR.position(deg);
    float FRpos = FR.position(deg);
    float LMpos = LM.position(deg);
    float FLpos = FL.position(deg);

    // START LAT PID
    // _-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-___

    averagePos = (BLpos + BRpos + FLpos + FRpos + LMpos + RMpos) / 6;

    //bettererror = desValue - (((BLpos*(36/60))/360)*(M_1_PI*3.25));
    bettererror = desValue - averagePos*(36/60);

    totalError += bettererror;
    if(bettererror == 0 || averagePos > desValue){
      totalError = 0;
    }
    if(totalError > 1000){
      totalError = 1000;
    }

    derivative = bettererror - previousError;

    latmotorPower = (bettererror * kP + derivative * kD + totalError * kI)/12;

    BR.spin(forward,latmotorPower, voltageUnits::volt);
    RM.spin(forward,latmotorPower, voltageUnits::volt);
    BL.spin(forward,latmotorPower, voltageUnits::volt);
    FR.spin(forward,latmotorPower, voltageUnits::volt);
    LM.spin(forward,latmotorPower, voltageUnits::volt);
    FL.spin(forward,latmotorPower, voltageUnits::volt);

    previousError = bettererror;

    if(std::abs(bettererror) < 0.05){
      enableDrivePID = false;
    }

    vex::task::sleep(10);

    // END LAT PID
    // _-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-___
  
  }
  FL.stop(brakeType::brake);
  FR.stop(brakeType::brake);
  LM.stop(brakeType::brake);
  BR.stop(brakeType::brake);
  BL.stop(brakeType::brake);
  RM.stop(brakeType::brake);
  return 1;
}

int turnPID(){
      while(enableTurnPID){

      if (resetDriveSensors) {
      resetDriveSensors = false;
      Inertia.calibrate();
      Inertia.setRotation(0, deg);
      while(Inertia.isCalibrating()){
        wait(10,msec);
      }

      /*Inertia2.calibrate();
      Inertia2.setRotation(0,deg);
      while(Inertia2.isCalibrating()){
        wait(10, msec);
      }*/
    }
    // START TURN PID
    // _-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-___

    double TurncurPos = Inertia.rotation(deg);

    TurnBettererror = desTurnValue - TurncurPos;
    if(fabs(TurnBettererror) >= 180){
    TurnBettererror = -fabs(TurnBettererror)/TurnBettererror * 360 - fabs(TurnBettererror);
    }

    TurnDerivative = TurnBettererror - TurnPreviousError;

    TurnTotalError += TurnBettererror;
    if(TurnBettererror == 0 || TurncurPos > desTurnValue){
      TurnTotalError = 0;
    }
    if(TurnTotalError > 1000){
      TurnTotalError = 1000;
    }

    double turnMotorPower = (TurnBettererror * TurnkP + TurnDerivative * TurnkD + TurnTotalError * TurnkI) /12.0;

    // END TURN PID
    // _-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-___

    BR.spin(forward,- turnMotorPower, voltageUnits::volt);
    RM.spin(forward,- turnMotorPower, voltageUnits::volt);
    BL.spin(forward, + turnMotorPower, voltageUnits::volt);
    FR.spin(forward, - turnMotorPower, voltageUnits::volt);
    LM.spin(forward, + turnMotorPower, voltageUnits::volt);
    FL.spin(forward, + turnMotorPower, voltageUnits::volt);

    TurnPreviousError = TurnBettererror;

    if(std::abs(TurnBettererror) > 0.1){
      enableTurnPID = false;
    }
    vex::task::sleep(10);
  }
  FL.stop(brakeType::brake);
  FR.stop(brakeType::brake);
  LM.stop(brakeType::brake);
  BR.stop(brakeType::brake);
  BL.stop(brakeType::brake);
  RM.stop(brakeType::brake);
  return 1;
}

and I call the PID in auton like this:

void autonomous(void) {
 
  enableDrivePID = true;
  vex::task drivePID(fwdPID);
  resetDriveSensors = true;

  desValue = 300;
  wait(1000,msec);
  Brain.Screen.print(bettererror);
}

I’m sure it’s just a really small error - it just seemed to happen out of nowhere while I was tuning it - but I can’t seem to figure it out, so I appreciate any help. Thanks!

#2

Have you tried lowering the desired value? Maybe try changing how you’re gathering sensor values and/or calculating errors. Also, I don’t really see the point in including the “motor stop brake type,” since a PID loop with the right tuned constants should stop on its own.

#3

Lowering the desired value slows the drive down, however it still spins indefinitely and the error printed on the brain still reads as the desired value. This to me proves that for some reason error isn’t updating, so the wheels are just spinning at the error*kP. This is super confusing, because as previously said, this code was running completely fine just a few hours ago. Also quick note, the desValue is in units of degrees, so thats why it might seem really big.

You’re right, I just do it so when the while loop ends it stays in place - its not really necessary, but I don’t think it negatively affects the code.

#4

Why do you use float instead of integer(int)? I use integer for my PID and it works fine.

#5

Usually when a PID goes infinity in the wrong direction it means it it going the opposite direction it thinks it should be going. For instance, it could be driving forward because it thinks it needs to go forwards more, but it actually needs to reverse. This means it will keep going forward faster and faster in order to get closer, but it is actually going further away.

Generally this is caused by motors being reversed incorrectly, or the code moving the motors the opposite way the PID thinks it will move it. Try multiplying your latmotorPower variable by -1 to flip it’s sign and see if it works.

Additionally, I would recommend logging the pid values to the terminal or on the brain screen as it is running to see what is going on. If you can see the values of all the different components of the PID, it’s easier to see what is going wrong.

Which variable(s) are you talking about? The OP’s code has a bunch of float variables, which all probably should be floats. Float has higher precision because it has decimal places, while int does not. There’s no reason to reduce the precision of the variables.

#6

I thought this was the case at first but when I slowed down the drive (using a smaller desired value) I could see it was still going forward.

For some reason, I cant print to terminal anymore but I do have the brain print the error after 2-3 seconds and it always is the exact same as the desired value, which probably means there’s an issue with it updating - if the drive spun forever forwards or backwards I would assume I would get at least a negative error or a error bigger than the desValue, but that doesn’t seem to happen. I also tried printing

to the brain and it seems to be updating for that (the averagePos is going really high) so I’m really confused on why the error isn’t changing at all…

#7

After looking over your code again, I think I found your problem

On this line you multiply by 36/60, which because of some weird C++ behavior is actually 0. This is because of integer devision. Basically, if you divide a number without a decimal by another number without a decimal, all of the decimal places of the result are discarded. This turns the 0.6 you should be getting as a result into 0.

You can fix this by telling the compiler that you want a decimal result by making the numbers you are dividing decimals, like this:

bettererror = desValue - averagePos*(36.0/60.0);
2 Likes
#8

That worked, thanks so much! I have another question - when I started testing my turn PID, it didn’t spin at all, so I printed

to the brain, and I saw it was outputting what I thought it would, but when I checked the voltage to the motors, it was at 0. If anyone could give me any tips for that, that would be great. Thanks for all the help!