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
 
 
     }