Team DIANA / dynamixel

d

Dependencies:   AX12_final MX106_not_working comunication_1 mbed-dev

Fork of MX106-finaltest by Team DIANA

MX.cpp@12:6ac3cac5677c, 2016-11-24 (annotated)

Committer:
clynamen
Date:
Thu Nov 24 15:29:50 2016 +0000
Revision:
12:6ac3cac5677c
Parent:
11:19e8022f60ea 
dynamixel

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clynamen 10:2acfa1a84c96 1 #include "MX.h"
clynamen 10:2acfa1a84c96 2
clynamen 12:6ac3cac5677c 3 #include <math.h>
clynamen 10:2acfa1a84c96 4
clynamen 11:19e8022f60ea 5 #define MOVING_SPEED_UNIT_RPM 0.114
clynamen 11:19e8022f60ea 6
clynamen 11:19e8022f60ea 7 MX::MX(communication_1& line, int ID, float gear_train)
clynamen 11:19e8022f60ea 8 : _line(line)
clynamen 11:19e8022f60ea 9 , _ID(ID)
clynamen 11:19e8022f60ea 10 , _gear_train(gear_train) {
clynamen 11:19e8022f60ea 11 maxSpeedDegreeS = 360;
clynamen 11:19e8022f60ea 12 limitCWDegrees = 0;
clynamen 11:19e8022f60ea 13 limitCCWDegrees = 0;
clynamen 10:2acfa1a84c96 14 }
clynamen 10:2acfa1a84c96 15
clynamen 10:2acfa1a84c96 16 MX::~MX() {
clynamen 10:2acfa1a84c96 17 }
clynamen 10:2acfa1a84c96 18
clynamen 10:2acfa1a84c96 19 void MX::setID(int newID) {
clynamen 10:2acfa1a84c96 20
clynamen 11:19e8022f60ea 21 char data[1];
clynamen 11:19e8022f60ea 22 data[0] = newID;
clynamen 11:19e8022f60ea 23
clynamen 11:19e8022f60ea 24 if (MX_DEBUG) {
clynamen 11:19e8022f60ea 25 printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
clynamen 11:19e8022f60ea 26 }
clynamen 10:2acfa1a84c96 27
clynamen 11:19e8022f60ea 28 _line.write(_ID, MX_REG_ID, 1, data);
clynamen 11:19e8022f60ea 29 _ID = newID;
clynamen 11:19e8022f60ea 30 }
clynamen 10:2acfa1a84c96 31
clynamen 11:19e8022f60ea 32 int MX::getID() {
clynamen 11:19e8022f60ea 33 return _ID;
clynamen 10:2acfa1a84c96 34 }
clynamen 10:2acfa1a84c96 35
clynamen 10:2acfa1a84c96 36 void MX::setGoalPosition(float degrees) {
clynamen 11:19e8022f60ea 37 setMode(2);
clynamen 11:19e8022f60ea 38 setCRSpeed(maxSpeedDegreeS);
clynamen 10:2acfa1a84c96 39 setGoal(degrees);
clynamen 10:2acfa1a84c96 40 }
clynamen 10:2acfa1a84c96 41
clynamen 10:2acfa1a84c96 42 int MX::setGoal(float degrees, int flags) {
clynamen 11:19e8022f60ea 43 if ((_mode == 0) && ((degrees < 0) || (degrees > 360))) {
clynamen 11:19e8022f60ea 44 if (MX_DEBUG == 1) {
clynamen 11:19e8022f60ea 45 printf("Error, wheel mode ");
clynamen 11:19e8022f60ea 46 }
clynamen 11:19e8022f60ea 47 return (1);
clynamen 10:2acfa1a84c96 48 }
clynamen 11:19e8022f60ea 49
clynamen 11:19e8022f60ea 50 if (_mode == 1) {
clynamen 11:19e8022f60ea 51 if (MX_DEBUG == 1) {
clynamen 11:19e8022f60ea 52 printf("Error, wheel mode ");
clynamen 11:19e8022f60ea 53 }
clynamen 11:19e8022f60ea 54 return (1);
clynamen 10:2acfa1a84c96 55 }
clynamen 11:19e8022f60ea 56
clynamen 11:19e8022f60ea 57 //if ((_mode == 2) && ((degrees < 0) || (degrees > 360))) {
clynamen 11:19e8022f60ea 58 //if (MX_DEBUG == 1) {
clynamen 11:19e8022f60ea 59 //printf("Error, wheel mode ");
clynamen 11:19e8022f60ea 60 //}
clynamen 11:19e8022f60ea 61 //return (1);
clynamen 11:19e8022f60ea 62 //}
clynamen 11:19e8022f60ea 63
clynamen 10:2acfa1a84c96 64 char reg_flag = 0;
clynamen 10:2acfa1a84c96 65 char data[2];
clynamen 10:2acfa1a84c96 66
clynamen 10:2acfa1a84c96 67 // set the flag is only the register bit is set in the flag
clynamen 10:2acfa1a84c96 68 if (flags == 0x2) {
clynamen 10:2acfa1a84c96 69 reg_flag = 1;
clynamen 10:2acfa1a84c96 70 }
clynamen 10:2acfa1a84c96 71
clynamen 11:19e8022f60ea 72 if (degrees > limitCWDegrees && limitCWDegrees != 0) {
clynamen 11:19e8022f60ea 73 degrees = limitCWDegrees;
clynamen 11:19e8022f60ea 74 } else if (degrees < limitCCWDegrees && limitCCWDegrees != 0){
clynamen 11:19e8022f60ea 75 degrees = limitCCWDegrees;
clynamen 11:19e8022f60ea 76 }
clynamen 11:19e8022f60ea 77
clynamen 11:19e8022f60ea 78 uint16_t goal = (int)(degrees * _gear_train / goal_resolution);
clynamen 10:2acfa1a84c96 79 if (MX_DEBUG) {
clynamen 11:19e8022f60ea 80 printf("SetGoal to 0x%x\n", goal);
clynamen 10:2acfa1a84c96 81 }
clynamen 10:2acfa1a84c96 82
clynamen 10:2acfa1a84c96 83 data[0] = goal & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 84 data[1] = goal >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 85
clynamen 10:2acfa1a84c96 86 // write the packet, return the error code
clynamen 10:2acfa1a84c96 87 int rVal = _line.write(_ID, MX_REG_GOAL_POSITION, 2, data, reg_flag);
clynamen 11:19e8022f60ea 88
clynamen 10:2acfa1a84c96 89 if (flags == 1) {
clynamen 10:2acfa1a84c96 90 // block until it comes to a halt
clynamen 11:19e8022f60ea 91 while (isMoving()) {
clynamen 11:19e8022f60ea 92 }
clynamen 10:2acfa1a84c96 93 }
clynamen 10:2acfa1a84c96 94 }
clynamen 10:2acfa1a84c96 95
clynamen 11:19e8022f60ea 96 float MX::getVolts(void) {
clynamen 10:2acfa1a84c96 97 if (MX_DEBUG) {
clynamen 11:19e8022f60ea 98 printf("\nGetVolts(%d)", _ID);
clynamen 10:2acfa1a84c96 99 }
clynamen 10:2acfa1a84c96 100 char data[1];
clynamen 10:2acfa1a84c96 101 int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
clynamen 11:19e8022f60ea 102 float volts = data[0] / 10.0;
clynamen 11:19e8022f60ea 103 return (volts);
clynamen 10:2acfa1a84c96 104 }
clynamen 10:2acfa1a84c96 105
clynamen 11:19e8022f60ea 106 float MX::getCurrent(void) {
clynamen 10:2acfa1a84c96 107 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 108 printf("\nGetCurrent(%d)", _ID);
clynamen 10:2acfa1a84c96 109 }
clynamen 10:2acfa1a84c96 110 char data[1];
clynamen 10:2acfa1a84c96 111 int ErrorCode = _line.read(_ID, MX_REG_CURRENT, 1, data);
clynamen 11:19e8022f60ea 112 float ampere = 4.5 * (data[0] - 2048);
clynamen 11:19e8022f60ea 113 return (ampere);
clynamen 11:19e8022f60ea 114 }
clynamen 11:19e8022f60ea 115
clynamen 11:19e8022f60ea 116 float MX::getPGain() {
clynamen 11:19e8022f60ea 117 char data[1];
clynamen 11:19e8022f60ea 118 _line.read(_ID, MX_REG_PGAIN, 1, data);
clynamen 11:19e8022f60ea 119 return (float) *data / 8;
clynamen 11:19e8022f60ea 120 }
clynamen 11:19e8022f60ea 121
clynamen 11:19e8022f60ea 122 float MX::getIGain() {
clynamen 11:19e8022f60ea 123 char data[1];
clynamen 11:19e8022f60ea 124 _line.read(_ID, MX_REG_IGAIN, 1, data);
clynamen 11:19e8022f60ea 125 return (float) *data * 1000 / 2048;
clynamen 11:19e8022f60ea 126 }
clynamen 11:19e8022f60ea 127
clynamen 11:19e8022f60ea 128 float MX::getDGain() {
clynamen 11:19e8022f60ea 129 char data[1];
clynamen 11:19e8022f60ea 130 _line.read(_ID, MX_REG_DGAIN, 1, data);
clynamen 11:19e8022f60ea 131 return (float) *data * 4 / 1000;
clynamen 10:2acfa1a84c96 132 }
clynamen 10:2acfa1a84c96 133
clynamen 11:19e8022f60ea 134 void MX::setPGain(float gain) {
clynamen 11:19e8022f60ea 135 char data[1];
clynamen 11:19e8022f60ea 136 *data = gain * 8;
clynamen 11:19e8022f60ea 137 _line.write(_ID, MX_REG_PGAIN, 1, data);
clynamen 11:19e8022f60ea 138 }
clynamen 11:19e8022f60ea 139 void MX::setIGain(float gain) {
clynamen 11:19e8022f60ea 140 char data[1];
clynamen 11:19e8022f60ea 141 *data = gain / 1000 * 2048;
clynamen 11:19e8022f60ea 142 _line.write(_ID, MX_REG_IGAIN, 1, data);
clynamen 11:19e8022f60ea 143 }
clynamen 11:19e8022f60ea 144 void MX::setDGain(float gain) {
clynamen 11:19e8022f60ea 145 char data[1];
clynamen 11:19e8022f60ea 146 *data = gain / 4 * 1000;
clynamen 11:19e8022f60ea 147 _line.write(_ID, MX_REG_DGAIN, 1, data);
clynamen 11:19e8022f60ea 148 }
clynamen 10:2acfa1a84c96 149
clynamen 11:19e8022f60ea 150 void MX::setMaxTorque(float maxTorque) {
clynamen 11:19e8022f60ea 151 char data[2];
clynamen 11:19e8022f60ea 152 short torqueValue = maxTorque * 0x3ff;
clynamen 11:19e8022f60ea 153 data[0] = torqueValue & 0xff; // bottom 8 bits
clynamen 11:19e8022f60ea 154 data[1] = torqueValue >> 8; // top 8 bits
clynamen 11:19e8022f60ea 155 // write the packet, return the error code
clynamen 11:19e8022f60ea 156 _line.write(_ID, MX_REG_MAXTORQUE, 2, data);
clynamen 11:19e8022f60ea 157 }
clynamen 11:19e8022f60ea 158
clynamen 11:19e8022f60ea 159 void MX::setMotorEnabled(bool enabled) {
clynamen 11:19e8022f60ea 160 char data[1];
clynamen 11:19e8022f60ea 161 data[0] = enabled; // bottom 8 bits
clynamen 11:19e8022f60ea 162 // write the packet, return the error code
clynamen 11:19e8022f60ea 163 _line.write(_ID, MX_REG_MOTORONOFF, 1, data);
clynamen 11:19e8022f60ea 164 }
clynamen 10:2acfa1a84c96 165
clynamen 10:2acfa1a84c96 166 float MX::getPosition(void) {
clynamen 10:2acfa1a84c96 167 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 168 printf("\nGetPosition(%d)", _ID);
clynamen 10:2acfa1a84c96 169 }
clynamen 10:2acfa1a84c96 170
clynamen 10:2acfa1a84c96 171 char data[2];
clynamen 10:2acfa1a84c96 172
clynamen 10:2acfa1a84c96 173 int ErrorCode = _line.read(_ID, MX_REG_POSITION, 2, data);
clynamen 10:2acfa1a84c96 174 short position = data[0] + (data[1] << 8);
clynamen 11:19e8022f60ea 175 float angle = (float)position * 0.088;
clynamen 10:2acfa1a84c96 176
clynamen 10:2acfa1a84c96 177 return (angle);
clynamen 10:2acfa1a84c96 178 }
clynamen 10:2acfa1a84c96 179
clynamen 10:2acfa1a84c96 180
clynamen 10:2acfa1a84c96 181 bool MX::isMoving() {
clynamen 10:2acfa1a84c96 182 char data[1];
clynamen 11:19e8022f60ea 183 _line.read(_ID, MX_REG_MOVING, 1, data);
clynamen 11:19e8022f60ea 184 return (data[0]);
clynamen 10:2acfa1a84c96 185 }
clynamen 10:2acfa1a84c96 186
clynamen 11:19e8022f60ea 187 void MX::setMode(int mode) {
clynamen 11:19e8022f60ea 188 switch (mode) {
clynamen 11:19e8022f60ea 189 //Wheel Mode
clynamen 11:19e8022f60ea 190 case (0):
clynamen 11:19e8022f60ea 191 setCWLimitUnits(0);
clynamen 11:19e8022f60ea 192 setCCWLimitUnits(0);
clynamen 11:19e8022f60ea 193 //setCRSpeed(0.0);
clynamen 11:19e8022f60ea 194 break;
clynamen 11:19e8022f60ea 195 //Joint Mode
clynamen 11:19e8022f60ea 196 case (1):
clynamen 11:19e8022f60ea 197 setCWLimitUnits(4000);
clynamen 11:19e8022f60ea 198 setCCWLimitUnits(4000);
clynamen 11:19e8022f60ea 199 //setCRSpeed(0.0);
clynamen 11:19e8022f60ea 200 break;
clynamen 11:19e8022f60ea 201 //Multi-turn Mode
clynamen 11:19e8022f60ea 202 case (2):
clynamen 11:19e8022f60ea 203 setCWLimitUnits(4095);
clynamen 11:19e8022f60ea 204 setCCWLimitUnits(4095);
clynamen 11:19e8022f60ea 205 //setCRSpeed(0.0);
clynamen 11:19e8022f60ea 206 break;
clynamen 11:19e8022f60ea 207 //other cases
clynamen 11:19e8022f60ea 208 default:
clynamen 11:19e8022f60ea 209 if (READ_DEBUG) {
clynamen 11:19e8022f60ea 210 printf("Not valid mode");
clynamen 11:19e8022f60ea 211 return;
clynamen 11:19e8022f60ea 212 }
clynamen 11:19e8022f60ea 213 }
clynamen 11:19e8022f60ea 214 _mode = mode;
clynamen 10:2acfa1a84c96 215 }
clynamen 10:2acfa1a84c96 216
clynamen 11:19e8022f60ea 217 void MX::setCWLimitUnits(short limit) {
clynamen 11:19e8022f60ea 218 char data[2];
clynamen 11:19e8022f60ea 219 data[0] = limit & 0xff; // bottom 8 bits
clynamen 11:19e8022f60ea 220 data[1] = limit >> 8; // top 8 bits
clynamen 11:19e8022f60ea 221 // write the packet, return the error code
clynamen 11:19e8022f60ea 222 _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
clynamen 11:19e8022f60ea 223 }
clynamen 11:19e8022f60ea 224 void MX::setCCWLimitUnits(short limit) {
clynamen 11:19e8022f60ea 225 char data[2];
clynamen 11:19e8022f60ea 226 data[0] = limit & 0xff; // bottom 8 bits
clynamen 11:19e8022f60ea 227 data[1] = limit >> 8; // top 8 bits
clynamen 11:19e8022f60ea 228 // write the packet, return the error code
clynamen 11:19e8022f60ea 229 _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
clynamen 10:2acfa1a84c96 230 }
clynamen 10:2acfa1a84c96 231
clynamen 11:19e8022f60ea 232 void MX::setCWLimit(float degrees) {
clynamen 11:19e8022f60ea 233 limitCWDegrees = degrees;
clynamen 11:19e8022f60ea 234 //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
clynamen 11:19e8022f60ea 235 //setCWLimit(limit);
clynamen 11:19e8022f60ea 236 }
clynamen 10:2acfa1a84c96 237
clynamen 11:19e8022f60ea 238 void MX::setCCWLimit(float degrees) {
clynamen 11:19e8022f60ea 239 limitCCWDegrees = degrees;
clynamen 11:19e8022f60ea 240 //short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
clynamen 11:19e8022f60ea 241 //setCCWLimit(limit);
clynamen 10:2acfa1a84c96 242 }
clynamen 10:2acfa1a84c96 243
clynamen 10:2acfa1a84c96 244
clynamen 11:19e8022f60ea 245 void MX::setSpeed(float goalSpeedDegreeS) {
clynamen 11:19e8022f60ea 246 setMode(0);
clynamen 11:19e8022f60ea 247 setCRSpeed(goalSpeedDegreeS);
clynamen 11:19e8022f60ea 248 }
clynamen 10:2acfa1a84c96 249
clynamen 11:19e8022f60ea 250 void MX::setMaxSpeed(float maxSpeedDegreeS) {
clynamen 11:19e8022f60ea 251 this->maxSpeedDegreeS = maxSpeedDegreeS;
clynamen 11:19e8022f60ea 252 }
clynamen 11:19e8022f60ea 253
clynamen 12:6ac3cac5677c 254 int min(int a, int b) {
clynamen 12:6ac3cac5677c 255 if(a < b) return a; else return b;
clynamen 12:6ac3cac5677c 256 }
clynamen 12:6ac3cac5677c 257
clynamen 11:19e8022f60ea 258 void MX::setCRSpeed(float goalSpeedDegreeS) {
clynamen 10:2acfa1a84c96 259 // bit 10 = direction, 0 = CCW, 1=CW
clynamen 10:2acfa1a84c96 260 // bits 9-0 = Speed
clynamen 10:2acfa1a84c96 261 char data[2];
clynamen 10:2acfa1a84c96 262
clynamen 11:19e8022f60ea 263 float rpm = std::abs(goalSpeedDegreeS * _gear_train) / 360 * 60;
clynamen 11:19e8022f60ea 264 int goal = rpm / MOVING_SPEED_UNIT_RPM;
clynamen 12:6ac3cac5677c 265 goal = min(goal, 0x3ff);
clynamen 10:2acfa1a84c96 266
clynamen 10:2acfa1a84c96 267 // Set direction CW if we have a negative speed
clynamen 11:19e8022f60ea 268 if (goalSpeedDegreeS < 0) {
clynamen 10:2acfa1a84c96 269 goal |= (0x1 << 10);
clynamen 10:2acfa1a84c96 270 }
clynamen 10:2acfa1a84c96 271
clynamen 10:2acfa1a84c96 272 data[0] = goal & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 273 data[1] = goal >> 8; // top 8 bits
clynamen 11:19e8022f60ea 274
clynamen 10:2acfa1a84c96 275 // write the packet, return the error code
clynamen 10:2acfa1a84c96 276 _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
clynamen 10:2acfa1a84c96 277 }
clynamen 10:2acfa1a84c96 278
clynamen 11:19e8022f60ea 279 //void MX::setCRSpeed(float speed) {
clynamen 11:19e8022f60ea 280 //// bit 10 = direction, 0 = CCW, 1=CW
clynamen 11:19e8022f60ea 281 //// bits 9-0 = Speed
clynamen 11:19e8022f60ea 282 //char data[2];
clynamen 10:2acfa1a84c96 283
clynamen 11:19e8022f60ea 284 //int goal = (0x3ff * std::abs(speed));
clynamen 10:2acfa1a84c96 285
clynamen 11:19e8022f60ea 286 //// Set direction CW if we have a negative speed
clynamen 11:19e8022f60ea 287 //if (speed < 0) {
clynamen 11:19e8022f60ea 288 //goal |= (0x1 << 10);
clynamen 11:19e8022f60ea 289 //}
clynamen 10:2acfa1a84c96 290
clynamen 11:19e8022f60ea 291 //data[0] = goal & 0xff; // bottom 8 bits
clynamen 11:19e8022f60ea 292 //data[1] = goal >> 8; // top 8 bits
clynamen 11:19e8022f60ea 293
clynamen 11:19e8022f60ea 294 //// write the packet, return the error code
clynamen 11:19e8022f60ea 295 //int rVal = _line.write(_ID, 0x20, 2, data);
clynamen 11:19e8022f60ea 296 //}
clynamen 10:2acfa1a84c96 297
clynamen 10:2acfa1a84c96 298 float MX::getTemp(void) {
clynamen 10:2acfa1a84c96 299 char data[1];
clynamen 11:19e8022f60ea 300 int ErrorCode = _line.read(_ID, MX_REG_TEMP, 1, data);
clynamen 10:2acfa1a84c96 301 float temp = data[0];
clynamen 11:19e8022f60ea 302 return (temp);
clynamen 10:2acfa1a84c96 303 }