Ricky Kwon
Fish Feeder
Fork of
AX12
by 
Chris Styles
Diff: AX12.cpp
- Revision:
- 2:5ea99c37a2d7
- Parent:
- 1:93ad80f5fde7
- Child:
- 3:ced71d1b2558
--- a/AX12.cpp Wed Mar 30 16:08:02 2011 +0000
+++ b/AX12.cpp Sun Apr 10 20:58:21 2011 +0000
@@ -24,11 +24,12 @@
#include "AX12.h"
#include "mbed.h"
-AX12::AX12(PinName tx, PinName rx, int ID)
+AX12::AX12(PinName tx, PinName rx, int ID, int baud)
: _ax12(tx,rx) {
+ _baud = baud;
+ _ID = ID;
+ _ax12.baud(_baud);
- _ax12.baud(1000000);
- _ID = ID;
}
// Set the mode of the servo
@@ -64,9 +65,9 @@
// 1023 / 300 * degrees
short goal = (1023 * degrees) / 300;
- if (AX12_DEBUG) {
- printf("SetGoal to 0x%x\n",goal);
- }
+#ifdef AX12_DEBUG
+ printf("SetGoal to 0x%x\n",goal);
+#endif
data[0] = goal & 0xff; // bottom 8 bits
data[1] = goal >> 8; // top 8 bits
@@ -109,13 +110,13 @@
int AX12::SetCWLimit (int degrees) {
char data[2];
-
+
// 1023 / 300 * degrees
short limit = (1023 * degrees) / 300;
- if (AX12_DEBUG) {
- printf("SetCWLimit to 0x%x\n",limit);
- }
+#ifdef AX12_DEBUG
+ printf("SetCWLimit to 0x%x\n",limit);
+#endif
data[0] = limit & 0xff; // bottom 8 bits
data[1] = limit >> 8; // top 8 bits
@@ -132,9 +133,9 @@
// 1023 / 300 * degrees
short limit = (1023 * degrees) / 300;
- if (AX12_DEBUG) {
- printf("SetCCWLimit to 0x%x\n",limit);
- }
+#ifdef AX12_DEBUG
+ printf("SetCCWLimit to 0x%x\n",limit);
+#endif
data[0] = limit & 0xff; // bottom 8 bits
data[1] = limit >> 8; // top 8 bits
@@ -148,9 +149,11 @@
char data[1];
data[0] = NewID;
- if (AX12_DEBUG) {
- printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
- }
+
+#ifdef AX12_DEBUG
+ printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
+#endif
+
return (write(CurrentID, AX12_REG_ID, 1, data));
}
@@ -170,14 +173,11 @@
char TxBuf[16];
char sum = 0;
- if (AX12_TRIGGER_DEBUG) {
- printf("\nTriggered\n");
- }
-
+#ifdef AX12_TRIGGER_DEBUG
// Build the TxPacket first in RAM, then we'll send in one go
- if (AX12_TRIGGER_DEBUG) {
- printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
- }
+ printf("\nTriggered\n");
+ printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
+#endif
TxBuf[0] = 0xFF;
TxBuf[1] = 0xFF;
@@ -186,29 +186,31 @@
TxBuf[2] = 0xFE;
sum += TxBuf[2];
- if (AX12_TRIGGER_DEBUG) {
- printf(" ID : %d\n",TxBuf[2]);
- }
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
// Length
TxBuf[3] = 0x02;
sum += TxBuf[3];
- if (AX12_TRIGGER_DEBUG) {
- printf(" Length %d\n",TxBuf[3]);
- }
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Length %d\n",TxBuf[3]);
+#endif
// Instruction - ACTION
TxBuf[4] = 0x04;
sum += TxBuf[4];
- if (AX12_TRIGGER_DEBUG) {
- printf(" Instruction 0x%X\n",TxBuf[5]);
- }
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Instruction 0x%X\n",TxBuf[5]);
+#endif
// Checksum
TxBuf[5] = 0xFF - sum;
- if (AX12_TRIGGER_DEBUG) {
- printf(" Checksum 0x%X\n",TxBuf[5]);
- }
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Checksum 0x%X\n",TxBuf[5]);
+#endif
// Transmit the packet in one burst with no pausing
for (int i = 0; i < 6 ; i++) {
@@ -216,16 +218,15 @@
}
// This is a broadcast packet, so there will be no reply
-
return;
}
float AX12::GetPosition(void) {
- if (AX12_DEBUG) {
- printf("\nGetPosition(%d)",_ID);
- }
+#ifdef AX12_DEBUG
+ printf("\nGetPosition(%d)",_ID);
+#endif
char data[2];
@@ -239,9 +240,10 @@
float AX12::GetTemp (void) {
- if (AX12_DEBUG) {
- printf("\nGetTemp(%d)",_ID);
- }
+#ifdef AX12_DEBUG
+ printf("\nGetTemp(%d)",_ID);
+#endif
+
char data[1];
int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
float temp = data[0];
@@ -250,9 +252,11 @@
float AX12::GetVolts (void) {
- if (AX12_DEBUG) {
- printf("\nGetVolts(%d)",_ID);
- }
+
+#ifdef AX12_DEBUG
+ printf("\nGetVolts(%d)",_ID);
+#endif
+
char data[1];
int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
float volts = data[0]/10.0;
@@ -269,14 +273,14 @@
Status[4] = 0xFE; // return code
- if (AX12_READ_DEBUG) {
- printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
- }
+#ifdef AX12_READ_DEBUG
+ printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
+#endif
// Build the TxPacket first in RAM, then we'll send in one go
- if (AX12_READ_DEBUG) {
- printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
- }
+#ifdef AX12_READ_DEBUG
+ printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
+#endif
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
@@ -284,43 +288,48 @@
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
- if (AX12_READ_DEBUG) {
- printf(" ID : %d\n",TxBuf[2]);
- }
+
+#ifdef AX12_READ_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
// Packet Length
TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
sum += TxBuf[3]; // Accululate the packet sum
- if (AX12_READ_DEBUG) {
- printf(" Length : 0x%x\n",TxBuf[3]);
- }
+
+#ifdef AX12_READ_DEBUG
+ printf(" Length : 0x%x\n",TxBuf[3]);
+#endif
// Instruction - Read
TxBuf[4] = 0x2;
sum += TxBuf[4];
- if (AX12_READ_DEBUG) {
- printf(" Instruction : 0x%x\n",TxBuf[4]);
- }
+
+#ifdef AX12_READ_DEBUG
+ printf(" Instruction : 0x%x\n",TxBuf[4]);
+#endif
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
- if (AX12_READ_DEBUG) {
- printf(" Start Address : 0x%x\n",TxBuf[5]);
- }
+
+#ifdef AX12_READ_DEBUG
+ printf(" Start Address : 0x%x\n",TxBuf[5]);
+#endif
// Bytes to read
TxBuf[6] = bytes;
sum += TxBuf[6];
- if (AX12_READ_DEBUG) {
- printf(" No bytes : 0x%x\n",TxBuf[6]);
- }
+
+#ifdef AX12_READ_DEBUG
+ printf(" No bytes : 0x%x\n",TxBuf[6]);
+#endif
// Checksum
TxBuf[7] = 0xFF - sum;
- if (AX12_READ_DEBUG) {
- printf(" Checksum : 0x%x\n",TxBuf[7]);
- }
+#ifdef AX12_READ_DEBUG
+ printf(" Checksum : 0x%x\n",TxBuf[7]);
+#endif
// Transmit the packet in one burst with no pausing
for (int i = 0; i<8 ; i++) {
@@ -333,9 +342,30 @@
// Skip if the read was to the broadcast address
if (_ID != 0xFE) {
- // Receive the Status packet 6+ number of bytes read
- for (int i=0; i<(6+bytes) ; i++) {
- Status[i] = _ax12.getc();
+
+
+ // response packet is always 6 + bytes
+ // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+ // timeout is a little more than the time to transmit
+ // the packet back, i.e. (6+bytes)*10 bit periods
+
+ int timeout = 0;
+ int plen = 0;
+ while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
+
+ if (_ax12.readable()) {
+ Status[plen] = _ax12.getc();
+ plen++;
+ timeout = 0;
+ }
+
+ // wait for the bit period
+ wait (1.0/_baud);
+ timeout++;
+ }
+
+ if (timeout == ((6+bytes)*10) ) {
+ return(-1);
}
// Copy the data from Status into data for return
@@ -343,20 +373,20 @@
data[i] = Status[5+i];
}
- if (AX12_READ_DEBUG) {
- printf("\nStatus Packet\n");
- printf(" Header : 0x%x\n",Status[0]);
- printf(" Header : 0x%x\n",Status[1]);
- printf(" ID : 0x%x\n",Status[2]);
- printf(" Length : 0x%x\n",Status[3]);
- printf(" Error Code : 0x%x\n",Status[4]);
+#ifdef AX12_READ_DEBUG
+ printf("\nStatus Packet\n");
+ printf(" Header : 0x%x\n",Status[0]);
+ printf(" Header : 0x%x\n",Status[1]);
+ printf(" ID : 0x%x\n",Status[2]);
+ printf(" Length : 0x%x\n",Status[3]);
+ printf(" Error Code : 0x%x\n",Status[4]);
- for (int i=0; i < Status[3]-2 ; i++) {
- printf(" Data : 0x%x\n",Status[5+i]);
- }
+ for (int i=0; i < Status[3]-2 ; i++) {
+ printf(" Data : 0x%x\n",Status[5+i]);
+ }
- printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
- }
+ printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+#endif
} // if (ID!=0xFE)
@@ -364,21 +394,21 @@
}
-int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
+int AX12::write(int ID, int start, int bytes, char* data, int flag) {
// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
char TxBuf[16];
char sum = 0;
char Status[6];
- if (AX12_WRITE_DEBUG) {
- printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
- }
+#ifdef AX12_WRITE_DEBUG
+ printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
+#endif
// Build the TxPacket first in RAM, then we'll send in one go
- if (AX12_WRITE_DEBUG) {
- printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
- }
+#ifdef AX12_WRITE_DEBUG
+ printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
+#endif
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
@@ -387,17 +417,17 @@
TxBuf[2] = ID;
sum += TxBuf[2];
- if (AX12_WRITE_DEBUG) {
- printf(" ID : %d\n",TxBuf[2]);
- }
+#ifdef AX12_WRITE_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
// packet Length
TxBuf[3] = 3+bytes;
sum += TxBuf[3];
- if (AX12_WRITE_DEBUG) {
- printf(" Length : %d\n",TxBuf[3]);
- }
+#ifdef AX12_WRITE_DEBUG
+ printf(" Length : %d\n",TxBuf[3]);
+#endif
// Instruction
if (flag == 1) {
@@ -408,31 +438,35 @@
sum += TxBuf[4];
}
- if (AX12_WRITE_DEBUG) {
- printf(" Instruction : 0x%x\n",TxBuf[4]);
- }
+#ifdef AX12_WRITE_DEBUG
+ printf(" Instruction : 0x%x\n",TxBuf[4]);
+#endif
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
- if (AX12_WRITE_DEBUG) {
- printf(" Start : 0x%x\n",TxBuf[5]);
- }
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Start : 0x%x\n",TxBuf[5]);
+#endif
// data
for (char i=0; i<bytes ; i++) {
TxBuf[6+i] = data[i];
sum += TxBuf[6+i];
- if (AX12_WRITE_DEBUG) {
- printf(" Data : 0x%x\n",TxBuf[6+i]);
- }
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Data : 0x%x\n",TxBuf[6+i]);
+#endif
+
}
// checksum
TxBuf[6+bytes] = 0xFF - sum;
- if (AX12_WRITE_DEBUG) {
- printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
- }
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
+#endif
// Transmit the packet in one burst with no pausing
for (int i = 0; i < (7 + bytes) ; i++) {
@@ -448,20 +482,35 @@
// we'll only get a reply if it was not broadcast
if (_ID!=0xFE) {
- // response is always 6 bytes
+
+ // response packet is always 6 bytes
// 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
- for (int i=0; i < 6 ; i++) {
- Status[i] = _ax12.getc();
+ // timeout is a little more than the time to transmit
+ // the packet back, i.e. 60 bit periods, round up to 100
+ int timeout = 0;
+ int plen = 0;
+ while ((timeout < 100) && (plen<6)) {
+
+ if (_ax12.readable()) {
+ Status[plen] = _ax12.getc();
+ plen++;
+ timeout = 0;
+ }
+
+ // wait for the bit period
+ wait (1.0/_baud);
+ timeout++;
}
+
// Build the TxPacket first in RAM, then we'll send in one go
- if (AX12_WRITE_DEBUG) {
- printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
- printf(" ID : %d\n",Status[2]);
- printf(" Length : %d\n",Status[3]);
- printf(" Error : 0x%x\n",Status[4]);
- printf(" Checksum : 0x%x\n",Status[5]);
- }
+#ifdef AX12_WRITE_DEBUG
+ printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
+ printf(" ID : %d\n",Status[2]);
+ printf(" Length : %d\n",Status[3]);
+ printf(" Error : 0x%x\n",Status[4]);
+ printf(" Checksum : 0x%x\n",Status[5]);
+#endif
}