1 & \

1 * 1

1 1 * 1

No. 5

fV^|

I I I

yj

UNIVERSITY OF KWA-ZULU NATAL

|

WShop Technician

II II II II

i ° 1

U

•

DATE CHECKED SCALE 1

—

—

: 1

MATERIAL: Nylon

UNITS : mm

NOTES: None 111LK: Upper Arm Ball Mounting

1 •"1 1

⁰

1

V 1

No. 6

150

UNIVERSITY OF

KWA-ZULU NATAL WShop Technician

DATE CHECKED SCALE 1:2

MATERIAL: Stcc!

UNITS: mm

NOTES: N o n e TITLE: Lower Leg No. 7

L P i

-4- e g

1 LJ

n ,31,56 J x

- —t i_ _

on II

LJ

n

<fc ^VI

-0s

s.e ?

R2S.

UNIVERSITY OF

KWA-ZULU NATAL WShop Technician

DATE CHECKED SCALE 1:2

MATERIAL: Nylu:

UNITS: mm

NOTES: None r n i E : End Effector No.

33

4

•m • »

0 0

o

CO

23

< ^

/?

-" i > -^{• i i}

0 0

UNIVERSITY OF

KWA-ZULU NATAL WShop Technician

DATE CHECKED SCALE 1: 1

MATERIAL: Aluminium

UNITS: mm

NOTHS: None TITLE: .Second Laser Mounting No. 9

CNI r--

CsJ CD

r~- 39

0 o

112

170

"1

UNIVERSITY OF

KWA-ZULU NATAL WShop Technician

DATE CHECKED SCALE 1:5

MATERIAL: Aluminium

UNITS: mm

NOTES: N o n e MILE: Servo mounting frame No. 10

LO

r—

Csl

cr-T

Csl oo

o

3,5

UNIVERSITY OF

KWA-ZULU NATAL WShop Technician

DATE CHECKED SCALE 1: 1

MATERIAL: Aluminium

UNITS: mm

None TITLE: Servo Mounting Bracket No. 11

Appendix F - Code

MATLAB CODE FLEXPICKER SIMULATION

% Solving the forward kinematics and describing the workspace

% Clear Command window clc

S_Angles = [000 0];

coee = [0 0 0];

menuselection = 4;

while (menuselection ~= 0) fprintfC\nMENU...');

fprintfi('\n\nl. Inverse Kinematics (with Forward Kinematics Check)');

fprintf('\n2. Forward Kinematics (with Inverse Kinematics Check) ');

menu_selection = inputC\n3. Vibration Model ...');

%menu_selection = 1; % remove if (menuselection = 1)

fprintfC\nEnter end effector coordinates ... \t');

fprintf('x Range: -7.5 to 7.5 \ty Range: -7.5 to 7.5 \tz Range: -25 to -15 ');

coee(l) = input('\n\nx0 :');

coee(2) = input('\ny0 :');

coee(3) = input('\nz0 :');

fprintf('\n\nThe coordinates you have selected ...');

fprintf('[%8.4f\t%8.4f\t%8.4fJ',coee);

[SAngles(l), S_Angles(2), S_Angles(3), S_Angles(4), kjl, kj2, kj3, kj4] = Inverse_Kinematics(coee(l), coee(2), coee(3));

fprintf('\n\nAnglel = %8.4f\t\t[xl yl zl] = [%8.4f\t%8.4f\t%8.4fJ',S_Angles(l) , kjl);

fprintfC\nAngle2 = %8.4f\t\t[x2 y2 z2] = [%&AWo8Af\t%8Af\\ S_Angles(2), kj2);

fprintfC\nAngle3 - %8.4f\t\t[x3 y3 z3] = [%8.4f\t%8.4f\t%8.4fJ', S_Angles(3), kj3);

fprintf('\nAngle4 = %8.4f\t\t[x4 y4 z4] = [%8.4f\t%8.4f\t%8.4f]', S_Angles(4), kj4);

fprintfC\n\n\nChecking solution with Forward Kinematics...1);

