PID not exiting loop, working well otherwise

code is here:

int DrivePID(int setpoint){
  LF.setPosition(0, degrees);
  LB.setPosition(0, degrees);
  RF.setPosition(0, degrees);
  RB.setPosition(0, degrees);
  float error = 1;
  float integral = 0;
  float derivative = 0;
  float preverror = 0;
  while (error > 0.05 || error < 0.05){
   //basic values for P I and D
   //create values for them
   error = setpoint - ((LF.position(degrees) + LB.position(degrees) + RF.position(degrees) + RB.position(degrees))/4);
   preverror = error;
   if(200 > error && -200 > error){
     integral += error;
   }
   derivative = error - preverror;
   //regulating values for power
   float kP = 0.05;
   float kI = 0.05;
   float kD = 0.05;
   //integral regulator
   //motor power
   float velo = (error*kP) + (integral*kI) + (derivative*kD);
   if(velo > 1){
     velo = 1;
   }
   if(velo < -1){
     velo = -1;
   }
   //run motors
   LF.spin(forward, 12*velo, volt); //sigma sigma on the wall
   LB.spin(forward, 12*velo, volt);
   RF.spin(forward, 12*velo, volt);
   RB.spin(forward, 12*velo, volt);
  }
  
  if(error <=0.05 && error >= -0.05) {
    LF.stop();
    LB.stop();
    RF.stop();
    RB.stop();
  }
  Brain.Screen.print("done");
  wait(20, msec);
  return 0;
}

int TurnPID(int setpoint){
  LF.setPosition(0, degrees);
  LB.setPosition(0, degrees);
  RF.setPosition(0, degrees);
  RB.setPosition(0, degrees);
  float error = 1;
  float integral = 0;
  float derivative = 0;
  float preverror = 0;
  while (error > 0.1 || error < -0.1){
   //basic values for P I and D
   float initial = Inertial.rotation(degrees);
   //create values for them
   error = setpoint - initial;
   preverror = error;
   if(10 > error && -10 < error){
     integral += error;
   }
   derivative = error - preverror;
   //regulating values for power
   float kP = 0.02;
   float kI = 0.8;
   float kD = 0.1;
   //integral regulator
   //motor power
   float velo = (error*kP) + (integral*kI) + (derivative*kD);
   if(velo > 1){
     velo = 1;
   }
   if(velo < -1){
     velo = -1;
   }
   //run motors 
   LF.spin(forward, -12*velo, volt); //sigma sigma on the wall
   LB.spin(forward, -12*velo, volt);
   RF.spin(forward, 12*velo, volt);
   RB.spin(forward, 12*velo, volt);
  }
  if (error <= 0.05 && error >= -0.05){
    LF.stop();
    LB.stop();
    RF.stop();
    RB.stop();
  }
  Brain.Screen.print("done"); 
  wait(20, msec);
  return 0;
}

It’s on a 4 motor, 4 wheel drive testing bot (not our actual comp bot) and the drivePID hasn’t had tuning yet. When we run this code, the turn works perfectly, but it never goes on to the next thing.
Our auton is
TurnPID(90);
wait(20, msec);
DrivePID(400);
it does the turn perfectly, but never moves on to the drive, and vice versa when you change the order. I’ve had my coach, more experienced programmers from my school, and more look at this and I’ve been thinking about it for hours, I’ve tried every change I can think of.

Also, sometimes it will start spinning and not stop after a slight hesitation after a perfect turn, rather than completely stopping - but it never does exit the loop as it does not print the “done”.

The PID is self-made and self-tuned, though if you are curious I used Ascend and Orange Depot and George Gillard’s descriptions/guides for help in developing it.

1 Like

You probably meant error > 0.05 || error < -0.05.

9 Likes

That’s how it was before, I was tweaking some stuff that i forgot to change back to how i originally had it, but the issue seems to be a different one in the while loop of the turn (and logically the drive one too) however i noticed an issue with the integral thing that i can fix that stops the whole “turn, stop for a second, permanently turn” but even then it still just turns normally and doesnt move on.

I’ll try adding this
else if(error <= 0.1 && error >= -0.1){
integral = 0;
}
as well as a wait(20, msec) INSIDE the while error loop to potentially make it stabler / faster / less overwhelming on the brain
if this fixes the issue and lets it out of the loop (though I doubt it - the issue seems to be about the error, not the integral) i will mark this as the solution.

Still not working, even after a few other tweaks

Your error bound is really too small to be usable in competition. For example, JAR Template by default uses 1 degree for turning and 1 inch for driving. Your error bound is 20 times smaller than that.

3 Likes

A nice shorthand for this is abs(error) <= 0.1 (though you should use a value larger than 0.1).

I wouldn’t recommend using degrees for your position (you should use something like inches instead), since if you ever change your wheel size or gearing, then you will have to rewrite all of your auton distance values (this can be solved with inches by having a gear ratio/wheel diameter constant that you only have to change once). Also, in my opinion it is much easier to write auton code with measurements in inches rather than degrees, because, for me at least, it is easier to get a sense of real life scale with inches so that I can make a rough guess at a distance before fine-tuning it, rather than making a complete guess with degrees.

This will only run when the error is less than -200, which is probably not what you intended. You should change it to something like if (abs(error) < 200) {. Also, (I know you were using degrees but for reference I use around 5 inches for integral cutoff).

Normally, your kP, kI, and kD values should be different: kP being the largest, kD the second, and kI the smallest (for some reason in my code my kD is much larger than my kP but that shouldn’t normally be the case (also in my code my kD is 19000 so maybe it isn’t the best example lol (I think it’s just scaled weirdly though since pros voltage is from -12000 to 12000)), what matters is that kI is the smallest).

This is not strictly necessary, but it can help with integral windup issues if you reset the integral when the error is 0 or passes 0 (what I do to code this is just check if the error and integral are opposite signs, and if so, then reset the integral).

Wow, this enlightening comment really helped me understand your code!

3 Likes

Thanks, I figured this out myself and I fixed it, I made a post here but it didn’t come up for some reason, marking this as solution for others who see this.
P.S. I know the values for kP kI and kD will be different, I just hadn’t tuned it yet on account of not being able to run it right in the first place.

1 Like