AX12.cpp

00001 #include "AX12.h"
00002 #include "mbed.h"
00003 #include "string"
00004 
00005 AX12::AX12(Serial& bus, Serial& PCbus, PinName dir, int ID)
00006     : _bus(bus), _PCbus(PCbus), _dir(dir){
00007     
00008     _bus.baud(1000000);
00009     _ID = ID;
00010     _dir = TRANSMIT;
00011 }
00012 
00013 int AX12::SetGoal(int degrees, int speed){
00014       
00015     short goal = (1023*degrees)/300;
00016     char data[4];
00017     data[0] = goal & 0xff;
00018     data[1] = goal >> 8;
00019     data[2] = 0x00;
00020     data[3] = 0x00;
00021   
00022     int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data);
00023     
00024     return (rVal);
00025 }
00026 
00027 // Set the mode of the servo
00028 //  0 = Positional (0-300 degrees)
00029 //  1 = Rotational -1 to 1 speed
00030 
00031 int AX12::SetMode(int mode) {
00032  
00033     if (mode == 1) {
00034         
00035         SetCWLimit(0);
00036         SetCCWLimit(0);
00037         SetCRSpeed(0.0);
00038     } else {
00039         SetCWLimit(0);
00040         SetCCWLimit(300);
00041         SetCRSpeed(0.0);
00042     }
00043     return(0);
00044 }
00045 
00046 int AX12::SetTorqueLimit (int torque) {
00047  
00048     char data[2];
00049     int set = (torque*1023)/10;
00050 
00051     data[0] = set && 0xff; // bottom 8 bits
00052     data[1] = set >> 8;   // top 8 bits
00053     
00054  
00055     // write the packet, return the error code
00056     return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); 
00057     
00058 }
00059 
00060 int AX12::TorqueEnable (int enable) {
00061  
00062     char data[1];
00063     
00064     data[0] = enable;
00065  
00066     return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data)); 
00067     
00068 }
00069 
00070 int AX12::SetCWLimit (int degrees) {
00071  
00072     char data[2]; 
00073     // 1023 / 300 * degrees
00074     short limit = (1023 * degrees) / 300;
00075  
00076     data[0] = limit & 0xff; // bottom 8 bits
00077     data[1] = limit >> 8;   // top 8 bits
00078  
00079     // write the packet, return the error code
00080     return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); 
00081 }
00082 
00083 int AX12::SetCCWLimit (int degrees) {
00084  
00085     char data[2];
00086  
00087     // 1023 / 300 * degrees
00088     short limit = (1023 * degrees) / 300;
00089  
00090     data[0] = limit & 0xff; // bottom 8 bits
00091     data[1] = limit >> 8;   // top 8 bits
00092  
00093     // write the packet, return the error code
00094     return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
00095 }
00096 
00097 int AX12::SetCRSpeed(float speed) {
00098  
00099     // bit 10     = direction, 0 = CCW, 1=CW
00100     // bits 9-0   = Speed
00101     char data[2];
00102  
00103     int goal = (0x3ff * abs(speed));
00104  
00105     // Set direction CW if we have a negative speed
00106     if (speed < 0) {
00107         goal |= (0x1 << 10);
00108     }
00109  
00110     data[0] = goal & 0xff; // bottom 8 bits
00111     data[1] = goal >> 8;   // top 8 bits
00112  
00113     // write the packet, return the error code
00114     int rVal = write(_ID, 0x20, 2, data);
00115  
00116     return(rVal);
00117 }
00118 
00119 int AX12::isMoving(void) {
00120  
00121     char data[1];
00122     read(_ID,AX12_REG_MOVING,1,data);
00123     return(data[0]);
00124 }
00125 
00126 int AX12::GetLoad(void){
00127     char data[2];
00128     int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
00129     int rVal = data[0] + (data[1] << 8);
00130     return (rVal);
00131 }
00132 
00133 int AX12::GetPosition(void){
00134     
00135     char data[2];
00136     
00137     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
00138     short position = data[0] + (data[1] << 8);
00139     float angle  = ((position * 300)/1023);
00140 
00141     return (angle);
00142 }
00143 
00144 int AX12::GetVoltage(void){
00145     
00146     char data[2];
00147     
00148     int ErrorCode = read(_ID, AX12_REG_VOLTAGE, 2, data);
00149     short volts = data[0] + (data[1] << 8);
00150 
00151     return (volts);
00152 }
00153 
00154 int AX12::read(int ID, int start, int bytes, char* data){
00155 
00156     char TxBuf[16];
00157     char sum = 0;
00158     char Status[16];
00159 
00160     TxBuf[0] = 0xff;
00161     TxBuf[1] = 0xff;
00162     TxBuf[2] = ID;
00163     TxBuf[3] = 0x04;
00164     TxBuf[4] = 0x02;
00165     TxBuf[5] = start;
00166     TxBuf[6] = bytes;
00167 
00168     sum+= TxBuf[2];
00169     sum+= TxBuf[3];
00170     sum+= TxBuf[4];
00171     sum+= TxBuf[5];
00172     sum+= TxBuf[6];
00173     TxBuf[7] = 0xff - sum;
00174 
00175     _dir = TRANSMIT;
00176     
00177     for(int i = 0; i <8; i++){
00178         _bus.putc(TxBuf[i]);
00179     }
00180     
00181     wait(0.00004);
00182     _dir = RECEIVE;
00183 
00184     Status[4] = 0XFE;
00185     if(_ID!=0xFE){
00186         for(int i = 0; i<(6+bytes); i++){
00187         Status[i] = _bus.getc();
00188         }
00189         for(int i = 0; i< Status[3]-2;i++){
00190         data[i] = Status[5+i];
00191         }
00192     }
00193     return(Status[4]);
00194 }
00195 
00196 int AX12::write(int ID, int start, int bytes, char* data){
00197 
00198     char TxBuf[16];
00199     char sum = 0;
00200     char Status[6];
00201 
00202     TxBuf[0] = 0xff;
00203     TxBuf[1] = 0xff;
00204     TxBuf[2] = ID;
00205     TxBuf[3] = 3+bytes;
00206     TxBuf[4] = 0x03;
00207     TxBuf[5] = start;
00208     for(char i = 0; i<bytes; i++){
00209         TxBuf[6+i] = data[i];
00210         sum+= TxBuf[6+i];
00211     }
00212 
00213     sum+= TxBuf[2];
00214     sum+= TxBuf[3];
00215     sum+= TxBuf[4];
00216     sum+= TxBuf[5];
00217     TxBuf[6+bytes] = 0xff - sum;
00218 
00219     _dir = TRANSMIT;
00220     for(int i = 0; i <(7+bytes); i++){
00221         _bus.putc(TxBuf[i]);
00222     }
00223     
00224     wait(0.00004);
00225     _dir = RECEIVE;
00226 
00227     Status[4] = 0X00;
00228     if(_ID!=0xFE){
00229         for(int i = 0; i<(6); i++){
00230         Status[i] = _bus.getc();
00231         }
00232     }
00233     return(Status[4]);
00234 }