GHCHS Robodox 599D Sets World Record at Golden State VEX Robotics Championship

Programming skills world record = 35 points.

Video of world record.‎

Quoted from the Robodox Facebook page:

559A, B, and D competed at CAMS’ VEX competition today. 599B made it to the quarterfinals, 599A made it to the semifinals, and 559D won in their division and were comp finalists, losing in the first match they didn’t play in, making a great comeback by winning the next match (and beating the tactics of this robot-entangling-and-what-I-thought-was-pinning-wall-’bot with their alliance partner) and finally, losing the last match of the round by two points. And 599D won the Excellence Award!! They also beat the world record for the Programming Skills Challenge and won the Drive Skills Challenge at the comp 599B was the second place for the Drive Skills Challenge.

A GHCHS writeup

 

As a mentor, I’m proud to say I didn’t have much to do with this:)

Note: Velocity PID loop driving a torque input

In the previous note I mentioned that there were 3 basic methods of implementing a velocity PID loop in software.   A variation of the first  method which combines a feed forward with a PID loop can  make the motor system look like a torque device rather than speed device in the steady state.   The torque command is the output of the PID loop and goes to zero when the PID error is zero.    In this case no speed command is necessary to hold the motor speed since it is the integration of the torque and remains steady when the torque command goes to zero.

Canceling the Back Emf Voltage makes a motor a torque device.

With a stead state voltage command a motor would continue to increase its speed were it not for the opposing back emf voltage generated by the motor.   In the resistive model of a motor torque is proportional to the current which in turn is proportional to the difference between the input voltage and the back emf voltage generated by the flux rate of change in the motor armature.

torq = kt*(V_cmd – V_bemf)/R – torq_o

where

V_cmd = the input voltage command

V_bemf = the motor induced back emf voltage

kt = the motor torque constant

R = the motor resistance

torq_0 =  the motor drag torque loss due to friction.

We can rewrite V_cmd as a sum of a torque command plus the back emf voltage:

        V_cmd = Vcmd_torq + V_bemf

then the resistive model reduces to a torque that is  proportional to Vcmd_torq:

torq = kt* Vcmd_torq/R -torq_0

Where do we get back emf?

There are ways to exactly measure the back emf using an A/D to sample the motor voltage when the controller PWM signal is in its off state, however, this technique is not available to most FRC teams.  We can instead approximate the voltage using specification parameters of the motor, gear box and measured wheel speed.

Software Implementation:  Click on Figure to see full size.

If in software we feedback a computed back emf of the opposite sign then the input will be a torque command.

The back emf voltage =- ke*w_m

A JAG or Victor output to the motor must contain a back emf canceling term = +  ke*w_m

where ke is the motor back emf constant, w_m = motor speed rad/s .

This term can be computed indirectly from the measured wheel speed knowing w_max, the maximum wheel speed and the motor supply voltage V_s, ke and the transmission gear ratio: ie

w_max = V_s/ke/Gear_ratio ;

Then the controller output back emf term   =   w/w_max *V_s    where w is the wheel speed.

The cancellation scheme works so long as the system remains linear.  The JAG/Victor limits the feedback to +-1 so large torque inputs will saturate the controller and the cancellation term will not work. The motor will return to a speed device while the command is saturated.

This topology has a nice feature in that the torque command is broken out explicitly and hence if there are large loads on the wheel that can cause excessive currents in the motor, a torque command limit can be placed after the pid  controller to limit the currents.   This is not particularly useful in a shooter wheel, however, if used during autonomous drive modes it can protect the motors from an over current if the robot were to inadvertently contact an object and stall the motors.

Computing the PID gains:

In Laplace notation, the motor system transfer function G(s)  before back emf cancellation is  1/(tau_m*s +1) .  With cancellation,

G(s) = 1/(tau_m *s);

Recall that the PID compensator transfer function is of the form GC(s) = KP + KD*s +KI/s

We would like to have KD be zero to avoid feeding back differentiated motor speed because differentiating a noisy signal amplifies the noise considerably.  Also there is not much need for KI since we have a forward path integrator in G(s).   So the open loop transfer function is

