EXAMPLE ADVANCED PROGRAMMING WITH ROBOFORTH

BIOMASH HANDLING ROBOT SYSTEM

Robot takes a pestle from tray on left, vertical lift then sideways over to an Eppendorf well in central tray from A to B.
The Eppendorf contains a sample.
The robot then mashes the sample at B using rotation in both directions and up and down movements. User wanted movements up and down of 1mm.
Rotation
The rotation is a DC motor controlled by the electronic module normally fitted for an electric gripper and is controlled for speed in the same way using variable mark-space ratio. The ratio is TON to 1000 as below
To do this I set up the rotation using the GRIP code and the motion on interrupt.
The interrupt is set like this:

: JOG-TEST
SET INTVEC TFLASH
500 INT-TIME !
START-TIMER
;

TFLASH is

: 1FLASH JUP JDN ;
: TFLASH 1FLASH RETURN ;

to return from interrupt as per the RoboForth manual.
The 1mm movements themselves are
up:

: JUP 10 Z +! TRANSFORM DSPSMOOTH DROP ?STOP DSPASSUME ;

10 Z +! - change Z in the Cartesian coordinates,
TRANSFORM - calculate joint coordinates
DSPSMOOTH DROP - tell DSP to make the motion without interaction with the CPU (which is busy)
?STOP - wait for DSP to finish (also checks stop button)
DSPASSUME - have to read back the joint coordinates from the DSP
Same for down

: JDN -10 Z +! TRANSFORM DSPSMOOTH DROP ?STOP DSPASSUME ;

JUP and JDN run in background on timer interrupts.
Then there are 2 words CW and ACW to keep the motor spinning using PWM; mark is TON, space is 1000

: CW
0 PA OUT
RESETMS
100 SPINSPEED @ * 10 / TON ! ( SPINSPEED is provided for the user.
BEGIN
  PA 0 ON
  TON @ USECS
  PA 0 OFF
  1000 USECS
MSECS? SPINTIME @ > UNTIL
FGRIP C1SET
STOP-TIMER
;

ACW is same but PA 1
Then to jog while spinning clockwise:

: CWJOG
1 PA OUT JOG-TEST
CW
;

and ACWJOG is same but 2 PA OUT

Path programming

Once the biomash is complete the robot has to take the used pestle to a dropoff tray on the right.
But it can't just take the quickest route. It is not allowed to pass over other samples in case it drips.
The solution is to create a path alongside a column, 10mm to the left. I created a route DRIP with 10 positions. 8 of them correspond with the Eppendorf and dropoff positions and the extra 2 are so the robot can get clear to the front of the system.
The first step is to get the coordinates of well #1. Add 10mm to the X axis to shift the route to the side. Then use START-HERE to shift the route to start from this position.
I break this process into smaller parts - never try to write the whole function in one go. It's a good Forth rule.
I also need two functions to decide row and column. The well number is in WELL
To find the row number subtract 1 (because calcs start from zero) then divide by 3 then add the 1 back again.
To find the column number again subtract 1 then divide by 3 and use the modulus.

: ROW ( ROW NUMBER
1- 3 / 1+
;
: COL
1- 3 /MOD DROP ( GIVE COLUMN No. 0 1 OR 2
;

Then the actual path, A in the image. The rows are 45mm apart so use the COL value * 450 to get the X value.

: AV1 ( PATH TO SIDE AND FRONT
EPTRAY 1 LINE ASSUME
100 X +! ( SHIFT LEFT 10MM
WELL @ COL 450 * 0 SWAP - X +!
DRIP START-HERE

now need to run it.
But we wont want to run from line 1 to the end. If we are taking say from well 7 that would be in row 3 so we want to run from line 3 to the end. So I use the SEQUENCE feature. This will run from any line to any other line, forward or backwards.

WELL @ ROW 10 SEQUENCE SMOOTH RUN RESTORE
;

for say well 7 the RUN would be from 3 to 10. Using SMOOTH.
Next we need to get across to the same column on the dropoff tray. again shifted 10mm to left. I did this crudely just moving fxed amounts plus a change of ROLL (180 deg the other way) and using the COL function to decide what amount and at same time add in another 20mm on Z. Line B in the image.

: AV2 ( MOVE READY FOR SAME COLUMN ON DROPOFF
WELL @ COL
DUP 0 = IF -750 SWAP THEN ( these are the differences between where we come from (Eppendorf column) to where we want to go to.
DUP 1 = IF -550 SWAP THEN
    2 = IF -300 THEN
-900 ROLL !
FAST
 0 200 MOVE
NORMAL
;

Now to head back down the other way to put the used pestle in the dropoff tray, a SEQUENCE from 9 down to line number from ROW ie backwards. Line C in the image.

: AV3 ( GO BACK DOWN COLUMN LEVEL WITH INTENDED DROPOFF
-1750 Y +! ( LEVEL WITH WELL 1
DRIP START-HERE
9 ( START OF MOVE
WELL @ ROW ( LINE NUMBER OF END OF MOVE
SEQUENCE ( GOING BACKWARDS
SMOOTH RUN RESTORE
( WELL @ DROPOFF
;