File content as of revision 25:d345ef4cb2b5:

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

// Set the mode of the servo
//  0 = Positional (0-300 degrees)
//  1 = Rotational -1 to 1 speed

int AX12::SetMode(int mode) {
 
    if (mode == 1) {
        
        SetCWLimit(0);
        SetCCWLimit(0);
        SetCRSpeed(0.0);
    } else {
        SetCWLimit(70);
        SetCCWLimit(270);
        SetCRSpeed(0.0);
    }
    return(0);
}

int AX12::SetTorqueLimit (int torque) {
 
    char data[2];
    int set = (torque*1023)/10;

    data[0] = set && 0xff; // bottom 8 bits
    data[1] = set >> 8;   // top 8 bits
    
 
    // write the packet, return the error code
    return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); 
    
}

int AX12::TorqueEnable (int enable) {
 
    char data[1];
    
    data[0] = enable;
 
    return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data)); 
    
}

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::SetCRSpeed(float speed) {
 
    // bit 10     = direction, 0 = CCW, 1=CW
    // bits 9-0   = Speed
    char data[2];
 
    int goal = (0x3ff * abs(speed));
 
    // Set direction CW if we have a negative speed
    if (speed < 0) {
        goal |= (0x1 << 10);
    }
 
    data[0] = goal & 0xff; // bottom 8 bits
    data[1] = goal >> 8;   // top 8 bits
 
    // write the packet, return the error code
    int rVal = write(_ID, 0x20, 2, data);
 
    return(rVal);
}

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);
    short position = data[0] + (data[1] << 8);
    float angle  = ((position * 300)/1023);

    return (angle);
}

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