GC(s)*G(s) = KP/(tau_m*s)

The closed loop transfer function

GC(s)*G(s)/(1 + GC(s)*G(s) )  =  1/(1 + tau_d*s)

where tau_d  = tau_m/KP is the desired closed loop system time constant.

Since the PID loop doesn’t contain an integrator now, any biases down stream of the PID loop that create a command offset must be held by an error in velocity = command_bias/KP.   Typically , with a properly tuned motor system, the bias will be less than 5% of the speed.   This error will be reduced by 1/KP .  So we like to set KP as high as possible while minimizing system noise levels  and  saturation effects.   We know speed derived from encoders is noisy if the bandwidth is high.  Typically unfiltered it runs about 5% of the speed.  This noise if multiplied by KP so usually the encoder rate is filtered and KP is kept less than 3.

The KP used above assumes that the feedbacks are normalized to the max wheel speed w_max.

If you use rpm directly as a feedback, then  divide KP by max_rpm = w_max*60/2/pi;

Note: Velocity PID Loop

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.

Note: Precomputing PID gains for a velocity shooter using pole/zero placement

I wanted to post some formula’s for deriving P and I gains for a velocity PID control loop such as a shooter used in the 2012 Rebound Rumble game.

Assumptions:
a) motor L/R time constant is small relative to motor control frequencies.
b) The system ( motor , gearbox and wheel)  first order time constant tau_m is known.
c) The input speeds are normalized to the maximum motor speed, w_max. w_max = 12v/ke*60/2pi rpm which is approx w_free of the motor.
d) The desired motor response time constant, tau_d, is smaller than the motor tau_m by a factor of r.

Then the integral gain KI is given by

KI = 1/tau_d

and the proportional gain KP is given by

KP = r = tau_m/tau_d ;

If the integral computation used in the PID loop doesn’t contain a dt factor (i.e it is just an error accumulation) as is typical of the WPILIB PIDcontroller then the KI must be multiplied by the PID update period.

KI_WPILIB = T/tau_d where T is the PID period.

If you are feeding back RPM then the gains must be divided by  w_max.

Example:

The 599 shooter wheel has an open loop time constant tau_m = .68 sec

We want to have a tau_d  =  .33  sec which means the error should be around 5%  of the target by 1 second (three time constants).   During autonomous this would be the delay before we can shoot.

KI = 1/tau_d = 3.

KP = tau_m/tau_d = 3*.68 = 2.04 ;

For WPILIB  we want KI_WPILIB  = T*KI = .05*3 = .15   ;  KP remains the same.

The shooter has a w_max = 5614 rpm  so if the feedbacks are not normalized then KP and KI must be divided by w_max. or

KP = 2.04/5614 = 3.63 e-4 ;  KI_WPILIB = .15/5614 = 2.49e-5 ;

That’s it.   The main factor limiting the value of KP is the magnitude of the noise on the RPM feedback signal.   Typically after filtering this can get down to 1% of the RPM.  KP multiplies the noise so to keep the noise below 5% then the max KP would be 5.

theoretical derivation:

The transfer function G(s) of the motor is  1/(tau_m*s + 1).

The transfer function of the PID compensator  GC(s) is  (KP + KI/s) or KI*(KP/KI*s + 1)/s

The closed loop transfer function   =   GC(s)*G(s)/(1+GC(s)*G(s))

Lets look at the product  GC(s)*G(s) = (KI/s)*(KP/KI*s +1)*/(tau_m*s +1)

If we place the zero exactly at the motor pole then they will essentially cancel each other and we are left with GC(s)*G(s) = KI/s  and  now KP/KI = tau_m.

So the resulting closed loop transfer function is now   KI/s/(1 + KI/s)  = 1/(1/KI*s +1)  and since we defined 1/KI as tau_d we have a new closed loop response that looks like a more responsive motor with a transfer function of  1/(tau_d*s + 1).

See  this for additional discussion on obtaining tau_m and software implementation of PID loops.