File content as of revision 2:3000c6778d1c:

/* mbed AX-12+ Servo Library - External hardware version */

#include "AX12.h"
#include "mbed.h"

AX12::AX12(Serial& bus, PinName dir, int ID)
        : _bus(bus), _dir(dir) {

    _bus.baud(1000000);
    _ID = ID;
    _dir = TRANSMIT; 
}

int AX12::SetGoal(int degrees) {

    char data[2];

    short goal = (1023 * degrees) / 300;                    // 1023 / 300 * degrees
    data[0] = goal & 0xff;                                  // bottom 8 bits
    data[1] = goal >> 8;                                    // top 8 bits
    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); // write the packet, return the error code

    return(rVal); 
}

float AX12::GetPosition(void) {

    char data[2];

    int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
    short position = data[0] + (data[1] << 8);
    float angle = (position * 300)/1024;

    return (angle);
}

//********************************************************************************************
// Method used to read data from the regiesters of the AX-12+ Servo
//********************************************************************************************
int AX12::read(int ID, int start, int bytes, char* data) {

    char TxBuf[16];
    char sum = 0;
    char Status[16];

    // Build the TxPacket first in RAM, then we'll send in one go...
    TxBuf[0] = 0xff;                // Header byte 1
    TxBuf[1] = 0xff;                // Header byte 2   
    TxBuf[2] = ID;                  // ID byte 
    TxBuf[3] = 0x04;                // Packet Length (always 4 bytes)
    TxBuf[4] = 0x02;                // Instruction byte (READ - Section 4.2 on datasheet)
    TxBuf[5] = start;               // First AX-12 reg Address to read from (Parameter 1)
    TxBuf[6] = bytes;               // Bytes to read

    //Work out checksum...  
    sum += TxBuf[2];
    sum += TxBuf[3];
    sum += TxBuf[4];
    sum += TxBuf[5];
    sum += TxBuf[6];
    TxBuf[7] = 0xFF - sum;

    // And send the packet to th AX-12+ Servo...
    _dir = TRANSMIT;                            // Switch the hardware to transmit...   
    for (int i = 0; i < 8 ; i++) {              // Transmit the packet in one burst with no pausing
        _bus.putc(TxBuf[i]);
    }  
    wait (0.00004);                             // Wait for data to transmit  
    _dir = RECEIVE;                             // Switch the hardware back to receive...

    //********************************************************************************************
    // Now we should get a response back from the AX-12+ servo...
    //********************************************************************************************
    Status[4] = 0xFE;                           // Initailise status[4] return code
    if (_ID!=0xFE) {                            // We'll only get a reply if it was not broadcast
        for (int i=0; i<(6+bytes) ; i++) {      // Receive the Status packet 6+ number of bytes read
            Status[i] = _bus.getc();
        }
        for (int i=0; i < Status[3]-2 ; i++) {  // Copy the data from Status into data for return
            data[i] = Status[5+i];
        }
    } // if (ID!=0xFE)
    return(Status[4]);
}

//********************************************************************************************
// Method used to write data into the regiesters of the AX-12+ Servo
//********************************************************************************************
int AX12:: write(int ID, int start, int bytes, char* data) {

    // Format is - 0xff, 0xff, ID, Length, Intruction(write), Reg Address, Param(s), Checksum
    char TxBuf[16];
    char sum = 0;
    char Status[6];

    // Build the TxPacket first in RAM, then we'll send in one go...
    TxBuf[0] = 0xff;                // Header byte 1
    TxBuf[1] = 0xff;                // Header byte 2   
    TxBuf[2] = ID;                  // ID byte 
    TxBuf[3] = 3+bytes;             // Packet Length byte
    TxBuf[4] = 0x03;                // Instruction byte (WRITE - Section 4.1 on datasheet)
    TxBuf[5] = start;               // First AX-12 reg Address to write to (Parameter 1)
    for (char i=0; i<bytes ; i++) { // Data to be written (Parameters 2 - N+1)
        TxBuf[6+i] = data[i];
        sum += TxBuf[6+i];
    }
    
    //Work out checksum...  
    sum += TxBuf[2];
    sum += TxBuf[3];
    sum += TxBuf[4];
    sum += TxBuf[5];
    TxBuf[6+bytes] = 0xFF - sum;

    // And send the packet to th AX-12+ Servo...
    _dir = TRANSMIT;                            // Switch the hardware to transmit...   
    for (int i = 0; i < (7 + bytes) ; i++) {    // Transmit the packet in one burst with no pausing
        _bus.putc(TxBuf[i]);
    }  
    wait (0.00004);                             // Wait for data to transmit  
    _dir = RECEIVE;                             // Switch the hardware back to receive...

    //********************************************************************************************
    // Now we should get a response back from the AX-12+ servo...
    //********************************************************************************************
    Status[4]=0x00;                             // Initailise status[4] to get correct response 
    if (_ID!=0xFE) {                            // We'll only get a reply if it was not broadcast
        for (int i=0; i < 6 ; i++) {            // Response is 6 bytes - 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
            Status[i] = _bus.getc();
        }
    } // if (ID!=0xFE)
    return(Status[4]);                          // return error code - if no error will return 0x00
}