Diff: MX.cpp

Revision:

10:2acfa1a84c96

Child:

11:19e8022f60ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MX.cpp	Sat Jul 02 18:06:59 2016 +0000
@@ -0,0 +1,242 @@
+#include "MX.h"
+
+#include <cmath>
+
+MX::MX(communication_1& line, int ID, float gear_train) : 
+  _line(line), 
+  _ID(ID),
+  _gear_train(gear_train) {
+  
+}
+
+MX::~MX() {
+}
+
+void MX::setID(int newID) {
+
+  char data[1];
+  data[0] = newID;
+
+  if (MX_DEBUG) {
+    printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
+  }
+
+  _line.write(_ID, MX_REG_ID, 1, data);
+  _ID = newID;
+}
+
+void MX::setGoalPosition(float degrees) {
+    setGoal(degrees);
+}
+
+int MX::setGoal(float degrees, int flags) {
+if((_mode==0)&&((degrees<0)||(degrees>360))) 
+    {
+    if(MX_DEBUG==1) 
+    {  
+    printf("Error, wheel mode ");
+    }
+    return(1);
+        
+    }
+if (_mode == 1) 
+    { 
+    if(MX_DEBUG==1) 
+    {  
+    printf("Error, wheel mode ");
+    }
+    return(1);
+        
+    }
+    if((_mode==2)&&((degrees<0)||(degrees>360)))
+    {
+       if(MX_DEBUG==1) 
+    {  
+    printf("Error, wheel mode ");
+    }
+    return(1);
+       
+    }  
+    
+    char reg_flag = 0;
+    char data[2];
+
+    // set the flag is only the register bit is set in the flag
+    if (flags == 0x2) {
+        reg_flag = 1;
+    }
+
+    
+    short  goal = (int)(degrees/goal_resolution );     /// chiedere se può andar bene short int****
+    if (MX_DEBUG) {
+        printf("SetGoal to 0x%x\n",goal);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = _line.write(_ID, MX_REG_GOAL_POSITION, 2, data, reg_flag);
+    
+    if (flags == 1) {
+        // block until it comes to a halt
+        while (isMoving()) {}
+    }
+}
+
+float MX::getVolts (void) {
+    if (MX_DEBUG) {
+        printf("\nGetVolts(%d)",_ID);
+    }
+    char data[1];
+    int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
+    float volts = data[0]/10.0;
+    return(volts);
+}
+
+float MX::getCurrent(void)
+{
+    if (MX_DEBUG) {
+        printf("\nGetCurrent(%d)", _ID);
+    }
+    char data[1];
+    int ErrorCode = _line.read(_ID, MX_REG_CURRENT, 1, data);
+    float ampere  = 4.5 * (data[0] - 2048 )  ;
+    return(ampere);
+    
+}
+
+
+
+float MX::getPGain() { return 0; }
+float MX::getIGain() { return 0; }
+float MX::getDGain() { return 0; }
+void MX::setMaxTorque(float torque) { } 
+void MX::setMotorEnabled(bool enabled) { } 
+
+float MX::getPosition(void) {
+    if (MX_DEBUG) {
+        printf("\nGetPosition(%d)", _ID);
+    }
+
+    char data[2];
+
+    int ErrorCode = _line.read(_ID, MX_REG_POSITION, 2, data);
+    short position = data[0] + (data[1] << 8);
+    float angle = (position * 300)/4093;
+
+    return (angle);
+}
+
+
+bool MX::isMoving() {
+    char data[1];
+    _line.read(_ID,MX_REG_MOVING,1,data);
+    return(data[0]);
+}
+
+void MX::setMode(int mode){
+  switch (mode){
+    //Wheel Mode
+    case (0): 
+      setCWLimit(0);
+      setCCWLimit(0);
+      setCRSpeed(0.0);
+      break;
+      //Joint Mode
+    case (1): 
+      setCWLimit(MX_RESOLUTION);
+      setCCWLimit(MX_RESOLUTION);
+      setCRSpeed(0.0);
+      break;
+      //Multi-turn Mode 
+    case (2): 
+      setCWLimit(360);
+      setCCWLimit(360);
+      setCRSpeed(0.0);
+      break;
+      //other cases
+    default: 
+      if(READ_DEBUG){
+        printf("Not valid mode"); 
+        return;
+      }   
+  }
+  _mode = mode;
+}
+
+void MX::setCWLimit(float degrees){
+  char data[2];
+
+  short limit = (short)(MX_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
+  _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
+}
+
+void MX::setCCWLimit(float degrees){
+  char data[2];
+
+  // 1023 / 300 * degrees
+  short limit = (4093 * degrees) / 300;
+
+  if (MX_DEBUG) {
+    printf("SetCCWLimit to 0x%x\n",limit);
+  }
+
+  data[0] = limit & 0xff; // bottom 8 bits
+  data[1] = limit >> 8;   // top 8 bits
+
+  // write the packet, return the error code
+  _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
+}
+
+
+void MX::setSpeed(float goal_speed) {
+
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+
+    int goal = (0x3ff * std::abs(goal_speed * _gear_train));
+
+    // Set direction CW if we have a negative speed
+    if (goal_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
+    _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
+}
+
+void MX::setCRSpeed(float speed) {
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+
+    int goal = (0x3ff * std::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 = _line.write( _ID, 0x20, 2, data);
+}
+
+float MX::getTemp(void) {
+    char data[1];
+    int ErrorCode = _line.read( _ID, MX_REG_TEMP, 1, data);
+    float temp = data[0];
+    return(temp);
+}