A library for the AX-12+ servo using external ICs instead of the SerialHalf-Duplex class
A library based on the AX-12+ library but using external hardware to convert from full to half duplex as suggested in the AX-12 datasheet. The buffers and NOT gate need to connected as shown below.
Don't forget the pull-up resistor!
AX12.cpp
- Committer:
- ms523
- Date:
- 2012-10-07
- Revision:
- 2:3000c6778d1c
- Parent:
- 1:1dd9cd18975d
File content as of revision 2:3000c6778d1c:
/* mbed AX-12+ Servo Library - External hardware version */ #include "AX12.h" #include "mbed.h" AX12::AX12(Serial& bus, PinName dir, int ID) : _bus(bus), _dir(dir) { _bus.baud(1000000); _ID = ID; _dir = TRANSMIT; } int AX12::SetGoal(int degrees) { char data[2]; short goal = (1023 * degrees) / 300; // 1023 / 300 * degrees data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); // write the packet, return the error code return(rVal); } float 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)/1024; return (angle); } //******************************************************************************************** // Method used to read data from the regiesters of the AX-12+ Servo //******************************************************************************************** int AX12::read(int ID, int start, int bytes, char* data) { char TxBuf[16]; char sum = 0; char Status[16]; // Build the TxPacket first in RAM, then we'll send in one go... TxBuf[0] = 0xff; // Header byte 1 TxBuf[1] = 0xff; // Header byte 2 TxBuf[2] = ID; // ID byte TxBuf[3] = 0x04; // Packet Length (always 4 bytes) TxBuf[4] = 0x02; // Instruction byte (READ - Section 4.2 on datasheet) TxBuf[5] = start; // First AX-12 reg Address to read from (Parameter 1) TxBuf[6] = bytes; // Bytes to read //Work out checksum... sum += TxBuf[2]; sum += TxBuf[3]; sum += TxBuf[4]; sum += TxBuf[5]; sum += TxBuf[6]; TxBuf[7] = 0xFF - sum; // And send the packet to th AX-12+ Servo... _dir = TRANSMIT; // Switch the hardware to transmit... for (int i = 0; i < 8 ; i++) { // Transmit the packet in one burst with no pausing _bus.putc(TxBuf[i]); } wait (0.00004); // Wait for data to transmit _dir = RECEIVE; // Switch the hardware back to receive... //******************************************************************************************** // Now we should get a response back from the AX-12+ servo... //******************************************************************************************** Status[4] = 0xFE; // Initailise status[4] return code if (_ID!=0xFE) { // We'll only get a reply if it was not broadcast for (int i=0; i<(6+bytes) ; i++) { // Receive the Status packet 6+ number of bytes read Status[i] = _bus.getc(); } for (int i=0; i < Status[3]-2 ; i++) { // Copy the data from Status into data for return data[i] = Status[5+i]; } } // if (ID!=0xFE) return(Status[4]); } //******************************************************************************************** // Method used to write data into the regiesters of the AX-12+ Servo //******************************************************************************************** int AX12:: write(int ID, int start, int bytes, char* data) { // Format is - 0xff, 0xff, ID, Length, Intruction(write), Reg Address, Param(s), Checksum char TxBuf[16]; char sum = 0; char Status[6]; // Build the TxPacket first in RAM, then we'll send in one go... TxBuf[0] = 0xff; // Header byte 1 TxBuf[1] = 0xff; // Header byte 2 TxBuf[2] = ID; // ID byte TxBuf[3] = 3+bytes; // Packet Length byte TxBuf[4] = 0x03; // Instruction byte (WRITE - Section 4.1 on datasheet) TxBuf[5] = start; // First AX-12 reg Address to write to (Parameter 1) for (char i=0; i<bytes ; i++) { // Data to be written (Parameters 2 - N+1) TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; } //Work out checksum... sum += TxBuf[2]; sum += TxBuf[3]; sum += TxBuf[4]; sum += TxBuf[5]; TxBuf[6+bytes] = 0xFF - sum; // And send the packet to th AX-12+ Servo... _dir = TRANSMIT; // Switch the hardware to transmit... for (int i = 0; i < (7 + bytes) ; i++) { // Transmit the packet in one burst with no pausing _bus.putc(TxBuf[i]); } wait (0.00004); // Wait for data to transmit _dir = RECEIVE; // Switch the hardware back to receive... //******************************************************************************************** // Now we should get a response back from the AX-12+ servo... //******************************************************************************************** Status[4]=0x00; // Initailise status[4] to get correct response if (_ID!=0xFE) { // We'll only get a reply if it was not broadcast for (int i=0; i < 6 ; i++) { // Response is 6 bytes - 0xFF, 0xFF, ID, Length Error, Param(s) Checksum Status[i] = _bus.getc(); } } // if (ID!=0xFE) return(Status[4]); // return error code - if no error will return 0x00 }