% Equations of motion
% x = [x(1); x(2)] = [pos, vel]
g = 9.81;
m = 1;
l = 1;
f =@(t,x,u) [x(2); 1/(m*l^2)*u-g/l*sin(x(1))];

% time vector
dt = 1/1000;
T = 20;
t = 0:dt:(T-dt);

% Simulate
x0 =[3*pi/4;0];
[t,x] = ode45(@(t,x) f(t,x,0),t,x0);
figure; hold all;
for i=1:100:numel(t)
  clf; hold all;
  plotPendulum(x(i,1),l);
  hold on;
  title({'Simulation 1, Free Vibration', ...
          ['t = ',num2str(t(i),'%.2f')]});
  pause(0.01);
  axis square;
end

pause

% Add damping
b = 0.1;
u =@(x) -x(2)*b;
[t,x] = ode45(@(t,x) f(t,x,u(x)),t,x0);
figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(x(i,1),l);
  hold on;
  title({'Simulation 2, Damped Vibration', ...
          ['t = ',num2str(t(i),'%.2f')]});
  pause(0.01);
  axis square;
end

pause

% Damping + P-controller, with xd = [pi,0]
kp = 5;
u = @(x) -x(2)*b + (pi - x(1))*kp;
x0 = [0; 0];
[t,x] = ode45(@(t,x) f(t,x,u(x)),t,x0);
figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(x(i,1),l);
  hold on;
  title({'Simulation 3, Damped + P-Control', ...
          ['t = ',num2str(t(i),'%.2f')]});
  pause(0.01);
  axis square;
end

pause

% Damping + PD-controller, with xd = [pi,0]
kp = 100;
kd = 100;
u = @(x) -x(2)*b + (pi - x(1))*kp + (-x(2))*kd;
[t,x] = ode45(@(t,x) f(t,x,u(x)),t,x0);
figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(x(i,1),l);
  hold on;
  title({'Simulation 4, Damped + PD-Control', ...
          ['t = ',num2str(t(i),'%.2f')]});
  pause(0.01);
  axis square;
end

pause

% ---- PID
kp = 100;
kd = 100;
ki = 20;
F =@(t,x,u) [f(t,x,u); pi - x(1)];
u = @(x) -x(2)*b + (pi - x(1))*kp + (-x(2))*kd + x(3)*ki;
x0 = [x0; 0];
[t,x] = ode45(@(t,x) F(t,x,u(x)),t,x0);
figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(x(i,1),l);
  hold on;
  title({'Simulation 5, Damped + PID-Control', ...
          ['t = ',num2str(t(i),'%.2f')]});
  pause(0.01);
  axis square;
end

% -----------------------------------------------------------------------------
function plotPendulum(x,a)
  xx = a*sin(x);
  yy = -a*cos(x);
  plot([-0.1 0.1],[0,0],'k','linewidth',20);
  plot([0,xx],[0,yy],'k','linewidth',10);
  scatter(xx,yy,1000,'k','filled','o');
  xlim([-2,2]);
  ylim([-2,2]);
  axis off;
end









