VIII. Appendix - CZ

// Motor A connections
int enA = 9;
int in1 = 8;
int in2 = 7;
// Motor B connections
int enB = 3;
int in3 = 5;
int in4 = 4;

void setup() {
// Set all the motor control pins to outputs
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);

// Turn off motors - Initial state
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}

void loop() {
directionControl();
delay(1000);
}

// This function lets you control spinning direction of motors
void directionControl() {
// Set motors to maximum speed
// For PWM maximum possible values are 0 to 255
analogWrite(enA, 40);
analogWrite(enB, 20);

// Turn on motor A & B
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
delay(2000);

}

%Calculate all leg lengths

%coordinates
%fixed
rocker1 = [1.366,1.366];
rocker2 = [1.009,0.574];
shaft = [1.599,0.750];
%extended
elbow = [0.741,0.750];
crank = [1.331,0.750];
foot = [0,0];
knee = [0.232,0.866];
hip = [0.866,1.5];

scale = 10; %scale factor for all leg lengths. 1 = no scaling

%lengths
leg_lower = scale*Leg_length(foot,knee)
leg_upper = scale*Leg_length(knee,hip)
arm_outer = scale*Leg_length(knee,elbow)
arm_inner = scale*Leg_length(elbow,crank)
arm_total = scale*Leg_length(knee,crank)
rocker_upper = scale*Leg_length(hip,rocker1)
rocker_lower = scale*Leg_length(elbow,rocker2)
crank_length = scale*Leg_length(crank,shaft)

triangle_outer = scale*Leg_length(rocker1,rocker2)
triangle_inner = scale*Leg_length(rocker1,shaft)
triangle_lower = scale*Leg_length(rocker2,shaft)

%angles
leg_angle = Leg_angle(foot,knee,hip)
arm_angle = Leg_angle(knee,elbow,crank)
triangle_lower_angle = Leg_angle(rocker2,shaft,crank)

%calculate leg length

function [length] = Leg_length(pos1,pos2) %calculate the length of a leg given two xy coordinates
x1 = pos1(1);
y1 = pos1(2);
x2 = pos2(1);
y2 = pos2(2);
delx = abs(x2-x1);
dely = abs(y2-y1);
length = sqrt(delx^2+dely^2);
end

%calculate angles for leg

function [angle] = Leg_length(pos1,pos2,pos3)
x1 = pos1(1);
y1 = pos1(2);
x2 = pos2(1);
y2 = pos2(2);
x3 = pos3(1);
y3 = pos3(2);

delx1 = abs(x2-x1);
dely1 = abs(y2-y1);
delx2 = abs(x3-x2);
dely2 = abs(y3-y2);

slope1 = dely1/delx1; %find slopes
slope2 = dely2/delx2;

angle = atan(abs((slope1-slope2)/(1+slope1*slope2)))*(180/pi); %formula for angle
end

%Robot Position, Velocity, and Acceleration calculations

% Constants
O2A = 2.68; % Crank_length
AB = 5.9; % Arm_inner
BO4 = 3.2062; %rocker_lower
O4O2 = 6.1569; %triangle_lower
AC = 11.051; %arm_total
CD = 8.9661; %Leg_upper
DO6 = 5.1764; %rocker_upper
O6O2 = 6.5859; %triangle_inner

omega2 = -10 %rpm

theta1 = 16.61*(pi/180); %triangle_lower_angle (radians)
theta2 = 0:0.1:2*pi+0.1;
psi = -1;

for i = 1:length(theta2)
%First 4-bar analysis
%calculate angle of arm
[A,B,C,D] = fourbarpos(O2A,AB,BO4,O4O2,0,theta2(i)-theta1,psi);
theta3(i) = C+theta1;
theta4(i) = D+theta1;

%calculate velocity of arm
omega3(i) = (O2A*omega2*(sin(theta2(i))*cos(theta4(i))-cos(theta2(i))*sin(theta4(i))))/(AB*(sin(theta3(i))*cos(theta4(i))-cos(theta3(i))*sin(theta4(i))));
omega4(i) = (O2A*omega2*sin(theta2(i))-AB*omega3(i)*sin(theta3(i)))/(BO4*sin(theta4(i)));

