Diff: MX28.cpp

Revision:

10:8ed335824731

Parent:

8:4b2c48ab30fe

--- a/MX28.cpp	Fri Apr 24 03:21:14 2015 +0000
+++ b/MX28.cpp	Sat Apr 25 06:01:21 2015 +0000
@@ -66,7 +66,7 @@
     // 4095 / 360 * degrees
     short goal = (4095 * degrees) / 360;
     if (MX28_DEBUG) {
-        printf("SetGoal to 0x%x\n",goal);
+        printf("SetGoal to %d[degrees], 0x%x\n",degrees,goal);
     }
 
     data[0] = goal & 0xff; // bottom 8 bits
@@ -274,7 +274,9 @@
     }
     char data[2];
     int ErrorCode = read(_ID, MX28_REG_CURRENT, 2, data);
-    float current = (data[0]+(data[1] << 8)-0x800)*0.0045;
+//    float current = ((data[0]+(data[1] << 8))-0x800)*0.0045;
+    short nama = (data[0]+(data[1] << 8));
+    float current = nama*10;
     return(current);
 }
 
@@ -397,6 +399,7 @@
         }
 
     } // if (ID!=0xFE)
+    if(Status[4]!=0) printf("\nError Code : 0x%x\n", Status[4]);
 
     return(Status[4]);
 }
@@ -503,6 +506,7 @@
 
 
     }
+    if(Status[4]!=0) printf("\nError Code : 0x%x\n", Status[4]);
 
     return(Status[4]); // return error code
 }