File content as of revision 21:ab49231d1479:

#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, int flags){
    
    char reg_flag = 0;
    char data[2];
    
    if (flags == 0x2) {
        reg_flag = 1;
    }
      
    short goal = (1023*degrees)/300;
    data[0] = goal & 0xff;
    data[1] = goal >> 8;
    
    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data);

if (flags == 1){
    while (isMoving()) {}
    }
    return(rVal);
}

int AX12::SetCWLimit (int degrees) {
 
    char data[2]; 
    // 1023 / 300 * degrees
    short limit = (1023 * degrees) / 300;
 
    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits
 
    // write the packet, return the error code
    return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); 
}

int AX12::SetCCWLimit (int degrees) {
 
    char data[2];
 
    // 1023 / 300 * degrees
    short limit = (1023 * degrees) / 300;
 
    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits
 
    // write the packet, return the error code
    return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
}

int AX12::isMoving(void) {
 
    char data[1];
    read(_ID,AX12_REG_MOVING,1,data);
    return(data[0]);
}

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::GetPosition(void){
    
    char data[2];
    
    int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
    int position = data[0] + (data[1] << 8);
//    int angle  = (position * 300)/1023;
//    (1023 * degrees) / 300
    return (position);
}

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