stateSpaceModel.asv

%% State-Space catcher thing
clear; close all

%% define vars

% motor parameters
    Kt = 0.0967;        % Nm/A torque (from ServoSysCo spec sheet)
    %Kv = 105;          % V/rad/s back-emf constant
    L = .0041;          % H motor inductance  (from ServoSysCo spec sheet)
    R = 1.6;            % ohms  motor resistance (from ServoSysCo spec sheet)
    Jm = .05648;        % kgm^2 moment of inertial (from ServoSysCo spec sheet)
    b3 = 1.7e-5;        % Nms/rad motor damping (from ServoSysCo spec sheet)
    
% physical parameters    
    m1 = 3;             % kg mass of puck
    m2 = 5;             % kg mass of platform
    k1 = 18500;         % N/m mechanical spring element 
    k2 = 1000410;       % Nm/rad rotational spring element  
    r = .013;           % m   radius of screw
    rho_al = 2.70;      % kg/m3  density of alluminum
    Vol_al = pi*r^2*.013;    % vol of pulley
    mp = rho_al*Vol_al;      % mass of pulley
    Jp = mp*r^2/2;           % inertia of pulleys
    J = 2*Jp + Jm;           % inertia of rotational system
    nb = 3;                  % number of bearings
    b1 = 250;                % Ns/m   damping coeff for pad
    b2 = nb*r*500;           % Nms/rad guide bearing friction  

% transformer 
    TF12 = 2*pi*r;           % transformer translation to rotation
    TF34 = Kt;               % transformer rotation to electrical

% time
dt=.0001;
t = 0:dt:1;                  % time array


%% define matricies
x0 = [...
    -2.445;...    % Vm1 (if platform did not move)
    0;...         % Vm2
    0;...         % Fk1
    0;...         % Tk2
    0;...         % Wj
    1;];          % iL

A = [...
    -b1/m1, b1/m1, 1/m1, 0, 0, 0;...
    b1/m2, (-b1*TF12 - b2)/(TF12*m2), -1/m1, 1/(TF12*m2), 0, 0;...
    -k1, k1, 0, 0, 0, 0;...
    0, -k2/TF12, 0, 0, k2, 0;...
    0, 0, 0, -1/J, -b3/J, TF34/J;...
    0, 0, 0, 0, -TF34/L, - R/L ];
    
B = [...
    0;...
    0;...
    0;...
    0;...
    0;...
    1/L];

C = [1,0,0,0,0,0;...    % Vm1
    0,1,0,0,0,0;...     % Vm2
    0,0,1,0,0,0;...     % Fk1
    0,0,0,1,0,0;...     % Tk2
    0,0,0,0,1,0;...     % Wj 
    0,0,0,0,0,1];       % iL
          
D = 0;                  

%% create state-space
sys = ss(A,B,C,D);
sys.InputName = 'Vs';
sys.OutputName = {'Vm1';'Vm2';'Fk1';'Tk2';'Wj';'iL'};
u = zeros(1,length(t));

% for j = 1:length(u)         % uncomment for step input
%     if u(j) < median(u)
%         u(j) = 24;
%     else
%         u(j) = 0;
%     end
% end

y = lsim(sys,u,t,x0);
a = cat(1,NaN, diff(y(:,1))/dt);

% plot initial velocity response
figure
plot(t,y(:,1));    % velocity
grid on
hold on
plot(t,y(:,2));
title('Velocity Response');
xlabel('time (s)')
ylabel('velocity of puck (m/s)')
legend('Vm1','Vm2')
save2pdf('VelRes',gcf,300);

% position of puck
x = cumtrapz(t,y(:,1));

figure
plot(t,x);      % initial position response
grid on
title('position')
xlabel('time (s)')
ylabel('position (m)')
save2pdf('PosRes',gcf,300);

