clc; clear all
global b u xd s
x0 = rand(2,1); Ts = 0.001;
Tf = 10; px = []; pu = [];
px2 = []; t0 = 0;
xd = 0; e1 = x0(1)-xd;
e2 = x0(2)-0.1*cos(0.1*0);
b = 100; s = e2 + b*e1;
for i = 0:Ts:Tf
    px = [px x0];
    px2 = [px2 0.5*s^2];
    [t,y]=ode45(@myfun3,[t0 t0+Ts],x0);
    t0 = t0 + Ts;
    x0 = y(end,:)';
    pu = [pu u];
end
t = 0:Ts:Tf;
semilogx(t,px(1,:),'b-.',t,0.1*sin(t),'k')
legend('\theta','\theta_d')
grid on; xlabel('Time (Second)')
ylabel('rad')
figure(2)
semilogx(t,pu); grid on
xlabel('Time (Second)');ylabel('u')
figure(3)
loglog(t,px2); grid on
xlabel('Time (Second)'); ylabel('V(t)')
function dx = myfun3(t,x)
global b u xd s gxm
mc = 1; m = 0.1; l = 0.5; g = 9.81;
xd = 0.1*sin(t); e1 = x(1)-xd;
e2 = x(2)-0.1*cos(0.1*t);
s = e2 + b*e1;
a = 200;
msm = l*(4/3-m*(cos(pi/2))^2)/(mc+m);
gxm = cos(pi/3)/(mc+m)/msm;
u = 2*(-a*norm(s)-10)*sign(s)/gxm;
ms = l*(4/3-m*(cos(x(1)))^2)/(mc+m);
fx = g*sin(x(1))-m*l*x(2)^2*cos(x(1))*sin(x(1))/(mc+m)/ms;
gx = cos(x(1))/(mc+m)/ms;
dx = [x(2);fx]+[0;gx]*(u+0.3*sin(0.5*t));
end