Revision 7:d0c0d11907e2, committed 2015-04-23

Comitter:

yusuke_kyo

Date:

Thu Apr 23 06:40:59 2015 +0000

Parent:

5:950c4f4f73c6

Commit message:

correct MX28 GetGoal

Changed in this revision

--- 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]);
     }

--- a/AX12.h	Tue Apr 21 16:08:24 2015 +0000
+++ b/AX12.h	Thu Apr 23 06:40:59 2015 +0000
@@ -30,7 +30,7 @@
 #define AX12_WRITE_DEBUG 0
 #define AX12_READ_DEBUG 0
 #define AX12_TRIGGER_DEBUG 0
-#define AX12_DEBUG 0
+#define AX12_DEBUG 1
 
 #define AX12_REG_ID 0x3
 #define AX12_REG_CW_LIMIT 0x06

--- a/MX28.cpp	Tue Apr 21 16:08:24 2015 +0000
+++ b/MX28.cpp	Thu Apr 23 06:40:59 2015 +0000
@@ -92,6 +92,10 @@
 
     int goal = (0x3ff * abs(speed));
 
+    if (MX28_DEBUG) {
+        printf("SetCRSpeed to 0x%x\n",goal);
+    }
+    
     // Set direction CW if we have a negative speed
     if (speed < 0) {
         goal |= (0x1 << 10);
@@ -233,7 +237,7 @@
 
     int ErrorCode = read(_ID, MX28_REG_POSITION, 2, data);
     short position = data[0] + (data[1] << 8);
-    float angle = (position * 300)/1024;
+    float angle = (position * 360) / 4095;
 
     return (angle);
 }
@@ -270,7 +274,7 @@
     }
     char data[2];
     int ErrorCode = read(_ID, MX28_REG_CURRENT, 2, data);
-    float current = ((data[0]+(data[1] << 8))-0x8FF)*0.0045;
+    float current = ((data[0]+(data[1] << 8))-0x800)*0.0045;
     return(current);
 }
 

--- a/MX28.h	Tue Apr 21 16:08:24 2015 +0000
+++ b/MX28.h	Thu Apr 23 06:40:59 2015 +0000
@@ -30,7 +30,7 @@
 #define MX28_WRITE_DEBUG 0
 #define MX28_READ_DEBUG 0
 #define MX28_TRIGGER_DEBUG 0
-#define MX28_DEBUG 0
+#define MX28_DEBUG 1
 
 #define MX28_REG_ID 0x3
 #define MX28_REG_CW_LIMIT 0x06