I have a few things I want to try to figure out. One is the possibility of doing direct memory assignments in order to construct a ROUTE, given that my controller doesn't appear to provide the $JL command, and I'm not getting any response from ST-Robotics support.
One thing I'm doing to figure this out is to do a run-through of the manuals again. Somewhere, I recall having seen a memory layout, so that might help narrow down where the route point storage is in memory.
primer7.pdf
Along the way, I started reading primer7.pdf. I don't think I'd read that before. Notes are below.
UNTWIST
Rotates the wrist back to its zero count position.
(Same as TELL WRIST 0 MOVETO)
That suggests that the wrist position (in joint 5) could be measured as the hand position (TELL L-HAND n MOVE) changes. Hand is also referred to as pitch.
The SPEED variable range
Range: 2-65000
The ACCEL variable range
Range 100-5000
Teach pad: "tick" or checkmark is "learn". X is delete.
in Cartesian coordinates, PITCH and W are usable for hand rotation (pitch) and wrist (roll), but in Robwin they are expressed in degrees.
The TOOLSET command appears to be one of those that the stock Python Shell would not allow through, because it's one that prompts the user for additional information after the command is issued. It looks like it says
TOOLSET
then asks for WRIST PITCH, WRITE ROLL, TOOL-LENGTH, ALIGN, and EXECUTE.
It also says that beforehand, you can use the command READY to "get the robot down into a safe working position."
After READY, the Cartesian movement operators are x y z MOVETO or MOVE, and TOOLSET followed by responses.
TOOLSET tests.
> CALIBRATE
> HOME
> TELL ELBOW E-RATIO MOVE
> TELL SHOULDER S-RATIO 2 / MOVE
> TOOLSET
WRIST PITCH 45.0 ?
> 900
900
WRIST ROLL 0.0 ?
> 450
450
TOOL LENGTH 0.0?
>
ALIGN ROLL?
> <---- here you can just hit Enter, or enter Y or N. Y invokes ALIGN mode
EXECUTE?
> Y
Y
OK
Now hand is pointing straight down. Wrist is not obviously positioned differently, but it's hard to tell. It's not clear what the appropriate responses to TOOL LENGTH and ALIGN ROLL should be.
Important: the doc says, "EXECUTE? Y to execute the above changes immediately.
Or press enter to defer the changes. They will take place the next time you use a MOVE
or MOVETO command."
So that's the secret sauce for moving the joints and hand and wrist all in one go. If you were to have a route position stored with a bunch of joint locations, you record their CARTESIAN position. Then, issue the TOOLSET command to get the hand and wrist ready, but don't execute the change. Then you do an X Y Z MOVE or MOVETO command to complete the move.
The tutorial says that there are ROUTEs and PLACEs. However, I think the system doesn't actually support a command or variable called PLACE. Instead, you end up with numbered places in a route, and you can use the n GOTO command to go to one of the places.
An Approach position appears to be some Place that is associated to an existing, numbered PLACE within a route.
Can there be multiple Approach points for a given Place?
Can an Approach place have its own Approach place?
To make a route run in a continuous way, you say routeName CONTINUOUS ADJUST and then follow up with RUN.
R12 manual.pdf
A few more interesting finds here.
Joint steps (per 90 degrees)
Where previously I thought I'd gleaned magic information from the github repo about the joint-to-angle ratios, they appear to be provided in these constants:
So, one quick experiment is to just . those values and see what appears.
> B-RATIO .
B-RATIO . 3640 OK
> S-RATIO .
S-RATIO . 8400 OK
> E-RATIO .
E-RATIO . 6000 OK
> W-RATIO .
W-RATIO . 4000 OK
> T-RATIO .
T-RATIO . 4500 OK
The 90-degree relationship is implied later on, too, in the calibration step (page 22) where it says you can execute
> TELL ELBOW E-RATIO MOVE
Memory and data dump
The parameters appear to be stored at address A200, length 200. They mention these parameters:
bank 0
start address A200
length 200
Presumably, those are responses to a "Save binary" command that's only in Robwin, and the values are hexadecimal, not decimal.
Test to perform:
1. run Save binary on other segments of memory and inspect for Point coordinates
2. run Save binary and see if there are limits on the start address and length
Memory dump, flash backup, and original flash RAM
Supposedly there's a backup file supplied with the robot, e.g., R12C123.RAM, that can be loaded to re-flash the memory to a good, known state.
Do I have one of those? Yes, maybe. In the original disk, the following were provided:
Backups/R12B725.SIG.RAM 512
Backups/R12VA161.RAM 24576
Backups/R12VA6161.ram 24576
There are also R17 and Nexus files in there.
The R12VA161.RAM has clear strings of FORTH commands within it, as does R12VA6161.ram
One can hope that the SIG.RAM file is some kind of digital signature than can be used to roughly verify that the contents of the other files are legit. It's only 512 bytes long.
The two *161 files differ starting at byte 683, and the VA6161.ram file has a 13-DEC-2016 timestamp, whereas the VA161.RAM file is dated 19-JUL-2016.
If I back up the existing RAM to a file, how does it compare to either of the VA files?
Warning: do not run PSAVE as that could corrupt the FORTH image
More commands
DATUM appears to be a command to seek out a calibration sensor. The example shown is
> TELL WRIST DATUM
USAVE appears to save "user memory" (not "program memory" as with PSAVE?) to flash.
LIMITS and MAXON may be interesting to explore later. Also, other calibration commands CAL1, CAL2, CAL3, CAL4, and SETLIMITS
HERALD and WRU ("who are you?") provide version and serial number information that inform the file naming convention for the RAM files above. There is a suggestion to save a file of the form "serial number.SIG.RAM" after calibration.
> HERALD
HERALD ROBOFORTH II V17.4 for R12, Forth vA (C) D.N.Sands
> WRU
WRU R12B725 OK
That matches up with the filename I'm seeing under Backups.
Page 25 suggests pin 16 of the arm's Dsub25 male port provides +24v to LEDs, but does not mention current limits.
It may be necessary to quote the WRU serial number when requesting support. (See page 26)
Help Sheets / HELP17 Downloading coordinates.pdf
Until now, I hadn't bothered to look in the Help Sheets folder for any information!
There is useful information in here.
Apparently, you can transmit route coordinates to the arm using the RX command, followed by (0D) (is that a carriage-return character?) and then send x, y, z, pitch, and roll positions (x, y, z in decimeters, pitch and roll in decidegrees).
The first line of GETLINE says LINE# !. I take that to mean that GETLINE is given a line number, and it's stashed in the pre-defined variable LINE# upon entry.
What does E! do? Google says it stores a floating point value in Extended Memory. E@ is the opposite, fetching the value.
What does MOVES do? It's not clear, but it seems like MOVES is a pre-defined value that represents the count of spots stored for route. The code starts with 0 stored in Extended Memory in the MOVES location (specific to the route). It then loops by adding 1 to MOVES and then calling GETLINE with that value, so GETLINE starts at line 1 and goes up from there. As each
What does GETLINE do? GETLINE is a function defined in the sample code. It reads a line of input from the user, parses out five values from it, and stores the values in memory relative to the start point of a ROUTE.
Each line is stored relative to a ROUTE's line #0 offset. Each line is a set of 8 16-bit quantities representing joint positions (motor steps) for WAIST, SHOULDER, ELBOW; angular positions for L-HAND and WRIST; and extra flags for relative or absolute position or function ID, object ID occupying a space, and perhaps a flag for approach vs. not. There's a doc on that somewhere in the set.
What does ETX mean? The term ETX represents a ctrl-C character and the digital value 3. The user runs the RX command, provides sets of 5 values for coordinates on each line, and finally sends a ctrl+C on an empty line to stop transmission. The names TX and RX here are used as transmit-receive terms, viewed from the perspective of the controller. When RX is running, it's receiving data, and the computer sends an end "transmission" code when done. The code isn't really clean in that it uses ETX both as a stop signal and as a comparator, allowing two other states (0 and 1) to be returned in FLAG1 from within GETVAL. This code is from the days before #define constants in C, or similar in other languages.
The sample code in HELP17 defines an RX command, where earlier code examples in the page suggest RX is built-in.e The RX command does not exist natively.
It defines GETVAL which parses a "value", presumably, out of FLAG1 (?) until a 0D or 03 or delimiter is seen. It does not process ASCII characters <= 0x2f, meaning some symbols, including dots and dashes, are excluded.
GETLINE calls GETVAL five times, hoping to find non-negative numerical input. As values are seen, it maintains a total value, shifting values. It's odd in that it multiplies the result by 10 with each new digit, but then ANDs the new character with 0xf without checking for values in the ASCII range 3A..3F range. It really should ensure the new character is a digit. In its current form, one could enter the characters of this set -- :;<=>? -- or even higher values, and still get data stored.
As each numerical value is parsed (including part-way through a line), the value is stored in memory. The ones provided by the user are stored at memory offsets 0, 2, 4, 6, and 8 relative to the line start. Then, the values 0, 2, and 0 are stored in relative positions 10, 12, and 14. (The code is inefficient in that the final three values are stored every time a new number is parsed.)
The line that stores a data entry value looks like this:
LINE# @ LINE I 2* + E!
Since LINE# represents the value passed in from the outside, and starts at 1, then "LINE# @" would be 1 initially.
LINE is meant "give back the relative address in Extended Memory where this line is located", so it gives back values like 16, 32, or 48 for the first route defined.
"I" is the iterator of the DO loop, which is called with limit 5, index 0. That gets multiplied by 2, ending up yielding the values 0, 2, 4, 6, and 8.
So in the end, this line says, "value = getval(); addr = LINE(LINE#) + 2*i; poke(addr, value);", where LINE() already knows its memory offset based on the selected ROUTE.
What does this mean: 0 LINE# @ LINE 0A + E! ( 6th value
The comment "6th" value refers to the sixth 16-bit quantity stored for that line. Since we're storing relative to position 0, the first five values are at positions 0, 2, 4, 6, and 8. The 0A value shown represents hex value 10, which is an offset pointer to the 6th datum for the line. The seventh and eighth values are also assigned using offsets 0C and 0E in subsequent lines.
Test
> connect
> ROBOFORTH
> START
> CALIBRATE
> TELL WAIST B-RATIO 2 / MOVE
> TELL ELBOW E-RATIO MOVE
> CARTESIAN
> COMPUTE
> WHERE
X Y Z PITCH W(ROLL) LEN. OBJECT
176.7 176.7 250.0 0.0 0.0 0.0
> ROUTE TMI
> 10 RESERVE
> LEARN
> L.
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
01 176.7 176.7 250.0 0.0 0.0 0.0
> JOINT
> WHERE
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
1820 0 6000 0 0
(and other output)
> LINE# @ .
LINE# @ . -1 OK <--- this shows that the variable LINE# already exists in the system.
> 1 LINE .
. 32 OK <--- this shows that the memory offset to line #1 for this route is 32.
> 2 LINE .
. 48 OK
In HEX, the WHERE output in JOINT mode looks like this now
> HEX
> WHERE
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
01 6E.7 6E.7 9C.4 0.0 0.0 0.0
> DECIMAL
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
01 1820 0 6000 0 0
> CARTESIAN
X Y Z PITCH W(ROLL) LEN. OBJECT
176.7 176.7 250.0 0.0 0.0 0.0
> JOINT
> HEX
> WHERE
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
71C 0 1770 0 0
What about E@? That appears to be presenting values in decimal and 16-bit quantities.
> 0 E@ . 10 OK
> 1 E@ . -7000 OK
> 3 E@ . 469 OK
Wider ranging data dump
OK, so I wrote this:
> : DISP
64 0 DO
LINE# @ LINE I 2* + E@ .
LOOP
;
and then ran
> DISP
and got back
DISP 10 6990 4 0 0 0 8 -1 C0 -57F6 4 1 0 -1 8 0 6E7 6E7 9C4 0 0 0 2 0 -1 -1 -1 ... -1
So the data storage is visible at the 16th value (or 32 offset, given these are 16-bit quantities) for line 1.
> 10 E@ .
C0
> 12 E@ .
-57F6
> HEX
> 20 E@ .
6E7
> JOINT
> TELL WAIST 200 MOVE
> WHERE
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
91C 0 1770 0 0
> LEARN
LEARN OK
> L.
WAIST SHOULDER ELBOW L-HAND WRIST OBJECT
01
02 91C 0 1770 0 0 0.0 (not displayed is that this was a relative move?)
and sure enough those values are now shown by re-running DISP and doing
> 30 E@ . 91C OK
The next thing I don't quite get, though, is: what if there are multiple ROUTEs? Maybe I don't really need to know that, given I want to transfer more of the control to the Python layer.
<soapbox>
And really, that's what gets me about this system. I liken it to what I saw in the Tandy and Panasonic dot-matrix printers of olde. Back in the day, they would put more and more features into the firmware layer of the printer device. They would add character sets, and fonts, and such. But eventually, everything became just a pixel. It became more important to have fonts defined in the software, and have the printer drivers just send graphics.
Similar has happened to the FDM 3d printer world. There were basic, and good, and really good 3d printers out there, but eventually they figured out that the computations in an external computer (be it a Raspberry Pi or a PC), rather than updating firmware with limited computational horsepower within the printer.
So with this robot arm, you have to ask: maybe it is a better to circumvent the whole FORTH and ROBOFORTH layers and operate directly against the Gecko motor controllers? The physical build of the robot arm is really good, including its calibration sensors. We already have ROS and similar doing kinematics computations, so why can't we just let ROS do that work, and have a more basic system whose responsibility is motor control?
</soapbox>
The 6th, 7th, and 8th values in a LINE
Referring back to tutorial5-axis.pdf page 37, a memory layout per line is described, but you have to tie that together to HELP17 to really understand how to get to that data.
The line is laid out as
EXAD lookup
0 ABSOLUTE JOINT
1 RELATIVE JOINT
2 ABSOLUTE CARTESIAN
3 RELATIVE CARTESIAN
Any "much larger" value means it's the CFA of a function to perform. This can be tested by finding the CFA of GRIP (using FIND GRIP) and storing the term GRIP in the route. The doc doesn't make it clear where a function parameter value is stored in a given LINE.
Note that there's a separate section about PLACES that might also store location information.
There's a command shown, "VIEW LINE 5", that reports an individual line of a route.
i NEAR can be used to move to a location 50mm "higher" than position i.
i INTO can be used via the i NEAR point and into the target position i.
UP moves away from a target position to its approach position. (It's not clear how the distinction between approach and non-approach is represented.)
i GOTO goes to position i directly, apparently without consideration for a "NEAR" point.
ENCTEST apparently lets you see encoder positions repeatedly while de-energized, so that would potentially be part of "lead by the nose" training.
INTVEC can be used for writing custom interrupt-handling code. Examples of interrupt coding and timer interrupts are around page 54 of tutorial5-axis.pdf.
?TERMINAL indicates whether an Escape key press occurred on the terminal, but it's likely that that doesn't work with the Python shell. Similar for CTRL-C.
Next experiments
Relative moves and LEARN operations
Approach settings and memory. tutorial5-axis.pdf says that an approach position is marked with an 'R' in the coordinates. Something in the way it's described suggests there's an Approach position per route, not per point. As such, maybe having an Approach position works only with a route where all positions are ABSOLUTE CARTESIAN except for the one relative position? Nah, that'd be nutty.
L-HAND and WRIST value representation
How are negative relative moves handled? Given S-RATIO is the largest value of the ratios at 8400 for a 90 degree move, then a 180-degree move would be 16800. Since that's the case, then that's within range if the joint movement is recorded as a 16-bit signed int, having a max positive (32767) or max negative (-32768) value.
HELP20 data logging.pdf
This document describes where user RAM is stored, and how to get to it.
It shows a little routine that starts at 0x2000 and stores a value from the stack to RAM, 2 bytes at a time, until it reaches 0x2fa0.
It also suggests using Robwin and its Save Binary function to pull the data from "bank 0", starting at address 0x2000, and of length 0x1000 or less (rounding length up in 0x100 increments) to a file. Presumably, similar could be done using custom code.
It also uses the term CONSTANT for setting up the value of LRAM.
It also uses a function FILL that appears to be of the forms byteVal startAddr endAddr FILL and that appears to operate in bytes, not 16-bit values.
It's not clear if that kind of storage / logging would be useful for ROS purposes. For ROS, it'd be more useful to have a timed trigger that's regularly capturing the encoder positions (ENCASSUME) and storing those in a known RAM location, perhaps with a RAMBUSY flag to avoid partial data being returned. Ideally, a caller could get the data set (including the RAMBUSY flag) in one go, and if it's valid (RAMBUSY == 0), use it to capture joint positions, while the arm is moving.