fprintfC\n\n[Anglel Angle2 Angle3 Angle4] = [%8.4f\t%8.4f\t%8.4f\t%8.4f]', S_Angles);

[coee(l), coee(2), coee(3), kjl, kj2, kj3, kj4] = Forward_Kinematics(S_Angles(l), S_Angles(2), S_Angles(3), S_Angles(4));

fprintfC\n\nAnglel = %8.4f\t\t[xl yl zl] = [%8.4f\t%8.4f\t%8.4fJ',S_Angles(l) , kjl);

fprintfC\nAngle2 - %8.4f\t\t[x2 y2 z2] = [%8.4f\t%8.4f\t%8.4f]', S_Angles(2), kj2);

fprintf('\nAngle3 = %8.4f\t\t[x3 y3 z3] = [%8.4f\t%8.4f\t%8.4fJ', S_Angles(3), kj3);

fprintf('\nAngle4 = %8.4f\t\t[x4 y4 z4] - [%8.4f\t%8.4f\t%8.4fJ', S_Angles(4), kj4);

fprintf('\n\nConvert to Servo Rotation Angles....1);

SRotAngles = Convert_to_servo_rotation_angles (S_Angles);

fprintf('\n[Anglel Angle2 Angle3 Angle4] = [%8.4f\t%8.4f\t%8.4f\t%8.4f]', S_Rot_Angles);

elseif (menuselection = 2)

fprintfC\nEnter 4 actuation Angles ... \t');

fprintf('Servo Angles Range: 30 - 170 Degrees (140 Degree Range)');

S_Angles( 1) = input('\n\nServo Angle 1 :');

S_Angles(2) = input('\nServo Angle 2 :');

S_Angles(3) = input('\nServo Angle 3 :');

S_Angles(4) = input('\nServo Angle 4 :');

fprintf('\n\nConvert to Coordinate System Angles....');

SAngles = Convert_to_coordinate_system_angles (S_Angles);

fprintf('\n[Anglel Angle2 Angle3 Angle4] = [%8.4i\t%8.4f\t%8.4f\t%8.4f]', SAngles);

[coee(l), coee(2), coee(3), kjl, kj2, kj3, kj4] = Forward_Kinematics(S_Angles(l), S_Angles(2), S_Angles(3), S_Angles(4));

%Forward_Kinematics

fprintf('\n\n[x0 yO zO] = [%8.4f\t%8.4f\t%8.4f]', coee);

fprintfC\nAngle2 = %8.4f\t\t[x2 y2 z2] = [%8.4f\t%8.4f\t%8.4f]', S_Angles(2), kj2);

fprintfC\nAngle3 = %8.4f\t\t[x3 y3 z3] = [%8.4f\t%8.4f\t%8.4f],) S_Angles(3), kj3);

fprintf('\nAngle4 - %8.4f\t\t[x4 y4 z4] = [%8.4f\t%8.4I^t%8.4f]•, S_Angles(4), kj4);

fprintfC\n\nChecking solution with Inverse Kinematics...');

[S_Angles(l), S_Angles(2), S_Angles(3), S_Angles(4), kjl, kj2, kj3, kj4] = Inverse_Kinematics(coee(l), coee(2), coee(3));

fprintf('\n\nAnglel = %8.4f\t\t[xl yl zl] = [%8.4f\t%8.4f\t%8.4f]',S_Angles(l), kjl);

fprintf('\nAngle2 = %8.4f\t\t[x2 y2 z2] = [%8.4f\t%8.4f\r%8.4f|,, S_Angles(2), kj2);

fprintfl>Angle3 = %8.4f\t\t[x3 y3 z3] = [%8.4f\t%8.4f\t%8.4f]', S_Angles(3), kj3);

fprintfi>Angle4 = %8.4f\t\t[x4 y4 z4] = [%8.4f\t%8.4f\t%8.4f]', S_Angles(4), kj4);

fprintf('\n\n[xO yO zO] - [%8.4f\t%8.4f\t%8.4f]', coee);

elseif (menuselection = 3) vf= 500;

va= 1;

vp = [0, pi/2, pi];

time = 2/500;

dt = 2/(500*99);

fprintfC\nFrequency (Hz) : %9.4f, vf);

fprintfC\nAmplirude (Degrees): %9.4f, va);

