File content as of revision 10:2acfa1a84c96:

#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);
}