d
Dependencies: AX12_final MX106_not_working comunication_1 mbed-dev
Fork of MX106-finaltest by
Diff: MX.cpp
- Revision:
- 10:2acfa1a84c96
- Child:
- 11:19e8022f60ea
diff -r 34f4eb572c47 -r 2acfa1a84c96 MX.cpp --- /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); +}