fprintf('\nSimulation Time (S) : %9.4f, time);

%Call vibration function .... Vibration (vf, va, vp, time, dt) Vibration (vf, va, vp, time, dt);

end end

CONVERT TO SERVO ROTATION ANGLES function [n] = Convert_to_servo_rotation_angles (s)

%Convert to servo angles

n(l) = mod(405 - abs(s(l)), 360); %from 45 to -135, clockwise,

%convertto0tol80 n(2) = s(2) -135; %from 135 to 315, anti-clockwise,

%convert to 0 to 180 n(3)= s(3) - 135; %from 135 to 315, anti-clockwise,

Voconvert to 0 to 180 n(4) = mod(405 - abs(s(4)), 360); %from 45 to -135, clockwise,

%convert to 0 to 180

INVERSE KINEMATICS

function [servo_angle_l, servo_angle_2, servo_angle_3, servo_angle_4, kjl, kj2, kj3, kj4] = Inverse_Kinematics(xO, yO, zO)

%function [servoanglel, servo_angle_2, servo_angle_3, servo_angle_4, kjl, kj2, kj3, kj4] = Inverse_Kinematics(xO, yO, zO)

% This Program will solve the Inverse Kinematic equations of the modified Delta

% Robot for points in space.

% Coordinates for the centre of the end effector... coee = [xO yO zO]

coee = [000];

coee(l) = x0;

coee(2) = yO;

coee(3) = zO;

if (coee(3) ~= 0) % Proceed only if zO is not equal to 0

"/(.coordinates of ankle joint 1

coajl = [(coee(l)+4.45), coee(2), coee(3)];

coaj2 = [coee(l), (coee(2)-4.45), coee(3)];

%coordinates of ankle joint 3

coaj3 • [(coee(l)-4.45), coee(2), coee(3)];

"/(.coordinates of ankle joint 4

coaj4 = [coee(l), (coee(2)+4.45) coee(3)];

%coordinates of thigh joint 1 cotjl = [10 0 0];

%coordinates of thigh joint 2 cotj2 = [0-10 0];

%coordinates of thigh joint 3 cotj3 = [-10 0 0];

%coordinates of thigh joint 4 cotj4=[0 10 0];

%A11 equations have been solved —> 1 ; if any cannot be resolved —> 0 all_equations_solved = 1;

%fprintfC\nSolving the knee coordinates for each leg ...\n')

%LEG 1

%coaj 1 = [xO+4.45 yO z0];

%cl = (5-x0)/z0 cl=(5.55-coee(l))/coee(3);

% c2 = (xO^A2 + y0^A2 + zO^A2 + 10x0 - 200)/2z0

c2 =• (coee(l)A2 + coee(2)A2 + coee(3)A2 + 8.9*coee(l) - 300.607)/(2*coee(3)); %was 200 instead of 295.41 tempi = 100-20*cl*c2-c2A2;

%if tempi < 0 then we have no real solutions

if (tempi < 0) | (allequationssolved = 0)

%Place and escape sequence here allequationssolved = 0;

servoanglel = -400;

kjl = H00,-400,-400];

else

%Solution set 1

% cokjlsl - coordinates of knee joint 1 solution 1

% c o k j l s l = [ x l y l z l ] cokjlsl = [0 0 0];

