APPENDIX C

Figure C.1 � Kinematic Model

In the linkage displayed in the figure the links are represented by lengths r₁, r₂ and r₃.� These lengths correspond to the link lengths of the arm where r₁ is the length of the lower arm, r₂is the length of the forearm, and r₃ is the length of the gripper.� The angles q₁, q₂�, q₃� represent the angles of the links from the horizontal.� Furthermore angle q₂ represents the angle between link 1 and 2, and q₃ represents the angle between links 2 and 3.� The kinematics analysis is detailed in the following paragraphs and equations.

The following relationships were determined by analyzing the geometry of the linkage:

The position of the end of link 3, i.e. the tip of the gripper, was evaluated analytically using open-loop kinematic analysis of the linkage.� In the following equation x is the position of the gripper in the horizontal direction:

Equations (1) and (2) were substituted into equation (3) to obtain position equations in terms of the angles between the links instead of from the horizontal.� In the following equations y represents the position of the end of link 3 in the vertical direction:

An assumption was made that angle q₃� must remain constant.� Equation (2) was then differentiated, yielding the following equation:

Solving (6) for the derivative of q₃ yielded the following relationship:

Equations (4) and (5) were then differentiated, incorporating the simplifications from (6) and (7).� This yielded the following equations:

Equations (8) and (9) were rearranged as follow:

10)

11)

Finally equations (10) and (11) were set equal to each other and solved for the derivative of angle q₁.� The equation is as follows:

12)

C.2 MASTER OOPIC CONTROL CODE

//� MASTER CODE FOR CONTROL OF THE ARM

//� FILE NAME:� MASTER_1

//� CREATED:� 5/08/01

//� LAST MODIFIED:� 5/29/01

//� AUTHORS:� MICHAEL STEVENS AND AARON WEAVER

//� NOTES:� THIS CODE WILL OPERATE THE ARM

//�� USING FOUR DIRECTIONS ON THE

//�� JOYSTICK.� ANOTHER OOPIC WILL BE USED TO

//�� OPERATE THE WRIST.� MODES OF COMMUNICATION

//�� WILL BE SENT FROM THIS MASTER CODE TO THE

//�� SLAVE VIA I/O LINES.

//� MOTORS ARE NUMBERED AS FOLLOWS:

//�� #1 = SHOULDER BEND

//�� #2 = SHOULDER TWIST

//�� #3 = ELBOW BEND

//�� #4 & #5 = FOREARM

//�� #6 = GRIPPER

opwm pwm1 = new opwm;�� // PWM #1, for m3 & m6

opwm pwm2 = new opwm;�� // PWM #2, for m1 & m2

odio1 m1f = new odio1;�� // Motor #1 forward line

odio1 m1e = new odio1;�� // Motor #1 enable line

odio1 m1b = new odio1;�� // Motor #1 backward line

odio1 m2f = new odio1;�� // Motor #2 forward line

odio1 m2e = new odio1;�� // Motor #2 enable line

odio1 m2b = new odio1;�� // Motor #2 backward line

odio1 m3f = new odio1;�� // Motor #3 forward line

odio1 m3e = new odio1;�� // Motor #3 enable line

odio1 m3b = new odio1;�� // Motor #3 backward line

odio1 m6f = new odio1;�� // Motor #6 forward line

odio1 m6e = new odio1;�� // Motor #6 enable line

odio1 m6b = new odio1;�� // Motor #6 backward line

odio1 jf = new odio1;�� // Joystick forward line

odio1 jb = new odio1;�� // Joystick backward line

odio1 jl = new odio1;�� // Joystick left line

odio1 jr = new odio1;�� // Joystick right line

ogate jfgate = new ogate;�� // Gate to link jfgo to jf

ogate jbgate = new ogate;�� // Gate to link jbgo to jb

ogate jlgate = new ogate;�� // Gate to link jlgo to jl

ogate jrgate = new ogate;�� // Gate to link jrgo to jr

oevent jfgo = new oevent;�� // Event for joystick forward

oevent jbgo = new oevent;�� // Event for joystick backward

oevent jlgo = new oevent;�� // Event for joystick left

oevent jrgo = new oevent;�� // Event for joystick right

odio1 s1 = new odio1;�� // Switch #1

odio1 s2 = new odio1;�� // Switch #2

odio1 s3 = new odio1;�� // Switch #3

