File content as of revision 0:79e2a8171b16:

#include "Dynamixel.h"
#include "mbed.h"

Serial pc(USBTX, USBRX);

Dynamixel::Dynamixel(PinName tx, PinName rx, PinName txEnable, uint8_t motorID, int baudrate) :  m_link(tx, rx), m_txEnable(txEnable), m_motorID(motorID), m_baudrate(baudrate)
{
    m_link.baud(m_baudrate);
}

void Dynamixel::ping()
{
    uint8_t elements = 6;
    uint8_t packetBuffer[elements];
    if (m_link.writeable() ) {
        packetBuffer[0] = 0xff;
        packetBuffer[1] = 0xff;
        packetBuffer[2] = m_motorID;    // ID
        packetBuffer[3] = 0x02;         // Length
        packetBuffer[4] = PING;         // Instruction
        packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF;   // Check sum

        pc.printf("\n Instruction packet:\n");
        for (int i = 0; i<= elements; i++) {
            pc.printf("0x%x\t", packetBuffer[i]);
        }

        m_txEnable = 1;       // Enable Tx / Disable Rx
        for (int i = 0; i<= elements; i++) {
            m_link.putc(packetBuffer[i]);
        }
        wait_ms(1);         // fix this!!!
        m_txEnable = 0;       // Disable Tx / Enable Rx
    } else {
        pc.printf("Dynamixel not writeable\n");
    }
    //pc.baud(9600);
    wait_ms(10);
    pc.printf("\n Status packet:\n");
    while ( m_link.readable() ) {
        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
    }
    pc.printf("\n PING: Done.\n");
}

void Dynamixel::toggleLED(uint8_t ledState)
{
    uint8_t elements = 8;
    uint8_t packetBuffer[elements];
    if (m_link.writeable() ) {
        packetBuffer[0] = 0xff;
        packetBuffer[1] = 0xff;
        packetBuffer[2] = m_motorID;    // ID
        packetBuffer[3] = 0x04;         // Length
        packetBuffer[4] = WRITE_DATA;   // Instruction
        packetBuffer[5] = ADDRESS_LED;  // Parameter 1: Starting address
        packetBuffer[6] = ledState;     // Parameter 2: First value to be writen
        packetBuffer[7] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6]) & 0xFF;   // Check sum

        m_txEnable = 1;       // Enable Tx / Disable Rx
        for (int i = 0; i<= elements; i++) {
            m_link.putc(packetBuffer[i]);
        }
        wait_ms(2);         // fix this!!!
        m_txEnable = 0;       // Disable Tx / Enable Rx
    } else {
        //pc.printf("Dynamixel not writeable\n");
    }
    wait_ms(10);
    pc.printf("\n Status packet:\n");
    while ( m_link.readable() ) {
        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
    }
}

void Dynamixel::move(uint16_t position)
{
    // 0 to 1023 (0x3FF)
    uint8_t elements = 9;
    uint8_t packetBuffer[elements];
    if (m_link.writeable() ) {
        packetBuffer[0] = 0xff;
        packetBuffer[1] = 0xff;
        packetBuffer[2] = m_motorID;   // ID
        packetBuffer[3] = 0x05;   // Length
        packetBuffer[4] = WRITE_DATA;               // Instruction
        packetBuffer[5] = ADDRESS_GOAL_POSITION;    // Parameter 1: Starting address
        packetBuffer[6] = (uint8_t) position & 0xff;    // Parameter 2: First value to be writen
        packetBuffer[7] = (uint8_t) (position >> 8);    // Parameter 3: Second value to be writen
        packetBuffer[8] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + + packetBuffer[7]) & 0xff;   // Check sum

        /*
        for (int i = 0; i<= elements; i++) {
            pc.printf("%c ", packetBuffer[i]);
        }
        pc.printf("\n");
        */
        m_txEnable = 1;       // Enable Tx / Disable Rx
        for (int i = 0; i<= elements; i++) {
            m_link.putc(packetBuffer[i]);
        }
        wait_ms(2);         // fix this!!!
        m_txEnable = 0;       // Disable Tx / Enable Rx
    } else {
        //pc.printf("Dynamixel not writeable\n");
    }
    wait_ms(10);
    pc.printf("\n Status packet:\n");
    while ( m_link.readable() ) {
        pc.printf("0x%x\t", m_link.getc());  // Read status packet if available
    }
}