add to all - this will add the same value to all the lines at once, useful for shifting an entire route up or down.
Typical editing dialog: 
Typing the name of the route makes it the currently active route.
All commands thereafter such as LISTROUTE, RUN, LINE, GOTO, LEARN, REPLACE, DELETE, RETRACE etc.
will apply to the currently active route and no other route.
To see which route is currently active enter:-
WHICH TEST1 ABSOLUTE OK
The routes window lists all the routes created so far or in the command window enter:-
ROUTES
nnnn 6 TEST2 nnnn 9 TEST1
2 words
OK
The "nnnn n" in the above are the RAM address and lengths of each
word in the dictionary. See VLIST under INFORMATION.
Editing in the comms window alone
The route window provides all necessary editing buttons. However if you use any of the following commands they will change data in the controller but not in RobWin. To update RobWin click on 
LEARN - adds one line to the route.
(n) DELETE deletes the line number (n) and moves all the following lines down one.
(n) INSERT moves all lines from (n) up one and inserts a new line (n) with the current co-ordinates of the robot.
INSERT may produce a FULL error even when no recent entities have
been added, in which case reserve more space with (n) RESERVE.
(n) REPLACE
replaces the co-ordinates of line n with the current co-ordinates of the robot. This command can also be used in user's software (new definitions).
You would not normally use any of the above commands while using RobWin as RobWin sends these commands when you click the various buttons.
However (n) REPLACE is often used within a program to self-learn a particular position, for example by computation or to synchronize the end of one route with the start of another. See below and example in section 12.
ERASE erases the entire list.
UNLEARN deletes the last line learned.
STARTOVER would do the same but would also restore the NEXT
pointer to initial value which would be disastrous for any
entities which had been created. If no entities have been created
then it is OK to use STARTOVER. Normally STARTOVER and ROBOFORTH
are used together. ROBOFORTH clears out all the user's words from
the dictionary requiring a fresh load from the text file.
A list of co-ordinates is not necessarily to be RUN in sequence. It can also be used as an array or matrix of positions for example a grid of test tubes. See Matrices. To access the data in individual lines:
(n) LINE returns the memory address of the data in line (n).
This address is suitable for words like GOTO and ASSUME. e.g.
(n) LINE GOTO
GOTO requires the address of some positional data. If you leave
out LINE then GOTO assumes the argument is a line number and finds the data e.g.
(n) GOTO
Click 'goto' on the route window.
For ASSUME the syntax is (n) LINE ASSUME
For VIEW the syntax is VIEW LINE (n)
To run the route type RUN
Click 'run' on the route window. The robot will progress from line to line, stopping at each point in the route. This is the default SEGMENTED mode. To run through the route without stopping see CONTINUOUS mode.
To RUN a limited amount of the route enter:
(n1) (n2) SEQUENCE RUN
which runs the route only from line n1 to line n2 (inclusive).
Note that n1 can be bigger than n2 in which case the route runs
in reverse order.
In segmented mode as a route is being executed it is listed out line by line. This actually takes a significant time and can slow down the robot, especially for small movements. The listing can be turned off with the command LISTFLAG C0SET (pronounced "list-flag see-nought-set").
To turn listing back on enter LISTFLAG C1SET ("list-flag see-one-set").
NOTE: in C0SET the 0 is a zero not letter O.
Segmented mode only: To introduce a delay into the program highlight the line before which you wish to insert the delay and click 'insert func'. In the dialog box enter the word MSECS and in the lower space enter the number of milliseconds to pause. Click the space after the last line to 'insert' the delay onto the end of the route.
To abort the route while it is running press the escape key - the robot will stop
after executing the current line. The stop button stops the robot
immediately. The line at which the route aborted is contained in
the variable LINE#. Inspect with LINE# ?
FIRST# and LAST# are variables containing the first and last
lines to be RUN (set by SEQUENCE).
MOVES is a pseudo-variable i.e. a routine which behaves like a variable and contains
the total number of lines in the list or route. e.g.:
MOVES ?
The address left by MOVES is in high memory so must be accessed with E@ and E!.
Example use to self learn the last position in a route:
MOVES E@ REPLACE
See example in section 12.
To have the gripper operate during progress of the program first enter GRIP or UNGRIP as appropriate to immediately operate the gripper then 'insert func' GRIP or UNGRIP. Leave the value at 0.
In Cartesian mode LEARN learns the Cartesian co-ordinates instead
of the joint co-ordinates. The same applies to REPLACE and INSERT.
However, you can force the system to learn the joint co-ordinates
instead with:
JL
Likewise a line containing any kind of data may be replaced with joint co-ordinates with
JR
Then upload the data to the computer with
7.3.1
PUTTING FORTH WORDS INTO ROBOT PROGRAMS
( "TICK" COMMANDS )
It is possible during the progress of a route to have the controller execute a FORTH program or "word" for example GRIP and UNGRIP, GET and PUT, PLACE names, other routes, changes in SPEED or other variables, or user defined words.
Use insert func
However it is not a good idea to 'LEARN (insert func) words which you have created because your word is looked up in the dictionary and it's code field address (CFA) is compiled into the route. This is then executed when that line is reached (using the word EXECUTE). In FORTH this is known as "vectored execution ". This address could change if you edit the text file rendering the address in the route invalid and the system will crash (and probably the robot as well). You would have to edit all occurrences of your word using edit line. Therefore stick to words in ROBOFORTH. Any ROBOFORTH words can be inserted as functions, leaving the argument (value) at zero. Words which can have values are: MSECS GRIPPER SPEED SETPA
If you must use new definitions and subsequently re-compile a number of words from the editor so that the address of the word changes then a better practice is to put the line 'REPLACE (word) in your text file to re-install the correct CFA each time the text file is reloaded. Don't forget to precede it with the name of your route or it will be put in the wrong route e.g. if the route is called PATH1 and you have your function in line 2 then enter
PATH1 2 'REPLACE PRESENT
If you wish to update the listing in RobWin then click the red up-arrow
Example FORTH word to make the robot wait until an object is
present to be picked up, indicated by bit 7 of port PB going to a
logical '1':-
: PRESENT BEGIN PB 7 BIT? UNTIL ;
Then click insert func and enter PRESENT
In the comms window or hyperterminal you can type 'LEARN PRESENT
WARNING: If you remove or FORGET a word which you have 'LEARNed or 'insert func' into a route then you must also DELETE the line in the route which holds it. Otherwise you will leave an invalid address in the route to be executed and the system will crash.
7.3.2
SUB-ROUTES
The following procedures apply only to JOINT mode operation. To do something similar in Cartesian mode go to Relative Cartesian routes.
In the majority of cases we want the arm to do a similar movement in many different positions, that is to say that the movement should be relative to its starting position and not to its home position. To make this happen first put the robot into RELATIVE mode BEFORE creating the route. Enter:
RELATIVE
Let's call the relative route WAVE.
Click 'new' in routes window, then in the route create dialog box click 'relative' and enter the name WAVE.
( The command sent to RoboForth would be SUB ROUTE WAVE )
WAVE is now a sub-route.
WHERE now has an additional line labelled LOCL and with initial starting values of zero. (example values, typical headers):
WAIST SHOULDER ELBOW HAND WRIST OBJECT
TRACK LIFT EXTEND HAND WRIST OBJECT
GLOB 1000 2000 3000 0 0
LOCL 0 0 0 0 0
642 2406 3604 0
1001 2005 3003 0
WAIST SHOULDER ELBOW L-HAND R-H/YAW WRIST OBJECT (R12/R17 - six axis)
2000 0 0 0 0 0
LOCL 0 0 0 0 0 0
642 2406 3604 0 0
2010 0 0 0 0
(encoder values shown are examples only)
If a joint is moved now, say the shoulder or lift by 1000, the result is:-
WAIST SHOULDER ELBOW HAND WRIST OBJECT
TRACK LIFT EXTEND HAND WRIST OBJECT
GLOB 1000 3000 3000 0 0
LOCL 0 1000 0 0 0
xxxx xxxx xxxx 0
1001 3000 3003 0
When co-ordinates are LEARNed into the new sub-route, WAVE, the LOCL (local) position rather than the GLOBal position are learned. These are given ABSOLUTE type flags and will list as absolute positions but when WAVE is RUN it will run the co-ordinates in the list relative to
the initial position of the robot before RUN was typed, rather than relative to the HOME position.
By inserting another route name into a route using 'insert func' (or in comms window by using 'LEARN or 'INSERT or 'REPLACE ) a robot route can be made to execute the other route then return to complete the first (calling) route.
If the sub-route WAVE is tick-learned into an absolute route using 'insert func' and
the absolute route is run then when the line containing the sub-route is executed the sub-route will run relative to the robot's position at the previous line.
If RELATIVE is typed AFTER THE ROUTE IS CREATED then the route
will be absolute but LEARN will learn the LOCAL positions. These
positions are given RELATIVE type flags indicated by an 'R' e.g.
WAIST SHOULDER ELBOW HAND WRIST OBJECT
TRACK LIFT EXTEND HAND WRIST OBJECT
01 1000 3000 3000 0 0 R
If a route is already created and you want to change it into a
sub-route then type:-
RELATIVE RTYPE then upload to the computer with
ALL 0 MOVETO makes the arm go back to the LOCAL home position
ABSOLUTE cancels RELATIVE mode.
NOTES
(1) RELATIVE or SUB designates the next ROUTE created to be a
sub-route, designates all joints to be local and zeros all the
local position counts. Thereafter LEARN will learn the LOCAL
positions but with absolute type flags.
(2) When a RELATIVE route is executed it saves the current
position (i.e. the position last at before the route is executed)
on the stack. If the current position is itself relative (e.g.
when sub-routes are nested) it is the local position which is
saved. When the route finishes the position values are 'popped'
off the stack again. If the sub-route does not actually finish at
the same position it started then the difference between starting
and finishing positions is added to the saved position values as
they are restored from the stack.
(3) If the escape key is pressed any route will abort after the
current line is complete. In the case of nested sub-routes each
level will abort leaving a true GLOBAL position.
(4) For information in nested sub-routes, each route announces
itself as it begins (unless LISTFLAG is zero). When a sub-route
finishes it announces the name of the route which called it.
ROW
Click new in routes window; in route create box click row. Then enter the number of points in the row in the columns box. Make sure the number of lines reserved is at least 1 greater than the number in the columns box. Move robot to first position, click the first line in the route window and click set-to-here. Then move robot to last position in the row, click the last line in the route window and click set-to-here. Finally press interpolate.
The number of positions in the row is held in the variable POSNS
GRID
This sets up a 2-dimensional array.
1. Click new in routes window; in route create box click grid. Select whether the numbers should be JOINT or Cartesian co-ordinates. Then enter the number of rows and columns. Make sure the number of lines reserved is at least 1 greater than rows times columns.
2. Guide robot to the first position in the matrix, that will be row 1 column 1. Highlight line 1 (which has an asterisk next to it) and press set-to-here
3. Guide robot to the end of the last column, row 1 i.e. the next corner of the matrix. Highlight the next line with an asterisk and click set-to-here
4. Guide robot to the last position i.e. last row last column. Highlight the last line (which has an asterisk) and click set-to-here
5. Click interpolate.
Dialog box showing creation of a new matrix TRAY1 in Cartesian mode, 10 columns by 5 rows.
The number of positions in each row of the GRID is held in the variable POSNS. The number of rows is in ROWS.
A number of words are provided to work with ROWs and GRIDs. They all require an extra line added to the list which contains a relative (type R) co-ordinate which is added to any line to make an approach position for every line. This line is always the last line and the address of the data is data is MOVES E@ LINE
You can create this approach position as follows:
Joint mode
1. Guide the robot to one of the target positions (one of the lines). The robot might already be there after learning the third corner.
2. Enter RELATIVE
3. Guide the robot to an approach position using commands or the teach pad - for example a vertical movement on lift alone might suffice for a position over a test tube rack.
4. Press set aproach
5. Enter ABSOLUTE
Cartesian mode
1. Guide the robot to one of the target positions (one of the lines). The robot might already be there after learning the third corner.
3. Command the robot to an approach position using a Cartesian MOVE or use JOG. For example a vertical movement on Z alone might suffice for a position over a test tube rack.
4. Press set aproach
In joint mode the co-ordinates learned are those shown against LOCL in the WHERE display and in Cartesian mode the co-ordinates learned are the difference between the current position and the PREVious position shown in the WHERE display.
Method A is best suited to joint mode and method B is best suited to Cartesian mode.
Once the relative position, line n+1 has been learned a number of other words may be used:
(n) NEAR
This sends the robot to the approach position for line (n) obtained by ADDing the co-ordinates in the last (type R) line to the co-ordinates of line (n).
(n) NEARAD
This looks up the memory address of the near position and leaves it on the stack. It does nothing otherwise. NOTE in v10.0 and below this command actually sent the robot to the near position. The new form of this command is useful for tail-ending other routes. For example suppose you have a matrix route TRAY with an approach position and another route PATH to get you to the general area of the tray from somewhere else. You can amend the last line of PATH so the robot finishes up at the required near position of the tray. The command string would be:
TRAY (n) NEARAD AXES PATH LASTLINE REPLACE
where TRAY is the name of the matrix route, (n) is the position within that matrix you want to go to. NEARAD produces the address of the approach position of line (n) and AXES extracts the Cartesian co-ordinates of that approach position into the variables X Y Z W etc. Next PATH invokes the route which gets the robot there. LASTLINE leaves the line number of the last line of that route on the stack and REPLACE replaces that (last) line with the co-ordinates in the variables X Y Z W etc. extracted from the NEAR position of TRAY. So to get smoothly from the starting position of PATH to, say position 5 of TRAY you could use this sequence of commands.
TRAY 5 NEARAD AXES
PATH LASTLINE REPLACE RUN
TRAY 5 GOTO
or a generic word to take the line number off the stack:
: GOTRAY
DUP ( needed by NEARAD and GOTO
TRAY NEARAD AXES
PATH LASTLINE REPLACE
RUN
TRAY GOTO
;
usage
(n) GOTRAY
NOTE in version 10 the command NEAR was GONEAR and NEARAD was just NEAR
(n) INTO
This sends the robot to line (n) of the row or grid via the approach position.
DOWN
Moves the robot from the approach position to the target position - use only after (n) NEAR.
UP
Moves the robot up from the target position to the approach position. (Actually moves the robot up from any position!)
Before using any of the above words the route to which they refer must be selected. If in doubt enter the name of the route again; Remember to use the route name in any definitions.
An example of how to use these commands.
Suppose you were taking parts from a TRAY and putting them on a BELT
: EMPTY-TRAY
TRAY
MOVES E@ 1 DO
I INTO
GRIP
UP
BELT
UNGRIP
LOOP
;
Note the use of MOVES E@ above. In the first place MOVES is one more than we need, being the number of the line with the RELATIVE position. In the second place, due to a FORTH idiosyncracy DO LOOPs require one more than the actual number of loops, e.g. 9 1 DO LOOP will do 8 loops not 9. Remember also that MOVES is in high memory so must be read with E@ not just @
A route may be used to generate an almost continuous path. For this the CPU passes all the route data to the DSP line by line and the DSP executes the motion without the CPU being involved (see later, three phases of operation). Learn a route as normal but to RUN it first enter
CONTINUOUS
to put the system into continuous path mode.
SMOOTH is an identical command that includes ADJUST (v11 up) (see 7.3.5)click 
Then when you type RUN
the robot will run through all the points without stopping. Even if the route is small enough to be loaded into the DSP in one go the CPU waits for the DSP to finish driving the robot before control comes back to the user or the software moves to the next word.
To run the route backwards enter
RETRACE
To cancel CONTINUOUS mode enter
SEGMENTED
which returns motion to line by line mode.
The speed in continuous path mode is determined by the variable SPEED. A value of 1000 is a low speed, 10,000 is a high speed. Highest possible is 32767 but of course the robot may not be able to do that. Acceleration is determined by the variable ACCEL 100 is very low acceleration, 5000 would be a high acceleration.
The route may contain contain joint or Cartesian values.
To change speed within the route go to the route box and highlight the required line before which the speed must change. Click 'insert func' then enter SPEED in the dialog box and the required speed below it.
To turn the gripper on or off within the route go to the route box and highlight the required line at which the gripper should operate. Click 'insert func' then enter GRIPPER in the dialog box and the value 1 below it to turn it on, or 0 to turn it off. You cannot use the word GRIP which only works in segmented mode.
STOP BUTTON:
If the stop button is pressed while a continuous path is running then the entire route is abandoned even if the stop button does not itself cause an abort (i.e. re-programmed with STOPVEC). The task programmed into STOPVEC will be executed but the run will not resume and the next word will be executed.
When a route is run in continuous path mode the DSP computes the
required speed for each motor to get from present count to the
counts in the next line. The path between one line and the next
is called a segment. The speed for a given motor will be
different in one segment from the next. To change speed from one
segment to the next the DSP computes a ramp using the value of
ACCEL. If the motor cannot reach the required speed before the
next segment comes up then an error message
"Too tight, line (n)" is issued and the system aborts with error 22. This means one
motor cannot get from current speed to the computed speed within
the segment.
This programmed path comprises 2 right-angle turns. The DSP rounds off the corners according to the acceleration programmed. A high value for ACCEL means a tighter turn (smaller radius). In the above the robot is able to change direction by 90 degrees and back again in the space between lines 2 and 3.
In the above lines 2 and 3 are too close together. The DSP obviously cannot round off both corners so the CPU issues the message "too tight, line 3". Only if ACCEL is increased i.e. reduce the radius of the two curves can the robot make it round both corners. If SPEED is reduced this has the same effect i.e. smaller radius.
For similar mathematical reasons if the distance between lines 1 and 2 is too short then the DSP can not get the robot up to set speed before the first change of direction so would issue the message "too tight line 2". Similarly if lines 3 and 4 are too close the message would be "too tight line 4".
The cure is to reduce the value of SPEED or increase the
acceleration ACCEL. The route is tested first so that the error
does not occur while the robot is in motion and if there is no
such error in the route then the route is run. You can test the
route yourself with the command
DRY RUN
There are 2 modes of RUN for the R19, DRY (2) and CONTINUOUS (1). After a DRY RUN, CONTINUOUS is re-asserted. Whenever RUN is executed a dry run is automatically run first to make sure settings are valid.
There are 3 modes of RUN for the R12/R17, DRY (2), CONTINUOUS (1) and NOCHECK (3). After a DRY RUN, CONTINUOUS is re-asserted. In CONTINUOUS mode a dry run is automatically run first to make sure settings are valid. In NOCHECK mode a dry run check is not performed. The reason for this is that for a very long route a CONTINUOUS RUN takes a significant time resulting in a delay before the robot moves whereas a NOCHECK RUN moves the robot immediately. It is up to the user to ensure that settings are still valid before a NOCHECK RUN. If not the results can be catastrophic.
There exists the possibility to reduce the speed of a route automatically and temporarily with the command
ADJUST (then RUN)
or
SMOOTH which includes ADJUST so should only be used if a route is selected e.g.
(route-name) SMOOTH RUN
Note that this command will not work if there is a SPEED command
embedded in the route. The system will hang and you will
have to press the escape key to get the system
back. In this case ERR value will be 23 (See section 11.2).
When ADJUST is used the value of SPEED is reduced
until a workable value is found. You will need to restore the
original value yourself with (value) SPEED !.
Besides the gripper other output bits may also be operated by inserting the function SETPA with other values:
SETPA 3 - turns on PA 0 (gripper)
SETPA 2 - turns off PA 0
SETPA 5 - turns on PA 1
SETPA 4 - turns off PA 1
SETPA 7 - turns on PA 2
SETPA 6 - turns off PA 2
SETPA 9 - turns on PA 3
SETPA 8 - turns off PA 3
SETPA 11 - turns on PA 4
SETPA 10 - turns off PA 4
SETPA 13 - turns on PA 5
SETPA 12 - turns off PA 5
Note these functions only work with RUN not with CRUN - see three phase operation below.
2. ?RUN
This leaves a value on the stack as follows:
0 - DSP is not running anything
1 - DSP is running a route
2 - DSP has read a line GRIPPER 0 which normally means turn off gripper
3 - DSP has read a line GRIPPER 1 which normally means turn on gripper
4 - DSP has read a line SETPA 4 which normally means turn off PA 1
5 - DSP has read a line SETPA 5 which normally means turn on PA 1
6 - DSP has read a line SETPA 6 which normally means turn off PA 2
7 - DSP has read a line SETPA 7 which normally means turn on PA 2
8 - DSP has read a line SETPA 8 which normally means turn off PA 3
9 - DSP has read a line SETPA 9 which normally means turn on PA 3
10 - DSP has read a line SETPA 10 which normally means turn off PA 4
11 - DSP has read a line SETPA 11 which normally means turn on PA 4
12 - DSP has read a line SETPA 12 which normally means turn off PA 5
13 - DSP has read a line SETPA 13 which normally means turn on PA 5
All values for SETPA and ?RUN from 14 to 255 are available to operate other functions.
CRUN
WARNING If you use this command the robot will not stop when you press the stop button. To stop the robot execute the command STOP.
WARNING If the value of SPEED is invalid the robot will go out of control. Always test the route first with RUN or DRY RUN and reduce speed, increase acceleration or use ADJUST (7.3.5) if necessary.
CRUN passes the route data to the DSP but does not wait for it to finish. If the route is short enough control passes straight back to the CPU i.e. you will get OK or the next word will be executed while the DSP is still running the robot. If the route is a long one then the CPU will be held up for a while. CRUN ought not be used with routes that contain SETPA or GRIPPER commands because the CPU must remain monitoring the DSP to get these commands as they arise. When the robot has stopped WHERE will show the robot to be still where it was before you executed CRUN therefore you must, as soon as robot motion finished, or at worst before you execute the
next command, execute
DSPASSUME - this reads back from the DSP the position the robot got to when it was stopped and changes the counts to the correct values.
To determine if the DSP has finished use EITHER:
1. ?STOP - this traps until the DSP has stopped.
It also checks the stop button and stops the DSP prematurely if pressed. As long as the CPU is in command mode or executing some other command the stop button is ignored.
If the stop button is pressed while executing ?STOP then a flag FSTOP is set to a '1', see below.
OR
2. ?RUN
This leaves a value on the stack as follows: 0 - DSP not running, 1 - DSP running a route, 2 or more - controlling an output (see later)
Three phases of operation
A DSP controlled move may proceed in three phases. In the first phase the CPU is downloading positions (route lines) to the DSP. The DSP fills up a buffer with a capacity of about 50 lines. If there are not enough lines to fill the buffer then once all the lines are downloaded then the robot begins to move. If there are more lines than the buffer can hold then when the buffer is full the robot begins to move and while the DSP is moving the robot it gradually empties the buffer. After each line is executed space is freed and the next line is downloaded into the DSP. When there are no more lines to download the second phase ends. If RUN is being executed the CPU enters phase 3, a loop monitoring the DSP, polling for a motion complete flag. RUN finishes only when all motion is finished. If CRUN is being executed it (CRUN) finishes after phase 2 when there are no lines left to download. At this point there could be up to 50 lines in the DSP buffer so the robot will carry on moving, but the CPU will either be waiting for a command or executing the next word in the definition.
During the first phase (loading buffer) no other activity takes place. During the second and third phases ?GRIP is executed which transfers SETPA or GRIPPER flags to the PA output port. Therefore if you use CRUN, when control is returned to the CPU, no further SETPA commands will be executed. The DSP continues to issue flags but the CPU is not reading them. You must therefore include ?GRIP or ?RUN in the code following CRUN if you want to use SETPA. ?GRIP is a self contained word that operates the output register directly. ?RUN polls the DSP and leaves a flag on the stack as listed above.
STOP
This tells the DSP to stop moving the robot.
ESTOP?
If the flag FSTOP is set then ESTOP? executes the stop procedure,
normally STOPABORT i.e. aborts with "stop button pressed"
message, or else whatever word is SET into STOPVEC (See section 6.1).
For example the above definition could be expanded to test the
stop button and stop the robot if pressed:
: TASK
CRUN
BEGIN
?STOP ( Waits for DSP to finish
ESTOP? ( Executes new definition
?RUN ( Gets status of DSP
0= UNTIL
;
Words of the above type are only necessary if you intend to send
a command to the DSP and do something else while the DSP controls
motion. The definition above could be expanded further to do all
kinds of things, for example take analog measurements, while the
robot is running a route in continuous path mode.
If ?RUN ever returns a true value when it should not be, for example due to a programming error confusing the DSP you can send the command:
CLRDSP
The code field (CFA) of a word is executable and can be found using FIND (or using ' (tick) minus two), e.g. FIND GRIP EXECUTE is the same as GRIP. So you can put this value in a variable and have it executed by some other routine which has already been defined. e.g.
VARIABLE GVEC
FIND GRIP GVEC ! ( FINDs the CFA (code field address) of GRIP and stores it in GVEC
GVEC @ EXECUTE ( fetch the value from the variable GVEC and execute that
An easier way to set the CFA into a variable is using the word SET e.g.
SET GVEC GRIP
In the example above it is assumed that CRUN has completed as far as the CPU is concerned and the DSP is busy running motors, and issuing flags as programmed. However in the case of a long route the software might be in the second phase of running, and therefore the robot will be moving without MONITOR being executed.
Vectored execution is a way of making sure that a given word is executed all the time the robot is running regardless of which phase the CPU/DSP communication is in. A variable DSPVEC is provided which if non zero will be executed at every opportunity during 2nd and 3rd phases. Remember that this will be executed repeatedly, maybe thousands of times so you need to write appropriate logic for a single operation of something.
Example: This will 'chase' lamps connected to the PA output port as the robot moves. Each lamp will stay on for 100mS.
USER CYCLES
USER LAMP
: SCAN
LAMP @ 5 > IF 0 LAMP ! THEN
LAMP @ BIT PA OUT
LAMP INC
100 MSECS
;
: TEST
SET DSPVEC SCAN
TR ( NAME OF A ROUTE
10000 SPEED !
SMOOTH
RUN
;
Although stated that you can only include SETPA I/O control when using RUN, it is possible to set up a loop that monitors the DSP responses to SETPA and operates an output depending on what comes back from the DSP.
: TASK
CRUN
BEGIN
?STOP ( Waits for DSP to finish
ESTOP? ( Executes new definition
?RUN ( Gets status of DSP
DUP 15 = PB 7 BIT? 0 > AND IF CYLINDER ON THEN ( SETPA 15 learned
DUP 14 = PB 7 BIT? 0= AND IF CYLINDER OFF THEN ( SETPA 14 learned
0= UNTIL
;
For example the following definition will call another routine to
turn on or off a device depending on the state of another input:
: CYLINDER PA 5 ;
: TASK
CRUN
BEGIN
?RUN
DUP 15 = PB 7 BIT? 0 > AND IF CYLINDER ON THEN ( SETPA 15 learned
DUP 14 = PB 7 BIT? 0= AND IF CYLINDER OFF THEN ( SETPA 14 learned
0= UNTIL
;
7.3.9 Straight lines
A straight line in any robot is merely a succession of points in a line. The robot passes from point to point. The robot does not move in a straight line between points so to make the path straighter you need more points. However more points will slow down the robot.
Create a ROUTE e.g. SLINE, click Cartesian and Row. Choose number of Columns depending on how straight you want your line. If this is greater than Reserved then increase Reserved. Remember that 50 segments is 51 lines.
Method 1
Using commands or JOG
1. move the robot to one end of the straight line, highlight line 1 of the route and click 'set to here'.
2. Then move the robot to the other end of the straight line, highlight the last line and click 'set to here'.
3. Then click 'interpolate'.
Method 2
1. Determine the exact co-ordinates of each end of the line.
2. Highlight the first line and click 'edit line'. Enter the co-ordinates.
3. Highlight the last line and click 'edit line'. Enter the co-ordinates.
4. Click 'interpolate'.
Enter CONTINUOUS RUN
If you get "too tight" message try
ADJUST to reduce the speed, or else increase value of ACCEL
Insert function into a straight line
You can't easily do this because of the limitations of RobWin. Here are two work-arounds:
1. In the communications window do a ROBOFORTH insert, for example to insert GRIP before line 5 enter
5 'INSERT GRIP
2. Type L. to confirm
3. Click the red up-arrow in Robwin to upload the changed data to Robwin. Close and re-open the route to confirm.
You can no longer use interpolate.
or
1. Create a second route with a different name, for example SPATH. Make sure you reserve enough lines for the co-ordinates and the functions.
2. Enter this definition to copy the straight line to the new route: (you can copy and paste this text)
: COPY
SLINE MOVES E@ SPATH MOVES E! ( MAKE #MOVES THE SAME IN EACH
SLINE 1 LINE ( SOURCE ADDR
SPATH 1 LINE ( DEST ADDR
MOVES E@ 16 * ( BYTES TO MOVE IN MEMORY
ECMOVE ( MOVES BYTES IN EXTENDED MEMORY
;
3. Type COPY then Click the red up-arrow to upload the data to the new route. Open SPATH to confirm.
4. Use insert function in the new route SPATH.
You can also use the straight line function in the curve generator commands of RobWin7. See RobWin7.pdf
7.3.10 RELATIVE Cartesian routes
Version 10.1 up only.
Sometimes it is desirable to program a motion in Cartesian co-ordinates and have it repeated starting from various positions. For example to pull out a shelf might require a move towards the shelf, then down onto the handle then pull out the shelf. Having programmed this for one shelf you can use the same route for the other shelves.
From any position in the workspace you can run the route as if it was starting from that position. Use
START-HERE
This adjusts all the positions in the route relative to where the robot is now. Note that although the data in the controller will change the data on screen in RobWin will not change. The name of the route must be declared before you use START-HERE
Example usage:
P1 GOTO PATH1 START-HERE CONTINUOUS RUN
The robot will go to point P1 then run the route PATH1 starting at the position P1.
A similar word is
END-THERE
In this case you need to supply the position where the route is to end without the robot being at that position, for example:
P1 PATH1 END-THERE CONTINUOUS RUN
The robot will run PATH1 from whatever position is necessary to finish at P1.
TRAY 1 LINE PATH1 END-THERE CONTINUOUS RUN
The robot will run PATH1 from whatever position is necessary to finish at line 1 of TRAY.
Version v13.5 up only.
Consider a route created as a ROW in which the points are equally spaced. This could be a straight line, or perhaps a circle comprising 36 points as in the following example. There would be 37 lines, line 37 being the same as line 1, so the robot could draw a circle comprising 36 segments of 10 degrees each.
A problem arises because as various axes become dominant in different parts of the circle, or as reach changes, the speed of the arm in real terms changes. This can be corrected by specifiying a time for each segment. This is a completely different mode. To invoke this mode enter
CONTINUOUS TIMED
To cancel this mode and return to the usual mode enter
CONTINUOUS
Having specified the mode you now need to specify the time for each segment - i.e. the time between one line and the next. Enter a value in milliseconds for SEGTIME e.g.
1000 SEGTIME !
Will make each segment 1 second long. This results in the circle being drawn at 10 degrees per second and takes 36 seconds all the way round.
Obviously this can be applied to any path where exact linear speed is critical. The DSP adjusts the speed so that the time between points (lines) is as specified. Clearly if the distance between one pair of points is very large then the DSP will speed up the robot to cross that distance in the specified time.
finally
RUN
Pitfalls
As with normal continuous mode if the move is too long or the time is too short then the DSP can not compute a viable speed and you will get the "too tight" error. ADJUST will not work. The solution is to increase the time, or bring the points closer together, or increase the value of ACCEL.
Unlike the normal mode you can never have two points too close together. In fact you can have two lines with identical coordinates (not possible in the normal mode). The robot would simply pause at that position for the specified time before moving on.
Version v13.61 and V14 up only.
The basic code for this is below.
The principle of operation is as follows:
The DSP can compute a trajectory for a multi-axis move. It works only with motor counts and only with the change in motor counts, not the actual values.
Normally DSP moves (such as DSPMOVE and CONTINUOUS RUN) subtract the current joint coordinates from the target coordinates and send the difference to the DSP. In a route RUN situation the differences from line to line are computed by the CPU in FORTH and sent to the DSP as fast as possible. The DSP stores them in a buffer. If the buffer is full then the CPU program waits for space to be available. If Cartesian coordinates are used (which they usually are) these are converted to joint coordinates on the fly using TRANSFORM
The DSP uses the whole list of values to compute speeds and ramps for each joint. If this can not be computed (see manual) then a “too tight” error is raised.
As in the previous section the second mode of operation in RUN is TIMED. In this each segment (distance from one line to the next) is executed in a fixed time, the value in SEGTIME in mS. The speed is adjusted by the DSP so that the move is completed in that exact time. A long move requires a higher speed. CHASE is related to that.
A mode in the DSP allows us to check if the buffer is empty. When we send a move to the DSP it immediately starts on it and empties the buffer. We can then send the next move while the DSP is working on the last one. The buffer is now not empty and we must wait for it to be empty before we can send the next. Sequence is therefore:
send move n – DSP starts on them, buffer is empty
send move n+1 – DSP buffers them, buffer not empty
DSP completes move n and starts on n+1, buffer is empty
now we can send move n+2 and so on.
So we can never have more than one move in hand. If you are using a PC program to send these coordinates then it will need to know when the controller is ready for the next coordinates.
In the program CHASE below, to start with we initialize the DSP and set a starting condition. Then we enter a loop: BUFZ is a trap that waits for the buffer to be empty. While in this trap nothing else can be done.
As soon as the buffer is empty BUFZ drops through to
GETCART – this is where you can send the new coordinates to the robot. You have some time because in theory the DSP is still working on the last segment and the robot is still moving. If you are using a PC supervisor then you need to send a message or a character from GETCART to the PC it to indicate GETCART is ready for the coordinates. The PC then sends the coordinates in some form. GETCART needs to parse the string and get the values in to X Y Z PITCH and W (roll) (5-axes) or X Y Z PITCH YAW and ROLL (6-axes). Or just send X Y and Z if the hand parameters do not change.
As soon as the new coordinates are received NEXTSEG computes the changes in motor counts and sends them to the DSP. The DSP attempts to do this move within the time in SEGTIME. For a short move the speed will be low and for a long move it will be high.
NEXTSEG returns a status which should be zero. If you sent impossible coordinates such that the speed required to do that move within SEGTIME is too great then NEXTSEG returns false and we need to stop everything. The procedure prints the “segment too long” message and aborts with error 31. (that value goes into ERR.) If using a supervisor you can change this to some other string or single character or even an output on the PA port.
A project is provided, CHASE. It may be downloaded from the ST Robotics downloads page as CHASE.ZIP. In this project is a route called LIST1 which is just a list of Cartesian coordinates.
INIT sets workable values for ACCEL and SEGTIME. Change them as necessary.
To test the algorithm a program RJ is provided (random jump). A number is picked from the sequence
CREATE SEQUENCE
1 , 5 , 3 , 9 , 10 , 4 , 2 , 8 , 7 , 3 , 1 , 0 , ( 0 means stop
so that 1 means get the coordinates from line 1, 5 means get coords from line 5 etc. The numbers are picked at random although a jump from 10 to 1 could result in the too long error.
The result is as on the youtube video at
http://www.youtube.com/watch?v=1j_o_vwruGI (chase.avi)
This shows two clips:
The first is with ACCEL set to 3000 and SEGTIME set to 500mS
As you can see the robot speeds up to complete the longer moves.
Next we changed the SEGTIME to 300mS
As you can see the robot stops part way through because the segment can not be achieved in 300mS, with the appropriate error message:
In other words the robot could not make it from line 10 to line 4 in 300mS.
What happens if the move is very small?
The robot moves the short distance in SEGTIME time, so it will move very slowly. Remember also it has to complete each move before it can start on the next. However if the next move is a large one the robot will start accelerating before it finishes the current slow move ready for the faster move.
What happens if the coordinates do not change?
The robot then continues to do zero movement in SEGTIME time. And the segment stored for next move may also be zero so as the next move will not begin until the current one is finished if, for example SEGTIME is 1000, then you may wait up to a second before motion starts again.
( CHASE COORDINATES )
: BUFZ ( wait for buffer empty
BEGIN
24 SPSB SPRB
78 < UNTIL
;
: NEXTSEG
TRANSFORM DROP ( convert to motor values
DSPCHANS @ 0 DO
TARGET I IND @ DUP ( IND is just 2* +
OLDP I IND @
- SWAP ( leave rel pos on stack, axis 6 on top
OLDP I IND ! ( store rel pos in OLDP
LOOP
1 ( flag for DSP ) TSEG
?STAT
;
: GETCART
( GET THE NEXT COORDINATES TO AIM FOR
;
: CHASE
VSET ( send ACCEL to DSP
CFLAG C1SET ( enable DSPASSUME
( set starting position
DSPCHANS @ 0 DO
I GLOBALS @ OLDP I IND !
LOOP
( send initial zero move to DSP
MVST ( start new move
0 0 0 0 0 0 1 TSEG ( first segment zero move
MVRN ( start a run sequence
( now enter loop to collect coordinates and send to DSP
BEGIN ( loop that collects next coordinates
BUFZ ( wait for buffer empty before sending next coords
GETCART ( get the next coordinates
NEXTSEG ( send motor values to DSP
( status from NEXTSEG ) 0 > IF ( fail if non zero
STOP
BEGIN ?RUN 0= UNTIL ( wait for DSP to stop
DSPASSUME
." Segment too long" 31 ABORT
THEN
?TERMINAL UNTIL
MVND ( end move sequence
BEGIN ?RUN 0= UNTIL ( wait for DSP to stop
DSPASSUME ( update counts from DSP
;
button. The communications window will show a line of chevrons