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)');