// Read (and set) the rotation mode from servo
int
dynamixel_read_rotation_mode(dynamixel_device_t *device)
{
int mode = 1;
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x06;
msg->buf[1] = 4;
dynamixel_msg_t *resp = device->read(device, msg, 1);
dynamixel_msg_destroy(msg);
if (resp == NULL || resp->len != 6) {
printf("WRN: Invalid read of continuous state: len=%d\n",
resp == NULL ? 0 : resp->len);
dynamixel_msg_destroy(resp);
return device->rotation_mode; // best guess
}
for (int i = 1; i < 5; i++) {
if (resp->buf[i] != 0) {
mode = 0;
break;
}
}
device->rotation_mode = mode;
dynamixel_msg_destroy(resp);
return mode;
}
// === AX series device creation ===================
dynamixel_device_t* dynamixel_axseries_create(dynamixel_bus_t* bus,
uint8_t id)
{
dynamixel_device_t* device = dynamixel_device_create(id);
// Bus stuff
device->bus = bus;
device->is_address_EEPROM = axseries_is_address_EEPROM;
device->get_min_position_radians = axseries_get_min_position_radians;
device->get_max_position_radians = axseries_get_max_position_radians;
device->set_joint_goal = axseries_set_joint_goal;
device->get_status = axseries_get_status;
device->set_rotation_mode = axseries_set_rotation_mode;
device->get_name = axseries_get_name;
// Return delay time
uint8_t delay = 0x02; // each unit = 2 usec
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x5;
msg->buf[1] = delay;
dynamixel_msg_t *resp = device->ensure_EEPROM(device, msg);
dynamixel_msg_destroy(msg);
if (resp != NULL)
dynamixel_msg_destroy(resp);
return device;
}
// speedfrac [-1,1] pos for CCW, neg for CW
// torquefrac [0,1]
void
dynamixel_set_continuous_goal(dynamixel_device_t *device,
double speedfrac,
double torquefrac)
{
assert (device->rotation_mode);
speedfrac = dmax(-1, dmin(1, speedfrac));
torquefrac = dmax(0, dmin(1, torquefrac));
int speedv = (int)abs(speedfrac * 0x3ff);
if (speedfrac < 0)
speedv |= 0x400; // CW direction
int torquev = (int)(0x3ff * torquefrac);
dynamixel_msg_t *msg = dynamixel_msg_create(5);
msg->buf[0] = 0x20;
msg->buf[1] = speedv & 0xff;
msg->buf[2] = (speedv >> 8) & 0xff;
msg->buf[3] = torquev & 0xff;
msg->buf[4] = (torquev >> 8) & 0xff;
dynamixel_msg_t *resp = device->write_to_RAM(device, msg, 1);
dynamixel_msg_destroy(msg);
if (resp != NULL)
dynamixel_msg_destroy(resp);
}
// Ensure the data at specified EEPROM address
//
// First, read EEPROM bytes and write if different from desired.
// params: parameters of write command. First byte is address in servo
// control table, followed by data to write beginning at that
// address. No retry allowed
//
// returns servo response from bus.
//
// User is responsible for cleaning up params and the response
dynamixel_msg_t *
dynamixel_ensure_EEPROM(dynamixel_device_t *device,
dynamixel_msg_t *params)
{
if (!device->is_address_EEPROM(0xff & params->buf[0])) {
printf("WRN: Write failed because EEPROM address given is in RAM area.\n");
return NULL;
}
int num_bytes = params->len - 1;
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = params->buf[0];
msg->buf[1] = num_bytes & 0xff;
dynamixel_msg_t *resp = device->read(device, msg, 0);
dynamixel_msg_destroy(msg);
if (resp == NULL || resp->len != (num_bytes+2) || resp->buf[0] != 0) {
printf("WRN: Invalid EEPROM read: ");
dynamixel_msg_dump(resp);
return resp;
}
else {
int differ = 0;
for (int i = 1; i <= num_bytes && !differ; i++)
differ |= (params->buf[i] != resp->buf[i]);
if (!differ) {
dynamixel_msg_destroy(resp);
resp = dynamixel_msg_create(1);
resp->buf[0] = 0;
return resp; // as if no error write occured (w/o checksum)
}
printf("WRN: Writing to EEPROM (address %d)\n", (0xff & params->buf[0]));
}
dynamixel_msg_destroy(resp);
resp = device->bus->send_command(device->bus,
device->id,
INST_WRITE_DATA,
params,
0);
if (resp == NULL || resp->len != 2 || resp->buf[0] != 0) {
printf("WRN: Error occurred while writing to EEPROM");
dynamixel_msg_dump(resp);
}
return resp;
}
int
dynamixel_get_firmware_version(dynamixel_device_t *device)
{
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x2;
msg->buf[1] = 8;
dynamixel_msg_t *resp = device->read(device, msg, 1);
int version = resp->buf[1]&0xff;
dynamixel_msg_destroy(msg);
dynamixel_msg_destroy(resp);
return version;
}
static void axseries_set_rotation_mode(dynamixel_device_t *device, int mode)
{
dynamixel_msg_t *msg = dynamixel_msg_create(5);
msg->buf[0] = 0x06;
msg->buf[1] = 0;
msg->buf[2] = 0;
msg->buf[3] = mode ? 0 : 0xff;
msg->buf[4] = mode ? 0 : 0x03;
dynamixel_msg_t *resp = device->ensure_EEPROM(device, msg);
dynamixel_msg_destroy(msg);
if (resp != NULL)
dynamixel_msg_destroy(resp);
}
int dynamixel_bus_get_servo_model(dynamixel_bus_t *bus, uint8_t id)
{
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x00;
msg->buf[1] = 3;
dynamixel_msg_t *resp = bus->send_command(bus,
id,
INST_READ_DATA,
msg,
0);
dynamixel_msg_destroy(msg);
if (resp == NULL)
return -1;
int v = (resp->buf[1] & 0xff) + ((resp->buf[2] & 0xff) << 8);
dynamixel_msg_destroy(resp);
return v;
}
void
dynamixel_set_id(dynamixel_device_t *device, int newid)
{
assert(newid >=0 && newid < 254);
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x03;
msg->buf[1] = (newid & 0xff);
dynamixel_msg_t *resp = device->ensure_EEPROM(device, msg);
if (resp == NULL || resp->len < 1 || resp->buf[0] != 0) {
printf("set_id failed for %d. Aborting in order to avoid EEPROM wear-out.", device->id);
exit(-1);
}
dynamixel_msg_destroy(msg);
dynamixel_msg_destroy(resp);
}
static dynamixel_device_status_t* axseries_get_status(dynamixel_device_t *device)
{
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x24;
msg->buf[1] = 8;
dynamixel_msg_t *resp = device->bus->send_command(device->bus,
device->id,
INST_READ_DATA,
msg,
1);
dynamixel_msg_destroy(msg);
if (resp == NULL)
return NULL;
dynamixel_device_status_t *stat = dynamixel_device_status_create();
stat->position_radians = ((resp->buf[1] & 0xff) +
((resp->buf[2] & 0x3) << 8)) *
to_radians(300) / 1024.0 - to_radians(150);
int tmp = ((resp->buf[3] & 0xff) + ((resp->buf[4] & 0x3f) << 8));
if (tmp < 1024)
stat->speed = tmp / 1023.0;
else
stat->speed = -(tmp - 1024)/1023.0;
// load is signed, we scale to [-1, 1]
tmp = (resp->buf[5] & 0xff) + ((resp->buf[6] & 0xff) << 8);
if (tmp < 1024)
stat->load = tmp / 1023.0;
else
stat->load = -(tmp - 1024) / 1023.0;
stat->voltage = (resp->buf[7] & 0xff) / 10.0; // scale to voltage
stat->temperature = (resp->buf[7] & 0xff); // deg celcius
stat->continuous = device->rotation_mode;
stat->error_flags = (resp->buf[0] & 0xff);
return stat;
}
void
dynamixel_set_baud(dynamixel_device_t *device, int baud)
{
int code = 0;
switch (baud) {
case 1000000:
code = 1;
break;
case 500000:
code = 3;
break;
case 115200:
code = 16;
break;
case 57600:
code = 24;
break;
default:
// Unknown baud rate
assert(0);
}
dynamixel_msg_t *msg = dynamixel_msg_create(2);
msg->buf[0] = 0x04;
msg->buf[1] = code;
dynamixel_msg_t *resp = device->ensure_EEPROM(device, msg);
if (resp == NULL || resp->len < 1 || resp->buf[0] != 0) {
printf("set_baud failed for %d. Aborting in order to avoid EEPROM wear-out.", device->id);
exit(-1);
}
dynamixel_msg_destroy(msg);
dynamixel_msg_destroy(resp);
}
// === Bus specific implementation ===================
// Send an instruction with the specified parameters. The error code,
// body, and checksum of the response are returned (while the initial 4
// bytes of the header are removed)
static dynamixel_msg_t *
send_command_raw(dynamixel_bus_t *bus,
uint8_t id,
int instruction,
dynamixel_msg_t *params)
{
dynamixel_serial_bus_impl_t *impl = (bus->impl);
// Missing synchronization
int parameterlen = (params == NULL) ? 0 : params->len;
uint8_t *cmd = malloc((6+parameterlen)*sizeof(uint8_t));
cmd[0] = 255; // MAGIC
cmd[1] = 255; // MAGIC
cmd[2] = id; // servo id
cmd[3] = (uint8_t)(parameterlen+2) & 0xff; // Length
cmd[4] = (uint8_t)(instruction & 0xff);
if (params != NULL) {
for (int i = 0; i < params->len; i++)
cmd[5+i] = params->buf[i];
}
int checksum = 0;
for (int i = 2; i < parameterlen+6-1; i++) {
checksum += (cmd[i] & 0xff);
}
cmd[5+parameterlen] = (uint8_t)((checksum ^ 0xff) & 0xff);
int res = write(impl->fd, cmd, 6+parameterlen);
if (VERBOSE) {
// XXX Dump cmd...which isn't a msg
}
free(cmd);
// Read response. The header is really 5 bytes, but we put the
// error code in the body so that the caller knows what went wrong
// if something bad happens. Synchronize on the first two 0xffff
// characters.
dynamixel_msg_t *header = dynamixel_msg_create(4);
int header_have = 0;
while (header_have < 4) {
res = read_fully_timeout(impl->fd,
(header->buf)+header_have,
4 - header_have,
TIMEOUT_MS);
if (VERBOSE) {
printf("READ: res = %d : ", res);
dynamixel_msg_dump(header);
}
if (res < 1)
return NULL;
assert (res <= (4 - header_have));
//assert (res + header_have == 4);
// If the first two bytes are the sync bytes, we're done
if ((header->buf[0] & 0xff) == 0xff && (header->buf[1] & 0xff) == 0xff)
break;
// Shift buffer, read one more character
header_have = 3;
for (int i = 0; i < 3; i++)
header->buf[i] = header->buf[i+1];
}
if ((header->buf[2] & 0xff) != id) {
printf("serial_bus: Received response for wrong servo %d\n",
header->buf[2] & 0xff);
return NULL;
}
//int thisid = header->buf[2] & 0xff;
int length = header->buf[3] & 0xff;
if (length < 2)
return NULL;
dynamixel_msg_t *body = dynamixel_msg_create(length);
res = read_fully_timeout(impl->fd,
body->buf,
body->len,
TIMEOUT_MS);
if (VERBOSE) {
printf("READ: res = %d : ", res);
dynamixel_msg_dump(body);
}
if (1) {
int checksum = 0;
for (int i = 2; i < header->len; i++)
checksum += (header->buf[i] & 0xff);
for (int i = 0; i < body->len-1; i++)
checksum += (body->buf[i] & 0xff);
checksum = (checksum & 0xff) ^ 0xff;
if ((body->buf[body->len - 1] & 0xff) != checksum) {
printf("serial_bus: Bad checksum %02x %02x\n",
body->buf[body->len - 1] & 0xff,
checksum);
return NULL;
}
}
dynamixel_msg_destroy(header);
return body;
}
// radians [-pi, pi]
// speedfrac [0,1]
// torquefrac [0,1]
void
dynamixel_set_joint_goal_default(dynamixel_device_t *device,
int pmask,
double radians,
double speedfrac,
double torquefrac)
{
assert (!device->rotation_mode && (pmask == 0xfff || pmask == 0x3ff));
// Ensure proper ranges
radians = mod2pi(radians);
speedfrac = dmax(0.0, dmin(1.0, dabs(speedfrac)));
torquefrac = dmax(0.0, dmin(1.0, torquefrac));
double min = device->get_min_position_radians(device);
double max = device->get_max_position_radians(device);
radians = dmax(min, dmin(max, radians));
int stop = speedfrac < (1.0/0x3ff);
int posv = ((int) round((radians - min) / (max - min) * pmask)) & pmask;
// in joint-mode, speed == 0 --> maxspeed
int speedv = stop ? 0x1 : (int)(speedfrac * 0x3ff);
int torquev = (int)(torquefrac * 0x3ff);
dynamixel_msg_t *msg = dynamixel_msg_create(7);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv >> 8) & 0xff;
msg->buf[3] = speedv & 0xff;
msg->buf[4] = (speedv >> 8) & 0xff;
msg->buf[5] = torquev & 0xff;
msg->buf[6] = (torquev >> 8) & 0xff;
dynamixel_msg_t *resp = device->write_to_RAM(device, msg, 1);
dynamixel_msg_destroy(msg);
if (resp != NULL);
dynamixel_msg_destroy(resp);
// Handle speed == 0 case (after slowing down, above) by relaying current
// position back to servo. Do not set torque == 0, b/c that is possibly not
// desired...
if (stop) {
msg = dynamixel_msg_create(2);
msg->buf[0] = 0x24;
msg->buf[1] = 2;
resp = device->bus->send_command(device->bus,
device->id,
INST_READ_DATA,
msg,
1);
dynamixel_msg_destroy(msg);
if (resp != NULL) {
dynamixel_msg_destroy(resp);
posv = (resp->buf[1] & 0xff) + ((resp->buf[2] & 0xff) << 8);
msg = dynamixel_msg_create(3);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv > 8) & 0xff;
resp = device->write_to_RAM(device, msg, 1);
}
if (resp != NULL)
dynamixel_msg_destroy(resp);
}
}