%-------------------------------------------------------------------------
%-------------------------------------------------------------------------
% COURSE:   RBE502 ROBOT CONTROL 
%
% ASSIGNMENT:TERM PROJECT 
%
% AUTHORS:  KYLE CANTRELL (KJCANTRELL@WPI.EDU)
%           CRAIG MILLER  (CDMILLER@WPI.EDU)
%           JORDAN NELSON (JVNELSON@WPI.EDU)
%
% DATE:     04/05/2019
%-------------------------------------------------------------------------
% TITLE: MCG_Calc_Open_Manipulator
%-------------------------------------------------------------------------

clear all
clc

%% OpenManipulator Parameter Symbols 

syms t l1 l2 l3 l4 l5 lc1 lc2 lc4 lc5 m1 m2 m4 m5 I1 I2 I4 I5 g temp real

%Output based on elbow up or down configuration:
elb_pos = input('Enter elbow position (0 - elbow down, 1 - elbow up):');

theta0 = str2sym('theta0(t)');
theta1 = str2sym('theta1(t)');
theta2 = str2sym('theta2(t)');
theta3 = str2sym('theta3(t)');

%% Position Vectors 

%X(:,1) theta0
%X(:,2) theta1
%X(:,3) theta2
%X(:,4) theta3

%Joint 1 CG Position (Theta 0)
p1 = [0; 0; lc1];

%Joint 2 CG Position (Theta 1)
p2 = [(lc2.*cos(theta1))*cos(theta0);(lc2.*cos(theta1))*sin(theta0);l1 + lc2.*sin(theta1)];


if (elb_pos == 1)
    
