So, my team and I have been trying to figure out PID on our shooter. We are using the built-in PID subsystem in command-based Java. Our code spins the shooter and compensates for the strain on the shooting wheel slightly, but we had to jack up our PID Inputs. I feel like we’re doing something wrong. If anyone knows anything your help is greatly appreciated.
package org.usfirst.frc.team3039.robot.subsystems;
import org.usfirst.frc.team3039.robot.RobotMap;
import com.ctre.CANTalon;
import com.ctre.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class ShooterPID extends PIDSubsystem {
public CANTalon Shooter = new CANTalon(RobotMap.shootMotor);
public double shootVoltage;
public double setpoint;
public ShooterPID() {
super("PIDControl", 150250, 10500, 5);
setPercentTolerance(0.001);
getPIDController().setContinuous(false);
Shooter.setFeedbackDevice(FeedbackDevice.QuadEncoder);
setInputRange(.7, .97);
setOutputRange(.61, .8);
setSetpoint(setpoint);
}
public void enablePID() {
enable();
rampUp();
}
public void disablePID() {
disable();
Shooter.pidWrite(0);
}
public void rampUp() {
for(setpoint = 0; setpoint < 25500; setpoint += 10000) {
System.out.println("Setpoint " + setpoint);
}
}
public void initDefaultCommand() {
}
protected double returnPIDInput() {
return Shooter.getEncVelocity();
}
protected void usePIDOutput(double output) {
Shooter.pidWrite(output);
}
}
PID will generally respond more slowly than a direct approach. With PID, it is essential that you either have a significant I term or a feed-forward term. For a shooter (which I understand to mean a flywheel based shooter, please let me know if this is not what you mean), you would probably be better off with “bang-bang”, that is, running the motor at 100% when it’s below speed, and 0% when it’s above speed. (DO NOT use brake mode with bang-bang!)
Another system you may want to look at is “Take Back Half”. I haven’t used it, but it sounds promising. Ether recommended these resources about a year and a half ago:
Edit/Addition: Another system which I have thought about but never implemented is an adjustment on bang-bang. What you need to do is figure out a percentage (or perhaps absolute voltage) which you know will always be a bit less than the speed you want - this voltage/percentage is calculated dynamically based on the desired speed, and is likely a linear function of speed. Then, do bang-bang, but instead of zero, use the “lower bound” speed I described above. This should reduce the number of speed switches required to hold speed in a spinning but not shooting condition, but will spin up as rapidly as possible from a cold start or after inertia is given to the launched game pieces.
Mind giving some details about your setup? What encoders are you using, where are they on the shooter, what gearing/motor (basically what encoder and how fast will they spin).
So first of all, those are obviously some incredibly high PID values. That honestly shouldn’t even work, especially with that input range.
First thing you should do is calculate the max speed of your shooter and scale your input so that the input range (and your setpoints) are (-1, 1) for max reverse and max forwards speed. That’s purely optional, but it makes it a bit easier to play with PID constants.
Second, output range should also be (-1, 1), since you’re controlling the Talon with PercentVBus (which is -1, 1).
What’s currently happening is, because your output range is .61 to .8, your motor will always be running at a duty cycle of .61 (61%) minimum and .8 maximum. That, coupled with the high PI coefficients, means you’re essentially just running your motor in open loop mode.
You should probably throw out the existing PID constants and start from scratch.
I’d strongly recommend using a PIDF loop instead of a PID loop. F in PIDF stands for feedforward, which is an open-loop (doesn’t have anything to do with the error) term that simply adds to the output a number proportional to the setpoint. It’s generally used in velocity control systems, like drivetrains and shooters. F is used so that the output is “good enough,” then the PID terms are used to close the gap between the open-loop (F-only) speed and the target speed.
In this case, since a setpoint of 1 is max speed (if you scale your inputs from -1 to 1) , and to make a Talon output max power you give it a setpoint of 1, Kf should simply be able to be 1.
From there, set your P, I, and D values to zero, and graph the shooter speed in response to different setpoints. That’ll give you a good idea of how much you’re going to have to adjust your PID values (spoiler: probably not too much). Chances are, you’ll only have to adjust P and D by a bit to provide appropriate disturbance rejection. If you use an I term, be sure to limit/cap it to a relatively small value (setpoint/10?), otherwise it’ll become problematically large when the system is accelerating and hasn’t yet reached the setpoint.
One more thing — in your rampUp() loop, that actually won’t ramp anything up. It’ll simply add 10,000 to your setpoint until it’s 30,000 (not 25,500), and probably complete it . If you actually want the setpoint to ramp up, spawn off a thread or something that will slowly ramp it up (if you do it in the main thread, it’ll block all other threads until complete, which is a Bad Thing™).
Out of curiosity, why not just use the built-in Talon PID(F)? It’s much faster than you’re going to get on the Rio.
If I got anything wrong in there, feel free to call me an idiot! I’m no control systems engineer.
What are your velocity units, and have you scaled the encoder any to make them realistic? From looking at your ramp function, your velocities look too high. Your values may actually be quite realistic if your velocity is weird. Try spinning at a known speed (1 revolution/second) and seeing what you measure. I like to work in radians/second, others like rpm.
In a nutshell when tuning the kp, ki, and kd values you start with kp and set the ki and kd values to zero. The largest part of correction should come from the P-term
Once you get the kp into the ballpark then work with kp and ki while kd is zero. The second largest part of the correction should be the I-term
Once kp and ki are ball parked then work with kd. If at any point you feel that any value needs a tweak be sure all of the lesser terms are at zero.
The smallest part of the correction should be the D-Term.
Example:
Only attempt to set kp while ki and kd are zero
Only attempt to set ki after kp is in in the ballpark and kd is zero
Only after kp and ki are close then attempt to set kd to non-zero.
This is a good starting point, but there’s a few things that I think are missing or wrong:
I think the correct order should be in most cases, P, then D, then I. Often PD is enough - steady state error that is consistent and repeatable in FRC isn’t really a massive problem. In any case, it’s often a lot of P, a healthy amount of D, and a tiiiny amount of I that makes loops work - doing I first just means you’ll retune I later…
This leaves out feed-forward, which is pretty essential on a velocity controller and generally what you want to do first. This lets the PID loop focus on just correcting error and not bringing power up to speed in the first place. First tune feed forward on a full battery to just barely have enough power to dead-reckon a single ball into a goal at full charge with no other mechanism running.
For both of these I like looking at the CTRE guide which has a helpful few sentences on this.
This procedure is appropriate for staying on a line, driving to a target, or moving an actuator to a position, but it is not appropriate for controlling speed. No matter how big kp is, if ki and kd are zero (and you don’t have a feed forward), you will never be able to sustain the desired speed: when you are at the desired speed, the motor drive would be cut to zero!
When running a sufficiently-fast control loop (such as the kilohertz loop on the Talon SRX), steady state error can be made acceptably small with sufficiently-large P gains in combination with feedforward. None of our control loops last year used any integral gain at all.
I was skeptical of this, too, before we tried it and it worked.
Fair enough, however velocity control without feedforward is a bad idea in general even if you’re tuning a slower loop to be driven primarily by integral gain. It can “work” in a technical sense of the word, but it will be sluggish and possibly hampered by windup.
Agreed, feed-forward is the proper way to deal with this. Using the I term for this is a huge bandaid, when feed forward is what you actually want the motor to be doing (a constant voltage when at zero error to try and maintain zero error as much as you can). Using the I gain for this purpose is sorta hacky.
Sure, but, honestly speaking, I’d much prefer to be driving a little bit slower than have an integral controller. I hate integral control. With a fiery passion. It took a long time before we used integral on 971. It’s very unintuitive and has some nasty interactions with saturation and windup. It’s hard to tune effectively. I will always design a control loop without integral control, see if it’s good enough, and if it isn’t, then try adding integral control.
If 971 doesn’t like integral control, you should think pretty seriously about not using it too.
The corollary here is that if you can crank up Kp high enough, you will get a smaller and smaller error. For a lot of applications, that will be close enough that you don’t need to make everything more complicated.
Also, I don’t know why you are mentioning Kd for a velocity loop. The differential equations don’t have any acceleration state, so you aren’t buying yourself anything there. (I’m ignoring inductor current as a state since it doesn’t matter for FRC frequencies). Kd adds damping to a position loop because there is a velocity term in motor equations.
I’m not sure what you mean by this? You certainly can reduce error / time-to-error by referencing acceleration in a velocity loop (a nonzero D term), thpugh it’s less dramatic / important than in a position loop. A D term will deal with disturbances in a shooter for example - cranking the D term much higher was one of the better ways I could improve shooter consistency when tuning loops on various shooters this year. Is there something huge I’m missing here? I’m definitely a little rusty on my controls, so I very well could be wrong.
Agreed on the marginal utility of Ki being very very low though. The benefit seemed very marginal for the huge amount of work it took to get it to not occasionally ruin the loop.
I have refrained from supporting either, having experienced a few successes with both feed-forward and traditional PWM. They are different, but both have worked for us. YMMV.
I was only mentioning it negatively to explain the flaws of using a particular tuning method for velocity control. That out of the way, why wouldn’t Kd provide damping for velocity overshoots, especially as you are advocating aggressive Kp values? The PID loop typically controls voltage to the motor, independently of the equations of motion of that motor. One of the strengths of PID is its capacity to control systems which do NOT have easily-defined performance curves.
Because Kp isn’t going to generate overshoot… the proportional term is kinda like a less extreme version of bang-bang. When Kp is set really high the output voltage is 100% until you get REALLY close to setpoint. As long as you have a fast controller, like the talon SRX, you won’t get overshoot.
Also, the adjustment you thought of earlier on bang-bang is equivalent to feed forward. If you know the max velocity of your system, the correct feedforward voltage is vbat * setpoint / vmax
PID is designed to control second order and first order systems well. It can be used to control a lot of things, but starts to struggle mightily when given higher dimensional systems. It has 3 degrees of freedom. 2 are used to place the 2 poles of the system, and the third is used to remove steady state error. You are kidding yourself if you think PID is going to control a 1 input, 7 state system well. Try working through the laplace transforms some time for a 7 dimensional system and see how well you can control it.
The math for PID doesn’t assume voltage, motor, etc. It defines an output based on derivatives and integrals of the input. We happen to use it for motors because it actually works pretty well for it because motors are second order systems.
It’s kind of hard to get into this without some advanced math, so here we go. I’m going to do this in continuous time but the same ideas apply to discrete time. This is all assuming a velocity loop.
Our simple motor model hooked up to a mass is
V = I * R + omega / Kv
torque = I * Kt
torque = J * d omega/dt
Reshuffle to get this in terms of d omega/dt
*V = J * d omega/dt / Kt * R + omega / Kv
d omega/dt = Kt / (J * R) * (V - omega/Kv)*
This has a pole at -(Kt / (J * R * Kv) + Kt * Kp / (J * R)). Assuming that that quantity is negative (ie we are stable), that pole corresponds to a time constant of 1 / (Kt / (J * R * Kv) + Kt * Kp / (J * R)).
As you can see above, a flywheel has a single pole. It therefore only needs a single pole controller to place all of its poles anywhere.
Note: I’ve made some assumptions here. I’m assuming that the motor is well coupled to the mass. I’m assuming that the time constant of the inductor is fast enough that it doesn’t factor into the motor equations. My experience on 971 has shown that, for our robots, these are pretty good assumptions.
For grins, let’s try a PD loop. (I’m going to do a perfect derivative here, but anyone following along closely already knows that we can’t really take a derivative here so the math will need to be updated at some point. We could switch to discrete time and pick a differentiation method, or pick some other way of modeling the derivative.)
So, we added a zero at -Kp/Kd and moved our pole to -(Kt / (J * R * Kv) + Kt * Kp / (J * R)) / (1 + Kt * Kd / (J * R)) I’m going to just outright say that that’s not progress. We’ve added more complexity to our system and practically speaking, gotten nothing good out of it. Zeros should be avoided if at all possible. At least this is a stable zero, but still.
(Hopefully I didn’t miss any signs or invert anything. I’m sure someone will tell me if I did)
TLDR: Derivative doesn’t help on a flywheel.
In reality, if your system isn’t ideal, D might help some, but I wouldn’t count on it.