File content as of revision 14:3be8ca0c066d:

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

AX12::AX12(Serial& bus, Serial& PCbus, PinName dir, int ID)
    : _bus(bus), _PCbus(PCbus), _dir(dir){
    
    _bus.baud(1000000);
    _ID = ID;
    _dir = TRANSMIT;
}

int AX12::SetGoal(int degrees){

    char data[4];
    short int goal = (1023*degrees)/300;
    data[0] = goal & 0xff;
    data[1] = goal >> 8;
    data[2] = 0xf4;
    data[3] = 0x01;
    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data);

    return(rVal);
}

int AX12::GetLoad(void){
    char data[2];
    int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
    int rVal = data[0] + (data[1] << 8);
    return (rVal);
}

int AX12::read(int ID, int start, int bytes, char* data){

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

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;
    TxBuf[2] = ID;
    TxBuf[3] = 0x04;
    TxBuf[4] = 0x02;
    TxBuf[5] = start;
    TxBuf[6] = bytes;

    sum+= TxBuf[2];
    sum+= TxBuf[3];
    sum+= TxBuf[4];
    sum+= TxBuf[5];
    sum+= TxBuf[6];
    TxBuf[7] = 0xff - sum;

    _dir = TRANSMIT;
    
    for(int i = 0; i <8; i++){
        _bus.putc(TxBuf[i]);
    }
    
    wait(0.00004);
    _dir = RECEIVE;

    Status[4] = 0XFE;
    if(_ID!=0xFE){
        for(int i = 0; i<(6+bytes); i++){
        Status[i] = _bus.getc();
        }
        for(int i = 0; i< Status[3]-2;i++){
        data[i] = Status[5+i];
        }
    }
    return(Status[4]);
}

int AX12::write(int ID, int start, int bytes, char* data){

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

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;
    TxBuf[2] = ID;
    TxBuf[3] = 3+bytes;
    TxBuf[4] = 0x03;
    TxBuf[5] = start;
    for(char i = 0; i<bytes; i++){
        TxBuf[6+i] = data[i];
        sum+= TxBuf[6+i];
    }

    sum+= TxBuf[2];
    sum+= TxBuf[3];
    sum+= TxBuf[4];
    sum+= TxBuf[5];
    TxBuf[6+bytes] = 0xff - sum;

    _dir = TRANSMIT;
    for(int i = 0; i <(7+bytes); i++){
        _bus.putc(TxBuf[i]);
    }
    
    wait(0.00004);
    _dir = RECEIVE;

    Status[4] = 0X00;
    if(_ID!=0xFE){
        for(int i = 0; i<(6); i++){
        Status[i] = _bus.getc();
        }
    }
    return(Status[4]);
}