Support Dynamixel XC430 on Tool Interface

[JamesNewton]

The XL-320s have a limited rotational range which sometimes limits our ability to do things. The XC430 doesn't cost much more and provides higher torque, stronger body, and continuous rotation modes. It is, however, physically larger and so is not a direct replacement. It also has a completely different memory map and more complex operating modes.

We are looking at the XC-430-W240:
https://emanual.robotis.com/docs/en/dxl/x/xc430-w240/

r 0 #Servo ID, Addr, Len

There is a new #Servo # Addr Len for the 'r' (read from robot) oplet that lets you query a servo directly and get back the result. I'm hoping it might be helpful for reading back torques really quickly (faster than the monitor mode) or for diagnosing Servo issues. e.g.

r 0 #Servo 1 32 2; Read Max Voltage (currently returning A0 or 160 or 16 volts)
r 0 #Servo 1 34 2; Read Min Voltage (currently returning 3C or 60 or 6 volts)
r 0 #Servo 1 70 1; Read Hardware error (returns 1 which is "Input Voltage Error")

Sending an S RebootServo 1; does not resolve the error. Sending an S ServoSet2X 1 34 59; Set Max Min voltage 0x22 0x3B gets me a packet status error of 0x84 which is "Data Range Error: Data to be written in the corresponding Address is outside the range of the minimum/maximum value"

So it's looking like our days of running the servos at 5 volts when they are spec'd for a min of 6 volts are over. Supporting the XC430's will apparently require another power supply... Not sure why the XL320's were willing to work at 5 volts and the XC430's don't seem to be.

We do, as it happens, have an extra power supply ready to go:
https://workspace.circuitmaker.com/Projects/Details/James-Newton-2/Dexter-Tool-Interface-Servo-Power-Supply
But it's another item on the BOM, it's hard to find a place to mount it, and getting them made is a pain. When I designed it, I looked for anything we could just buy from China, but didn't find anything that met the requirements. The XL320 max voltage is 8.4, but the XC430 will run on up to 14.8, so a more commonly available 12 volt unit like:
http://filecenter.deltaww.com/Products/download/01/0102/datasheet/DS_V36SE12004.pdf
will now work. If you get the "negative logic" version, be sure to short the "ON/OFF" input the ground to get it to work. On the "positive logic" version, you must leave that pin floating.

