MX.cpp

00001 #include "MX.h"
00002 
00003 #include <math.h>
00004 
00005 #define MOVING_SPEED_UNIT_RPM 0.114
00006 
00007 MX::MX(communication_1& line, int ID, float gear_train)
00008     : _line(line)
00009     , _ID(ID)
00010     , _gear_train(gear_train) {
00011       maxSpeedDegreeS = 360;
00012       limitCWDegrees = 0;
00013       limitCCWDegrees = 0;
00014 }
00015 
00016 MX::~MX() {
00017 }
00018 
00019 void MX::setID(int newID) {
00020 
00021     char data[1];
00022     data[0] = newID;
00023 
00024     if (MX_DEBUG) {
00025         printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
00026     }
00027 
00028     _line.write(_ID, MX_REG_ID, 1, data);
00029     _ID = newID;
00030 }
00031 
00032 int MX::getID() {
00033   return _ID;
00034 }
00035 
00036 void MX::setGoalPosition(float degrees) {
00037     setMode(2);
00038     setCRSpeed(maxSpeedDegreeS);
00039     setGoal(degrees);
00040 }
00041 
00042 int MX::setGoal(float degrees, int flags) {
00043     if ((_mode == 0) && ((degrees < 0) || (degrees > 360))) {
00044         if (MX_DEBUG == 1) {
00045             printf("Error, wheel mode ");
00046         }
00047         return (1);
00048     }
00049 
00050     if (_mode == 1) {
00051         if (MX_DEBUG == 1) {
00052             printf("Error, wheel mode ");
00053         }
00054         return (1);
00055     }
00056 
00057     //if ((_mode == 2) && ((degrees < 0) || (degrees > 360))) {
00058         //if (MX_DEBUG == 1) {
00059             //printf("Error, wheel mode ");
00060         //}
00061         //return (1);
00062     //}
00063 
00064     char reg_flag = 0;
00065     char data[2];
00066 
00067     // set the flag is only the register bit is set in the flag
00068     if (flags == 0x2) {
00069         reg_flag = 1;
00070     }
00071 
00072     if (degrees > limitCWDegrees && limitCWDegrees != 0) {
00073       degrees = limitCWDegrees;
00074     } else if (degrees < limitCCWDegrees && limitCCWDegrees != 0){
00075       degrees = limitCCWDegrees;
00076     }
00077 
00078     uint16_t goal = (int)(degrees * _gear_train / goal_resolution); 
00079     if (MX_DEBUG) {
00080         printf("SetGoal to 0x%x\n", goal);
00081     }
00082 
00083     data[0] = goal & 0xff; // bottom 8 bits
00084     data[1] = goal >> 8;   // top 8 bits
00085 
00086     // write the packet, return the error code
00087     int rVal = _line.write(_ID, MX_REG_GOAL_POSITION, 2, data, reg_flag);
00088 
00089     if (flags == 1) {
00090         // block until it comes to a halt
00091         while (isMoving()) {
00092         }
00093     }
00094 }
00095 
00096 float MX::getVolts(void) {
00097     if (MX_DEBUG) {
00098         printf("\nGetVolts(%d)", _ID);
00099     }
00100     char data[1];
00101     int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
00102     float volts = data[0] / 10.0;
00103     return (volts);
00104 }
00105 
00106 float MX::getCurrent(void) {
00107     if (MX_DEBUG) {
00108         printf("\nGetCurrent(%d)", _ID);
00109     }
00110     char data[1];
00111     int ErrorCode = _line.read(_ID, MX_REG_CURRENT, 1, data);
00112     float ampere = 4.5 * (data[0] - 2048);
00113     return (ampere);
00114 }
00115 
00116 float MX::getPGain() {
00117     char data[1];
00118     _line.read(_ID, MX_REG_PGAIN, 1, data);
00119     return (float) *data / 8;
00120 }
00121 
00122 float MX::getIGain() {
00123     char data[1];
00124     _line.read(_ID, MX_REG_IGAIN, 1, data);
00125     return (float) *data * 1000 / 2048;
00126 }
00127 
00128 float MX::getDGain() {
00129     char data[1];
00130     _line.read(_ID, MX_REG_DGAIN, 1, data);
00131     return (float) *data * 4 / 1000;
00132 }
00133 
00134 void MX::setPGain(float gain) {
00135     char data[1];
00136     *data = gain * 8;
00137     _line.write(_ID, MX_REG_PGAIN, 1, data);
00138 }
00139 void MX::setIGain(float gain) {
00140     char data[1];
00141     *data = gain / 1000 * 2048;
00142     _line.write(_ID, MX_REG_IGAIN, 1, data);
00143 }
00144 void MX::setDGain(float gain) {
00145     char data[1];
00146     *data = gain / 4 * 1000;
00147     _line.write(_ID, MX_REG_DGAIN, 1, data);
00148 }
00149 
00150 void MX::setMaxTorque(float maxTorque) {
00151     char data[2];
00152     short torqueValue = maxTorque * 0x3ff; 
00153     data[0] = torqueValue & 0xff; // bottom 8 bits
00154     data[1] = torqueValue >> 8;   // top 8 bits
00155     // write the packet, return the error code
00156     _line.write(_ID, MX_REG_MAXTORQUE, 2, data);
00157 }
00158 
00159 void MX::setMotorEnabled(bool enabled) {
00160     char data[1];
00161     data[0] = enabled; // bottom 8 bits
00162     // write the packet, return the error code
00163     _line.write(_ID, MX_REG_MOTORONOFF, 1, data);
00164 }
00165 
00166 float MX::getPosition(void) {
00167     if (MX_DEBUG) {
00168         printf("\nGetPosition(%d)", _ID);
00169     }
00170 
00171     char data[2];
00172 
00173     int ErrorCode = _line.read(_ID, MX_REG_POSITION, 2, data);
00174     short position = data[0] + (data[1] << 8);
00175     float angle = (float)position * 0.088;
00176 
00177     return (angle);
00178 }
00179 
00180 
00181 bool MX::isMoving() {
00182     char data[1];
00183     _line.read(_ID, MX_REG_MOVING, 1, data);
00184     return (data[0]);
00185 }
00186 
00187 void MX::setMode(int mode) {
00188     switch (mode) {
00189         //Wheel Mode
00190         case (0):
00191             setCWLimitUnits(0);
00192             setCCWLimitUnits(0);
00193             //setCRSpeed(0.0);
00194             break;
00195         //Joint Mode
00196         case (1):
00197             setCWLimitUnits(4000);
00198             setCCWLimitUnits(4000);
00199             //setCRSpeed(0.0);
00200             break;
00201         //Multi-turn Mode
00202         case (2):
00203             setCWLimitUnits(4095);
00204             setCCWLimitUnits(4095);
00205             //setCRSpeed(0.0);
00206             break;
00207         //other cases
00208         default:
00209             if (READ_DEBUG) {
00210                 printf("Not valid mode");
00211                 return;
00212             }
00213     }
00214     _mode = mode;
00215 }
00216 
00217 void MX::setCWLimitUnits(short limit) {
00218     char data[2];
00219     data[0] = limit & 0xff; // bottom 8 bits
00220     data[1] = limit >> 8;   // top 8 bits
00221     // write the packet, return the error code
00222     _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
00223 }
00224 void MX::setCCWLimitUnits(short limit) {
00225     char data[2];
00226     data[0] = limit & 0xff; // bottom 8 bits
00227     data[1] = limit >> 8;   // top 8 bits
00228     // write the packet, return the error code
00229     _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
00230 }
00231 
00232 void MX::setCWLimit(float degrees) {
00233     limitCWDegrees = degrees;
00234     //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
00235     //setCWLimit(limit);
00236 }
00237 
00238 void MX::setCCWLimit(float degrees) {
00239     limitCCWDegrees = degrees;
00240     //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
00241     //setCCWLimit(limit);
00242 }
00243 
00244 
00245 void MX::setSpeed(float goalSpeedDegreeS) {
00246   setMode(0);
00247   setCRSpeed(goalSpeedDegreeS);
00248 }
00249 
00250 void MX::setMaxSpeed(float maxSpeedDegreeS) {
00251   this->maxSpeedDegreeS = maxSpeedDegreeS;
00252 }
00253 
00254 int min(int a, int b) {
00255     if(a < b) return a; else return b;
00256 }
00257 
00258 void MX::setCRSpeed(float goalSpeedDegreeS) {
00259     // bit 10     = direction, 0 = CCW, 1=CW
00260     // bits 9-0   = Speed
00261     char data[2];
00262 
00263     float rpm = std::abs(goalSpeedDegreeS * _gear_train) / 360 * 60;
00264     int goal = rpm / MOVING_SPEED_UNIT_RPM;
00265     goal = min(goal, 0x3ff);
00266 
00267     // Set direction CW if we have a negative speed
00268     if (goalSpeedDegreeS < 0) {
00269         goal |= (0x1 << 10);
00270     }
00271 
00272     data[0] = goal & 0xff; // bottom 8 bits
00273     data[1] = goal >> 8;   // top 8 bits
00274 
00275     // write the packet, return the error code
00276     _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
00277 }
00278 
00279 //void MX::setCRSpeed(float speed) {
00280     //// bit 10     = direction, 0 = CCW, 1=CW
00281     //// bits 9-0   = Speed
00282     //char data[2];
00283 
00284     //int goal = (0x3ff * std::abs(speed));
00285 
00286     //// Set direction CW if we have a negative speed
00287     //if (speed < 0) {
00288         //goal |= (0x1 << 10);
00289     //}
00290 
00291     //data[0] = goal & 0xff; // bottom 8 bits
00292     //data[1] = goal >> 8;   // top 8 bits
00293 
00294     //// write the packet, return the error code
00295     //int rVal = _line.write(_ID, 0x20, 2, data);
00296 //}
00297 
00298 float MX::getTemp(void) {
00299     char data[1];
00300     int ErrorCode = _line.read(_ID, MX_REG_TEMP, 1, data);
00301     float temp = data[0];
00302     return (temp);
00303 }