PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.cpp@17:6f85605f793d, 2019-07-26 (annotated)

Committer:
marcodesilva
Date:
Fri Jul 26 12:40:22 2019 +0000
Revision:
17:6f85605f793d
Child:
18:862a4afc1fbe
Child:
19:16d5f8a3311f
Last Version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 17:6f85605f793d 1
marcodesilva 17:6f85605f793d 2 #include "MX.h"
marcodesilva 17:6f85605f793d 3
marcodesilva 17:6f85605f793d 4 MX::MX(){
marcodesilva 17:6f85605f793d 5 }
marcodesilva 17:6f85605f793d 6
marcodesilva 17:6f85605f793d 7 MX::MX(int* broadcastID, int Nmotor, int MotorBaud, UARTSerial_half* pCommLayer){
marcodesilva 17:6f85605f793d 8 _MotorBaud = MotorBaud;
marcodesilva 17:6f85605f793d 9 _pCommLayer = pCommLayer;
marcodesilva 17:6f85605f793d 10 _broadcastID = broadcastID ;
marcodesilva 17:6f85605f793d 11 _Nmotor = Nmotor;
marcodesilva 17:6f85605f793d 12 }
marcodesilva 17:6f85605f793d 13
marcodesilva 17:6f85605f793d 14
marcodesilva 17:6f85605f793d 15 int MX::SyncTorqueEnable(bool enableVal[], int ID) {
marcodesilva 17:6f85605f793d 16
marcodesilva 17:6f85605f793d 17 int Nmotor = _Nmotor;
marcodesilva 17:6f85605f793d 18 char data[1*Nmotor];
marcodesilva 17:6f85605f793d 19
marcodesilva 17:6f85605f793d 20 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 21 data[i] = enableVal[i];
marcodesilva 17:6f85605f793d 22 }
marcodesilva 17:6f85605f793d 23
marcodesilva 17:6f85605f793d 24 #ifdef SYNC_TORQUE_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 25 printf("\n SYNC TORQUE ENABLE\n");
marcodesilva 17:6f85605f793d 26 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 27 printf("\n Torque Enable value for ID[%d] = %d",_broadcastID[i], enableVal[i]);
marcodesilva 17:6f85605f793d 28 }
marcodesilva 17:6f85605f793d 29 #endif
marcodesilva 17:6f85605f793d 30
marcodesilva 17:6f85605f793d 31 int rVal = SyncSendPacket(MX_REG_TORQUE_ENABLE, 1, data, ID);
marcodesilva 17:6f85605f793d 32
marcodesilva 17:6f85605f793d 33 return(rVal);
marcodesilva 17:6f85605f793d 34 }
marcodesilva 17:6f85605f793d 35
marcodesilva 17:6f85605f793d 36
marcodesilva 17:6f85605f793d 37 int MX::SyncSetBaud(int MotorBaud[], int ID) {
marcodesilva 17:6f85605f793d 38
marcodesilva 17:6f85605f793d 39 int Nmotor = _Nmotor;
marcodesilva 17:6f85605f793d 40 char data[1*Nmotor];
marcodesilva 17:6f85605f793d 41
marcodesilva 17:6f85605f793d 42 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 43 data[i] = MotorBaud[i];
marcodesilva 17:6f85605f793d 44 }
marcodesilva 17:6f85605f793d 45
marcodesilva 17:6f85605f793d 46 #ifdef SYNC_SET_BAUD_DEBUG
marcodesilva 17:6f85605f793d 47 printf("\n SYNC_SET_BAUD_DEBUG\n");
marcodesilva 17:6f85605f793d 48 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 49 printf("Set Baud rate value to: 0x%02x\n",MotorBaud[i]);
marcodesilva 17:6f85605f793d 50 }
marcodesilva 17:6f85605f793d 51 printf( "* 0x00 = 9,600 bps \n"
marcodesilva 17:6f85605f793d 52 "* 0x01 = 57,600 bps (DEFAULT)\n"
marcodesilva 17:6f85605f793d 53 "* 0x02 = 115,200 bps \n"
marcodesilva 17:6f85605f793d 54 "* 0x03 = 1 Mbps \n"
marcodesilva 17:6f85605f793d 55 "* 0x04 = 2 Mbps \n"
marcodesilva 17:6f85605f793d 56 "* 0x05 = 3 Mbps \n"
marcodesilva 17:6f85605f793d 57 "* 0x06 = 4 Mbps \n"
marcodesilva 17:6f85605f793d 58 "* 0x06 = 4,5 Mbps \n");
marcodesilva 17:6f85605f793d 59 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 60 printf("\n Operating mode value for ID[%d] = %d",_broadcastID[i], MotorBaud[i]);
marcodesilva 17:6f85605f793d 61 }
marcodesilva 17:6f85605f793d 62 #endif
marcodesilva 17:6f85605f793d 63
marcodesilva 17:6f85605f793d 64 int rVal = SyncSendPacket(MX_REG_OPERATING_MODE, 1, data, ID);
marcodesilva 17:6f85605f793d 65
marcodesilva 17:6f85605f793d 66 return(rVal);
marcodesilva 17:6f85605f793d 67 }
marcodesilva 17:6f85605f793d 68
marcodesilva 17:6f85605f793d 69
marcodesilva 17:6f85605f793d 70
marcodesilva 17:6f85605f793d 71 int MX::SyncSetGoal(float degrees, int ID) {
marcodesilva 17:6f85605f793d 72
marcodesilva 17:6f85605f793d 73 float goal = (degrees) * (float)(4095/360);
marcodesilva 17:6f85605f793d 74 char data[4];
marcodesilva 17:6f85605f793d 75
marcodesilva 17:6f85605f793d 76 for (int j = 0 ; j < 4 ; j++){
marcodesilva 17:6f85605f793d 77 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 78 }
marcodesilva 17:6f85605f793d 79
marcodesilva 17:6f85605f793d 80 #ifdef SYNC_SETGOAL_DEBUG
marcodesilva 17:6f85605f793d 81 printf("\n SYNC SET GOAL\n ");
marcodesilva 17:6f85605f793d 82 printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 83 printf("goal[ID]: value(0-4095) - value(0 - 360)\n");
marcodesilva 17:6f85605f793d 84 printf("goal[%d]: %f - %.02f\n",ID,goal,degrees);
marcodesilva 17:6f85605f793d 85 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 86 for (int j = 0 ; j < 4; j++){
marcodesilva 17:6f85605f793d 87 printf("data[%d]: %02x\n",j+(4),data[j+(4)]); //debug data
marcodesilva 17:6f85605f793d 88 }
marcodesilva 17:6f85605f793d 89 #endif
marcodesilva 17:6f85605f793d 90
marcodesilva 17:6f85605f793d 91 int rVal = SyncSendPacket(MX_REG_GOAL_POSITION, 4, data, ID);
marcodesilva 17:6f85605f793d 92
marcodesilva 17:6f85605f793d 93 return(rVal);
marcodesilva 17:6f85605f793d 94 }
marcodesilva 17:6f85605f793d 95
marcodesilva 17:6f85605f793d 96 int MX::SyncSetGoal(float degrees[]) {
marcodesilva 17:6f85605f793d 97
marcodesilva 17:6f85605f793d 98 char data[4*_Nmotor]; //4 is dimension in bytes of instruction
marcodesilva 17:6f85605f793d 99 int goal[_Nmotor];
marcodesilva 17:6f85605f793d 100
marcodesilva 17:6f85605f793d 101 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
marcodesilva 17:6f85605f793d 102 goal[i] = (degrees[i]) * (float)(4095/360);
marcodesilva 17:6f85605f793d 103 }
marcodesilva 17:6f85605f793d 104
marcodesilva 17:6f85605f793d 105 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
marcodesilva 17:6f85605f793d 106 for (int j = 0 ; j < 4; j++){
marcodesilva 17:6f85605f793d 107 data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 108 }
marcodesilva 17:6f85605f793d 109 }
marcodesilva 17:6f85605f793d 110
marcodesilva 17:6f85605f793d 111 #ifdef SYNC_SETGOAL_DEBUG
marcodesilva 17:6f85605f793d 112 printf("\n SYNC SET GOAL\n ");
marcodesilva 17:6f85605f793d 113 printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 114 printf("goal[ID]: value(0-4095) - value(0 - 360)\n");
marcodesilva 17:6f85605f793d 115 for (int i=0; i<_Nmotor ; i++){
marcodesilva 17:6f85605f793d 116 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],degrees[i]);
marcodesilva 17:6f85605f793d 117 }
marcodesilva 17:6f85605f793d 118 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 119 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
marcodesilva 17:6f85605f793d 120 for (int j = 0 ; j < 4; j++){
marcodesilva 17:6f85605f793d 121 printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data
marcodesilva 17:6f85605f793d 122 }
marcodesilva 17:6f85605f793d 123 printf("\n");
marcodesilva 17:6f85605f793d 124 }
marcodesilva 17:6f85605f793d 125 #endif
marcodesilva 17:6f85605f793d 126
marcodesilva 17:6f85605f793d 127 // write the packet, and return the error code
marcodesilva 17:6f85605f793d 128 int rVal = SyncSendPacket(MX_REG_GOAL_POSITION, 4, data);
marcodesilva 17:6f85605f793d 129
marcodesilva 17:6f85605f793d 130 return(rVal);
marcodesilva 17:6f85605f793d 131 }
marcodesilva 17:6f85605f793d 132
marcodesilva 17:6f85605f793d 133
marcodesilva 17:6f85605f793d 134 int MX::SyncOperatingMode(int mode[], int ID) {
marcodesilva 17:6f85605f793d 135
marcodesilva 17:6f85605f793d 136 int Nmotor = _Nmotor;
marcodesilva 17:6f85605f793d 137 char data[1*Nmotor];
marcodesilva 17:6f85605f793d 138
marcodesilva 17:6f85605f793d 139 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 140 data[i] = mode[i];
marcodesilva 17:6f85605f793d 141 }
marcodesilva 17:6f85605f793d 142
marcodesilva 17:6f85605f793d 143 #ifdef SYNC_OPERATING_MODE_DEBUG
marcodesilva 17:6f85605f793d 144 printf("\n SYNC OPERATING MODE DEBUG\n");
marcodesilva 17:6f85605f793d 145 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 146 printf("Set Operating Mode value to: 0x%02x\n",mode[i]);
marcodesilva 17:6f85605f793d 147 }
marcodesilva 17:6f85605f793d 148 printf( "* 0x00 = Current Control Mode\n"
marcodesilva 17:6f85605f793d 149 "* 0x01 = Velocity Control Mode\n"
marcodesilva 17:6f85605f793d 150 "* 0x03 = Position Control Mode (DEFAULT)\n"
marcodesilva 17:6f85605f793d 151 "* 0x04 = Extended Position Control Mode(Multi-turn)\n"
marcodesilva 17:6f85605f793d 152 "* 0x05 = Current-based Position Control Mode\n"
marcodesilva 17:6f85605f793d 153 "* 0x16 = PWM Control Mode (Voltage Control Mode)\n");
marcodesilva 17:6f85605f793d 154 for (int i=0 ; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 155 printf("\n Operating mode value for ID[%d] = %d",_broadcastID[i], mode[i]);
marcodesilva 17:6f85605f793d 156 }
marcodesilva 17:6f85605f793d 157 #endif
marcodesilva 17:6f85605f793d 158
marcodesilva 17:6f85605f793d 159 int rVal = SyncSendPacket(MX_REG_OPERATING_MODE, 1, data, ID);
marcodesilva 17:6f85605f793d 160
marcodesilva 17:6f85605f793d 161 return(rVal);
marcodesilva 17:6f85605f793d 162 }
marcodesilva 17:6f85605f793d 163
marcodesilva 17:6f85605f793d 164
marcodesilva 17:6f85605f793d 165 int MX::SyncCurrentLimit(float mAmpere, int ID) {
marcodesilva 17:6f85605f793d 166
marcodesilva 17:6f85605f793d 167 uint8_t byte = 2; //2 is dimension in bytes of instruction
marcodesilva 17:6f85605f793d 168 float goal = (mAmpere) * (float)(1941/6521.76);
marcodesilva 17:6f85605f793d 169 char data[byte];
marcodesilva 17:6f85605f793d 170
marcodesilva 17:6f85605f793d 171 for (int j = 0 ; j < byte ; j++){
marcodesilva 17:6f85605f793d 172 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 173 }
marcodesilva 17:6f85605f793d 174
marcodesilva 17:6f85605f793d 175 #ifdef SYNC_CURRENT_LIMIT_DEBUG
marcodesilva 17:6f85605f793d 176 printf("\n SYNC CURRENT LIMIT DEBUG\n ");
marcodesilva 17:6f85605f793d 177 printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 178 printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n");
marcodesilva 17:6f85605f793d 179 printf("Current[%d]: %f - %.02f\n",ID,goal,mAmpere);
marcodesilva 17:6f85605f793d 180 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 181 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 182 printf("data[%d]: %02x\n",j+(byte),data[j+(byte)]); //debug data
marcodesilva 17:6f85605f793d 183 }
marcodesilva 17:6f85605f793d 184 #endif
marcodesilva 17:6f85605f793d 185
marcodesilva 17:6f85605f793d 186 int rVal = SyncSendPacket(MX_REG_CURRENT_LIMIT, byte, data, ID);
marcodesilva 17:6f85605f793d 187
marcodesilva 17:6f85605f793d 188 return(rVal);
marcodesilva 17:6f85605f793d 189 }
marcodesilva 17:6f85605f793d 190
marcodesilva 17:6f85605f793d 191
marcodesilva 17:6f85605f793d 192 int MX::SyncCurrentLimit(float mAmpere[]) {
marcodesilva 17:6f85605f793d 193
marcodesilva 17:6f85605f793d 194 uint8_t byte = 2; //2 is dimension in bytes of instruction
marcodesilva 17:6f85605f793d 195 char data[byte*_Nmotor];
marcodesilva 17:6f85605f793d 196 int goal[_Nmotor];
marcodesilva 17:6f85605f793d 197
marcodesilva 17:6f85605f793d 198 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
marcodesilva 17:6f85605f793d 199 goal[i] = (mAmpere[i]) * (float)(1941/6521.76);
marcodesilva 17:6f85605f793d 200 }
marcodesilva 17:6f85605f793d 201
marcodesilva 17:6f85605f793d 202 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
marcodesilva 17:6f85605f793d 203 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 204 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 205 }
marcodesilva 17:6f85605f793d 206 }
marcodesilva 17:6f85605f793d 207
marcodesilva 17:6f85605f793d 208 #ifdef SYNC_CURRENT_LIMIT_DEBUG
marcodesilva 17:6f85605f793d 209 printf("\n SYNC CURRENT LIMIT DEBUG\n ");
marcodesilva 17:6f85605f793d 210 printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 211 printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n");
marcodesilva 17:6f85605f793d 212 for (int i=0; i<_Nmotor ; i++){
marcodesilva 17:6f85605f793d 213 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],mAmpere[i]);
marcodesilva 17:6f85605f793d 214 }
marcodesilva 17:6f85605f793d 215 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 216 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
marcodesilva 17:6f85605f793d 217 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 218 printf("data[%d]: %02x\n",j+(i*byte),data[j+(i*byte)]); //debug data
marcodesilva 17:6f85605f793d 219 }
marcodesilva 17:6f85605f793d 220 printf("\n");
marcodesilva 17:6f85605f793d 221 }
marcodesilva 17:6f85605f793d 222 #endif
marcodesilva 17:6f85605f793d 223
marcodesilva 17:6f85605f793d 224 // write the packet, and return the error code
marcodesilva 17:6f85605f793d 225 int rVal = SyncSendPacket(MX_REG_CURRENT_LIMIT, byte, data);
marcodesilva 17:6f85605f793d 226
marcodesilva 17:6f85605f793d 227 return(rVal);
marcodesilva 17:6f85605f793d 228 }
marcodesilva 17:6f85605f793d 229
marcodesilva 17:6f85605f793d 230
marcodesilva 17:6f85605f793d 231 int MX::SyncGoalCurrent(float mAmpere, int ID) {
marcodesilva 17:6f85605f793d 232
marcodesilva 17:6f85605f793d 233 uint8_t byte = 2; //2 is dimension in bytes of instruction
marcodesilva 17:6f85605f793d 234 float goal = (mAmpere) * (float)(1941/6521.76);
marcodesilva 17:6f85605f793d 235 char data[byte];
marcodesilva 17:6f85605f793d 236
marcodesilva 17:6f85605f793d 237 for (int j = 0 ; j < byte ; j++){
marcodesilva 17:6f85605f793d 238 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 239 }
marcodesilva 17:6f85605f793d 240
marcodesilva 17:6f85605f793d 241 #ifdef SYNC_GOAL_CURRENT_DEBUG
marcodesilva 17:6f85605f793d 242 printf("\n SYNC GOAL CURRENT\n ");
marcodesilva 17:6f85605f793d 243 printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 244 printf("goal[ID]: value(-1941-1941) - value(-6521.76mA - 6521.76mA)\n");
marcodesilva 17:6f85605f793d 245 printf("goal[%d]: %f - %.02f\n",ID,goal,mAmpere);
marcodesilva 17:6f85605f793d 246 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 247 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 248 printf("data[%d]: %02x\n",j+(byte),data[j+(byte)]); //debug data
marcodesilva 17:6f85605f793d 249 }
marcodesilva 17:6f85605f793d 250 #endif
marcodesilva 17:6f85605f793d 251
marcodesilva 17:6f85605f793d 252 int rVal = SyncSendPacket(MX_REG_GOAL_CURRENT, byte, data, ID);
marcodesilva 17:6f85605f793d 253
marcodesilva 17:6f85605f793d 254 return(rVal);
marcodesilva 17:6f85605f793d 255 }
marcodesilva 17:6f85605f793d 256
marcodesilva 17:6f85605f793d 257
marcodesilva 17:6f85605f793d 258 int MX::SyncGoalCurrent(float mAmpere[]) {
marcodesilva 17:6f85605f793d 259
marcodesilva 17:6f85605f793d 260 uint8_t byte = 2; //2 is dimension in bytes of instruction
marcodesilva 17:6f85605f793d 261 char data[byte*_Nmotor];
marcodesilva 17:6f85605f793d 262 int goal[_Nmotor];
marcodesilva 17:6f85605f793d 263
marcodesilva 17:6f85605f793d 264 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
marcodesilva 17:6f85605f793d 265 goal[i] = (mAmpere[i]) * (float)(1941/6521.76);
marcodesilva 17:6f85605f793d 266 }
marcodesilva 17:6f85605f793d 267
marcodesilva 17:6f85605f793d 268 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
marcodesilva 17:6f85605f793d 269 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 270 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 271 }
marcodesilva 17:6f85605f793d 272 }
marcodesilva 17:6f85605f793d 273
marcodesilva 17:6f85605f793d 274 #ifdef SYNC_GOAL_CURRENT_DEBUG
marcodesilva 17:6f85605f793d 275 printf("\n SYNC GOAL CURRENT\n ");
marcodesilva 17:6f85605f793d 276 printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
marcodesilva 17:6f85605f793d 277 printf("goal[ID]: value(0-1941) - value(-6521.76mA - 6521.76mA)\n");
marcodesilva 17:6f85605f793d 278 for (int i=0; i<_Nmotor ; i++){
marcodesilva 17:6f85605f793d 279 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],mAmpere[i]);
marcodesilva 17:6f85605f793d 280 }
marcodesilva 17:6f85605f793d 281 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
marcodesilva 17:6f85605f793d 282 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
marcodesilva 17:6f85605f793d 283 for (int j = 0 ; j < byte; j++){
marcodesilva 17:6f85605f793d 284 printf("data[%d]: %02x\n",j+(i*byte),data[j+(i*byte)]); //debug data
marcodesilva 17:6f85605f793d 285 }
marcodesilva 17:6f85605f793d 286 printf("\n");
marcodesilva 17:6f85605f793d 287 }
marcodesilva 17:6f85605f793d 288 #endif
marcodesilva 17:6f85605f793d 289
marcodesilva 17:6f85605f793d 290 // write the packet, and return the error code
marcodesilva 17:6f85605f793d 291 int rVal = SyncSendPacket(MX_REG_GOAL_CURRENT, byte, data);
marcodesilva 17:6f85605f793d 292
marcodesilva 17:6f85605f793d 293 return(rVal);
marcodesilva 17:6f85605f793d 294 }
marcodesilva 17:6f85605f793d 295
marcodesilva 17:6f85605f793d 296
marcodesilva 17:6f85605f793d 297 float* MX::SyncGetPosition(void) {
marcodesilva 17:6f85605f793d 298 printf("\n SYNC GET POSITION ");
marcodesilva 17:6f85605f793d 299 char* data;
marcodesilva 17:6f85605f793d 300 int bytes = 4;
marcodesilva 17:6f85605f793d 301 int32_t position[bytes];
marcodesilva 17:6f85605f793d 302 int NumberOfMotor = sizeof(_Nmotor);
marcodesilva 17:6f85605f793d 303 int IDreceived[NumberOfMotor];
marcodesilva 17:6f85605f793d 304 float angle[NumberOfMotor] ;
marcodesilva 17:6f85605f793d 305 float ScaleFactor = (float)360/4095;
marcodesilva 17:6f85605f793d 306
marcodesilva 17:6f85605f793d 307
marcodesilva 17:6f85605f793d 308 //char Status[(11+bytes)*_Nmotor]; (11+bytes)
marcodesilva 17:6f85605f793d 309 data = SyncReadPacket(MX_REG_PRESENT_POSITION, bytes);
marcodesilva 17:6f85605f793d 310 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 311
marcodesilva 17:6f85605f793d 312 IDreceived[i] = (int)data[4 +(11+bytes)*i];
marcodesilva 17:6f85605f793d 313
marcodesilva 17:6f85605f793d 314 position[i] = (int)data[12+(11+bytes)*i] << 24;
marcodesilva 17:6f85605f793d 315 position[i] |= (int)data[11+(11+bytes)*i] << 16;
marcodesilva 17:6f85605f793d 316 position[i] |= (int)data[10+(11+bytes)*i] << 8;
marcodesilva 17:6f85605f793d 317 position[i] |= (int)data[9 +(11+bytes)*i];
marcodesilva 17:6f85605f793d 318 }
marcodesilva 17:6f85605f793d 319 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 320 // angle(degree) obtained from position(0 - 4095)
marcodesilva 17:6f85605f793d 321 angle[i] = (float)position[i]*ScaleFactor;
marcodesilva 17:6f85605f793d 322 }
marcodesilva 17:6f85605f793d 323
marcodesilva 17:6f85605f793d 324 #ifdef SYNCGETPOSITION_DEBUG
marcodesilva 17:6f85605f793d 325 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 326 printf("\nGet RAW position data from ID: %d\n",IDreceived[i]);
marcodesilva 17:6f85605f793d 327 printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 328 printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 329 printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 330 printf(" Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 331 printf("Converted position (0 - 4095): %d\n",position[i]);
marcodesilva 17:6f85605f793d 332 printf("Converted angle %f\n\n",angle[i]);
marcodesilva 17:6f85605f793d 333 }
marcodesilva 17:6f85605f793d 334 #endif
marcodesilva 17:6f85605f793d 335
marcodesilva 17:6f85605f793d 336 return(angle);
marcodesilva 17:6f85605f793d 337 }
marcodesilva 17:6f85605f793d 338
marcodesilva 17:6f85605f793d 339 int MX::SyncSendPacket(int start, int bytes, char* data, int ID) {
marcodesilva 17:6f85605f793d 340
marcodesilva 17:6f85605f793d 341 int Nmotor ;
marcodesilva 17:6f85605f793d 342 if (ID == -1){
marcodesilva 17:6f85605f793d 343 Nmotor = _Nmotor;
marcodesilva 17:6f85605f793d 344 }
marcodesilva 17:6f85605f793d 345 else{
marcodesilva 17:6f85605f793d 346 Nmotor = 1;
marcodesilva 17:6f85605f793d 347 }
marcodesilva 17:6f85605f793d 348
marcodesilva 17:6f85605f793d 349 char TxBuf[12+((bytes+1)*Nmotor)+2];
marcodesilva 17:6f85605f793d 350 char Status[11];
marcodesilva 17:6f85605f793d 351 int TxBufSize = sizeof(TxBuf);
marcodesilva 17:6f85605f793d 352
marcodesilva 17:6f85605f793d 353 // initialization of vector TxBuf
marcodesilva 17:6f85605f793d 354 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 355 TxBuf[i] = 0x00;
marcodesilva 17:6f85605f793d 356 }
marcodesilva 17:6f85605f793d 357
marcodesilva 17:6f85605f793d 358 // Inizialization
marcodesilva 17:6f85605f793d 359 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 360 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 361 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 362 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 363 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 364 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 365 TxBuf[4] = 0xfe; //Broadcast ID
marcodesilva 17:6f85605f793d 366 // Length
marcodesilva 17:6f85605f793d 367 TxBuf[5] = (7+( (bytes+1)*Nmotor) ) & 0xff; //packet length low 8 = inst(1bytes)+ param(instr.bytes+1)+CRC(2bytes)
marcodesilva 17:6f85605f793d 368 TxBuf[6] = (7+( (bytes+1)*Nmotor) ) >> 8; //packet length high
marcodesilva 17:6f85605f793d 369 // Instruction
marcodesilva 17:6f85605f793d 370 TxBuf[7] = 0x83; //Sync Write
marcodesilva 17:6f85605f793d 371 // Parameters
marcodesilva 17:6f85605f793d 372 TxBuf[8] = start & 0xff; //Low order byte from starting address
marcodesilva 17:6f85605f793d 373 TxBuf[9] = start >> 8; //High order byte from starting address
marcodesilva 17:6f85605f793d 374 // Bytes lenght needed for instruction
marcodesilva 17:6f85605f793d 375 TxBuf[10] = bytes & 0xff; //Low order byte from starting address
marcodesilva 17:6f85605f793d 376 TxBuf[11] = bytes >> 8; //High order byte from starting address
marcodesilva 17:6f85605f793d 377 // data
marcodesilva 17:6f85605f793d 378 // Motor parameters for each motors
marcodesilva 17:6f85605f793d 379
marcodesilva 17:6f85605f793d 380 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 381 printf("\nDEBUG DI SyncSendPacket");
marcodesilva 17:6f85605f793d 382 printf("\nTxBuf length : %d\n",TxBufSize );
marcodesilva 17:6f85605f793d 383 printf("\nTxBuf before data and CRC adding:\n ");
marcodesilva 17:6f85605f793d 384 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 385 printf("\nTxBuf[%d] = %x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 386 }
marcodesilva 17:6f85605f793d 387 printf("\n\nData to add");
marcodesilva 17:6f85605f793d 388 for (int i=0; i<(Nmotor*bytes) ; i++){
marcodesilva 17:6f85605f793d 389 printf("\ndata[%d] = %02x ",i,data[i]);
marcodesilva 17:6f85605f793d 390 }
marcodesilva 17:6f85605f793d 391 #endif
marcodesilva 17:6f85605f793d 392
marcodesilva 17:6f85605f793d 393 for (int i=0; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 394 // t is a index of TxBuf
marcodesilva 17:6f85605f793d 395 int t = 12 +((bytes+1)*i); // (bytes+1) => instruction dim + 1 (for ID) * i (for each motor)
marcodesilva 17:6f85605f793d 396
marcodesilva 17:6f85605f793d 397 if (Nmotor == 1){
marcodesilva 17:6f85605f793d 398 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 399 printf("\nSINGLE MOTOR ");
marcodesilva 17:6f85605f793d 400 #endif
marcodesilva 17:6f85605f793d 401 TxBuf[t] = ID;
marcodesilva 17:6f85605f793d 402 }else{
marcodesilva 17:6f85605f793d 403 TxBuf[t] = _broadcastID[i];
marcodesilva 17:6f85605f793d 404 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 405 printf("\n\nMULTI MOTOR");
marcodesilva 17:6f85605f793d 406 printf("\nbroadcastID: %d", _broadcastID[i]);
marcodesilva 17:6f85605f793d 407 #endif
marcodesilva 17:6f85605f793d 408 }
marcodesilva 17:6f85605f793d 409
marcodesilva 17:6f85605f793d 410 for (int j = 0; j < bytes ; j++){
marcodesilva 17:6f85605f793d 411 //Little endian
marcodesilva 17:6f85605f793d 412 TxBuf[t+j+1] = data[j+(i*(bytes))]; //(first byte is a low order byte, second byte are High order byte)
marcodesilva 17:6f85605f793d 413 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 414 printf("\nTxBuf[%d] = data[%d] = %x ",t+j+1,j+(i*bytes),data[j+(i*bytes)]);
marcodesilva 17:6f85605f793d 415 #endif
marcodesilva 17:6f85605f793d 416 }
marcodesilva 17:6f85605f793d 417 }
marcodesilva 17:6f85605f793d 418 // CRC //
marcodesilva 17:6f85605f793d 419 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 420 int dataSize = TxBufSize-2;
marcodesilva 17:6f85605f793d 421 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 422
marcodesilva 17:6f85605f793d 423 TxBuf[TxBufSize-2] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 424 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 425 //printf("\nCRC 1 : %02x CRC 1 : %02x ",TxBuf[14+((bytes)*Nmotor)],TxBuf[15+((bytes)*Nmotor)]);
marcodesilva 17:6f85605f793d 426
marcodesilva 17:6f85605f793d 427 #ifdef SYNC_SENDPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 428 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
marcodesilva 17:6f85605f793d 429 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 430 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 431 }
marcodesilva 17:6f85605f793d 432 printf("\n SYNC SEND PACKET \n");
marcodesilva 17:6f85605f793d 433 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 434 printf("|%02x",TxBuf[i]);
marcodesilva 17:6f85605f793d 435 }
marcodesilva 17:6f85605f793d 436 printf("\n \n");
marcodesilva 17:6f85605f793d 437 #endif
marcodesilva 17:6f85605f793d 438
marcodesilva 17:6f85605f793d 439 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 440 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 441 _pCommLayer->write(TxBuf,TxBufSize); // UART-write
marcodesilva 17:6f85605f793d 442
marcodesilva 17:6f85605f793d 443 return(Status[8]); // return error code
marcodesilva 17:6f85605f793d 444 }
marcodesilva 17:6f85605f793d 445
marcodesilva 17:6f85605f793d 446
marcodesilva 17:6f85605f793d 447 char* MX::SyncReadPacket(int start, int bytes) {
marcodesilva 17:6f85605f793d 448
marcodesilva 17:6f85605f793d 449 char TxBuf[12+(_Nmotor)+2];
marcodesilva 17:6f85605f793d 450 char Status[(11+bytes)*_Nmotor];
marcodesilva 17:6f85605f793d 451 int TxBufSize = sizeof(TxBuf);
marcodesilva 17:6f85605f793d 452 char* data;
marcodesilva 17:6f85605f793d 453 Status[8] = 0x00; // The error is set to zero
marcodesilva 17:6f85605f793d 454
marcodesilva 17:6f85605f793d 455 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 456 //Header
marcodesilva 17:6f85605f793d 457 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 458 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 459 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 460 // Reserved
marcodesilva 17:6f85605f793d 461 TxBuf[3] = 0x00;
marcodesilva 17:6f85605f793d 462 // ID
marcodesilva 17:6f85605f793d 463 TxBuf[4] = 0xfe; // Broadcast
marcodesilva 17:6f85605f793d 464 // Lenght 7 bytes = inst(1)+ param(4 + numbers of motors)+CRC(2)
marcodesilva 17:6f85605f793d 465 TxBuf[5] = 7+(_Nmotor) & 0xff; // packet length low
marcodesilva 17:6f85605f793d 466 TxBuf[6] = 7+(_Nmotor) >> 8; // packet length high
marcodesilva 17:6f85605f793d 467 // Instruction
marcodesilva 17:6f85605f793d 468 TxBuf[7] = 0x82;
marcodesilva 17:6f85605f793d 469 // Param
marcodesilva 17:6f85605f793d 470 TxBuf[8] = start & 0xFF; // Address
marcodesilva 17:6f85605f793d 471 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 472 TxBuf[10] = bytes & 0xFF;; // Lenght of reply
marcodesilva 17:6f85605f793d 473 TxBuf[11] = bytes >> 8;
marcodesilva 17:6f85605f793d 474 for (int i=0; i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 475 TxBuf[12+i] = _broadcastID[i];
marcodesilva 17:6f85605f793d 476 }
marcodesilva 17:6f85605f793d 477 // CRC
marcodesilva 17:6f85605f793d 478 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 479 int dataSize = TxBufSize-2;
marcodesilva 17:6f85605f793d 480 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 481 TxBuf[TxBufSize-2] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 482 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 483
marcodesilva 17:6f85605f793d 484 #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 485 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
marcodesilva 17:6f85605f793d 486 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 487 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 488 }
marcodesilva 17:6f85605f793d 489 printf("\n SYNC READ PACKET \n");
marcodesilva 17:6f85605f793d 490 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 491 printf("|%02x",TxBuf[i]);
marcodesilva 17:6f85605f793d 492 }
marcodesilva 17:6f85605f793d 493 printf("\n \n");
marcodesilva 17:6f85605f793d 494 #endif
marcodesilva 17:6f85605f793d 495
marcodesilva 17:6f85605f793d 496 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 497 _pCommLayer->write(TxBuf,TxBufSize); // UART-write
marcodesilva 17:6f85605f793d 498 wait(0.0002);
marcodesilva 17:6f85605f793d 499 _pCommLayer->read_timeout(Status, (11+bytes)*_Nmotor, 2.0);
marcodesilva 17:6f85605f793d 500
marcodesilva 17:6f85605f793d 501 // Take data from 8th cell of status array
marcodesilva 17:6f85605f793d 502 // first byte contain error code, for this we use (bytes+1)
marcodesilva 17:6f85605f793d 503 for (int i=0; i<(11+bytes)*_Nmotor ; i++){
marcodesilva 17:6f85605f793d 504 data[i] = Status[i];
marcodesilva 17:6f85605f793d 505 }
marcodesilva 17:6f85605f793d 506
marcodesilva 17:6f85605f793d 507 #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 508 printf("\n SYNC-READ (entire buffer RAW values)\n");
marcodesilva 17:6f85605f793d 509 for (int i=0; i<(11+bytes)*_Nmotor ; i++){
marcodesilva 17:6f85605f793d 510 data[i] = Status[i];
marcodesilva 17:6f85605f793d 511 printf("data[%d]:%02x\n",i,data[i]);
marcodesilva 17:6f85605f793d 512 }
marcodesilva 17:6f85605f793d 513 #endif
marcodesilva 17:6f85605f793d 514
marcodesilva 17:6f85605f793d 515 return(data); // return error code
marcodesilva 17:6f85605f793d 516 }
marcodesilva 17:6f85605f793d 517
marcodesilva 17:6f85605f793d 518
marcodesilva 17:6f85605f793d 519 ///////////////////////////////////////////////////////////////////////////////
marcodesilva 17:6f85605f793d 520 ////////////////////////SINGLE OPERATIONS//////////////////////////////////////
marcodesilva 17:6f85605f793d 521 ///////////////////////////////////////////////////////////////////////////////
marcodesilva 17:6f85605f793d 522
marcodesilva 17:6f85605f793d 523
marcodesilva 17:6f85605f793d 524 int MX::SetGoalSpeed(int speed, int flags) {
marcodesilva 17:6f85605f793d 525
marcodesilva 17:6f85605f793d 526 char reg_flag = 0;
marcodesilva 17:6f85605f793d 527 char data[4];
marcodesilva 17:6f85605f793d 528
marcodesilva 17:6f85605f793d 529 // set the flag is only the register bit is set in the flag
marcodesilva 17:6f85605f793d 530 if (flags == 0x2) {
marcodesilva 17:6f85605f793d 531 reg_flag = 1;
marcodesilva 17:6f85605f793d 532 }
marcodesilva 17:6f85605f793d 533
marcodesilva 17:6f85605f793d 534 int goal = (speed) ;
marcodesilva 17:6f85605f793d 535
marcodesilva 17:6f85605f793d 536 data[0] = goal & 0xff; // bottom 8 bits
marcodesilva 17:6f85605f793d 537 data[1] = goal >> 8 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 538 data[2] = goal >> 16 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 539 data[3] = goal >> 24 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 540
marcodesilva 17:6f85605f793d 541 // write the packet, return the error code
marcodesilva 17:6f85605f793d 542 int rVal = sendPacket( MX_REG_GOAL_VELOCITY, 4, data, reg_flag);
marcodesilva 17:6f85605f793d 543
marcodesilva 17:6f85605f793d 544 if (flags == 1) {
marcodesilva 17:6f85605f793d 545 // block until it comes to a halt
marcodesilva 17:6f85605f793d 546 while (isMoving()) {}
marcodesilva 17:6f85605f793d 547 }
marcodesilva 17:6f85605f793d 548
marcodesilva 17:6f85605f793d 549 #ifdef SETGOAL_SPEED_DEBUG
marcodesilva 17:6f85605f793d 550 int16_t sp = speed * float(234.27/1023); //rev/min
marcodesilva 17:6f85605f793d 551 printf("\nSetGoal to: %d Velocity in [rev/min]%d \n" ,speed,sp);
marcodesilva 17:6f85605f793d 552 printf("Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
marcodesilva 17:6f85605f793d 553 #endif
marcodesilva 17:6f85605f793d 554
marcodesilva 17:6f85605f793d 555 return(rVal);
marcodesilva 17:6f85605f793d 556 }
marcodesilva 17:6f85605f793d 557
marcodesilva 17:6f85605f793d 558
marcodesilva 17:6f85605f793d 559 int MX::SetGoal(float degrees, int flags) {
marcodesilva 17:6f85605f793d 560
marcodesilva 17:6f85605f793d 561 char reg_flag = 0;
marcodesilva 17:6f85605f793d 562 char data[4];
marcodesilva 17:6f85605f793d 563
marcodesilva 17:6f85605f793d 564 // set the flag is only the register bit is set in the flag
marcodesilva 17:6f85605f793d 565 if (flags == 0x2) {
marcodesilva 17:6f85605f793d 566 reg_flag = 1;
marcodesilva 17:6f85605f793d 567 }
marcodesilva 17:6f85605f793d 568
marcodesilva 17:6f85605f793d 569 int goal = (degrees) * (float)(4095/360);
marcodesilva 17:6f85605f793d 570
marcodesilva 17:6f85605f793d 571 data[0] = goal & 0xff; // bottom 8 bits
marcodesilva 17:6f85605f793d 572 data[1] = goal >> 8 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 573 data[2] = goal >> 16 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 574 data[3] = goal >> 24 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 575
marcodesilva 17:6f85605f793d 576 // write the packet, return the error code
marcodesilva 17:6f85605f793d 577 int rVal = sendPacket( MX_REG_GOAL_POSITION, 4, data, reg_flag );
marcodesilva 17:6f85605f793d 578
marcodesilva 17:6f85605f793d 579 if (flags == 1) {
marcodesilva 17:6f85605f793d 580 // block until it comes to a halt
marcodesilva 17:6f85605f793d 581 while (isMoving()) {}
marcodesilva 17:6f85605f793d 582 }
marcodesilva 17:6f85605f793d 583
marcodesilva 17:6f85605f793d 584 #ifdef SETGOAL_DEBUG
marcodesilva 17:6f85605f793d 585 printf("\n SetGoal to: %d in degree is: %f\n",goal,degrees);
marcodesilva 17:6f85605f793d 586 printf(" Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
marcodesilva 17:6f85605f793d 587 #endif
marcodesilva 17:6f85605f793d 588 return(rVal);
marcodesilva 17:6f85605f793d 589 }
marcodesilva 17:6f85605f793d 590
marcodesilva 17:6f85605f793d 591 // if flag[0] is set, were blocking
marcodesilva 17:6f85605f793d 592 // if flag[1] is set, we're registering
marcodesilva 17:6f85605f793d 593 // they are mutually exclusive operations
marcodesilva 17:6f85605f793d 594
marcodesilva 17:6f85605f793d 595 int MX::OperatingMode(int mode) {
marcodesilva 17:6f85605f793d 596
marcodesilva 17:6f85605f793d 597 char data[1];
marcodesilva 17:6f85605f793d 598 data[0] = mode; // bottom 8 bits
marcodesilva 17:6f85605f793d 599
marcodesilva 17:6f85605f793d 600 #ifdef OPERATING_MODE_DEBUG
marcodesilva 17:6f85605f793d 601 printf("\nOPERATING_MODE_DEBUG\n");
marcodesilva 17:6f85605f793d 602 printf("Set Operating Mode value to: 0x%02x\n",mode);
marcodesilva 17:6f85605f793d 603 printf( "* 0x00 = Current Control Mode\n"
marcodesilva 17:6f85605f793d 604 "* 0x01 = Velocity Control Mode\n"
marcodesilva 17:6f85605f793d 605 "* 0x03 = Position Control Mode (DEFAULT)\n"
marcodesilva 17:6f85605f793d 606 "* 0x04 = Extended Position Control Mode(Multi-turn)\n"
marcodesilva 17:6f85605f793d 607 "* 0x05 = Current-based Position Control Mode\n"
marcodesilva 17:6f85605f793d 608 "* 0x16 = PWM Control Mode (Voltage Control Mode)\n");
marcodesilva 17:6f85605f793d 609 #endif
marcodesilva 17:6f85605f793d 610
marcodesilva 17:6f85605f793d 611 //sendPacket(Motor ID, Address, Lenght, data);
marcodesilva 17:6f85605f793d 612 //return give back a error code of sendPacket function
marcodesilva 17:6f85605f793d 613 return (sendPacket( MX_REG_OPERATING_MODE, 1, data));
marcodesilva 17:6f85605f793d 614 }
marcodesilva 17:6f85605f793d 615
marcodesilva 17:6f85605f793d 616
marcodesilva 17:6f85605f793d 617 // return 1 is the servo is still in flight
marcodesilva 17:6f85605f793d 618 bool MX::isMoving(void) {
marcodesilva 17:6f85605f793d 619
marcodesilva 17:6f85605f793d 620 bool data[1];
marcodesilva 17:6f85605f793d 621 data[0] = 1;
marcodesilva 17:6f85605f793d 622 data[0] = readPacket( MX_REG_MOVING,1 );
marcodesilva 17:6f85605f793d 623 return(data[0]);
marcodesilva 17:6f85605f793d 624 }
marcodesilva 17:6f85605f793d 625
marcodesilva 17:6f85605f793d 626
marcodesilva 17:6f85605f793d 627 int MX::motorSetBaud (int MotorBaud) {
marcodesilva 17:6f85605f793d 628
marcodesilva 17:6f85605f793d 629 char data[1];
marcodesilva 17:6f85605f793d 630 data[0] = MotorBaud;
marcodesilva 17:6f85605f793d 631
marcodesilva 17:6f85605f793d 632 #ifdef SETBAUD_DEBUG
marcodesilva 17:6f85605f793d 633 printf("\nSTATUS Packet - SETBAUD_DEBUG\n");
marcodesilva 17:6f85605f793d 634 printf("Set Baud rate value to: 0x%02x\n",MotorBaud);
marcodesilva 17:6f85605f793d 635 printf( "* 0x00 = 9,600 bps \n"
marcodesilva 17:6f85605f793d 636 "* 0x01 = 57,600 bps (DEFAULT)\n"
marcodesilva 17:6f85605f793d 637 "* 0x02 = 115,200 bps \n"
marcodesilva 17:6f85605f793d 638 "* 0x03 = 1 Mbps \n"
marcodesilva 17:6f85605f793d 639 "* 0x04 = 2 Mbps \n"
marcodesilva 17:6f85605f793d 640 "* 0x05 = 3 Mbps \n"
marcodesilva 17:6f85605f793d 641 "* 0x06 = 4 Mbps \n"
marcodesilva 17:6f85605f793d 642 "* 0x06 = 4,5 Mbps \n");
marcodesilva 17:6f85605f793d 643 #endif
marcodesilva 17:6f85605f793d 644
marcodesilva 17:6f85605f793d 645 //sendPacket(Motor ID, Address, Lenght, data);
marcodesilva 17:6f85605f793d 646 //return give back a error code of sendPacket function
marcodesilva 17:6f85605f793d 647 return (sendPacket( MX_REG_BAUD, 1, data));
marcodesilva 17:6f85605f793d 648 }
marcodesilva 17:6f85605f793d 649
marcodesilva 17:6f85605f793d 650 void MX::Reboot() {
marcodesilva 17:6f85605f793d 651
marcodesilva 17:6f85605f793d 652 char TxBuf[10];
marcodesilva 17:6f85605f793d 653 char Status[11];
marcodesilva 17:6f85605f793d 654 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 655
marcodesilva 17:6f85605f793d 656 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 657 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 658 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 659 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 660 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 661 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 662 // packet Length
marcodesilva 17:6f85605f793d 663 TxBuf[5] = 0x03; //packet length low
marcodesilva 17:6f85605f793d 664 TxBuf[6] = 0x00; //packet length high
marcodesilva 17:6f85605f793d 665 // Instruction
marcodesilva 17:6f85605f793d 666 TxBuf[7] = 0x08; // Instruction to reboot Device
marcodesilva 17:6f85605f793d 667 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 668 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 669 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 670 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/);
marcodesilva 17:6f85605f793d 671 TxBuf[8] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 672 TxBuf[9] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 673 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 674 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 675 _pCommLayer->write(TxBuf,10); // UART-write
marcodesilva 17:6f85605f793d 676 _pCommLayer->read_timeout(Status, 11, 2.0);
marcodesilva 17:6f85605f793d 677
marcodesilva 17:6f85605f793d 678 #ifdef REBOOT_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 679 printf("\n Reboot Motor: (%d)",enableVal);
marcodesilva 17:6f85605f793d 680 #endif
marcodesilva 17:6f85605f793d 681 }
marcodesilva 17:6f85605f793d 682
marcodesilva 17:6f85605f793d 683
marcodesilva 17:6f85605f793d 684 int MX::TorqueEnable(bool enableVal) {
marcodesilva 17:6f85605f793d 685
marcodesilva 17:6f85605f793d 686 char data[1];
marcodesilva 17:6f85605f793d 687 data[0] = enableVal;
marcodesilva 17:6f85605f793d 688
marcodesilva 17:6f85605f793d 689 #ifdef TORQUE_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 690 printf("\n Setting Torque Enable value: (%d)",enableVal);
marcodesilva 17:6f85605f793d 691 #endif
marcodesilva 17:6f85605f793d 692
marcodesilva 17:6f85605f793d 693 return (sendPacket( MX_REG_TORQUE_ENABLE, 1, data));
marcodesilva 17:6f85605f793d 694 }
marcodesilva 17:6f85605f793d 695
marcodesilva 17:6f85605f793d 696
marcodesilva 17:6f85605f793d 697 float MX::GetPosition(void) {
marcodesilva 17:6f85605f793d 698 printf("\nGET POSITION ");
marcodesilva 17:6f85605f793d 699 char* data;
marcodesilva 17:6f85605f793d 700 int32_t position;
marcodesilva 17:6f85605f793d 701 float angle = 0;
marcodesilva 17:6f85605f793d 702 float ScaleFactor = (float)360/4095;
marcodesilva 17:6f85605f793d 703
marcodesilva 17:6f85605f793d 704 data = readPacket( MX_REG_PRESENT_POSITION, 4);
marcodesilva 17:6f85605f793d 705 position = (uint32_t)data[3] << 24;
marcodesilva 17:6f85605f793d 706 position |= (uint32_t)data[2] << 16;
marcodesilva 17:6f85605f793d 707 position |= (uint32_t)data[1] << 8;
marcodesilva 17:6f85605f793d 708 position |= (uint32_t)data[0];
marcodesilva 17:6f85605f793d 709 // angle(degree) obtained from position(0 - 4095)
marcodesilva 17:6f85605f793d 710 angle = (float)position*ScaleFactor;
marcodesilva 17:6f85605f793d 711
marcodesilva 17:6f85605f793d 712 #ifdef GETPOSITION_DEBUG
marcodesilva 17:6f85605f793d 713 printf("\nGetPosition from ID: %d\n",_ID);
marcodesilva 17:6f85605f793d 714 for (uint16_t i=0; i<4 ; i++) {
marcodesilva 17:6f85605f793d 715 printf(" Data[%d] : 0x%02x\n",(i+1),data[i]);
marcodesilva 17:6f85605f793d 716 }
marcodesilva 17:6f85605f793d 717 printf(" Read position (0 - 4095): %d\n",position);
marcodesilva 17:6f85605f793d 718 printf(" Converted angle %f\n",angle);
marcodesilva 17:6f85605f793d 719 #endif
marcodesilva 17:6f85605f793d 720 return (angle);
marcodesilva 17:6f85605f793d 721 }
marcodesilva 17:6f85605f793d 722
marcodesilva 17:6f85605f793d 723
marcodesilva 17:6f85605f793d 724 int MX::sendPacket( int start, int bytes, char* data, int flag) {
marcodesilva 17:6f85605f793d 725
marcodesilva 17:6f85605f793d 726 char TxBuf[12+bytes];
marcodesilva 17:6f85605f793d 727 char Status[11];
marcodesilva 17:6f85605f793d 728 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 729
marcodesilva 17:6f85605f793d 730 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 731 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 732 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 733 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 734 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 735 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 736 // packet Length
marcodesilva 17:6f85605f793d 737 TxBuf[5] = (5+bytes) & 0xff; //packet length low
marcodesilva 17:6f85605f793d 738 TxBuf[6] = (5+bytes) >> 8; //packet length high
marcodesilva 17:6f85605f793d 739 // Instruction
marcodesilva 17:6f85605f793d 740 if (flag == 1) {
marcodesilva 17:6f85605f793d 741 TxBuf[7]=0x04;
marcodesilva 17:6f85605f793d 742 } else {
marcodesilva 17:6f85605f793d 743 TxBuf[7]=0x03;
marcodesilva 17:6f85605f793d 744 }
marcodesilva 17:6f85605f793d 745 // Start Address
marcodesilva 17:6f85605f793d 746 TxBuf[8] = start & 0xFF;
marcodesilva 17:6f85605f793d 747 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 748 // data
marcodesilva 17:6f85605f793d 749 for (int i = bytes-1; i>=0 ; i--) { //little endian
marcodesilva 17:6f85605f793d 750 TxBuf[10+i] = data[i];
marcodesilva 17:6f85605f793d 751 }
marcodesilva 17:6f85605f793d 752 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 753 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 754 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 755 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/);
marcodesilva 17:6f85605f793d 756
marcodesilva 17:6f85605f793d 757 TxBuf[10+bytes] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 758 TxBuf[11+bytes] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 759 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 760 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 761 _pCommLayer->write(TxBuf,bytes+12); // UART-write
marcodesilva 17:6f85605f793d 762 _pCommLayer->read_timeout(Status, 11, 2.0);
marcodesilva 17:6f85605f793d 763
marcodesilva 17:6f85605f793d 764
marcodesilva 17:6f85605f793d 765 #ifdef SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 766 printf("\nWRITE input parameters:\n");
marcodesilva 17:6f85605f793d 767 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
marcodesilva 17:6f85605f793d 768 printf(" Data written:\n");
marcodesilva 17:6f85605f793d 769 for (int i=0; i < bytes ; i++) {
marcodesilva 17:6f85605f793d 770 printf(" Data[%d]: 0x%x\n",i,data[i]);
marcodesilva 17:6f85605f793d 771 }
marcodesilva 17:6f85605f793d 772 #endif
marcodesilva 17:6f85605f793d 773
marcodesilva 17:6f85605f793d 774 #ifdef SENDPACKET_DEBUG_INSTRUCTION_PKT
marcodesilva 17:6f85605f793d 775 printf("\nINSTRUCTIONS Packet - WRITE_DEBUG \n");
marcodesilva 17:6f85605f793d 776 printf(" H1 : 0x%02x\n",TxBuf[0]);
marcodesilva 17:6f85605f793d 777 printf(" H2 : 0x%02x\n",TxBuf[1]);
marcodesilva 17:6f85605f793d 778 printf(" H3 : 0x%02x\n",TxBuf[2]);
marcodesilva 17:6f85605f793d 779 printf(" Reserved : 0x%02x\n",TxBuf[3]);
marcodesilva 17:6f85605f793d 780 printf(" ID : 0x%02x\n",TxBuf[4]);
marcodesilva 17:6f85605f793d 781 printf(" Length L : 0x%02x\n",TxBuf[5]);
marcodesilva 17:6f85605f793d 782 printf(" Length H : 0x%02x\n",TxBuf[6]);
marcodesilva 17:6f85605f793d 783 printf(" Instruction : 0x%02x\n",TxBuf[7]);
marcodesilva 17:6f85605f793d 784 printf(" Cmd L : 0x%02x\n",TxBuf[8]);
marcodesilva 17:6f85605f793d 785 printf(" Cmd H : 0x%02x\n",TxBuf[9]);
marcodesilva 17:6f85605f793d 786 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 787 printf(" Data : 0x%02x\n",TxBuf[10+i]);
marcodesilva 17:6f85605f793d 788 }
marcodesilva 17:6f85605f793d 789 printf(" CRC1 : 0x%02x\n",TxBuf[10+bytes]);
marcodesilva 17:6f85605f793d 790 printf(" CRC2 : 0x%02x\n",TxBuf[11+bytes]);
marcodesilva 17:6f85605f793d 791 printf(" TxBuf = ");
marcodesilva 17:6f85605f793d 792 for (int i = 0; i < sizeof(TxBuf) ; i++) {
marcodesilva 17:6f85605f793d 793 printf(" [0x%02x]",TxBuf[i]);
marcodesilva 17:6f85605f793d 794 }
marcodesilva 17:6f85605f793d 795 printf("\n");
marcodesilva 17:6f85605f793d 796 #endif
marcodesilva 17:6f85605f793d 797
marcodesilva 17:6f85605f793d 798 #ifdef SENDPACKET_DEBUG_STATUS_PKT
marcodesilva 17:6f85605f793d 799 printf("\nSTATUS Packet - WRITE_DEBUG\n");
marcodesilva 17:6f85605f793d 800 printf(" H1 : 0x%02x\n",Status[0]);
marcodesilva 17:6f85605f793d 801 printf(" H2 : 0x%02x\n",Status[1]);
marcodesilva 17:6f85605f793d 802 printf(" H3 : 0x%02x\n",Status[2]);
marcodesilva 17:6f85605f793d 803 printf(" Reserved : 0x%02x\n",Status[3]);
marcodesilva 17:6f85605f793d 804 printf(" ID : 0x%02x\n",Status[4]);
marcodesilva 17:6f85605f793d 805 printf(" Length L : 0x%02x\n",Status[5]);
marcodesilva 17:6f85605f793d 806 printf(" Length H : 0x%02x\n",Status[6]);
marcodesilva 17:6f85605f793d 807 printf(" Instruction : 0x%02x\n",Status[7]);
marcodesilva 17:6f85605f793d 808 printf(" Error : 0x%02x\n",Status[8]);
marcodesilva 17:6f85605f793d 809 printf(" CRC1 : 0x%02x\n",Status[9]);
marcodesilva 17:6f85605f793d 810 printf(" CRC2 : 0x%02x\n",Status[10]);
marcodesilva 17:6f85605f793d 811 printf(" Status = ");
marcodesilva 17:6f85605f793d 812 for (int i = 0; i < sizeof(Status) ; i++) {
marcodesilva 17:6f85605f793d 813 printf(" [0x%02x]",Status[i]);
marcodesilva 17:6f85605f793d 814 }
marcodesilva 17:6f85605f793d 815 printf("\n");
marcodesilva 17:6f85605f793d 816 #endif
marcodesilva 17:6f85605f793d 817
marcodesilva 17:6f85605f793d 818 return(Status[8]); // return error code
marcodesilva 17:6f85605f793d 819 }
marcodesilva 17:6f85605f793d 820
marcodesilva 17:6f85605f793d 821
marcodesilva 17:6f85605f793d 822
marcodesilva 17:6f85605f793d 823 char* MX::readPacket(int start, int bytes, int flag) {
marcodesilva 17:6f85605f793d 824
marcodesilva 17:6f85605f793d 825 char TxBuf[14];
marcodesilva 17:6f85605f793d 826 char Status[11+bytes];
marcodesilva 17:6f85605f793d 827 char* data;
marcodesilva 17:6f85605f793d 828 Status[8] = 0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 829
marcodesilva 17:6f85605f793d 830 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 831 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 832 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 833 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 834 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 835 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 836 TxBuf[5] = 0x07; //packet length low
marcodesilva 17:6f85605f793d 837 TxBuf[6] = 0x00; //packet length high
marcodesilva 17:6f85605f793d 838 TxBuf[7] = 0x02; // Instruction
marcodesilva 17:6f85605f793d 839 TxBuf[8] = start & 0xFF; // Start Address
marcodesilva 17:6f85605f793d 840 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 841 // N param
marcodesilva 17:6f85605f793d 842 TxBuf[10] = 0x04;
marcodesilva 17:6f85605f793d 843 TxBuf[11] = 0x00;
marcodesilva 17:6f85605f793d 844 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 845 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 846 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 847 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 848 TxBuf[12] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 849 TxBuf[13] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 850
marcodesilva 17:6f85605f793d 851 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 852 _pCommLayer->write(TxBuf,sizeof(TxBuf)); // UART-write
marcodesilva 17:6f85605f793d 853 wait(0.0002);
marcodesilva 17:6f85605f793d 854 _pCommLayer->read_timeout(Status, 11+bytes, 2.0);
marcodesilva 17:6f85605f793d 855
marcodesilva 17:6f85605f793d 856
marcodesilva 17:6f85605f793d 857 #ifdef MX_READPACKET_DEBUG
marcodesilva 17:6f85605f793d 858 printf("\nREAD input parameters:\n");
marcodesilva 17:6f85605f793d 859 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
marcodesilva 17:6f85605f793d 860 printf(" Data written:\n");
marcodesilva 17:6f85605f793d 861 for (int i=0; i < bytes ; i++) {
marcodesilva 17:6f85605f793d 862 printf(" Data[%d]: 0x%x\n",i,data[i]);
marcodesilva 17:6f85605f793d 863 }
marcodesilva 17:6f85605f793d 864 #endif
marcodesilva 17:6f85605f793d 865
marcodesilva 17:6f85605f793d 866 #ifdef MX_READPACKET_DEBUG_INSTRUCTION_PKT
marcodesilva 17:6f85605f793d 867 printf("\nINSTRUCTIONS Packet - READ_DEBUG \n");
marcodesilva 17:6f85605f793d 868 printf(" H1 - Header : 0x%02x\n",TxBuf[0]);
marcodesilva 17:6f85605f793d 869 printf(" H2 - Header : 0x%02x\n",TxBuf[1]);
marcodesilva 17:6f85605f793d 870 printf(" H3 - Header : 0x%02x\n",TxBuf[2]);
marcodesilva 17:6f85605f793d 871 printf(" RESERVED : 0x%02x\n",TxBuf[3]);
marcodesilva 17:6f85605f793d 872 printf(" ID : 0x%02x\n",TxBuf[4]);
marcodesilva 17:6f85605f793d 873 printf(" Length L : 0x%02x\n",TxBuf[5]);
marcodesilva 17:6f85605f793d 874 printf(" Length H : 0x%02x\n",TxBuf[6]);
marcodesilva 17:6f85605f793d 875 printf(" Instruction : 0x%02x\n",TxBuf[7]);
marcodesilva 17:6f85605f793d 876 printf(" Param1 - Cmd L : 0x%02x\n",TxBuf[8]);
marcodesilva 17:6f85605f793d 877 printf(" Param2 - Cmd H : 0x%02x\n",TxBuf[9]);
marcodesilva 17:6f85605f793d 878 printf(" Param3 : 0x%02x\n",TxBuf[10]);
marcodesilva 17:6f85605f793d 879 printf(" Param4 : 0x%02x\n",TxBuf[11]);
marcodesilva 17:6f85605f793d 880 printf(" CRC1 : 0x%02x\n",TxBuf[12]);
marcodesilva 17:6f85605f793d 881 printf(" CRC2 : 0x%02x\n",TxBuf[13]);
marcodesilva 17:6f85605f793d 882 printf(" TxBuf = ");
marcodesilva 17:6f85605f793d 883 for (int i = 0; i < 14 ; i++) {
marcodesilva 17:6f85605f793d 884 printf(" [0x%02x]",TxBuf[i]);
marcodesilva 17:6f85605f793d 885 }
marcodesilva 17:6f85605f793d 886 printf("\n\n");
marcodesilva 17:6f85605f793d 887 #endif
marcodesilva 17:6f85605f793d 888
marcodesilva 17:6f85605f793d 889 #ifdef MX_READPACKET_DEBUG_STATUS_PKT
marcodesilva 17:6f85605f793d 890 printf("\nSTATUS Packet - READ_DEBUG \n");
marcodesilva 17:6f85605f793d 891 printf(" H1 - Header : 0x%02x\n",Status[0]);
marcodesilva 17:6f85605f793d 892 printf(" H2 - Header : 0x%02x\n",Status[1]);
marcodesilva 17:6f85605f793d 893 printf(" H3 - Header : 0x%02x\n",Status[2]);
marcodesilva 17:6f85605f793d 894 printf(" Reserved : 0x%02x\n",Status[3]);
marcodesilva 17:6f85605f793d 895 printf(" ID : 0x%02x\n",Status[4]);
marcodesilva 17:6f85605f793d 896 printf(" Length L : 0x%02x\n",Status[5]);
marcodesilva 17:6f85605f793d 897 printf(" Length H : 0x%02x\n",Status[6]);
marcodesilva 17:6f85605f793d 898 printf(" Instruction : 0x%02x\n",Status[7]);
marcodesilva 17:6f85605f793d 899 printf(" Error : 0x%02x\n",Status[8]);
marcodesilva 17:6f85605f793d 900 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 901 printf(" Param%d : 0x%02x\n",(i+1),Status[9+i]);
marcodesilva 17:6f85605f793d 902 }
marcodesilva 17:6f85605f793d 903 printf(" Crc1 : 0x%02x\n",Status[13]);
marcodesilva 17:6f85605f793d 904 printf(" Crc2 : 0x%02x\n",Status[14]);
marcodesilva 17:6f85605f793d 905 printf(" Status = ");
marcodesilva 17:6f85605f793d 906 for (int i = 0; i < sizeof(Status) ; i++) {
marcodesilva 17:6f85605f793d 907 printf(" [0x%02x]",Status[i]);
marcodesilva 17:6f85605f793d 908 }
marcodesilva 17:6f85605f793d 909 printf("\n");
marcodesilva 17:6f85605f793d 910 #endif
marcodesilva 17:6f85605f793d 911 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 912 data[i] = Status[9+i];
marcodesilva 17:6f85605f793d 913 }
marcodesilva 17:6f85605f793d 914 return(data); // return error code
marcodesilva 17:6f85605f793d 915 }
marcodesilva 17:6f85605f793d 916
marcodesilva 17:6f85605f793d 917 unsigned short MX::update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size)
marcodesilva 17:6f85605f793d 918 {
marcodesilva 17:6f85605f793d 919 unsigned short i, j;
marcodesilva 17:6f85605f793d 920 unsigned short crc_table[256] = {
marcodesilva 17:6f85605f793d 921 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
marcodesilva 17:6f85605f793d 922 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
marcodesilva 17:6f85605f793d 923 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
marcodesilva 17:6f85605f793d 924 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
marcodesilva 17:6f85605f793d 925 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
marcodesilva 17:6f85605f793d 926 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
marcodesilva 17:6f85605f793d 927 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
marcodesilva 17:6f85605f793d 928 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
marcodesilva 17:6f85605f793d 929 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
marcodesilva 17:6f85605f793d 930 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
marcodesilva 17:6f85605f793d 931 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
marcodesilva 17:6f85605f793d 932 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
marcodesilva 17:6f85605f793d 933 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
marcodesilva 17:6f85605f793d 934 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
marcodesilva 17:6f85605f793d 935 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
marcodesilva 17:6f85605f793d 936 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
marcodesilva 17:6f85605f793d 937 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
marcodesilva 17:6f85605f793d 938 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
marcodesilva 17:6f85605f793d 939 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
marcodesilva 17:6f85605f793d 940 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
marcodesilva 17:6f85605f793d 941 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
marcodesilva 17:6f85605f793d 942 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
marcodesilva 17:6f85605f793d 943 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
marcodesilva 17:6f85605f793d 944 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
marcodesilva 17:6f85605f793d 945 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
marcodesilva 17:6f85605f793d 946 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
marcodesilva 17:6f85605f793d 947 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
marcodesilva 17:6f85605f793d 948 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
marcodesilva 17:6f85605f793d 949 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
marcodesilva 17:6f85605f793d 950 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
marcodesilva 17:6f85605f793d 951 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
marcodesilva 17:6f85605f793d 952 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
marcodesilva 17:6f85605f793d 953 };
marcodesilva 17:6f85605f793d 954 for(j = 0; j < data_blk_size; j++)
marcodesilva 17:6f85605f793d 955 {
marcodesilva 17:6f85605f793d 956 i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
marcodesilva 17:6f85605f793d 957 crc_accum = (crc_accum << 8) ^ crc_table[i];
marcodesilva 17:6f85605f793d 958 }
marcodesilva 17:6f85605f793d 959 return crc_accum;
marcodesilva 17:6f85605f793d 960 }