Team DIANA / dynamixel

d

Dependencies:   AX12_final MX106_not_working comunication_1 mbed-dev

Fork of MX106-finaltest by Team DIANA

MX.cpp@10:2acfa1a84c96, 2016-07-02 (annotated)

Committer:
clynamen
Date:
Sat Jul 02 18:06:59 2016 +0000
Revision:
10:2acfa1a84c96
Child:
11:19e8022f60ea
use MX base class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clynamen 10:2acfa1a84c96 1 #include "MX.h"
clynamen 10:2acfa1a84c96 2
clynamen 10:2acfa1a84c96 3 #include <cmath>
clynamen 10:2acfa1a84c96 4
clynamen 10:2acfa1a84c96 5 MX::MX(communication_1& line, int ID, float gear_train) :
clynamen 10:2acfa1a84c96 6 _line(line),
clynamen 10:2acfa1a84c96 7 _ID(ID),
clynamen 10:2acfa1a84c96 8 _gear_train(gear_train) {
clynamen 10:2acfa1a84c96 9
clynamen 10:2acfa1a84c96 10 }
clynamen 10:2acfa1a84c96 11
clynamen 10:2acfa1a84c96 12 MX::~MX() {
clynamen 10:2acfa1a84c96 13 }
clynamen 10:2acfa1a84c96 14
clynamen 10:2acfa1a84c96 15 void MX::setID(int newID) {
clynamen 10:2acfa1a84c96 16
clynamen 10:2acfa1a84c96 17 char data[1];
clynamen 10:2acfa1a84c96 18 data[0] = newID;
clynamen 10:2acfa1a84c96 19
clynamen 10:2acfa1a84c96 20 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 21 printf("Setting ID from 0x%x to 0x%x\n", _ID, newID);
clynamen 10:2acfa1a84c96 22 }
clynamen 10:2acfa1a84c96 23
clynamen 10:2acfa1a84c96 24 _line.write(_ID, MX_REG_ID, 1, data);
clynamen 10:2acfa1a84c96 25 _ID = newID;
clynamen 10:2acfa1a84c96 26 }
clynamen 10:2acfa1a84c96 27
clynamen 10:2acfa1a84c96 28 void MX::setGoalPosition(float degrees) {
clynamen 10:2acfa1a84c96 29 setGoal(degrees);
clynamen 10:2acfa1a84c96 30 }
clynamen 10:2acfa1a84c96 31
clynamen 10:2acfa1a84c96 32 int MX::setGoal(float degrees, int flags) {
clynamen 10:2acfa1a84c96 33 if((_mode==0)&&((degrees<0)||(degrees>360)))
clynamen 10:2acfa1a84c96 34 {
clynamen 10:2acfa1a84c96 35 if(MX_DEBUG==1)
clynamen 10:2acfa1a84c96 36 {
clynamen 10:2acfa1a84c96 37 printf("Error, wheel mode ");
clynamen 10:2acfa1a84c96 38 }
clynamen 10:2acfa1a84c96 39 return(1);
clynamen 10:2acfa1a84c96 40
clynamen 10:2acfa1a84c96 41 }
clynamen 10:2acfa1a84c96 42 if (_mode == 1)
clynamen 10:2acfa1a84c96 43 {
clynamen 10:2acfa1a84c96 44 if(MX_DEBUG==1)
clynamen 10:2acfa1a84c96 45 {
clynamen 10:2acfa1a84c96 46 printf("Error, wheel mode ");
clynamen 10:2acfa1a84c96 47 }
clynamen 10:2acfa1a84c96 48 return(1);
clynamen 10:2acfa1a84c96 49
clynamen 10:2acfa1a84c96 50 }
clynamen 10:2acfa1a84c96 51 if((_mode==2)&&((degrees<0)||(degrees>360)))
clynamen 10:2acfa1a84c96 52 {
clynamen 10:2acfa1a84c96 53 if(MX_DEBUG==1)
clynamen 10:2acfa1a84c96 54 {
clynamen 10:2acfa1a84c96 55 printf("Error, wheel mode ");
clynamen 10:2acfa1a84c96 56 }
clynamen 10:2acfa1a84c96 57 return(1);
clynamen 10:2acfa1a84c96 58
clynamen 10:2acfa1a84c96 59 }
clynamen 10:2acfa1a84c96 60
clynamen 10:2acfa1a84c96 61 char reg_flag = 0;
clynamen 10:2acfa1a84c96 62 char data[2];
clynamen 10:2acfa1a84c96 63
clynamen 10:2acfa1a84c96 64 // set the flag is only the register bit is set in the flag
clynamen 10:2acfa1a84c96 65 if (flags == 0x2) {
clynamen 10:2acfa1a84c96 66 reg_flag = 1;
clynamen 10:2acfa1a84c96 67 }
clynamen 10:2acfa1a84c96 68
clynamen 10:2acfa1a84c96 69
clynamen 10:2acfa1a84c96 70 short goal = (int)(degrees/goal_resolution ); /// chiedere se può andar bene short int****
clynamen 10:2acfa1a84c96 71 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 72 printf("SetGoal to 0x%x\n",goal);
clynamen 10:2acfa1a84c96 73 }
clynamen 10:2acfa1a84c96 74
clynamen 10:2acfa1a84c96 75 data[0] = goal & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 76 data[1] = goal >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 77
clynamen 10:2acfa1a84c96 78 // write the packet, return the error code
clynamen 10:2acfa1a84c96 79 int rVal = _line.write(_ID, MX_REG_GOAL_POSITION, 2, data, reg_flag);
clynamen 10:2acfa1a84c96 80
clynamen 10:2acfa1a84c96 81 if (flags == 1) {
clynamen 10:2acfa1a84c96 82 // block until it comes to a halt
clynamen 10:2acfa1a84c96 83 while (isMoving()) {}
clynamen 10:2acfa1a84c96 84 }
clynamen 10:2acfa1a84c96 85 }
clynamen 10:2acfa1a84c96 86
clynamen 10:2acfa1a84c96 87 float MX::getVolts (void) {
clynamen 10:2acfa1a84c96 88 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 89 printf("\nGetVolts(%d)",_ID);
clynamen 10:2acfa1a84c96 90 }
clynamen 10:2acfa1a84c96 91 char data[1];
clynamen 10:2acfa1a84c96 92 int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
clynamen 10:2acfa1a84c96 93 float volts = data[0]/10.0;
clynamen 10:2acfa1a84c96 94 return(volts);
clynamen 10:2acfa1a84c96 95 }
clynamen 10:2acfa1a84c96 96
clynamen 10:2acfa1a84c96 97 float MX::getCurrent(void)
clynamen 10:2acfa1a84c96 98 {
clynamen 10:2acfa1a84c96 99 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 100 printf("\nGetCurrent(%d)", _ID);
clynamen 10:2acfa1a84c96 101 }
clynamen 10:2acfa1a84c96 102 char data[1];
clynamen 10:2acfa1a84c96 103 int ErrorCode = _line.read(_ID, MX_REG_CURRENT, 1, data);
clynamen 10:2acfa1a84c96 104 float ampere = 4.5 * (data[0] - 2048 ) ;
clynamen 10:2acfa1a84c96 105 return(ampere);
clynamen 10:2acfa1a84c96 106
clynamen 10:2acfa1a84c96 107 }
clynamen 10:2acfa1a84c96 108
clynamen 10:2acfa1a84c96 109
clynamen 10:2acfa1a84c96 110
clynamen 10:2acfa1a84c96 111 float MX::getPGain() { return 0; }
clynamen 10:2acfa1a84c96 112 float MX::getIGain() { return 0; }
clynamen 10:2acfa1a84c96 113 float MX::getDGain() { return 0; }
clynamen 10:2acfa1a84c96 114 void MX::setMaxTorque(float torque) { }
clynamen 10:2acfa1a84c96 115 void MX::setMotorEnabled(bool enabled) { }
clynamen 10:2acfa1a84c96 116
clynamen 10:2acfa1a84c96 117 float MX::getPosition(void) {
clynamen 10:2acfa1a84c96 118 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 119 printf("\nGetPosition(%d)", _ID);
clynamen 10:2acfa1a84c96 120 }
clynamen 10:2acfa1a84c96 121
clynamen 10:2acfa1a84c96 122 char data[2];
clynamen 10:2acfa1a84c96 123
clynamen 10:2acfa1a84c96 124 int ErrorCode = _line.read(_ID, MX_REG_POSITION, 2, data);
clynamen 10:2acfa1a84c96 125 short position = data[0] + (data[1] << 8);
clynamen 10:2acfa1a84c96 126 float angle = (position * 300)/4093;
clynamen 10:2acfa1a84c96 127
clynamen 10:2acfa1a84c96 128 return (angle);
clynamen 10:2acfa1a84c96 129 }
clynamen 10:2acfa1a84c96 130
clynamen 10:2acfa1a84c96 131
clynamen 10:2acfa1a84c96 132 bool MX::isMoving() {
clynamen 10:2acfa1a84c96 133 char data[1];
clynamen 10:2acfa1a84c96 134 _line.read(_ID,MX_REG_MOVING,1,data);
clynamen 10:2acfa1a84c96 135 return(data[0]);
clynamen 10:2acfa1a84c96 136 }
clynamen 10:2acfa1a84c96 137
clynamen 10:2acfa1a84c96 138 void MX::setMode(int mode){
clynamen 10:2acfa1a84c96 139 switch (mode){
clynamen 10:2acfa1a84c96 140 //Wheel Mode
clynamen 10:2acfa1a84c96 141 case (0):
clynamen 10:2acfa1a84c96 142 setCWLimit(0);
clynamen 10:2acfa1a84c96 143 setCCWLimit(0);
clynamen 10:2acfa1a84c96 144 setCRSpeed(0.0);
clynamen 10:2acfa1a84c96 145 break;
clynamen 10:2acfa1a84c96 146 //Joint Mode
clynamen 10:2acfa1a84c96 147 case (1):
clynamen 10:2acfa1a84c96 148 setCWLimit(MX_RESOLUTION);
clynamen 10:2acfa1a84c96 149 setCCWLimit(MX_RESOLUTION);
clynamen 10:2acfa1a84c96 150 setCRSpeed(0.0);
clynamen 10:2acfa1a84c96 151 break;
clynamen 10:2acfa1a84c96 152 //Multi-turn Mode
clynamen 10:2acfa1a84c96 153 case (2):
clynamen 10:2acfa1a84c96 154 setCWLimit(360);
clynamen 10:2acfa1a84c96 155 setCCWLimit(360);
clynamen 10:2acfa1a84c96 156 setCRSpeed(0.0);
clynamen 10:2acfa1a84c96 157 break;
clynamen 10:2acfa1a84c96 158 //other cases
clynamen 10:2acfa1a84c96 159 default:
clynamen 10:2acfa1a84c96 160 if(READ_DEBUG){
clynamen 10:2acfa1a84c96 161 printf("Not valid mode");
clynamen 10:2acfa1a84c96 162 return;
clynamen 10:2acfa1a84c96 163 }
clynamen 10:2acfa1a84c96 164 }
clynamen 10:2acfa1a84c96 165 _mode = mode;
clynamen 10:2acfa1a84c96 166 }
clynamen 10:2acfa1a84c96 167
clynamen 10:2acfa1a84c96 168 void MX::setCWLimit(float degrees){
clynamen 10:2acfa1a84c96 169 char data[2];
clynamen 10:2acfa1a84c96 170
clynamen 10:2acfa1a84c96 171 short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
clynamen 10:2acfa1a84c96 172
clynamen 10:2acfa1a84c96 173 data[0] = limit & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 174 data[1] = limit >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 175
clynamen 10:2acfa1a84c96 176 // write the packet, return the error code
clynamen 10:2acfa1a84c96 177 _line.write(_ID, MX_REG_CW_LIMIT, 2, data);
clynamen 10:2acfa1a84c96 178 }
clynamen 10:2acfa1a84c96 179
clynamen 10:2acfa1a84c96 180 void MX::setCCWLimit(float degrees){
clynamen 10:2acfa1a84c96 181 char data[2];
clynamen 10:2acfa1a84c96 182
clynamen 10:2acfa1a84c96 183 // 1023 / 300 * degrees
clynamen 10:2acfa1a84c96 184 short limit = (4093 * degrees) / 300;
clynamen 10:2acfa1a84c96 185
clynamen 10:2acfa1a84c96 186 if (MX_DEBUG) {
clynamen 10:2acfa1a84c96 187 printf("SetCCWLimit to 0x%x\n",limit);
clynamen 10:2acfa1a84c96 188 }
clynamen 10:2acfa1a84c96 189
clynamen 10:2acfa1a84c96 190 data[0] = limit & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 191 data[1] = limit >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 192
clynamen 10:2acfa1a84c96 193 // write the packet, return the error code
clynamen 10:2acfa1a84c96 194 _line.write(_ID, MX_REG_CCW_LIMIT, 2, data);
clynamen 10:2acfa1a84c96 195 }
clynamen 10:2acfa1a84c96 196
clynamen 10:2acfa1a84c96 197
clynamen 10:2acfa1a84c96 198 void MX::setSpeed(float goal_speed) {
clynamen 10:2acfa1a84c96 199
clynamen 10:2acfa1a84c96 200 // bit 10 = direction, 0 = CCW, 1=CW
clynamen 10:2acfa1a84c96 201 // bits 9-0 = Speed
clynamen 10:2acfa1a84c96 202 char data[2];
clynamen 10:2acfa1a84c96 203
clynamen 10:2acfa1a84c96 204 int goal = (0x3ff * std::abs(goal_speed * _gear_train));
clynamen 10:2acfa1a84c96 205
clynamen 10:2acfa1a84c96 206 // Set direction CW if we have a negative speed
clynamen 10:2acfa1a84c96 207 if (goal_speed < 0) {
clynamen 10:2acfa1a84c96 208 goal |= (0x1 << 10);
clynamen 10:2acfa1a84c96 209 }
clynamen 10:2acfa1a84c96 210
clynamen 10:2acfa1a84c96 211 data[0] = goal & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 212 data[1] = goal >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 213
clynamen 10:2acfa1a84c96 214 // write the packet, return the error code
clynamen 10:2acfa1a84c96 215 _line.write(_ID, MX_REG_MOVING_SPEED, 2, data);
clynamen 10:2acfa1a84c96 216 }
clynamen 10:2acfa1a84c96 217
clynamen 10:2acfa1a84c96 218 void MX::setCRSpeed(float speed) {
clynamen 10:2acfa1a84c96 219 // bit 10 = direction, 0 = CCW, 1=CW
clynamen 10:2acfa1a84c96 220 // bits 9-0 = Speed
clynamen 10:2acfa1a84c96 221 char data[2];
clynamen 10:2acfa1a84c96 222
clynamen 10:2acfa1a84c96 223 int goal = (0x3ff * std::abs(speed));
clynamen 10:2acfa1a84c96 224
clynamen 10:2acfa1a84c96 225 // Set direction CW if we have a negative speed
clynamen 10:2acfa1a84c96 226 if (speed < 0) {
clynamen 10:2acfa1a84c96 227 goal |= (0x1 << 10);
clynamen 10:2acfa1a84c96 228 }
clynamen 10:2acfa1a84c96 229
clynamen 10:2acfa1a84c96 230 data[0] = goal & 0xff; // bottom 8 bits
clynamen 10:2acfa1a84c96 231 data[1] = goal >> 8; // top 8 bits
clynamen 10:2acfa1a84c96 232
clynamen 10:2acfa1a84c96 233 // write the packet, return the error code
clynamen 10:2acfa1a84c96 234 int rVal = _line.write( _ID, 0x20, 2, data);
clynamen 10:2acfa1a84c96 235 }
clynamen 10:2acfa1a84c96 236
clynamen 10:2acfa1a84c96 237 float MX::getTemp(void) {
clynamen 10:2acfa1a84c96 238 char data[1];
clynamen 10:2acfa1a84c96 239 int ErrorCode = _line.read( _ID, MX_REG_TEMP, 1, data);
clynamen 10:2acfa1a84c96 240 float temp = data[0];
clynamen 10:2acfa1a84c96 241 return(temp);
clynamen 10:2acfa1a84c96 242 }