g = 9.81;
dynamics =@(m,l,t,q,u) [q(2); 1/(m*l^2)*u-g/l*sin(q(1))];

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

% input
u1 =@(t,q) t.*0;
u2 =@(t,q,b) -q(2)*b;
u3 = @(t,q,b,kp) -q(2)*b + (pi - q(1))*kp;
u4 = @(t,q,b,kp,kd) -q(2)*b + (pi - q(1))*kp + (-q(2))*kd

% simulation
m = 1;
l = 1;
b = 0.1;
kp = 100;
kd = 100;

% no input
%[t,q] = ode45(@(t,q) dynamics(m,l,t,q,u1(t)),t,[3*pi/4;0]);

% damping
%[t,q] = ode45(@(t,q) dynamics(m,l,t,q,u2(t,q,b)),t,[3*pi/4;0]);

% damping + P-control
%[t,q] = ode45(@(t,q) dynamics(m,l,t,q,u3(t,q,b,kp)),t,[0;0]);

% damping + PD-control
[t,q] = ode45(@(t,q) dynamics(m,l,t,q,u4(t,q,b,kp,kd)),t,[0;0]);

figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(q(i,1),l);
  pause(0.01);
  axis square;
end


% ---- PID
kp = 100;
kd = 100;
ki = 20;

dynamics =@(m,l,t,q,u) [q(2); 1/(m*l^2)*u-g/l*sin(q(1)); pi - q(1)];
u5 = @(t,q,b,kp,kd,ki) -q(2)*b + (pi - q(1))*kp + (-q(2))*kd + q(3)*ki;
[t,q] = ode45(@(t,q) dynamics(m,l,t,q,u5(t,q,b,kp,kd,ki)),t,[0;0;0]);

figure; hold all;
for i=1:10:numel(t)
  clf; hold all;
  plotPendulum(q(i,1),l);
  pause(0.01);
  axis square;
end


function plotPendulum(q,a)
  xx = a*sin(q);
  yy = -a*cos(q);
  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