Servo Read code: (place in the 'r' handler)

}else if (strcmp(token, "#Servo") == 0) { //Read from servo number at addr for length. 
  int servo_start = 0;
  int servo_length = 0;
  unsigned char servo = 0;
  token = strtok(NULL, delimiters); if (token) servo = token[0]-'0'; //which servo? Single digit 1..255
  token = strtok(NULL, delimiters); if (token) servo_start = atoi(token); //starting address, can be zero
  token = strtok(NULL, delimiters); if (token) servo_length = atoi(token); //data length, must be > 0
  printf("Reading servo %d at %d for %d\n", servo, servo_start, servo_length);
  if (servo && servo_length 
  	&& !SendReadPacket((unsigned char *)(sendBuff + sizeof(sendBuffReTyped[0])*7), servo, servo_start, servo_length)
	) { //got a valid reply. Save in sendBuff then convert to ASCII hex in mat_string (can't return binary 'cause strlcpy)
	servo_length += 11; //11 more because of protocal overhead (header, crc, etc...)
	for(i = 0; i < servo_length; i++) sprintf(mat_string+3*i, "%.2X ", sendBuff[i+sizeof(sendBuffReTyped[0])*7]);
	mat_string_length = servo_length * 3; //2 hex digits and a space for each byte
	//printf("Return %d\n", mat_string_length);
  	}

Useful instructions:

r 0 #Servo 1 32 2; Read Max Voltage
r 0 #Servo 1 34 2; Read Min Voltage
r 0 #Servo 1 70 1; Read Hardware error

S ServoSetX ID Addr Len [Data]

Added a new ServoSetX set parameter which can take several parameters:

• Servo ID: The expected ID number of the servo on the Dynamixel bus
• Address: The address in the header or ram table in the servo
• Data/Length: This can either be a single byte of data (which makes ServoSetX work just like ServoSet) or, if the next parameter is specified, then this is the length of the data string
• Data String: (optional) If specified, this is a string of data. Because 0x00 (nulls), and 0x3B (;) can't pass initial parse, these are esc'd w/ 0x25 (%). And of course, that means the % must be escaped as well. For example: 0x00,0x3B,0x25 would become %00%3B%25 and have a Length parameter of 9 but would end up writing 3 bytes to the servo. Another example: AB%20 would become AB%2520 (the % is replaced with %25) and would have a length of 7 but would actually send 5 bytes.

Internally ServoSet2X and ServoSet are replaced with ServoSetX.

TODO: Should ServoSet be removed from the code and ServoSetX be renamed to ServoSet.

S ServoSet2X 1 32 160; Set Max Max voltage 0x20 0xA0
S ServoSet2X 1 34 59; Set Max Min voltage 0x22 0x3B

[JamesNewton]

S RebootServo ID [Type [Slope Offset]]

Change S RebootServo _id_ to take S RebootServo _id_ _type_ e.g. S RebootServo 2 430 which adds a new servo to the list to be monitored with a type of XC430. The monitor has been updated to monitor up to 20 servos, but it only checks one per monitor loop. The loop speed has been increased to once every ~5000us. So if there are only 2 servos, they get checked a bit faster than they did before (was 15000) but if there are 10, the monitor doesn't lock up, it just checks each one every 500us or so.

Of course, there is currently NO way to get the data collected for any servo address other than 1 or 3 (joints 7 or 6) back from the robot, but that will come. Probably need to expand r 0 #Servo to return all monitored servos positions and loads, and r 0 #Servo _id_ to return the position and load of that servo.

Currently, @jonfurniss has shown that the Physical User Interface works for recording and playback with the 430's, but there are issues because the units are wrong. If you tell a 320 to move 1, that is 0.29' but a 430, it's 0.08789'

The plan is use JointsCal and HomeOffset values to correct for those units. The units for JointsCal are a bit confusing. They are a ratio between arcseconds and the amount a joint moves with a single step of the motor; or in the case of servos, a single unit change in the commanded goal. To avoid floating point numbers, they are specified by S AxisCal after multiplying the desired fractional value by 3600 * 360 i.e. by arcseconds per revolution. So any value specified is first divided by 3600 * 360 and saved as a floating point. So they are specified in arcseconds, but they are used as a ratio which does not have units.

In the past, values for Joint 6 or Joint 7 would NOT be changed by the firmware at all. They were just passed through to the servo. This was the way of it for 'a' and 'p' and for S EERoll or S EESpan

Now the firmware is adding the corresponding element of HomeOffset and then multiplying the value sent by the corresponding element of the JointsCal array. This correction is now in place, but there was no way to set numbers into those correction factors because the S AxisCal ... and S HomeOffset ... only take 5 parameters.

RebootServo has been modified to optionally accept JointsCal and HomeOffset values along with the servo type. If the servo type is not specified, JointsCal and HomeOffset are not changed. If only the type is specified, 1 and 0 are assumed for the slope and offset.

case 29:     // ServoReset
	//printf("Servo Reboot %d",a2);
	if (a3) {
		//need to translate 3->5, 1->6, 2->7, 4->8 and so on. ServoAddr(joint) does the opposite. 
		i = NUM_JOINTS + a2 - 1; //4->8, 5->9
		switch (a2) {
			case 1: i = 6; break; //J7, zero indexed
			case 2: i = 7; break; //J8
			case 3: i = 5; break; //J6
			default: break;
			}
		ServoData[a2].ServoType = (enum ServoTypes)a3;
		if (a3>0) { //if the servo type was supplied
			if (0 == a4) {a4=(3600*360);};
			JointsCal[i] = (float)a4 / (3600*360);
			HomeOffset[i] = a5;
			printf("With slope %f offset %d, ", JointsCal[i], HomeOffset[i]);
			}
		printf("Servo %d joint %d set to type %d \n", a2, i+1, a3);
		}
	RebootServo(a2); 
	return 0;

And now the

For example:

Default Mode Just powering on the robot, or doing a S RebootServo 3 320 1296000 0 would add a standard XL-320 expecting 320 units because 1296000 / (3600 * 360) is 1. You could also just do S RebootServo 3 320. If you told DexRun to move that servo 1 unit it would tell the servo to move 1 unit which is 0.29' on an XL-320. This is the "default" value, and it is the "guess" taken in the firmware on startup. By default, servo units are just passed through as always. So this new DexRun.c should not change the operation of existing robots.

Compatibility Mode But S RebootServo 3 430 4270909 0 would setup a 430 with a multiplier of 4270909/(3600 * 360) = ~3.295 because if you tell a 320 to move 1, that is 0.29' but a 430, it's 0.08789' (360/4096) and 0.08789 * 3.295 ~= 0.29' see? So DDE LTS says to move joint 6 by 1 unit, and we multiply that by 3.295 and move the 430 by 3 units or 0.254' (which is as close as we can get to 0.29' in 0.08789' steps). Call that "compatibility mode"

XL-430 Mode Then if you want better accuracy, with your XL-430 installed, you do S RebootServo 3 430 1296000 0 and you have 0.088' control, as 1 unit is 1 unit, but you have to work around DDE. Unless DDE looks at Defaults.make_ins and sees this command, and so changes the offset it applies to Joint 6 values; the 148. It would then make it 487. This would be the default setup on new robots for backwards compatibility with DDE LTS, in the Defaults.make_ins file:

;S RebootServo 1 430 4270909 0; Set 430 for LTS compatibility
S RebootServo 1 430 1296000 0; Set 430 for 430 units
;S RebootServo 1 430 316 0; Arcsecond mode: 1/3600' per unit
z 1000000; Let servo restart
S ServoSetX 1 65 1; Turn XC430 J7 LED ON
S ServoSetX 1 64 1; Turn XC430 torque on
z 1000000; 
S ServoSetX 1 65 0; Turn XC430 J7 LED OFF

Arcsecond Mode And finally, going forward, we should (perhaps) have DDE send J6 and J7 positions in arcseconds which would need S RebootServo <id> 430 316 0 for an XL 430 and S RebootServo <id> 320 1044 0 for an XL 320. DDE would know to use arcseconds, if it saw those commands in the Defaults.make_ins file. The offset would then be zero because the XC-430 can move both positive and negative. The offset was only there to allow movement in both directions on J6 in the 320's.

Notice that the HomeOffset is applied first and then the resulting value is multiplied by JointsCal. This means that the units for HomeOffset are those the user is sending. If you are sending XL-320 units, specify the offset in XL-320 units. If you are sending arcseconds, specify the offset in arcseconds.

e.g. When the 'P' move is done:

void moverobotPID(int a1,int a2,int a3,int a4,int a5) {
a1 -= HomeOffset[0];
...
a1=(int)((double)a1 * JointsCal[0]);

and then the movement is processed. And in the standard status values, the servo positions to be returned are computed like this:
if(param == SLOPE_END_POSITION){return (int)((float)ServoData[3].PresentPossition * JointsCal[6-1] + HomeOffset[6-1]);}

[JamesNewton]

Monitoring

Update monitor thread to read which ever servos have been rebooted, according to the type supplied. Supports up to 20 servo id's or rather servo id's 1 to 20. Only ID's 3 (J6) and 1 (J7) are currently readable via the standard status responses.

Possibilities

• Add additional g # status_mode's with one that only returns status data for the servos
• As above, but use the currently unused "completed time stamp" field to send in the version of the status you want back, without changing status_mode, so all the other commands will return the standard status (or which ever you selected with g # )
• As above but the special return status is a JSON array, instead of binary data.
• Add some options to the r 0 #Servo ID, Addr, Len read from robot command so that if only an ID is specified (no addr or len) then all the available monitor data for that servo is returned.
• As above but if no ID, addr, or len is supplied, return the ID and current position and load of all monitored servos.

[JamesNewton]

For joints 6 and 7, the standard move all joints and other commands work. For additional servo id's we need to send ServoSetX commands to set the goal position. James W wrote and tested this code. It translates the desired position setting to the hex format needed to send a binary value.

function num_to_hex(num, size = 12){
    let num_array = Vector.make_matrix(1, Math.round(2 * size / 3))[0]
    let num_str = num.toString(16)
    for(let i = 0; i < num_str.length; i++){
        num_array[i] = num_str[num_str.length - i - 1]
    }
    let str = ""
    for(let i = 0; i < num_array.length ; i += 2){
        str += "%" + num_array[i + 1] + num_array[i]
    }
    return str
}

function set_430_angle(id, angle){
    let size = 12
    return make_ins("S", "ServoSetX", id, 116, size, num_to_hex(Math.round(4096 * angle / 360), size))
}


new Job({
    name: "test",
    do_list: [
        set_430_angle(4, 0),
        Robot.wait_until(1),
        set_430_angle(4, 1*90),
        Robot.wait_until(1),
        set_430_angle(4, 2*90),
        Robot.wait_until(1),
        set_430_angle(4, 3*90)
    ]
})

The Servo at ID 4 was setup in Defaults.make_ins with

S, RebootServo, 4, 430, 1296000, 0; Set 430 ID 4 for 430 units
z, 1000000; Give it time to reboot
S, ServoSet, 4, 65, 1; Turn XC430 LED on
S, ServoSet, 4, 64, 1; Turn XC430 torque on
z, 1000000; Give the operator time to see it
S, ServoSet, 4, 65, 0; Turn XC430 LED off

[JamesNewton]

Note for the future: If you run an XC-430 with commands for an XL-320, it will probably cause the servo to start reporting a hardware error, the light will blink, and it will refuse to operate.

This happens because the goal position address for the XL-320 overlaps the Temperature Limit setting for the XC-430. And you can set a temperature limit of as little as 0'C. The servo will then only operate if it's frozen.

To verify this is the case, read back address 31 with r 0 #Servo 1 31 1 aka read_from_robot("#Servo 1 31 1") and the value returned after the ... 55 80 is the temp limit. Replace that first number with the address of the servo: 1 for J7 (as shown in the example) or 3 for J6.

To recover from this error, send S ServoSetX 1 31 80 (again set the first number to the servo address) and verify by reading back that the value was set. Now you can RebootServo (or just power cycle) and everything should be ok.

[jonfurniss]

jonfurniss commented on Oct 9

DDE currently has a built in offset when sending move all joints to J6 (servo ID 3) of 148.48° or 512 XL-320 (148.48°/0.29°/Dynamixel step) units. So a command of 0° from DDE has the XL-320 in the middle of its movement.

When operating the XC-430s from DDE in compatibility mode, we need to account for the larger motion range of the XC-430 as well as the built in DDE offset. When parsing the RebootServo command, DexRun.c first assigns the slope, then the offset, so we need to do the offset with respect to XL-320 units, not XC-430 units.

Joint 6 Compatibility

Dexter's 0° position is 180° off from the XC-430's 0 Dynamixel unit position, so 180°/0.29°/XL-320 step ~= 620 Dynamixel units. Then 620-512 = 108 is the offset in Defaults.make_ins that has the J6 (servo ID 3) XC-430 behaving as expected.

Note: Changes to the code to bring it in-line with the homeoffsets for other joints mean that this value must be negative because it will be subtracted from the commanded position.

Joint 7 Compatibility

Because the gripper end effector has TWO gears vs one between the power takeoff on the servo and the linear gear, larger values close the gripper rather than open it as it would on the XL-320.

Position	Degree (DDE)	XL-320 (*0.29)	XC-430 (*0.088)
Closed	0	0	3300
Remove gripper	100	345	2158
Max Open XL-320	150	517	1450
Max Open XC-430	277	957	0

In order to accomplish this, we must use a negative ratio and provide a positive offset to bias the results back into a positive range. Again, that bias must be in XL-320 units. So if the new gripper is closed when the XC-430 is at 3300 / (0.29 / 0.088) = 1001 for the correct offset. If we want to be at the removal point when DDE is told to go to 100, which is 345 to the XL-320, the offset and ratio moves the XC-430 to 2158 and that appears to be correct; the end effector is lined up for removal. The normal maximum open value for the old servos was about 150, but with the XC-320's we can go to 277 before the bias causes us to hit 0. Values larger than this will not work, as the location would then be negative.

Defaults.make_ins for Compatibility

For setting up a gripper with Servo 3 as joint 6 and Servo 1 as joint 7, paste the following code at the bottom of Defaults.make_ins.

S, RebootServo, 1, 430, -4270909 1001;
z, 1000000;
S, ServoSet2X 1, 64, 1; Turn J7 XC430 torque on
S, ServoSet2X 1, 65, 1; Turn J7 XC430 LED on
S, RebootServo, 3, 430, 4270909, -108;
z, 1000000;
S, ServoSet2X 3, 64, 1; Turn J6 XC430 torque on
S, ServoSet2X 1, 65,0; Turn J7 XC430 LED off