% Four-Bar Mechanism Parameters KCC 2022

% Parameters  SI Units
m2 = 0.108;             % mass of Link 2 (kg)
m3 = 0.324;             % mass of Link 3 (kg)
m4 = 0.216;             % mass of Link 4 (kg)
L1 =  1;                % length of Link 1 (m)      
L2 = 0.40;              % length of Link 2 (m)       
L3 = 1.20;              % length of Link 3 (m)
L4 = 0.80;              % length of Link 4 (m)  
rho = 2700;             % density of aluminum (kg/m^3)
                        % beam cross section: 1 cm by 1 cm
H = 0.01;
W = 0.01;
density = 2700;

theta2_0 = 40*pi/180;
theta3_0 = 20.3*pi/180;
theta4_0 = 57.3*pi/180;

T1 = 10;        % Move time 1
D = pi/2;       % Distance move: starts at 40 deg and ends at 130 deg
D1 = 0.77;      % Distance move for inverse kinetics for link 4
T2 = 5;         % Move time 2
T3 = 12;        % Start time of move 2

% Motion Profile   K. Craig    2022

% Information about the Reference Profile - the 5th Order Polynomial with
% zero initial and final velocities and accelerations, an initial position of
% zero and a final position of one.

% T = 0.5;
% t = [0:.001:.5];
% S = (10/T^3)*t.^3 - (15/T^4)*t.^4 + (6/T^5)*t.^5;
% V = (30/T^3)*t.^2 - (60/T^4)*t.^3 + (30/T^5)*t.^4;
% A = (60/T^3)*t.^1 - (180/T^4)*t.^2 + (120/T^5)*t.^3;
% J = (60/T^3) - (360/T^4)*t.^1 + (360/T^5)*t.^2;
