Diff: AX12.cpp

Revision:

3:97fc13b4168c

Parent:

1:3ac0919c093f

Child:

5:950c4f4f73c6

--- a/AX12.cpp	Thu Apr 09 04:23:51 2015 +0000
+++ b/AX12.cpp	Fri Apr 10 03:16:21 2015 +0000
@@ -25,6 +25,8 @@
 #include "mbed.h"
 //#include "SerialHalfDuplex.h"
 
+Serial pc2(USBTX, USBRX); // tx, rx
+
 AX12::AX12(PinName tx, PinName rx, int ID)
         : _ax12(tx,rx) {
 
@@ -108,6 +110,7 @@
 
 
 int AX12::SetCWLimit (int degrees) {
+    printf("SetCWLimit to %d\n",degrees);
 
     char data[2];
     
@@ -224,6 +227,7 @@
 
 
 float AX12::GetPosition(void) {
+    pc2.printf("\nGetPosition(ID%d)",_ID);
 
     if (AX12_DEBUG) {
         printf("\nGetPosition(%d)",_ID);
@@ -232,9 +236,13 @@
     char data[2];
 
     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
+    pc2.printf("\n1");
     short position = data[0] + (data[1] << 8);
+    pc2.printf("\n2");
     float angle = (position * 300)/1024;
-
+    pc2.printf("\n3");
+    
+    pc2.printf("\nGetPosition(angle%f)\n",angle);
     return (angle);
 }
 
@@ -345,9 +353,11 @@
     }
 
     // Transmit the packet in one burst with no pausing
+    pc2.printf("read:transmit the packet");
     for (int i = 0; i<8 ; i++) {
         _ax12.putc(TxBuf[i]);
     }
+    pc2.printf("read:transmit the packet : end");
 
     // Wait for the bytes to be transmitted
     wait (0.00002);
@@ -371,7 +381,7 @@
             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]);
+            pc2.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]);
@@ -457,6 +467,7 @@
     }
 
     // Transmit the packet in one burst with no pausing
+    pc2.printf("transmit the packet\n");
     for (int i = 0; i < (7 + bytes) ; i++) {
         _ax12.putc(TxBuf[i]);
     }