Diff: MX106.cpp

Revision:

1:555b9b4a1ecd

Parent:

0:8fda718cb527

Child:

2:6722a00e6184

--- a/MX106.cpp	Mon May 23 02:18:19 2016 +0000
+++ b/MX106.cpp	Sun Jun 26 20:46:08 2016 +0000
@@ -1,6 +1,6 @@
 #include "MX106.h"
 #include "mbed.h"
-
+#include "communication_1.h"
 //360°/4095
 #define RESOLUTION 0,088
 //bit per degrees 4095/360°
@@ -8,7 +8,7 @@
 
 #define READ_DEBUG 1
 
-MX106::MX106MX106(int ID, comunication_1 line, float gear_train) {
+MX106::MX106(int ID, communication_1 line, float gear_train) {
     _gear_train = gear_train;
     _line = line;
     _ID = ID;
@@ -76,7 +76,7 @@
 }
 
 
-int MX106::GoalPosition(float degrees); {
+int MX106::GoalPosition(float degrees) {
     
     char data[2];
     
@@ -109,11 +109,32 @@
     // write the packet, return the error code
     return(_line.write(_ID, REG_MOVING_SPEED, 2, data));
 }
+int MX106::SetCRSpeed(float speed) {
+
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+
+    int goal = (0x3ff * abs(speed));
+
+    // Set direction CW if we have a negative speed
+    if (speed < 0) {
+        goal |= (0x1 << 10);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+    
+    // write the packet, return the error code
+    int rVal = write( _ID, 0x20, 2, data);
+
+    return(rVal);
+}
 
 
-float MX106::GetTemp (void) {
+float MX106::GetTemp(void) {
     char data[1];
-    int ErrorCode = _line.read(_ID, REG_TEMP, 1, data);
+    int ErrorCode = _line.read( _ID, REG_TEMP, 1, data);
     float temp = data[0];
     return(temp);
 }