Code

Matlab Code for analysis and plots of the bear

%% Robotics Final Project Analysis

% Ella Small

%% step 1: Analysis of head position

%initialize the measured link lengths 

w2 = -10;

a2 = 0;

a=1;

b=5;

c=0.75;

t3 = 180;

t1 = 90;

%position loop

syms d t3 t2 

eqn1 = a*cosd(t2)+b*cosd(t3)-c*cosd(t1)-d*cosd(t1) == 0;

eqn2 = a*sind(t2)+b*sind(t3)-c*sind(t1)- d*sind(t1) == 0;

sol = solve([eqn1, eqn2], [d,t3]);

d = sol.d;

t3 = sol.t3;

%plug in input crank angle

dn= [];

t3n = [];

for i =1:1:360

  dn = [dn subs(d, t2, i)];

  t3n = [t3n subs(t3, t2, i)];

end

v_d = double(dn(2,:)); 

v_t3 = double(t3n(2,:));

v_t2 = [1:1:360];

% plot position of the head

figure;

hold on;

plot(v_t2,v_d);

title('Kinematic Analysis: Position of bear head');

xlabel('Theta 2 (deg): Crank angle');

ylabel('Position of head (in)');

%% velocity of head

%calculate the velocity

syms t2 w3 dd t3

eqn1 = -a*w2*sind(t2) - b*w3*sind(t3)-dd*cosd(t1)==0;

eqn2 = a*w2*cosd(t2)+b*w3*cosd(t3)-dd*sind(t1)==0;

sol = solve([eqn1, eqn2], [dd,w3]);

dd = sol.dd;

w3 = sol.w3;

ddn= [];

w3n = [];

for i =1:1:360

  ddn = [ddn subs(subs(dd, t3, v_t3(i)), t2, i)];

  w3n = [w3n subs(subs(w3, t3, v_t3(i)), t2, i)];

end

v_dd = double(ddn(1,:)); 

v_w3 = double(w3n(1,:));

v_t2 = [1:1:360];

%plot velocity

figure;

plot(v_t2,v_dd);

title('Kinematic Analysis: Velocity of the head');

xlabel('theta 2 (deg): Crank angle');

ylabel('Velocity of the head (in/s)');

% 

%% Acceleration of the head

%calculate the acceleration

syms t2n a3 ddd t3 w3

eqn1 = -a*w2^2*cosd(t2n)-b*a3*sind(t3)-b*w3^2*cosd(t3)-ddd*cosd(t1) == 0;

eqn2 = -a*w2^2*sind(t2n)+b*a3*cosd(t3)-b*w3^2*sind(t3)-ddd*sind(t1) == 0;

sol = solve([eqn1, eqn2], [ddd,a3]);

ddd = sol.ddd;

a3 = sol.a3;

dddn= [];

a3n = [];

for i =1:1:360

  dddn = [dddn subs(subs(subs(ddd, w3, v_w3(i)), t3, v_t3(i)), t2n, i)];

  a3n = [a3n subs(subs(subs(a3, w3, v_w3(i)), t3, v_t3(i)), t2n, i)];

end

v_ddd = double(dddn(1,:)); 

v_a3 = double(a3n(1,:));

v_t2 = [1:1:360];

%plot the acceleration

figure;

plot(v_t2,v_ddd);

title('Kinematic Analysis: Acceleration of the head');

xlabel('theta 2 (deg): Crank angle');

ylabel('Acceleration of the head (in/s^2)');

%% Step2: Analysis of the arm

%initialize measured link lengths of the arm

a = 1;

b = 2.75;

d = 2.1;

f = 1.8;

t1 = 111.8;

w2=-10;

%calculate position of arm

syms t2 t3 t4 t5 g 

eqn1 = a*cosd(t2)+b*cosd(t3)+d*cosd(t4)+f*cosd(t5)-g*cosd(t1)==0;

eqn2 = a*sind(t2)+b*sind(t3)+d*sind(t4)+f*sind(t5)-g*sind(t1)==0;

eqn3 = t5==t4-90;

sol = solve([eqn1, eqn2, eqn3],[g, t5, t4]);

g = sol.g;

t5=sol.t5;

t4=sol.t4;

gn= [];

t4n = [];

t5n = [];

for i =360:-1:1

  gn = [gn subs(subs(g, t3, v_t3(i)), t2, i)];

  t4n = [t4n subs(subs(t4, t3, v_t3(i)), t2, i)];

  t5n = [t5n subs(subs(t5, t3, v_t3(i)), t2, i)];

