% ONSITE 1

t1 = [0:0.1:1]
t2 = [1:0.5:31.4]
t3 = [31.4:0.1:32.4]
t = [t1 t2 t3]

s1 = -3.14 + 0.05.*t1.^2;
s2 = -3.19 + 0.1.*t2;
s3 = -52.488 + 3.24.*t3 - 0.05.*t3.^2;
s = [s1 s2 s3]

x1 = 1 + cos(s1);
x2 = 1 + cos(s2);
x3 = 1 + cos(s3);
x = [x1 x2 x3]

y1 = sin(s1);
y2 = sin(s2);
y3 = sin(s3);
y = [y1 y2 y3]

xq1 = -0.1.*t1.*sin(s1);
xq2 = -0.1.*sin(s2);
xq3 = (-3.24 +0.1.*t3).*sin(s3);
xq = [xq1 xq2 xq3]

yq1 = 0.1.*t1.*cos(s1);
yq2 = 0.1.*cos(s2);
yq3 = (3.24 - 0.1.*t3).*cos(s3);
yq = [yq1 yq2 yq3]

% ONSITE 2
d = sqrt(x.^2+y.^2);
if(d ~= 0)
theta = atan(y./x);

dq = xq.*cos(theta) + yq.*sin(theta);
theta1 = -xq.*sin(theta)./d + yq.*cos(theta)./d;
end

%ONSITE 3
function [tau1,tau2]= PDcontroller(err_t1,err_t1p,err_d2,err_d2p,theta1,d2)
tau1=0;
tau2=0;

Kp=0.2;
Kd=0.03;
r=0.5;
g=9.8;
m1 =1;
m2=1;
%dP/dq1
gq1= -g*m1*(r+d2)*cos(theta1);
%dP/dq2
gq2= g*m2*sin(theta1);

%Torque
tau1 = Kp*err_t1 - Kd*err_t1p + gq1;
tau2 = Kp*err_d2 - Kd*err_d2p + gq2;
end

function [theta1pp,d2pp] = invdyn(torque1,torque2,theta1,d2,theta1p,d2p)

%Initial
theta1pp=0;

d2pp=0;
g=9.8;
r=0.5;
m1=1;
m2=1;
qdd=0;

%dP/dq1
gq1= -g*m1*(r+d2)*cos(theta1);
%dP/dq2
gp2= g*m2*sin(theta1);
tau = [torque1;torque2];
gq=[gq1; gq2];

%Inertia matrix
d11 = m1*r+d2*m2;
d12=0;
d21=0;
d22=m2;
D=[d11 d12 ; d21 d22];

%Inverse matrix of D
Di = inv(D);
c11 = d2p;
c12= -theta1p/2;
c21 = theta1p;
c22 = 0;
C=[c11 c12; c21 c22];

Joint=[theta1p;d2p];
Cq=C*Joint;

%Calculate q''
qdd=Di*(tau-Cq-gq);
%q1"
theta1pp=qdd(1);
%q2"
d2pp=qdd(2);

end