figure
plot(t,a);      % initial acceleration response
grid on
title('Acceleration Response');
xlabel('time (s)')
ylabel('acceleration of puck (m/s^2)')
save2pdf('AccelRes',gcf,300);

%% closed loop transfer functions
Gp = tf(sys);

Gspuck = Gp(1);
Gsplatform = Gp(2);

% requirements
osTarget = 0.10;       % overshoot 
tsTarget = 0.1;        % sec  

%%  PID compensator Design for V1r -> Vm1 
figure
rlocus(Gspuck); grid on
KP = 1000;
sP = -33.2+97.2*1i;   % initial design point

% simulate 
sysP = feedback(KP*Gspuck,1);
tt = 0:dt:1;  % simulation time array
yP = step(sysP,tt);
Pinfo = stepinfo(yP,tt,yP(end));
Pinfo 

figure;
plot(tt,yP);
grid on;
xlabel('time (s)')
ylabel('unit step response')
legend('P controller')

%% add derivative to speed up response
s = tf('s');
tsP = Pinfo.SettlingTime;
tsFactor = tsP/tsTarget; 

sPD = tsFactor*sP;              % new design point
theta = pi - angle(evalfr(Gspuck,sPD));  % required angle contribution
zc = imag(sPD)/tan(theta)+real(sPD);
GcD = (s-zc);                   % derivative compensator
GsPD = GcD*Gspuck;              % create compensated system

figure;
rlocus(GsPD);
grid on

KPD = 2;

%% simulate PD 
sysPD = feedback(KPD*GsPD,1);
yPD = step(sysPD,tt);
PDinfo = stepinfo(yPD,tt,yPD(end));
PDinfo

figure;
plot(tt,yP,'r'); hold on;
plot(tt,yPD,'b');
grid on;
xlabel('time (s)')
ylabel('unit step response')
legend('P controller','PD controller')

%% add integrator to drive to setpoint
GcI = (s+50)/s;  % integrator compensator
GsPID = GcI*GsPD;

figure;
rlocus(GsPID)
grid on

KPID = 2;

%% simulate PID
sysPID = feedback(KPID*GsPID,1); % Vm1 veclocity command
yPID = step(sysPID,tt);
PIDinfo = stepinfo(yPID,tt,yPID(end));
PIDinfo 

figure;
plot(tt,yP,'r'); hold on;
plot(tt,yPD,'b');
plot(tt,yPID,'g');
grid on;
xlabel('time (s)')
ylabel('unit step response')
title('puck step response')
legend('P controller','PD controller','PID controller')

%% build compensator  (C1) for puck
Gc1 = tf(KPID*GcI*GcD);  % total PID compensator

Kp = KPID;
Ki = Gc1.num{:}(3)/Kp;
Kd = Gc1.num{:}(2)/Kp;
disp(['Kp = ',num2str(Kp),' | Ki = ',num2str(Ki),' | Kd = ', num2str(Kp)]);
disp(' ');
disp('total compensating controller:');
disp(['Gc = ',num2str(Kp),' + ',num2str(Ki),'/s + s',num2str(Kd)]);

%%  PID compensator Design for V2r -> Vm2  (platform)
figure
rlocus(Gsplatform); grid on
KP2 = 3700;
sP2 = -43.9+61.5*1i;   % initial design point

%% simulate P gain for platform
sysP2 = feedback(KP*Gsplatform,1);
yP2 = step(sysP2,tt);
P2info = stepinfo(yP2,tt,yP2(end));
P2info 

figure;
plot(tt,yP2);
grid on;
xlabel('time (s)')
ylabel('unit step response')
legend('P controller')

%% add integrator to drive to setpoint
GcI2 = (s+35)/s;  % integrator compensator
GsPID2 = GcI2*Gsplatform;

figure;
rlocus(GsPID2)
grid on

KPID2 = 200;

%% simulate PID
sysPID2 = feedback(KPID2*GsPID2,1); % Vm1/veclocity command
yPID2 = step(sysPID2,tt);
PIDinfo2 = stepinfo(yPID2,tt,yPID2(end));
PIDinfo2 

