% 18-9-2021 Max van Meer
% Derivation of cascaded control system
% based on https://ctms.engin.umich.edu/CTMS/index.php?example=MotorSpeed&section=SystemModeling
clc
clear
close all
format long

syms b K J R L

% Define state-space
A = [0 1 0; % first state is theta
    0 -b/J K/J;
    0 -K/L -R/L];
B = [0; 0; 1/L]; % input is voltage
C_theta = [1 0 0]; % output is first state: angle
C_curr = [0 0 1]; % output is third state: current

syms s
G_volt_to_theta = C_theta*(s*eye(3)-A)^-1 * B
G_volt_to_curr = C_curr*(s*eye(3)-A)^-1 * B

mysubs.J = 0.01;
mysubs.b = 0.1;
mysubs.K = 0.01;
mysubs.R = 1;
mysubs.L = 0.5;

G_volt_to_theta = subs(G_volt_to_theta,mysubs);
G_volt_to_theta = sym2tf(G_volt_to_theta); % see mathworks exchange

G_volt_to_curr = subs(G_volt_to_curr,mysubs);
G_volt_to_curr = sym2tf(G_volt_to_curr);

figure
bode(G_volt_to_theta)
hold on;
bode(G_volt_to_curr)
grid

s = tf('s');
current_controller = 1e6*(s+1e6) / s;
figure
step(feedback(current_controller*G_volt_to_curr,1)) % closed loop is stable with high bandwidth