odio1 wf = new odio1;�� // Command SLAVE to bend wrist forward

odio1 wb = new odio1;�� // Command SLAVE to bend wrist backward

odio1 wl = new odio1;�� // Command SLAVE to twist wrist left

odio1 wr = new odio1;�� // Command SLAVE to twist wrist right

obit nogo = new obit;

sub void main(void)

{

pwm1.ioline = 17;

pwm1.prescale = 3;

pwm1.period = 255;

pwm1.operate = 1;

pwm1.value = 0;

pwm2.ioline = 18;

pwm2.prescale = 3;

pwm2.period = 255;

pwm2.operate = 1;

pwm2.value = 0;

m1f.ioline = 31;

m1f.direction = cvoutput;

m1b.ioline = 30;

m1b.direction = cvoutput;

m1e.ioline = 29;

m1e.direction = cvoutput;

m2f.ioline = 28;

m2f.direction = cvoutput;

m2b.ioline = 27;

m2b.direction = cvoutput;

m2e.ioline = 26;

m2e.direction = cvoutput;

m3f.ioline = 25;

m3f.direction = cvoutput;

m3b.ioline = 24;

m3b.direction = cvoutput;

m3e.ioline = 23;

m3e.direction = cvoutput;

wf.ioline = 5;

wf.direction = cvoutput;

wb.ioline = 4;

wb.direction = cvoutput;

wl.ioline = 7;

wl.direction = cvoutput;

wr.ioline = 6;

wr.direction = cvoutput;

m6f.ioline = 3;

m6f.direction = cvoutput;

m6b.ioline = 2;

m6b.direction = cvoutput;

m6e.ioline = 1;

m6e.direction = cvoutput;

jf.ioline = 13;

jf.direction = cvinput;

jb.ioline = 14;

jb.direction = cvinput;

jl.ioline = 12;

jl.direction = cvinput;

jr.ioline = 11;

jr.direction = cvinput;

s1.ioline = 8;

s1.direction = cvinput;

s2.ioline = 9;

s2.direction = cvinput;

s3.ioline = 10;

s3.direction = cvinput;

m1f.value = 0;

m1b.value = 0;

m1e.value = 0;

m2f.value = 0;

m2b.value = 0;

m2e.value = 0;

m3f.value = 0;

m3b.value = 0;

m3e.value = 0;

wf.value = 0;

wb.value = 0;

wl.value = 0;

wr.value = 0;

m6f.value = 0;

m6b.value = 0;

m6e.value = 0;

jfgate.input1.link(jf);

jfgate.invertout = 1;

jfgate.output.link(jfgo);

jfgate.operate = 1;

jbgate.input1.link(jb);

jbgate.invertout = 1;

jbgate.output.link(jbgo);

jbgate.operate = 1;

jlgate.input1.link(jl);

jlgate.invertout = 1;

jlgate.output.link(jlgo);

jlgate.operate = 1;

jrgate.input1.link(jr);

jrgate.invertout = 1;

jrgate.output.link(jrgo);

jrgate.operate = 1;

}

//////////////////////////////////////////////////////////////////

// Joystick forward

Sub void jfgo_code(void)