figure;
plot(tt,yP2,'r'); hold on;
plot(tt,yPID2,'g');
grid on;
title('Platform Step Response');
xlabel('time (s)')
ylabel('unit step response')
legend('P controller','PID controller')

%% build compensator (C2)
Gc2 = tf(KPID2*GcI2);  % total PID compensator

Kp2 = KPID2;
Ki2 = Gc1.num{:}(3)/Kp2;
Kd2 = Gc1.num{:}(2)/Kp2;
disp(['Kp2 = ',num2str(Kp2),' | Ki2 = ',num2str(Ki2),' | Kd2 = ', num2str(Kp2)]);
disp(' ');
disp('total compensating controller:');
disp(['Gc2 = ',num2str(Kp2),' + ',num2str(Ki2),'/s + s',num2str(Kd2)]);


%% Picone input shaping trajectory tracking
F = ... % the tf from "input" desired platform traj to "output" platform command
        (1+Gc2*Gsplatform)/(Gc2*Gsplatform);

% specify the desired parameters as per design requirements
% (floor is position datum)
ft2m = @(x) x*12*2.54/100; % convert ft to m
g = -9.81;          % m/s^2
x00 = ft2m(4);      % m, initial puck height
xcatch = ft2m(2.5); % m, catch height
x10 = ft2m(3.9);      % m, height of puck passing break beam
x20 = ft2m(3);      % m, height of platform when break beam tripped
xf = ft2m(1);       % m, final height
tfin = 3;           % s, time when platform/puck at final height
v10 = -sqrt(2*abs(g)*abs(x10-x00));  % m/s, velocity of puck passing break beam
v20 = 0;            % m/s, velocity of platform when puck passing break beam

tp = 0:.0001:tfin; % have to simulate out to tfin, at least!

% compute platform desired trajectory (from Mathemtatica-generated function)
pt = zeros(length(tp),1);
for i = 1:length(tp)
    pt(i) = platform_trajectory_v(tp(i),tfin,xf,xcatch,x10,x20,v10,v20,g);
end

% plot
figure;
plot(tp,pt)
xlabel('time (s)')
ylabel('velocity (m/s)')
title('Velocity Trajectory')



%% Picone simulate by inserting "extra" poles
% We work around the noncausality of the system by inserting fake poles way
% out on the negative real axis that have no significant effect on the
% response. Note we have to compensate for the gain.

Fzpk = zpk(F);
fakepoles=[-400;-405;-410;-415];
Fz=Fzpk.z;
Fp=Fzpk.p;
Fp=vertcat(Fp{:},fakepoles);
Fk=abs(prod(fakepoles))*Fzpk.k; % fix scale with product of fake poles
F2=zpk(Fz,Fp,Fk); % new tf with fake poles inserted and gain corrected

vCmnd = lsim(F2,pt,tp);

% plot velocity command
figure;
plot(tp,vCmnd); hold on; grid on;
plot(tp,pt,'Color',[.5,.5,.5]);
xlabel('time (s)')
ylabel('velocity (m/s)')
title('Velocity Command for Platform')
legend('Velocity command', 'Applied signal','location','southeast')


%% simulate catch

pltSim = lsim(sysPID2,vCmnd,tp);

% get position command
xCmnd = cumtrapz(tp,pltSim) + x00;
for j = 1:length(tp)
    xPuck(j,1) = .5*g*tp(j)^2 + x00;
end

figure;
plot(tp,pltSim); grid on; hold on
plot(tp,g*tp);
xlabel('time (s)')
ylabel('velocity (m/s)')
title('velocity of platform')
legend('platform','puck (free fall)')

figure;
plot(tp,xCmnd); grid on; hold on
plot(tp,xPuck);
xlabel('time (s)')
ylabel('position (m)')
title('position of platform')
legend('platform','puck')