%calculate acceleration of arm
alpha3(i) = (O2A*(omega2^2)*(cos(theta2(i))*cos(theta4(i))+sin(theta2(i))*sin(theta4(i)))-AB*(omega3(i)^2)*(cos(theta3(i))*cos(theta4(i))+sin(theta3(i))*sin(theta4(i)))-BO4*(omega4(i)^2)*(cos(theta4(i))^2+sin(theta4(i))^2))/(AB*(sin(theta3(i))*cos(theta4(i))-cos(theta3(i))*sin(theta4(i))));
alpha4(i) = (O2A*(omega2^2)*cos(theta2(i))-AB*alpha3(i)*sin(theta3(i))-AB*(omega3(i)^2)*cos(theta3(i))-BO4*(omega4(i)^2)*cos(theta4(i)))/(BO4*sin(theta4(i)));

%Secondary 5-bar analysis
%calculate angle of leg
theta5(i) = C+theta1; % Need to figure out how to solve this
theta6(i) = D+theta1;

%calculate velocity of leg
omega5(i) = (O2A*omega2*(sin(theta2(i))*cos(theta6(i))-cos(theta2(i))*sin(theta6(i)))-AC*omega3(i)*(sin(theta3(i))*cos(theta6(i))-cos(theta6(i))*sin(theta6(i))))/(CD*(sin(theta5(i))*cos(theta6(i))-cos(theta5(i))*sin(theta6(i))));
omega6(i) = (O2A*omega2*sin(theta2(i))-AC*omega3(i)*sin(theta3(i))-CD*omega5(i)*sin(theta5(i)))/(DO6*sin(theta6(i)));

%calculate acceleration of leg
alpha5(i) = (O2A*(omega2^2)*(cos(theta2(i))*cos(theta6(i))+sin(theta2(i))*sin(theta6(i)))-AC*alpha3(i)*(sin(theta3(i))*cos(theta6(i))-cos(theta3(i))*sin(theta6(i)))-AC*(omega3(i)^2)*cos(theta3(i))*cos(theta6(i))+sin(theta3(i))*sin(theta6(i))-DO6*(omega6(i)^2)*(cos(theta6(i))^2+sin(theta6(i))^2))/(CD*(sin(theta5(i))*cos(theta6(i))-cos(theta5(i))*sin(theta6(i))));
alpha6(i) = (O2A*(omega2^2)*(cos(theta2(i)))-AC*alpha3(i)*sin(theta3(i))-AC*(omega3(i)^2)*cos(theta3(i))-CD*alpha5(i)*sin(theta5(i))-CD*(omega5(i)^2)*cos(theta5(i))-DO6*(omega6(i)^2)*cos(theta6(i)))/(DO6*sin(theta6(i)));

end

figure(1)
plot(theta2*(180/pi),theta3*(180/pi));
title("Arm Angle")
xlabel("Crank 2 Angle (degrees)");
ylabel("Linkage 3 Angle (degrees)");

figure(2)
plot(theta2*(180/pi),theta4*(180/pi));
title("Lower Rocker Angle")
xlabel("Crank 2 Angle (degrees)");
ylabel("Linkage 4 Angle (degrees)");

figure(2)
plot(theta2*(180/pi),omega3);
title("Arm velocity")
xlabel("Crank 2 Angle (degrees)");
ylabel("Omega3");

figure(3)
plot(theta2*(180/pi),alpha3);
title("Arm acceleration")
xlabel("Crank 2 Angle (degrees)");
ylabel("Alpha3");

%four bar position function

function [theta1,theta2,theta3,theta4] = fourbarpos(a,b,c,d,theta1,theta2,psi) % - psi is open, + psi is cross chain

K1 = d/a;
K2 = d/c;
K3 = (a^2-b^2+c^2+d^2)/(2*a*c);
K4 = d/b;
K5 = (c^2-a^2-b^2-d^2)/(2*a*b);

A = cos(theta2)-K1-K2*cos(theta2)+K3;
B = -2*sin(theta2);
C = K1 - (K2 + 1)*cos(theta2)+K3;
D = cos(theta2)-K1+K4*cos(theta2)+K5;
E = -2*sin(theta2);
F = K1 + (K4-1)*cos(theta2)+K5;

theta4 = 2*atan((-B+psi*sqrt(B^2-4*A*C))/(2*A));
theta3 = 2*atan((-E+psi*sqrt(E^2-4*D*F))/(2*D));

end