d
Dependencies: AX12_final MX106_not_working comunication_1 mbed-dev
Fork of MX106-finaltest by
MX.cpp
- Committer:
- clynamen
- Date:
- 2016-07-02
- Revision:
- 10:2acfa1a84c96
- Child:
- 11:19e8022f60ea
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); }