Implement LQR Control for shooter wheel

[gerth2]

See if we can make it spool up faster.

[gerth2]

Here's the basics: branch chrisg_initial_lqr_investigation has some GNU Octave code that characterizes the shooter system spoolup physics from log files.

Next steps involve designing and constructing a closed-loop controller as described by section 5.8 of Controls Engineering for FRC. Do this in Octave simulaton first, so we know what behavior we expect. Then, transfer this to a new LQR Shooter Controller class which accepts the same "RPM Setpoint" input, and then implements the LQR control on the shooter motor speed controllers (reading in velocity/accel, and assigning motor voltages).

[gerth2]

I whipped up the following over lunch to try to get LQR parameters and develop the control law:

% LQR Calc - reference
% https://file.tavsys.net/control/controls-engineering-in-frc.pdf
% throughout.

% Adjust these to taste.
maxRPMErr = 30;
maxVoltsApplied = 12;

% TODO - get this from other scripts. Hardcoding for now.
A = [-0.84606]; 
B = [323.67];

% Derive discrete-time lqr parameters
[K,S,P] = dlqr(A, B, [1/(maxRPMErr*maxRPMErr)],[1/(maxVoltsApplied*maxVoltsApplied)]);

% Calculate plant-inverstion params
%  Taken from Theorem 5.10.4 - linear plant inversion 

% The moore-penrose psuedoinverse
B_mpp = (transpose(B)*B) .^ -1 * transpose(B);

format long
disp(K)
disp(B_mpp)

%Feed-forward plus LQR control law becomes:
% u = B_mpp(spd_cmd - A*spd_cmd_prev) + K (spd_cmd - spd_act);
% Use S-curve to interpolate command

[gerth2]

From what I'm seeing, this thing will work best when the reference signal moves in physically plausible ways. That is to say, we should establish a maximum angular velocity, acceleration, and jerk of our reference speed, and "filter" the reference speed based on this. Will likely be a second class.

Additionally, I'm thinking all of this should run in its own, high-priorty thread to try to ensure we get max stability on the loop call time.