s=tf('s');
a=1800.0;
b=6.0;
Gpv=a/(s+b);
Gpp=a/(s*(s+b));

Kp=0.5;
Ki=0.001;
H=1;
Gc=(s*Kp+Ki)/s;

TFv=feedback(Gc*Gpv,H);

TFp=feedback(Gpp*(Gc),H);

 % Part 3
t=0:0.002:4; % time vector: start, step, stop
vel_ref = arrayfun(@trapezoidalRef,t); % make trapezoidal reference
[pos_ref,t]=lsim(1/s,vel_ref,t); % make position reference
[y,t]=lsim(TFp,pos_ref,t); % linear simulation
pos_err=pos_ref-y;

% Part 4 Pole zero compensator
C=5.625;
D=20;
Gpz= (D/C)*(s+C)/(s+D);
TFppz=feedback(Gc*Gpz*Gpp,H);  % Find transfer function of closed loop feedback with pole-zero comp
figure(6);
step(TFppz);

% digital controller delay model
Td=0.000268; % controller sample delay as measured in Arduino 
%Td=0.020268; % worst case delay with poor design of software
%Gd=(1-(Td/2)*s)/(1+(Td/2)*s); % Digital delay - pade delay approximation
%Gd=1/(1+(Td/2)*s); % Digital delay-single pole approximation
Gd=exp(-Td*s); % time delay model
% ref: https://www.mathworks.com/help/control/ug/time-delay-approximation-in-continuous-time-open-loop-model.html


TF=feedback(Gc*Gd*Gpp,H);  % can use this for feedback only or connect method below

figure(7);
[y,t]=lsim(TFppz,pos_ref,t); % linear simulation
plot(t,y)
hold on
plot(t,pos_ref)
title('trapezoidal reference response')
plot(t,vel_ref)
pos_err=pos_ref-y;
plot(t,150*Kp*pos_err)% scale by 150
hold off