%Joint 3 CG Position (Theta 2) ELBOW UP
p3 = [(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*cos(theta0)+(lc4.*cos(theta2+((pi/2)-theta1))*cos(cos(theta0)));(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*sin(theta0)+(lc4.*cos(theta2+((pi/2)-theta1))*cos(sin(theta0)));l1 + l2.*sin(theta1)-l3.*sin((pi/2)-(theta1))-(lc4.*sin(theta2+((pi/2)-theta1)))];

%End-Effector CG Position (Theta 3) ELBOW UP
p4 = [(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*cos(theta0)+(l4.*cos(theta2-((pi/2)-theta1))*cos(cos(theta0)))+(lc5.*cos(theta2+((pi/2)-theta1))*cos(theta0));(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*sin(theta0)+(l4.*cos(theta2-((pi/2)-theta1))*cos(sin(theta0)))+(lc5.*cos(theta2+((pi/2)-theta1))*sin(theta0));l1 + l2.*sin(theta1)-l3.*sin((pi/2)-(theta1))+(l4.*sin(theta2-((pi/2)-theta1)))];

end

if (elb_pos == 0)
    
%Joint 3 CG Position (Theta 2) ELBOW DOWN
p3 = [(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*cos(theta0)+(lc4.*cos(theta2-((pi/2)-theta1))*cos(cos(theta0)));(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*sin(theta0)+(lc4.*cos(theta2-((pi/2)-theta1))*cos(sin(theta0)));l1 + l2.*sin(theta1)-l3.*sin((pi/2)-(theta1))+(lc4.*sin(theta2-((pi/2)-theta1)))];

%End-Effector CG Position (Theta 3) ELBOW DOWN
p4 = [(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*cos(theta0)+(l4.*cos(theta2-((pi/2)-theta1))*cos(cos(theta0)))+(lc5.*cos(theta2-((pi/2)-theta1))*cos(theta0));(l2.*cos(theta1)+l3.*cos((pi/2)-(theta1)))*sin(theta0)+(l4.*cos(theta2-((pi/2)-theta1))*cos(sin(theta0)))+(lc5.*cos(theta2-((pi/2)-theta1))*sin(theta0));l1 + l2.*sin(theta1)-l3.*sin((pi/2)-(theta1))+(l4.*sin(theta2-((pi/2)-theta1)))];

end 


%% State Variable Derivatives

% Find the derivative of the state variables to be used as symbols in
% further computation

theta0dot = diff(theta0,t);
theta1dot = diff(theta1,t);
theta2dot = diff(theta2,t);
theta3dot = diff(theta3,t);

% Velocity
p1dot = diff(p1,t); %v1
p2dot = diff(p2,t); %v2
p3dot = diff(p3,t); %v3
p4dot = diff(p4,t); %v4

% Calculate Kinetic Energy
T1 = (1/2)*m1*(p1dot.'*p1dot); + ((1/2)*I1*theta0dot.^2);
T2 = (1/2)*m2*(p2dot.'*p2dot); + ((1/2)*I2*theta1dot.^2);
T3 = (1/2)*m4*(p3dot.'*p3dot); + ((1/2)*I4*theta2dot.^2);
T4 = (1/2)*m5*(p4dot.'*p4dot); + ((1/2)*I5*theta3dot.^2);

T = T1 + T2 + T3 + T4;

% Calculate Potential Energy
V1 = m1*g*p1(3);
V2 = m2*g*p2(3);
V3 = m4*g*p3(3);
V4 = m5*g*p4(3);

V = V1 + V2 + V3 + V4;

% Lagrangian
L = T-V;

% Use this symbolic variable to keep state variables' symbols intact
% for readability
temp = sym('temp');

% Compute required derivations for Lagrangian Equations
dLdtheta0 = subs(diff(subs(L,theta0,temp),temp),temp,theta0);
dLdtheta1 = subs(diff(subs(L,theta1,temp),temp),temp,theta1);
dLdtheta2 = subs(diff(subs(L,theta2,temp),temp),temp,theta2);
dLdtheta3 = subs(diff(subs(L,theta3,temp),temp),temp,theta3);

dLtheta0dot = subs(diff(subs(L,diff(theta0,t),temp),temp),temp,diff(theta0,t));
dLtheta1dot = subs(diff(subs(L,diff(theta1,t),temp),temp),temp,diff(theta1,t));
dLtheta2dot = subs(diff(subs(L,diff(theta2,t),temp),temp),temp,diff(theta2,t));
dLtheta3dot = subs(diff(subs(L,diff(theta3,t),temp),temp),temp,diff(theta3,t));

diffeq0 = diff(dLtheta0dot,t) - dLdtheta0;
diffeq1 = diff(dLtheta1dot,t) - dLdtheta1;
diffeq2 = diff(dLtheta2dot,t) - dLdtheta2;
diffeq3 = diff(dLtheta3dot,t) - dLdtheta3;

torques = [simplify(diffeq0); simplify(diffeq1); simplify(diffeq2); simplify(diffeq3)]

%Compact form

M10 = simplify((torques(1) - subs(torques(1),diff(theta0,t,t),0))/diff(theta0,t,t));
M11 = simplify((torques(1) - subs(torques(1),diff(theta1,t,t),0))/diff(theta1,t,t));
M12 = simplify((torques(1) - subs(torques(1),diff(theta2,t,t),0))/diff(theta2,t,t));
M13 = simplify((torques(1) - subs(torques(1),diff(theta3,t,t),0))/diff(theta3,t,t));

M20 = simplify((torques(2) - subs(torques(2),diff(theta0,t,t),0))/diff(theta0,t,t));
M21 = simplify((torques(2) - subs(torques(2),diff(theta1,t,t),0))/diff(theta1,t,t));
M22 = simplify((torques(2) - subs(torques(2),diff(theta2,t,t),0))/diff(theta2,t,t));
M23 = simplify((torques(2) - subs(torques(2),diff(theta3,t,t),0))/diff(theta3,t,t));

M30 = simplify((torques(3) - subs(torques(3),diff(theta0,t,t),0))/diff(theta0,t,t));
M31 = simplify((torques(3) - subs(torques(3),diff(theta1,t,t),0))/diff(theta1,t,t));
M32 = simplify((torques(3) - subs(torques(3),diff(theta2,t,t),0))/diff(theta2,t,t));
M33 = simplify((torques(3) - subs(torques(3),diff(theta3,t,t),0))/diff(theta3,t,t));

M40 = simplify((torques(4) - subs(torques(4),diff(theta0,t,t),0))/diff(theta0,t,t));
M41 = simplify((torques(4) - subs(torques(4),diff(theta1,t,t),0))/diff(theta1,t,t));
M42 = simplify((torques(4) - subs(torques(4),diff(theta2,t,t),0))/diff(theta2,t,t));
M43 = simplify((torques(4) - subs(torques(4),diff(theta3,t,t),0))/diff(theta3,t,t));

M = [M10 M11 M12 M13; M20 M21 M22 M23; M30 M31 M32 M33; M40 M41 M42 M43];

G = subs(torques, {diff(theta0,t,t),diff(theta1,t,t),diff(theta2,t,t),diff(theta3,t,t),diff(theta1,t), diff(theta1,t),diff(theta2,t),diff(theta3,t,t)},{0,0,0,0,0,0,0,0});

C1 = simplify(torques(1) - (M(1,:)*[diff(theta0,t,t) diff(theta1,t,t) diff(theta2,t,t) diff(theta3,t,t)].'+G(1)));
C2 = simplify(torques(2) - (M(2,:)*[diff(theta0,t,t) diff(theta1,t,t) diff(theta2,t,t) diff(theta3,t,t)].'+G(2)));
C3 = simplify(torques(3) - (M(3,:)*[diff(theta0,t,t) diff(theta1,t,t) diff(theta2,t,t) diff(theta3,t,t)].'+G(3)));
C4 = simplify(torques(4) - (M(4,:)*[diff(theta0,t,t) diff(theta1,t,t) diff(theta2,t,t) diff(theta3,t,t)].'+G(4)));

C = [C1; C2; C3; C4];