end

v_g = double(gn(2,:)); 

v_t4 = double(t4n(2,:));

v_t5 = double(t5n(2,:));

v_t2 = [1:1:360];

%plot position of arms

figure;

plot(v_t2, v_g);

title('Kinematic Analysis: Position of arms');

xlabel('Theta 2 (deg): Crank angle');

ylabel('Position of arm (in)');

%plot angle of arms

figure;

plot(v_t2, v_t5);

title('Kinematic Analysis: angle of arms');

xlabel('Theta 2 (deg): Crank angle');

ylabel('Theta 5 (deg): angle of upper arm');

%% velocity loop

%calculate the velocity

syms t2 t3 w3 w4 gd t4 t5

eqn1 = -a*w2*sind(t2)-b*w3*sind(t3)-d*w4*sind(t4)-f*w4*sind(t5)-gd*cosd(t1) ==0;

eqn2 = a*w2*cosd(t2)+b*w3*cosd(t3)+d*w4*cosd(t4)+f*w4*cosd(t5)-gd*sind(t1) ==0;

sol = solve([eqn1, eqn2], [w4, gd]);

w4 = sol.w4;

gd = sol.gd;

gdn= [];

w4n = [];

for i =1:1:360

  gdn = [gdn subs(subs(subs(subs(subs(gd, t5, v_t5(i)), t4, v_t4(i)), w3, v_w3(i)), t3, v_t3(i)), t2, i)];

  w4n = [w4n subs(subs(subs(subs(subs(w4, t5, v_t5(i)), t4, v_t4(i)), w3, v_w3(i)), t3, v_t3(i)), t2, i)];

end

v_gd = double(gdn(1,:)); 

v_w4 = double(w4n(1,:));

% plotting x and y velocity of arms

figure;

plot(v_t2, v_gd.*cosd(t1), v_t2, v_gd.*sind(t1));

title('Kinematic Analysis: linear velocity of arms');

xlabel('Theta 2 (deg): crank angle');

ylabel('velocity (in/s) of arm');

legend('x','y');

%plot angular velocity of arms

figure;

plot(v_t2, v_w4);

title('Kinematic Analysis: angular velocity of arms');

xlabel('Theta 2(deg): crank angle');

ylabel('w4 (deg/s)');

%% acceleration of arms

%calculate acceleration

syms t2 t3 a3 w3 a4 t4 w4 t5 gdd 

eqn1 = -a*w2^2*cosd(t2)-b*a3*sind(t3)-b*w3*cosd(t3)-d*w4^2*cosd(t4)-d*a4*sind(t4)...

    -f*a4*sind(t5)-f*w4^2*cosd(t5)-gdd*cosd(t1) == 0;

eqn2 = -a*w2^2*sind(t2)+b*a3*cosd(t3)-b*w3*sind(t3)-d*w4^2*sind(t4)+d*a4*cosd(t4)...

    +f*a4*cosd(t5)-f*w4^2*sind(t5)-gdd*sind(t1) == 0;

sol = solve([eqn1, eqn2], [a4, gdd]);

a4 = sol.a4;

gdd = sol.gdd;

gddn= [];

a4n = [];

for i =1:1:360

  gddn = [gddn subs(subs(subs(subs(subs(subs(subs(gdd, w4, v_w4(i)), a3,...

      v_a3(i)), t5, v_t5(i)), t4, v_t4(i)), w3, v_w3(i)), t3, v_t3(i)), t2, i)];

  a4n = [a4n subs(subs(subs(subs(subs(subs(subs(a4, w4, v_w4(i)), a3, v_a3(i)),...

      t5, v_t5(i)), t4, v_t4(i)), w3, v_w3(i)), t3, v_t3(i)), t2, i)];

end

v_gdd = double(gddn(1,:)); 

v_a4 = double(a4n(1,:));

% plotting x and y acceleration of arms

figure;

plot(v_t2, v_gdd.*cosd(t1), v_t2, v_gdd.*sind(t1));

title('Kinematic Analysis: linear acceleration of arms');

xlabel('Theta 2 (deg): crank angle');

ylabel('acceleration (in/s^2) of arm');

legend('x','y');

%plotting angular acceleration of arms 

figure;

plot(v_t2, v_a4);

title('Kinematic Analysis: angular acceleration of arms');

xlabel('Theta 2(deg): crank angle');

ylabel('a4 (deg/s^2)');