Diff: AX12.cpp

Revision:

7:d0c0d11907e2

Parent:

5:950c4f4f73c6

Child:

9:0927a7b74806

Child:

10:8ed335824731

diff -r 950c4f4f73c6 -r d0c0d11907e2 AX12.cpp
--- a/AX12.cpp	Tue Apr 21 16:08:24 2015 +0000
+++ b/AX12.cpp	Thu Apr 23 06:40:59 2015 +0000
@@ -25,8 +25,6 @@
 #include "mbed.h"
 //#include "SerialHalfDuplex.h"
 
-Serial pc2(USBTX, USBRX); // tx, rx
-
 AX12::AX12(PinName tx, PinName rx, int ID, int baudrate)
         : _ax12(tx,rx) {
 
@@ -94,6 +92,10 @@
 
     int goal = (0x3ff * abs(speed));
 
+    if (AX12_DEBUG) {
+        printf("SetCRSpeed to 0x%x\n",goal);
+    }
+    
     // Set direction CW if we have a negative speed
     if (speed < 0) {
         goal |= (0x1 << 10);
@@ -110,7 +112,6 @@
 
 
 int AX12::SetCWLimit (int degrees) {
-    printf("SetCWLimit to %d\n",degrees);
 
     char data[2];
     
@@ -227,7 +228,6 @@
 
 
 float AX12::GetPosition(void) {
-    pc2.printf("\nGetPosition(ID%d)",_ID);
 
     if (AX12_DEBUG) {
         printf("\nGetPosition(%d)",_ID);
@@ -236,13 +236,9 @@
     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);
 }
 
@@ -353,11 +349,9 @@
     }
 
     // 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);
@@ -381,7 +375,7 @@
             printf("  Header : 0x%x\n",Status[1]);
             printf("  ID : 0x%x\n",Status[2]);
             printf("  Length : 0x%x\n",Status[3]);
-            pc2.printf("  Error Code : 0x%x\n",Status[4]);
+            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]);
@@ -467,7 +461,6 @@
     }
 
     // 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]);
     }