53 lines
1.4 KiB
Matlab
53 lines
1.4 KiB
Matlab
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
|
|
|
|
|