Martin Smith / AX12_Hardware

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.

640

Don't forget the pull-up resistor!

AX12.cpp@2:3000c6778d1c, 2012-10-07 (annotated)

Committer:
ms523
Date:
Sun Oct 07 08:52:25 2012 +0000
Revision:
2:3000c6778d1c
Parent:
1:1dd9cd18975d 
Improved documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ms523 1:1dd9cd18975d 1 /* mbed AX-12+ Servo Library - External hardware version */
ms523 1:1dd9cd18975d 2
ms523 0:d4af99baf76f 3 #include "AX12.h"
ms523 0:d4af99baf76f 4 #include "mbed.h"
ms523 0:d4af99baf76f 5
ms523 2:3000c6778d1c 6 AX12::AX12(Serial& bus, PinName dir, int ID)
ms523 2:3000c6778d1c 7 : _bus(bus), _dir(dir) {
ms523 0:d4af99baf76f 8
ms523 2:3000c6778d1c 9 _bus.baud(1000000);
ms523 0:d4af99baf76f 10 _ID = ID;
ms523 0:d4af99baf76f 11 _dir = TRANSMIT;
ms523 0:d4af99baf76f 12 }
ms523 0:d4af99baf76f 13
ms523 1:1dd9cd18975d 14 int AX12::SetGoal(int degrees) {
ms523 0:d4af99baf76f 15
ms523 0:d4af99baf76f 16 char data[2];
ms523 0:d4af99baf76f 17
ms523 1:1dd9cd18975d 18 short goal = (1023 * degrees) / 300; // 1023 / 300 * degrees
ms523 1:1dd9cd18975d 19 data[0] = goal & 0xff; // bottom 8 bits
ms523 1:1dd9cd18975d 20 data[1] = goal >> 8; // top 8 bits
ms523 1:1dd9cd18975d 21 int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data); // write the packet, return the error code
ms523 0:d4af99baf76f 22
ms523 1:1dd9cd18975d 23 return(rVal);
ms523 0:d4af99baf76f 24 }
ms523 0:d4af99baf76f 25
ms523 0:d4af99baf76f 26 float AX12::GetPosition(void) {
ms523 0:d4af99baf76f 27
ms523 0:d4af99baf76f 28 char data[2];
ms523 0:d4af99baf76f 29
ms523 0:d4af99baf76f 30 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
ms523 0:d4af99baf76f 31 short position = data[0] + (data[1] << 8);
ms523 0:d4af99baf76f 32 float angle = (position * 300)/1024;
ms523 0:d4af99baf76f 33
ms523 0:d4af99baf76f 34 return (angle);
ms523 0:d4af99baf76f 35 }
ms523 0:d4af99baf76f 36
ms523 1:1dd9cd18975d 37 //********************************************************************************************
ms523 1:1dd9cd18975d 38 // Method used to read data from the regiesters of the AX-12+ Servo
ms523 1:1dd9cd18975d 39 //********************************************************************************************
ms523 0:d4af99baf76f 40 int AX12::read(int ID, int start, int bytes, char* data) {
ms523 0:d4af99baf76f 41
ms523 0:d4af99baf76f 42 char TxBuf[16];
ms523 0:d4af99baf76f 43 char sum = 0;
ms523 0:d4af99baf76f 44 char Status[16];
ms523 0:d4af99baf76f 45
ms523 1:1dd9cd18975d 46 // Build the TxPacket first in RAM, then we'll send in one go...
ms523 1:1dd9cd18975d 47 TxBuf[0] = 0xff; // Header byte 1
ms523 1:1dd9cd18975d 48 TxBuf[1] = 0xff; // Header byte 2
ms523 1:1dd9cd18975d 49 TxBuf[2] = ID; // ID byte
ms523 1:1dd9cd18975d 50 TxBuf[3] = 0x04; // Packet Length (always 4 bytes)
ms523 1:1dd9cd18975d 51 TxBuf[4] = 0x02; // Instruction byte (READ - Section 4.2 on datasheet)
ms523 1:1dd9cd18975d 52 TxBuf[5] = start; // First AX-12 reg Address to read from (Parameter 1)
ms523 1:1dd9cd18975d 53 TxBuf[6] = bytes; // Bytes to read
ms523 0:d4af99baf76f 54
ms523 1:1dd9cd18975d 55 //Work out checksum...
ms523 1:1dd9cd18975d 56 sum += TxBuf[2];
ms523 1:1dd9cd18975d 57 sum += TxBuf[3];
ms523 1:1dd9cd18975d 58 sum += TxBuf[4];
ms523 0:d4af99baf76f 59 sum += TxBuf[5];
ms523 0:d4af99baf76f 60 sum += TxBuf[6];
ms523 0:d4af99baf76f 61 TxBuf[7] = 0xFF - sum;
ms523 0:d4af99baf76f 62
ms523 1:1dd9cd18975d 63 // And send the packet to th AX-12+ Servo...
ms523 1:1dd9cd18975d 64 _dir = TRANSMIT; // Switch the hardware to transmit...
ms523 1:1dd9cd18975d 65 for (int i = 0; i < 8 ; i++) { // Transmit the packet in one burst with no pausing
ms523 2:3000c6778d1c 66 _bus.putc(TxBuf[i]);
ms523 1:1dd9cd18975d 67 }
ms523 1:1dd9cd18975d 68 wait (0.00004); // Wait for data to transmit
ms523 1:1dd9cd18975d 69 _dir = RECEIVE; // Switch the hardware back to receive...
ms523 0:d4af99baf76f 70
ms523 1:1dd9cd18975d 71 //********************************************************************************************
ms523 1:1dd9cd18975d 72 // Now we should get a response back from the AX-12+ servo...
ms523 1:1dd9cd18975d 73 //********************************************************************************************
ms523 1:1dd9cd18975d 74 Status[4] = 0xFE; // Initailise status[4] return code
ms523 1:1dd9cd18975d 75 if (_ID!=0xFE) { // We'll only get a reply if it was not broadcast
ms523 1:1dd9cd18975d 76 for (int i=0; i<(6+bytes) ; i++) { // Receive the Status packet 6+ number of bytes read
ms523 2:3000c6778d1c 77 Status[i] = _bus.getc();
ms523 0:d4af99baf76f 78 }
ms523 1:1dd9cd18975d 79 for (int i=0; i < Status[3]-2 ; i++) { // Copy the data from Status into data for return
ms523 0:d4af99baf76f 80 data[i] = Status[5+i];
ms523 0:d4af99baf76f 81 }
ms523 0:d4af99baf76f 82 } // if (ID!=0xFE)
ms523 0:d4af99baf76f 83 return(Status[4]);
ms523 0:d4af99baf76f 84 }
ms523 0:d4af99baf76f 85
ms523 1:1dd9cd18975d 86 //********************************************************************************************
ms523 1:1dd9cd18975d 87 // Method used to write data into the regiesters of the AX-12+ Servo
ms523 1:1dd9cd18975d 88 //********************************************************************************************
ms523 1:1dd9cd18975d 89 int AX12:: write(int ID, int start, int bytes, char* data) {
ms523 0:d4af99baf76f 90
ms523 1:1dd9cd18975d 91 // Format is - 0xff, 0xff, ID, Length, Intruction(write), Reg Address, Param(s), Checksum
ms523 0:d4af99baf76f 92 char TxBuf[16];
ms523 0:d4af99baf76f 93 char sum = 0;
ms523 0:d4af99baf76f 94 char Status[6];
ms523 0:d4af99baf76f 95
ms523 1:1dd9cd18975d 96 // Build the TxPacket first in RAM, then we'll send in one go...
ms523 1:1dd9cd18975d 97 TxBuf[0] = 0xff; // Header byte 1
ms523 1:1dd9cd18975d 98 TxBuf[1] = 0xff; // Header byte 2
ms523 1:1dd9cd18975d 99 TxBuf[2] = ID; // ID byte
ms523 1:1dd9cd18975d 100 TxBuf[3] = 3+bytes; // Packet Length byte
ms523 1:1dd9cd18975d 101 TxBuf[4] = 0x03; // Instruction byte (WRITE - Section 4.1 on datasheet)
ms523 1:1dd9cd18975d 102 TxBuf[5] = start; // First AX-12 reg Address to write to (Parameter 1)
ms523 1:1dd9cd18975d 103 for (char i=0; i<bytes ; i++) { // Data to be written (Parameters 2 - N+1)
ms523 0:d4af99baf76f 104 TxBuf[6+i] = data[i];
ms523 0:d4af99baf76f 105 sum += TxBuf[6+i];
ms523 0:d4af99baf76f 106 }
ms523 1:1dd9cd18975d 107
ms523 1:1dd9cd18975d 108 //Work out checksum...
ms523 1:1dd9cd18975d 109 sum += TxBuf[2];
ms523 1:1dd9cd18975d 110 sum += TxBuf[3];
ms523 1:1dd9cd18975d 111 sum += TxBuf[4];
ms523 1:1dd9cd18975d 112 sum += TxBuf[5];
ms523 0:d4af99baf76f 113 TxBuf[6+bytes] = 0xFF - sum;
ms523 0:d4af99baf76f 114
ms523 1:1dd9cd18975d 115 // And send the packet to th AX-12+ Servo...
ms523 1:1dd9cd18975d 116 _dir = TRANSMIT; // Switch the hardware to transmit...
ms523 1:1dd9cd18975d 117 for (int i = 0; i < (7 + bytes) ; i++) { // Transmit the packet in one burst with no pausing
ms523 2:3000c6778d1c 118 _bus.putc(TxBuf[i]);
ms523 0:d4af99baf76f 119 }
ms523 1:1dd9cd18975d 120 wait (0.00004); // Wait for data to transmit
ms523 1:1dd9cd18975d 121 _dir = RECEIVE; // Switch the hardware back to receive...
ms523 0:d4af99baf76f 122
ms523 1:1dd9cd18975d 123 //********************************************************************************************
ms523 1:1dd9cd18975d 124 // Now we should get a response back from the AX-12+ servo...
ms523 1:1dd9cd18975d 125 //********************************************************************************************
ms523 1:1dd9cd18975d 126 Status[4]=0x00; // Initailise status[4] to get correct response
ms523 1:1dd9cd18975d 127 if (_ID!=0xFE) { // We'll only get a reply if it was not broadcast
ms523 1:1dd9cd18975d 128 for (int i=0; i < 6 ; i++) { // Response is 6 bytes - 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
ms523 2:3000c6778d1c 129 Status[i] = _bus.getc();
ms523 0:d4af99baf76f 130 }
ms523 1:1dd9cd18975d 131 } // if (ID!=0xFE)
ms523 1:1dd9cd18975d 132 return(Status[4]); // return error code - if no error will return 0x00
ms523 1:1dd9cd18975d 133 }