% xl = (-clc2 + 10 + (templ)A0.5)/(clA2 + 1) cokjlsl(l) = (-cl*c2+ 10 + (templ)A0.5)/(clA2+ 1);

%zl = c l x l + c 2

cokjlsl(3) = cl*cokjlsl(l) + c2;

%Solution set 2

% cokj 1 s2 - coordinates of knee joint 1 solution 2

%cokjls2 = [xlyl zl]

cokj ls2 = [0 0 0];

% xl = (-clc2 + 10 - (templ)A0.5)/(clA2 + 1) cokjls2(l) = (-cl*c2+ 10-(templ)A0.5)/(clA2+ 1);

% z l = c l x l + c 2

cokj ls2(3) = cl*cokj ls2(l) + c2;

[servoanglel, kjl] = Determine_correct_solution(cokjlsl, cokjls2, coajl, cotjl); % (si, s2, a, t) end

%LEG 2

%coordinates of ankle joint 2 — coaj2 = [xO y0-5 zO]

%cll2 = (-5-y0)/z0 cl = (-5.55 - coee(2))/coee(3);

% c211 = (xO^A2 + y0^A2 + zO^A2 - lOyO - 200)/2z0

c2 = (coee(l)A2 + coee(2)A2 + coee(3)A2 - 8.9*coee(2) - 300.607)/(2*coee(3)); %was 200 instead of 295.41 temp2 = 100 + 20*cl *c2 - c2A2;

%if temp2 < 0 then we have no real solutions if (terap2 < 0) | (allequationssolved = 0)

%Place and escape sequence here allequationssolved = 0;

servo_angle_2 = -400;

kj2 = [-400, -400, -400];

else

%Solution set 1

% cokj2sl - coordinates of knee joint 2 solution 1

%cokj2sl = [x2y2z2]

cokj2sl = [0 0 0];

% y2 - (-clc2 -10 + (templ)A0.5)/(clA2 + 1) cokj2sl(2) = (-cl*c2 -10 + (temp2)A0.5)/(clA2 + 1);

%z2 = cly2 + c2

cokj2sl(3) = cl*cokj2sl(2) + c2;

%Solution set 2

% cokj2s2 - coordinates of knee joint 2 solution 2

% cokj2s2 = [x2 y2 z2]

cokj2s2 = [0 0 0];

% y2 = (-clc2 - 10 - (templ)^A0.5)/(cl^A2 + 1) cokj2s2(2) = (-cl*c2 - 10 - (temp2)A0.5)/(clA2 + 1);

%z2 = cly2 + c2

cokj2s2(3) = c 1 *cokj2s2(2) + c2;

[servo_angle_2, kj2] - Determine_correct_solution(cokj2sl, cokj2s2, coaj2, cotj2); % (si, s2, a, t) end

%LEG 3

%coaj3 = [xO-5yOzO];

%cl=(-5-x0)/z0

cl = (-5.55 - coee(l))/coee(3);

% c2 = (x0A2 + y0A2 + z0A2 + 10x0 - 200)/2z0

c2 = (coee(l)A2 + coee(2)A2 + coee(3)A2 - 8.9*coee(l) - 300.607)/(2*coee(3)); %was 200 instead of 295.41 temp3 = 100 + 20*cl *c2 - c2A2;

%if temp3 < 0 then we have no real solutions if (temp3 < 0) | (allequationssolved = 0)

%Place and escape sequence here allequationssolved = 0;

servo_angle_3 = -400;

kj3 = [-400, -400, -400];

else

%Solution set 1

% cokj3sl - coordinates of knee joint 3 solution 1

% cokj3sl = [x3 y3 z3]

cokj3sl = [0 0 0];

% x3 = (-clc2 + 10 + (templ)A0.5)/(clA2 + 1) cokj3sl(l) = (-cl*c2- 10 + (temp3)A0.5)/(clA2+ 1);

% z3 = clx3 + c2

cokj3sl(3) = cl*cokj3sl(l) + c2;