Ejemplo n.º 1
0
// === 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;
}
// 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;
}
// 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);
}
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;
}
Ejemplo n.º 5
0
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);
}
// 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;
}
Ejemplo n.º 7
0
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);
}
int
dynamixel_ping(dynamixel_device_t *device)
{
    dynamixel_msg_t *resp = device->bus->send_command(device->bus,
                                                      device->id,
                                                      INST_PING,
                                                      NULL,
                                                      0);

    if (resp == NULL || resp->len != 2)
        return 0;
    dynamixel_msg_destroy(resp);
    return 1;
}
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);
}
Ejemplo n.º 11
0
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;
}
// === 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);
    }
}