VexCode PID help

kp = 0.0 it should be >0, probably 0.07 or less (much less if you use ki)
while (1) { is commented out in drivePID (also remove extra { )
all varibles for the pid are int but should be double
that will require changing abs to fabs
Add fabs back tofabs(error) < deadBand
usercontrol needs an infinite loop …

vex::task t1(drivePID);

while (1) {
  <code>
  wait(20, msec); // Sleep the task for a short amount of time to allow other task to run
}

you can also add your usercontrol code for driving

    double turnVal = Controller1.Axis1.position(percent);
    double forwardVal = Controller1.Axis3.position(percent);

    double turnVolts = turnVal * 0.12;
    double forwardVolts =
        forwardVal * 0.12 * (1 - (fabs(turnVolts) / 12.0) * turnImportance);
3 Likes

Thanks, this seems to work pretty well. Do you have suggestions on how to tune the values? I have looked up ways, and seen some in tutorials, but was hoping you might have some extra tips.

I don’t have any practical experience in tuning PIDs. I believe most use th Ziegler-Nichols method. For this you need to set your motor’s brake mode to coast so you get overshoot and oscillations.

I can think of other ideas based on P-only. That would be make sure kP is big enough to move the motors at least to the dead band. Determine your how far from your setPos you want to slow down and make sure kP*error is 12 at that point.

However, now that you have it working I have some optimization for you.

add a clamp function so the PID output and Integral output don’t exceed +/-12V
VexCode is using an archaic version of C++ so you need to make your own. Google cpp clamp to see how it is called and match that so when VEX gets around to updating you can just remove your version.

fix integral:

      // Integral
      iTerm += kI * error; // allow live tuning for kI
      // there is accumulative windup so clamp output
      // if error is large for long time the integral can be come
      // very large and overwhelm the output
      iTerm = clamp(iTerm, -12.0, 12.0); 

Fix derivative (which is really delta)

      // Derivative Term
      // delta is de/dt (delta error w.r.t. time) = dsP/dt - dcP/dt
      // if setPosition is not changing dsP/dt = 0      
      // and when sP is changing treat it as if it isn't so it doesn't 
      // cause a large delta in error (i.e. derivative kick)
      // then dcP/dt is (curPos - lastPos)
      delta = -(currentPosition - lastPosition);
      dTerm = kD * delta;

finally clamp the output

      double lateralMotorPower =   clamp( pTerm + iTerm + dTerm, -12.0, 12.0);

notice i’ve changed some of the variables to read more like PID code

3 Likes

Okay, I’ll take a look. Thanks.

Sorry, one other thing I’ve been thinking about while looking into tuning. I’ve done some research, and I think I understand the PID oscillation, but how would PID’s on vex robots happen? I understand like a thermometer overshooting temperature, but what would oscillation look like on a vex robot? Would it just be driving past its target? if so, wouldn’t that be extremely minimal, and if it did overshoot, how would it fix it itself? Would it drive backwards? Sorry again for all the questions I was just thinking how that would work.

Questions are good. You’re making this a good thread to learn PID.

Yes, it would drive farther than necessary and then attempt to drive backwards. Thinking about it, it probably won’t oscillate continuously, just once or twice. Once error becomes negative say setPos=200, curPos=220 then error will be -20. Also you might need a very small deadband for this, maybe 3 to 6 degrees.

BTW, when (if) you add turnPID to the drivePID, the bumpless operation provided by using delta as above can allow you to update the turnValue midway through moving so you can get some nice S curves. I haven’t tried it yet but I’m think about it.

Also, when turning kI you can add a pot or using controller buttons (up/down) so you can change it and rerun the motion. I’d display the value on the screen so you know what it is. Probably doing that for all gain values would make tuning faster.

2 Likes

Okay, thanks, that makes sense. I figured the robot wouldn’t oscillate too much. S curves would be quite interesting, let me know if you do anything cool regarding that. I’ll try the controller for faster tuning, hopefully it helps. Thanks for your help, I’ll see how everything works out. Probably not the end of my questions though lol. Gotta build up the thread :wink:

1 Like

archaic ? can you elaborate ? We are using clang 8 (or perhaps 9, I forget, I think we did an interim build before they released 9), it’s pretty recent. Default build sets C++11, but if you want, and you know what you are doing, just change that.

5 Likes

Well that’s good to know. Okay, archaic might not have been the right word but there have been two major/stable release since 11.

So where do I add -std=c++17 so it is the default for all project using vexcode.

Also. this is what I get for venturing out of PROS land.

2 Likes

So should something be adjusted with the clamp?

Yes, clamp the output of the integral and the overall PID output to the min/max voltage.

Here is a reference for implementing clamp so it matches standard C++17.
https://en.cppreference.com/w/cpp/algorithm/clamp
You can make it solely for type double so the prototype would be

double clamp(double v, double lo, double hi); 

you just need to return the correct value nothing else (i.e. no assert).

1 Like

Ok; just curious, if there was no clamp, what would happen if it exceeded 12 volts?

Integral is based on accumulated error. So it can get very large over time. If it is used it should be clamped. For a drive, its gain should probably be set so it only gets big enough to move the robot when the proportional term is small b/c of closeness to target position. So it may need to be clamped to a much smaller value.

I think spin() clamps but it is good practice to limit the output of the pid to the min/max values the motors can handle.

1 Like

One more thing I remembered (telling someone else). Print values to the screen so you can see how the PID is working. This can give you a better feel of what the intermediate output values are and how many iterations it takes to get to the selected positions.

so add a loop count, which might help with understanding how the integral accumulates. Then display loop count, pTerm, iTerm, and dTerm. If you use very small value of kP then you can see the updates better b/c the movement is slower. (maybe you can write to the controller (but i think that requires 50ms between updates).

int drivePID() {
  int loopCount=0; // not technically needed but may help with debugging if reset is not called

  // rest of local PID variables

  while(1) {

    if (resetDrivePID) {   // was resetDriveSensors
      loopCount = 1;
      resetDrivePID = false;
      iTerm = 0.0;   // if needing hold (like for an arm) then this is the last output value.
      lastPosition = 0.0;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    }

    if (enableDrivePID) {
      Brain.Screen.printAt(10, 150, "LoopC %d", loopCount++);

      // PID CODE
      // display PID terms
      Brain.Screen.printAt(10, 30, "pTerm  %f", pTerm);
      Brain.Screen.printAt(10, 60, "iTerm  %f", iTerm);
      Brain.Screen.printAt(10, 90, "dTerm  %f", dTerm);
      Brain.Screen.printAt(10, 120, "curPos %f", currentPosition);
   
      // PID EXIT IF CLOSE ENOUGH
   } // while(1)
  return 1;
}// int drivePID()
1 Like

Okay so I looked at the clamping and tuning, and that’s coming along. Not perfect yet, but seems to be getting there. My biggest question now is why I can’t run it in auton. I tried testing it in auton to see how it would work, but for some reason, it’s not calling in auton. How would you call it in auton, without using the controller?

I was staring the task in usercontrol which runs after autonomous. So move that to main which run before auton or user.

int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  vex::task t1(drivePID);

...

then pidMoveToPosition(…) should work.

1 Like

I moved that to the autonomous. I put it in the beginning. That actually wasn’t the issue, I should have been more clear. It runs the first “pidmoveto” but I can’t get it to move again after that.

I need more info. I can call it multiple times from usercontrol as follows:

    if (Controller1.ButtonA.pressing()) {
      pidMoveToPosition(720, 0, true);
      pidMoveToPosition(-720, 0, true);
    }

it maybe stuck in a loop.
kI may be too small.

1 Like

It must be stuck in a loop. After it is called once, I can’t do anything else.

Ah. So nothing works, but once I press B, which is to abort PID, I can do stuff again. So that means that it isn’t aborting the PID. How would I make it do that after it runs without pressing B?