// 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); } }