clc;
close all;
clear all;
h=0.001;
tf=70;
thetaref=0;theta=30*(pi/180);tdot=-15*(pi/180);j=1;
kp=1;kw=1;DV=0;
Usat=0.04;
time=0;
etheta=thetaref-theta;
e=kp*etheta-kw*tdot;
X=[theta;tdot];
while time<tf,
theta=X(1);
tdot=X(2);
etheta=thetaref-theta;
e=kp*etheta-kw*tdot;
if e>4*(pi/180)
U=1*Usat;
elseif e<-4*(pi/180)
U=-1*Usat;
elseif e>2*(pi/180) && e<4*(pi/180)
U=Usat;
elseif e<-2*(pi/180) && e>-4*(pi/180)
U=-Usat;
elseif e<2*(pi/180) && e>-2*(pi/180)
U=0;
end
f=[tdot;U];
X=X+h*f;
for i=1:2
XX(i,j)=X(i);
end
uu(j)=U;
ee(j)=e;
t(j)=time;
j=j+1;
DV=DV+h*U;
time=time+h;
end
DV
t1=XX(1,:);t2=XX(2,:);
figure(1);plot(t1,t2);title('theta vs thetadot');ylabel('theta(rad)');xlabel('theta dot(rad/s)');
pause;
clf;
subplot(411);plot(t,t1);title('theta vs Time');ylabel('theta(rad)');xlabel('t(sec)');
subplot(412);plot(t,t2);title('theta dot vs Time');ylabel('theta dot(rad/s)');xlabel('t(sec)');
subplot(413);plot(t,ee);title('error vs Time');ylabel('error');xlabel('t(sec)');
subplot(414);plot(ee,uu);title('Input vs Time');ylabel('Input');xlabel('t(sec)');grid;