Diff: MX106.cpp

Revision:

2:6722a00e6184

Parent:

1:555b9b4a1ecd

Child:

3:adeaefc32a41

--- a/MX106.cpp	Sun Jun 26 20:46:08 2016 +0000
+++ b/MX106.cpp	Sun Jun 26 21:46:29 2016 +0000
@@ -2,15 +2,14 @@
 #include "mbed.h"
 #include "communication_1.h"
 //360°/4095
-#define RESOLUTION 0,088
+#define RESOLUTION 0.088
 //bit per degrees 4095/360°
-#define BIT_DEG 11,375
+#define BIT_DEG 11.375
 
 #define READ_DEBUG 1
 
-MX106::MX106(int ID, communication_1 line, float gear_train) {
+MX106::MX106(int ID, communication_1& line, float gear_train) : _line(line) {
     _gear_train = gear_train;
-    _line = line;
     _ID = ID;
 }
 
@@ -61,21 +60,21 @@
     return(_line.write(_ID, REG_CW_LIMIT, 2, data));
 }
 
+int MX106::SetCCWLimit (float degrees) {
 
-int MX106::SetCWLimit(float degrees) {
-     
     char data[2];
     
+    //TODO: review this
     short limit = (short)(BIT_DEG * degrees * _gear_train);
 
+
     data[0] = limit & 0xff; // bottom 8 bits
     data[1] = limit >> 8;   // top 8 bits
 
     // write the packet, return the error code
-    return(_line.write(_ID, REG_CCW_LIMIT, 2, data));
+    return (_line.write(_ID, REG_CCW_LIMIT, 2, data));
 }
 
-
 int MX106::GoalPosition(float degrees) {
     
     char data[2];
@@ -96,10 +95,10 @@
     // bits 9-0   = Speed
     char data[2];
 
-    int goal = (0x3ff * abs(speed * _gear_train));
+    int goal = (0x3ff * abs(goal_speed * _gear_train));
 
     // Set direction CW if we have a negative speed
-    if (speed < 0) {
+    if (goal_speed < 0) {
         goal |= (0x1 << 10);
     }
 
@@ -126,7 +125,7 @@
     data[1] = goal >> 8;   // top 8 bits
     
     // write the packet, return the error code
-    int rVal = write( _ID, 0x20, 2, data);
+    int rVal = _line.write( _ID, 0x20, 2, data);
 
     return(rVal);
 }