In the previous note I spoke about a method of setting PID gains based upon known dynamics of the motor system. All one needs is the motor system time constant tau_m. This is the time constant of the motor when connected to all its loads (i.e. gear box and shooter wheel). This can be derived by testing or from spec motor parameters.
Measuring tau_m:
Instrument the motor to measure speed versus time for a step input. The amplitude of the step response will be approximately exponential vs time t. The speed response, w_out =w_input* (1-exp(-t/tau_m)). If you measure t when w_out = .63*w_input then this time = tau_m. I.e. @ t = tau_m , vout = w_input*(1-exp(-1)) = w_input*.63.
Calculating tau_m using motor parameters;
The following figure shows a typical velocity control loop. Click on figure to see full size!!
The 12v motor parameters are:
kt = torque constant = torq_stall/I_max ;
ke=back emf speed constant = kt for most motors ;
R = motor resistance = 12v/I_stall (This is typically increased by a 1.5 factor to approx the effects of battery and wire series resistances)
I_load = moment of inertia of the motor with load attached.
num_motors = number of motors on the gear box
gear_ratio = motor_speed/wheel_speed
eff = efficiency of the gear box.
The tau_m = 1/loop_gain where the loop gain is the product of all the gains in the motor back_emf loop.
loop_gain = num_motors*kt/R/I_load*eff*gear_ratio^2*ke
One can also calculate tau_m by computing the maximum acceleration w_d_max and the maximum speed w_max.
Then tau_m = w_max/w_d_max;
w_d_max = num_motors*eff*gear_ratio*torq_stall/I_load;
w_max = V_s/ke/gear_ratio;
Software Implementation Schemes:
On Chiefdelfi there have been several methods discussed re implementing a velocity PID loop.
The main problem people face with a velocity loop is holding the motor speed command when the motor has reached the target speed. This can be done is three ways:
1)Apply a feed forward command that sums with the PID loop output so when the PID error goes to zero the motor still has a steady state command. This implementation allows the I term to be limited to a small value just necessary to compensate for down stream biases. The limit is typically around 5 to 10% of the total command. Usually, the D term is zero since it differentiates speed which is already noisy and it amplifies the noise considerably. Without the I and D terms the closed loop response is first order with a transfer function = 1/(tau_d*s +1) where KP is set by the desired tau_d at
KP = tau_m/tau_d – 1
Usually if KP is high enough the KI term is unnecessary since with a well tuned motor (i.e. linearized if a victor and static friction offsets compensated for) the offset bias errors with KP = 0 are less than 3%. With KP = 3 this can be dropped lower to 1%. KP cannot be made too large since it amplifies any noise present in the velocity feedback and will cause the command signal to saturate. The saturation usually occurs more in one direction and this causes rectification which can lead to biasing of the error. The higher KP the more the speed signal needs to be smoothed with filtering but this adds phase lag to the loop and degrades the stability so a balance must be reached.
2) Apply a normal PID loop with speed as the input, set the D term to zero and have a high integration gain , KI, that responds quickly to the error and holds the speed reference when the error goes to zero. The integrator must have full authority in this case. KP is adjusted to add damping and minimize the overshoot. This is the method I recommend and was the basis for the last post on precomputing PID gains.
Reviewing, the method creates a closed loop response that is first order except with faster response than that the original system motor response. The only parameter required to set KP and KI is the ratio r between the desired closed loop time constant , tau_d and the original system time constant tau_m or
r = tau_m/ tau_d.
Then
KI = 1/tau_d = r /tau_m;
KP = tau_m/tau_d = r ;
Again, as discussed in the first case, KP or r is primarily limited by the noise on the speed signal. KP amplifies the noise directly and can cause the system to saturate in addition to causing unwanted current spikes in the motor controller Typically it ranges between 2 and 5 with the encoders used in FRC.
As discussed in the note, if the WPILIB PIDController is used KI_WPILIB = KI*T
where T is the period of the PID loop. If non-normalized speeds are used then both KP and KI must be divided by the maximum speed.
When the WPILIB PIDController is disabled the command output is not updated and will remain at its last value. This will hold the speed open loop until the PIDController is reset.
3) Integrating the output of the PID loop. In this configuration, it is best to set KI = 0 to avoid having a double integral in the compensator. In controls speak the GC(s) compensator now looks like
GC(s) = (KP+KD*s + KI/s)/s . With KI = 0 and factoring KP we can write
GC(s) = KP*(1+ KD/KP*s)/s . Again we want to place the zero (1 + KD/KP*s) to match the open loop motor response 1/(1 + tau_m*s) and cancel it. Then the closed loop response of 1/(tau_d*s +1) is obtained by setting KP and KD as follows:
KP = 1/tau_d and KD/KP = tau_m. This is similar to case 2 except KP is used to set the desired response and KD is used to create a zero to cancel the open loop motor response.
When using KD in the WPILIB PIDController it must be divided by the PID update period T.
KD_WPILIB = KD/T
This implementation requires a non-zero D term so the noise on the speed term will be differentiated but it is then integrated. So long as there is no saturation of the PID terms prior to the final integration and double precision is used to minimize round off errors you can get away with this. The extra differencing and summing is why I prefer method 2 over this one.
This method requires the addition of another integration but you can disable the PID loop without losing the speed reference since it is held by the series integrator on the PID output.
Vamfun's Blog 