Figure C.1 – Kinematic Model

In the linkage displayed in the figure the links are represented by lengths r1, r2 and r3.  These lengths correspond to the link lengths of the arm where r1 is the length of the lower arm, r2 is the length of the forearm, and r3 is the length of the gripper.  The angles q1, q2’, q3’ represent the angles of the links from the horizontal.  Furthermore angle q2 represents the angle between link 1 and 2, and q3 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:

1)

2)

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:

3)

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:

4)

5)

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

6)

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

7)

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

8)

9)

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 q1.  The equation is as follows:

12)

//  MASTER CODE FOR CONTROL OF THE ARM

//

//  FILE NAME:  MASTER_1

//  CREATED:  5/08/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.invertout = 1;

jfgate.operate = 1;

jbgate.invertout = 1;

jbgate.operate = 1;

jlgate.invertout = 1;

jlgate.operate = 1;

jrgate.invertout = 1;

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;

}

}

}

//  SLAVE CODE FOR CONTROL OF THE ARM

//

//  DATE:  5/08/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.operate = 1;

wbgate.operate = 1;

wlgate.operate = 1;

wrgate.operate = 1;

}

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

// Wrist bend forward

Sub void wfgo_code(void)

{

m4f.value = 1;

m4e.value = 1;

m5f.value = 1;

m5e.value = 1;

do

{

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;

do

{

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;

do

{

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;

do

{

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;

}