Diff: MX28.cpp

Revision:

6:5ce46b6748fd

Parent:

5:950c4f4f73c6

Child:

8:4b2c48ab30fe

--- a/MX28.cpp	Tue Apr 21 16:08:24 2015 +0000
+++ b/MX28.cpp	Wed Apr 22 18:06:00 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);
 }