AX12.cpp

00001 /* mbed AX-12+ Servo Library - External hardware version */
00002 
00003 #include "AX12.h"
00004 #include "mbed.h"
00005 
00006 AX12::AX12(Serial& bus, PinName dir, int ID)
00007         : _bus(bus), _dir(dir) {
00008 
00009     _bus.baud(1000000);
00010     _ID = ID;
00011     _dir = TRANSMIT; 
00012 }
00013 
00014 int AX12::SetGoal(int degrees) {
00015 
00016     char data[2];
00017 
00018     short goal = (1023 * degrees) / 300;                    // 1023 / 300 * degrees
00019     data[0] = goal & 0xff;                                  // bottom 8 bits
00020     data[1] = goal >> 8;                                    // top 8 bits
00021     int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); // write the packet, return the error code
00022 
00023     return(rVal); 
00024 }
00025 
00026 float AX12::GetPosition(void) {
00027 
00028     char data[2];
00029 
00030     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
00031     short position = data[0] + (data[1] << 8);
00032     float angle = (position * 300)/1024;
00033 
00034     return (angle);
00035 }
00036 
00037 //********************************************************************************************
00038 // Method used to read data from the regiesters of the AX-12+ Servo
00039 //********************************************************************************************
00040 int AX12::read(int ID, int start, int bytes, char* data) {
00041 
00042     char TxBuf[16];
00043     char sum = 0;
00044     char Status[16];
00045 
00046     // Build the TxPacket first in RAM, then we'll send in one go...
00047     TxBuf[0] = 0xff;                // Header byte 1
00048     TxBuf[1] = 0xff;                // Header byte 2   
00049     TxBuf[2] = ID;                  // ID byte 
00050     TxBuf[3] = 0x04;                // Packet Length (always 4 bytes)
00051     TxBuf[4] = 0x02;                // Instruction byte (READ - Section 4.2 on datasheet)
00052     TxBuf[5] = start;               // First AX-12 reg Address to read from (Parameter 1)
00053     TxBuf[6] = bytes;               // Bytes to read
00054 
00055     //Work out checksum...  
00056     sum += TxBuf[2];
00057     sum += TxBuf[3];
00058     sum += TxBuf[4];
00059     sum += TxBuf[5];
00060     sum += TxBuf[6];
00061     TxBuf[7] = 0xFF - sum;
00062 
00063     // And send the packet to th AX-12+ Servo...
00064     _dir = TRANSMIT;                            // Switch the hardware to transmit...   
00065     for (int i = 0; i < 8 ; i++) {              // Transmit the packet in one burst with no pausing
00066         _bus.putc(TxBuf[i]);
00067     }  
00068     wait (0.00004);                             // Wait for data to transmit  
00069     _dir = RECEIVE;                             // Switch the hardware back to receive...
00070 
00071     //********************************************************************************************
00072     // Now we should get a response back from the AX-12+ servo...
00073     //********************************************************************************************
00074     Status[4] = 0xFE;                           // Initailise status[4] return code
00075     if (_ID!=0xFE) {                            // We'll only get a reply if it was not broadcast
00076         for (int i=0; i<(6+bytes) ; i++) {      // Receive the Status packet 6+ number of bytes read
00077             Status[i] = _bus.getc();
00078         }
00079         for (int i=0; i < Status[3]-2 ; i++) {  // Copy the data from Status into data for return
00080             data[i] = Status[5+i];
00081         }
00082     } // if (ID!=0xFE)
00083     return(Status[4]);
00084 }
00085 
00086 //********************************************************************************************
00087 // Method used to write data into the regiesters of the AX-12+ Servo
00088 //********************************************************************************************
00089 int AX12:: write(int ID, int start, int bytes, char* data) {
00090 
00091     // Format is - 0xff, 0xff, ID, Length, Intruction(write), Reg Address, Param(s), Checksum
00092     char TxBuf[16];
00093     char sum = 0;
00094     char Status[6];
00095 
00096     // Build the TxPacket first in RAM, then we'll send in one go...
00097     TxBuf[0] = 0xff;                // Header byte 1
00098     TxBuf[1] = 0xff;                // Header byte 2   
00099     TxBuf[2] = ID;                  // ID byte 
00100     TxBuf[3] = 3+bytes;             // Packet Length byte
00101     TxBuf[4] = 0x03;                // Instruction byte (WRITE - Section 4.1 on datasheet)
00102     TxBuf[5] = start;               // First AX-12 reg Address to write to (Parameter 1)
00103     for (char i=0; i<bytes ; i++) { // Data to be written (Parameters 2 - N+1)
00104         TxBuf[6+i] = data[i];
00105         sum += TxBuf[6+i];
00106     }
00107     
00108     //Work out checksum...  
00109     sum += TxBuf[2];
00110     sum += TxBuf[3];
00111     sum += TxBuf[4];
00112     sum += TxBuf[5];
00113     TxBuf[6+bytes] = 0xFF - sum;
00114 
00115     // And send the packet to th AX-12+ Servo...
00116     _dir = TRANSMIT;                            // Switch the hardware to transmit...   
00117     for (int i = 0; i < (7 + bytes) ; i++) {    // Transmit the packet in one burst with no pausing
00118         _bus.putc(TxBuf[i]);
00119     }  
00120     wait (0.00004);                             // Wait for data to transmit  
00121     _dir = RECEIVE;                             // Switch the hardware back to receive...
00122 
00123     //********************************************************************************************
00124     // Now we should get a response back from the AX-12+ servo...
00125     //********************************************************************************************
00126     Status[4]=0x00;                             // Initailise status[4] to get correct response 
00127     if (_ID!=0xFE) {                            // We'll only get a reply if it was not broadcast
00128         for (int i=0; i < 6 ; i++) {            // Response is 6 bytes - 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00129             Status[i] = _bus.getc();
00130         }
00131     } // if (ID!=0xFE)
00132     return(Status[4]);                          // return error code - if no error will return 0x00
00133 }