{

nogo = 0;

if (s1.value & s2.value)

{

�nogo = 1;

}

if (s1.value & s3.value)

{

�nogo = 1;

}

if (s2.value & s3.value)

{

�nogo = 1;

}

if (nogo == 0)

{

�if (s1.value)

�{

� m1f.value = 1;

� m1e.value = 1;

� do

� {

�� pwm2.value = 153;

�� wb.value = 1;

� } while (jf == 0);

� pwm2.value = 10;

� OOPic.delay = 20;

� pwm2.value = 0;

� m1f.value = 0;

� m1e.value = 0;

� wb.value = 0;

�}

�if (s2.value)

�{

� m3f.value = 1;

� m3e.value = 1;

� pwm1.value = 30;

� OOPic.delay = 10;

� do

� {

�� pwm1.value = 90;

�� wb.value = 1;

� } while (jf == 0);

� pwm1.value = 10;

� OOPic.delay = 20;

� pwm1.value = 0;

� m3f.value = 0;

� m3e.value = 0;

� wb.value = 0;

�}

�if (s3.value)

�{

� m6f.value = 1;

� m6e.value = 1;

� pwm1.value = 30;

� OOPic.delay = 10;

� do

� {

�� pwm1.value = 90;

� } while (jf == 0);

� pwm1.value = 10;

� OOPic.delay = 20;

� pwm1.value = 0;

� m6f.value = 0;

� m6e.value = 0;

�}

}

// Joystick back

Sub void jbgo_code(void)

{

nogo = 0;

if (s1.value & s2.value)

{

�nogo = 1;

}

if (s1.value & s3.value)

{

�nogo = 1;

}

if (s2.value & s3.value)

{

�nogo = 1;

}

if (nogo == 0)

{

�if (s1.value)

�{

� m1b.value = 1;

� m1e.value = 1;

� do

� {

�� pwm2.value = 153;

�� wf.value = 1;

� } while (jb == 0);

� pwm2.value = 10;

� OOPic.delay = 20;

� pwm2.value = 0;

� m1b.value = 0;

� m1e.value = 0;

� wf.value = 0;

�}

�if (s2.value)

�{

� m3b.value = 1;

� m3e.value = 1;

� pwm1.value = 30;

� OOPic.delay = 10;

� do

� {

�� pwm1.value = 90;

�� wf.value = 1;

� } while (jb == 0);

� pwm1.value = 10;

� OOPic.delay = 20;

� pwm1.value = 0;

� m3b.value = 0;

� m3e.value = 0;

� wf.value = 0;

�}

�if (s3.value)

�{

� m6b.value = 1;

� m6e.value = 1;

� pwm1.value = 30;

� OOPic.delay = 10;

� do

� {

�� pwm1.value = 90;

� } while (jb == 0);

� pwm1.value = 10;

� OOPic.delay = 20;

� pwm1.value = 0;

� m6b.value = 0;

� m6e.value = 0;

�}

}

// Joystick left

Sub void jlgo_code(void)

{

nogo = 0;

if (s1.value & s2.value)

{

�nogo = 1;

}

if (s1.value & s3.value)

{

�nogo = 1;

}

if (s2.value & s3.value)

{

�nogo = 1;

}

if (nogo == 0)

{

�if (s1.value)

�{

� m2f.value = 1;

� m2e.value = 1;

� pwm2.value = 30;

� OOPic.delay = 5;

� do

� {

�� pwm2.value = 90;

� } while (jl == 0);

� pwm2.value = 10;

� OOPic.delay = 20;

� pwm2.value = 0;

� m2f.value = 0;

� m2e.value = 0;

�}

�if (s2.value)

�{

� do

� {

�� wb.value = 1;

� } while (jl == 0);

� wb.value = 0;

�}

�if (s3.value)

�{

� do

� {

�� wl.value = 1;

� } while (jl == 0);

� wl.value = 0;

�}

}

// Joystick right

Sub void jrgo_code(void)

{

nogo = 0;

if (s1.value & s2.value)

{

�nogo = 1;

}

if (s1.value & s3.value)

{

�nogo = 1;

}

if (s2.value & s3.value)

{

�nogo = 1;

}

if (nogo == 0)

{

�if (s1.value)

�{

� m2b.value = 1;

� m2e.value = 1;

� pwm2.value = 35;

� OOPic.delay = 10;

� do

� {

�� pwm2.value = 80;

� } while (jr == 0);

� pwm2.value = 10;

� OOPic.delay = 20;

� pwm2.value = 0;

� m2b.value = 0;

� m2e.value = 0;

�}

�if (s2.value)

�{

� do

� {

�� wf.value = 1;

� } while (jr == 0);

� wf.value = 0;

�}

�if (s3.value)

�{

� do

� {

�� wr.value = 1;

� } while (jr == 0);

� wr.value = 0;

�}

}

C.3 SLAVE OOPIC CONTROL CODE

//� SLAVE CODE FOR CONTROL OF THE ARM

//� DATE:� 5/08/01

//� LAST MODIFIED:� 5/29/01

//� AUTHORS:� MICHAEL STEVENS AND AARON WEAVER

opwm pwm3 = new opwm;�� // PWM #3, for m4

opwm pwm4 = new opwm;�� // PWM #4, for m5

odio1 wf = new odio1;�� // Command from MASTER to bend wrist forward

odio1 wb = new odio1;�� // Command from MASTER to bend wrist backward

odio1 wl = new odio1;�� // Command from MASTER to twist wrist left

odio1 wr = new odio1;�� // Command from MASTER to twist wrist right

odio1 m4f = new odio1;�� // Motor #4 forward line

odio1 m4e = new odio1;�� // Motor #4 enable line

odio1 m4b = new odio1; ��// Motor #4 backward line

odio1 m5f = new odio1;�� // Motor #5 forward line

odio1 m5e = new odio1;�� // Motor #5 enable line

odio1 m5b = new odio1;�� // Motor #5 backward line

ogate wfgate = new ogate;�� // Gate to link wfgo to wf

ogate wbgate = new ogate;�� // Gate to link wbgo to wb

ogate wlgate = new ogate;�� // Gate to link wlgo to wl

ogate wrgate = new ogate;�� // Gate to link wrgo to wr

oevent wfgo = new oevent;�� // Event for wrist bend forward

oevent wbgo = new oevent;� ��// Event for wrist bend backward

oevent wlgo = new oevent;�� // Event for wrist bend left

oevent wrgo = new oevent;�� // Event for wrist bend right

sub void main(void)

{

wf.ioline = 5;

wf.direction = cvinput;

wb.ioline = 4;

wb.direction = cvinput;

wl.ioline = 7;

wl.direction = cvinput;

wr.ioline = 6;

wr.direction = cvinput;

pwm3.ioline = 17;

pwm3.prescale = 3;

pwm3.period = 255;

pwm3.operate = 1;

pwm3.value = 0;

pwm4.ioline = 18;

pwm4.prescale = 3;

pwm4.period = 255;

pwm4.operate = 1;

pwm4.value = 0;

m4f.ioline = 31;

m4f.direction = cvoutput;

m4b.ioline = 30;

m4b.direction = cvoutput;

m4e.ioline = 29;

m4e.direction = cvoutput;

m5f.ioline = 28;

m5f.direction = cvoutput;

m5b.ioline = 27;

m5b.direction = cvoutput;

m5e.ioline = 26;

m5e.direction = cvoutput;

wfgate.input1.link(wf);

wfgate.output.link(wfgo);

wfgate.operate = 1;

wbgate.input1.link(wb);

wbgate.output.link(wbgo);

wbgate.operate = 1;

wlgate.input1.link(wl);

wlgate.output.link(wlgo);

wlgate.operate = 1;

wrgate.input1.link(wr);

wrgate.output.link(wrgo);

wrgate.operate = 1;

}

/////////////////////////////////////////////////

// Wrist bend forward

Sub void wfgo_code(void)

{

m4f.value = 1;

m4e.value = 1;

m5f.value = 1;

m5e.value = 1;

{

�pwm3.value = 102;

�pwm4.value = 102;

} while (wf == 1);

pwm3.value = 10;

pwm4.value = 10;

OOPic.delay = 20;

pwm3.value = 0;

pwm4.value = 0;

m4f.value = 0;

m4e.value = 0;

m5f.value = 0;

m5e.value = 0;

}

// Wrist bend backward

Sub void wbgo_code(void)

{

m4b.value = 1;

m4e.value = 1;

m5b.value = 1;

m5e.value = 1;

{

�pwm3.value = 102;

�pwm4.value = 102;

} while (wb == 1);

pwm3.value = 10;

pwm4.value = 10;

OOPic.delay = 20;

pwm3.value = 0;

pwm4.value = 0;

m4b.value = 0;

m4e.value = 0;

m5b.value = 0;

m5e.value = 0;

}

// Wrist twist left

Sub void wlgo_code(void)

{

m4f.value = 1;

m4e.value = 1;

m5b.value = 1;

m5e.value = 1;

{

�pwm3.value = 102;

�pwm4.value = 102;

} while (wl == 1);

pwm3.value = 10;

pwm4.value = 10;

OOPic.delay = 20;

pwm3.value = 0;

pwm4.value = 0;

m4f.value = 0;

m4e.value = 0;

m5b.value = 0;

m5e.value = 0;

}

// Wrist twist right

Sub void wrgo_code(void)

{

m4b.value = 1;

m4e.value = 1;

m5f.value = 1;

m5e.value = 1;

{

�pwm3.value = 102;

�pwm4.value = 102;

} while (wr == 1);

pwm3.value = 10;

pwm4.value = 10;

OOPic.delay = 20;

pwm3.value = 0;

pwm4.value = 0;

m4b.value = 0;

m4e.value = 0;

m5f.value = 0;

m5e.value = 0;

}

Table of Contents

Appendix B

Appendix D