Velocity Control

VEX Robotics Competition Discussion VRC
1

I am doing a velocity control on my flywheel to increase recovery speed after a shot. How does PID control work? Can somebody explain the basic principles of a PID control, or some other velocity recovery type of code? Should I measure RPM through Encoders or Shaft Encoders? Thanks.

2

The wiki page PID controller - Wikipedia does a pretty good job of explaining the background on Proportional Integral Derivative or PID.

Assuming you mean IME vs Quad encoder, either works, but keep in mind that there is an upper measurement threshold on the Quad encoders which means that you cant mount them on the flywheel axle, you would need to mount it on the motor axle or gear it off. IME’s really work well since there is no fooling around with them all you gotta do is screw them to the motors.

There are a lot of threads on the forums that have example PID and TBH codes, good luck

3

So, PID is reliant on some kind of error (difference between what we want and what we have). For velocity, you need to store the encoder value, and the last encoder value. Also, PID to maintain velocity requires the addition of an experimentally determined motor power to maintain the velocity of the flywheel, since PID at 0 error tends to have 0 (or nearly 0) motor power. I think you can get the integral to do this, but I haven’t figured out how yet.


flyVel = encNow - encLast;
err = target - flyVel;

The first term is the Proportional. This is the error multiplied by an arbitrary constant less than one (way less, most of the time).


prop = err * kP;

The next is the Integral. This is the last to be implemented, so I will come back to it.
Then there is Derivative. This is the change in error between cycles, or the slope of the curve at a point (calculus derivative). It looks like this.


time = nSysTime;
deriv = (err - errLast) / time - timeLast;
errLast = err;
timeLast = time;
D = deriv * kD;

And now, back to the Integral. It is the sum of all of the error since the loop began. If you have taken calculus, this is called the integral because it is the area under the curve of the error. This often needs to be collared or zeroed because it builds very quickly, and can cause a lot of oscillation.


integ += err;
I = integ * kI;

Now it’s time to put it all together.


float kP = .05,
  kI = .001,
  kD = .01,
  P = 0,
  I = 0,
  D = 0,
  constPwr = 80.0,
  flyVel = 0,
  flyErr = 0,
  flyErrLast = 0,
  flyInteg = 0,
  flyTarget = 800,
  mtrPwr = 0;
int integLim = 1500;
//these numbers are arbitrary, you need to tune it
int flyVelTime = 0,
  flyVelTimeLast = 0,
  flyDerivTime = 0,
  flyDerivTimeLast = 0,
  flyEncNow = 0,
  flyEncLast = 0;

task main {
  while(true) {
    flyEncNow = SensorValue[flyEnc];
    flyVelTime = nSysTime;
    flyVel = (flyEncNow - flyEncLast) / (flyVelTime - flyVelTimeLast);
    flyEncLast = flyEncNow;
    flyVelTimeLast = flyVelTime;
    flyErr = flyTarget - flyVel;

    P = flyErr * kP;
    flyInteg += flyErr;
    I = flyInteg * kI;
    flyDerivTime = nSysTime;
    D = ((flyErr - flyErrLast) / (flyDerivTime - flyDerivTimeLast)) * kD;
    
    mtrPwr = P + I + D + C;
    setFly(mtrPwr);
    
     flyVelTimeLast = flyVelTime;
     flyDerivTimeLast = flyDerivTime;
     flyEncLast = flyEnc;
    
    wait1Msec(20);
  }
}

I’m not entirely sure how to tune a loop like this, because a) the motor power constant needs its own tuning (you can just play with that like you’re not using a PID loop for that), and b) we use a really, really weird controller that spans 3 header files, and I really can’t explain it.
To use a PID loop for position, just remove the motor power constant. Tuning that is fairly well documented here, and I think someone made a document and posted a link to it somewhere on the forum explaining how to tune that.

Edit: I realized I forgot a wait1Msec(), so I added it.

4

Thank you very much sir. Ur a good person!

5

TBH is a bit different, and is far easier to tune a flywheel. This is the code in C from a Chief Delphi thread:


e = S-P;                            // calculate the error;
Y += G*e;                           // integrate the output;
if (Y>1) Y=1; else if (Y<0) Y=0;    // clamp the output to 0..+1;
if (signbit(e)!=signbit(d)){        // if zero crossing,
  Y = b = 0.5*(Y+b);                // then Take Back Half
  d = e;}                           // and save the previous error;
...where:

S is the setpoint (target RPM)
P is the process variable (measured RPM)
G is the integral gain (the tuning parameter)
Y is the output command to the motor controller
e is the error
d is the previous error
b is the TBH variable

Basically, you find the error (target - now), then multiply it by a decimal constant, much like PID above. This is then added to another variable, which stores the sum of all iterations of the loop, just like the integral. The code here handles the integral differently from how I personally do, but it is just as valid. This is how I would do it:


float kI = .025, //again, this is arbitrary
  mtrOut = 0,
  flyTarget = 800, //still arbitrary
  flyVel = 0,
  flyErr = 0,
  flyErrLast = 0,
  I = 0,
  TBH = 0;
int flyEnc = 0,
  flyVelTime = 0,
  flyEncLast = 0,
  flyVelTimeLast = 0;


//This is for a 4 motor flywheel, add/remove motors as needed, and change names if you want
void setFly(float pwr) {
  motor[fly1] =
    motor[fly2] =
    motor[fly3] =
    motor[fly4] =
    pwr;
}

task main {
  while(true) {
    flyVelTime = nSysTime;
    flyVel = (flyEnc - flyEncLast) / (flyVelTime - flyVelTimeLast);
    flyErr = flyTarget - flyVel;
    I += flyErr;
    mtrOut = I * kI;
    if(mtrOut > 127) {
      mtrOut = 127;
    }
    else if(mtrOut < 0) {
//Keep the motor power positive, to prevent damaging gears or motors
      mtrOut = 0;
    }
    
    if(sgn(flyErr) != sgn(flyErrLast) {
//If the sign of the error changes, then the error crossed zero
      TBH = (mtrOut + TBH) / 2;
      mtrOut = TBH;
      flyErrLast = flyErr;
//the last error doesn't matter unless the sign is different, so the last error is only stored when necessary
    }
    setFly(mtrOut);
    wait1Msec(20);
  }
}

Also, I realized I didn’t declare the setFly function above. It’s not hard, but make sure you use a float as the input parameter, or change the mtrPwr variable to an int. It should have the same result, since motors can’t take decimal power levels, but I’m not sure if it starts throwing warnings for variables of different types.

I would recommend programming a button to adjust each of these constants by, say, .05 (smaller is better, but not so small that you don’t make any significant changes). This is especially useful for testing, but is also useful during a match, in case it starts behaving weirdly.

6

No problem, I’m happy to help. I’ve learned a lot from people on it as well, and I’m glad that I can give a little bit back. =)

7