PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.cpp@19:16d5f8a3311f, 2019-10-22 (annotated)

Committer:
marcodesilva
Date:
Tue Oct 22 12:39:36 2019 +0000
Revision:
19:16d5f8a3311f
Parent:
17:6f85605f793d
Child:
20:029e0fe30b0a
PWM implementata

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 19:16d5f8a3311f 105 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
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 19:16d5f8a3311f 297 int MX::SyncGoalPWM(float values[], int ID) {
marcodesilva 19:16d5f8a3311f 298
marcodesilva 19:16d5f8a3311f 299 uint8_t byte = 2; //2 is dimension in bytes of instruction
marcodesilva 19:16d5f8a3311f 300 char data[byte*_Nmotor];
marcodesilva 19:16d5f8a3311f 301 int goal[_Nmotor];
marcodesilva 19:16d5f8a3311f 302
marcodesilva 19:16d5f8a3311f 303 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
marcodesilva 19:16d5f8a3311f 304 goal[i] = (values[i]) * (float)(885/100);
marcodesilva 19:16d5f8a3311f 305 }
marcodesilva 19:16d5f8a3311f 306
marcodesilva 19:16d5f8a3311f 307 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
marcodesilva 19:16d5f8a3311f 308 for (int j = 0 ; j < byte; j++){
marcodesilva 19:16d5f8a3311f 309 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
marcodesilva 19:16d5f8a3311f 310 }
marcodesilva 19:16d5f8a3311f 311 }
marcodesilva 19:16d5f8a3311f 312 // write the packet, and return the error code
marcodesilva 19:16d5f8a3311f 313 int rVal = SyncSendPacket(MX_REG_GOAL_PWM, byte, data, ID);
marcodesilva 19:16d5f8a3311f 314
marcodesilva 19:16d5f8a3311f 315 return(rVal);
marcodesilva 19:16d5f8a3311f 316 }
marcodesilva 19:16d5f8a3311f 317
marcodesilva 19:16d5f8a3311f 318
marcodesilva 17:6f85605f793d 319 float* MX::SyncGetPosition(void) {
marcodesilva 17:6f85605f793d 320 printf("\n SYNC GET POSITION ");
marcodesilva 17:6f85605f793d 321 char* data;
marcodesilva 17:6f85605f793d 322 int bytes = 4;
marcodesilva 17:6f85605f793d 323 int32_t position[bytes];
marcodesilva 17:6f85605f793d 324 int NumberOfMotor = sizeof(_Nmotor);
marcodesilva 17:6f85605f793d 325 int IDreceived[NumberOfMotor];
marcodesilva 17:6f85605f793d 326 float angle[NumberOfMotor] ;
marcodesilva 17:6f85605f793d 327 float ScaleFactor = (float)360/4095;
marcodesilva 17:6f85605f793d 328
marcodesilva 17:6f85605f793d 329
marcodesilva 17:6f85605f793d 330 //char Status[(11+bytes)*_Nmotor]; (11+bytes)
marcodesilva 17:6f85605f793d 331 data = SyncReadPacket(MX_REG_PRESENT_POSITION, bytes);
marcodesilva 17:6f85605f793d 332 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 333
marcodesilva 17:6f85605f793d 334 IDreceived[i] = (int)data[4 +(11+bytes)*i];
marcodesilva 17:6f85605f793d 335
marcodesilva 17:6f85605f793d 336 position[i] = (int)data[12+(11+bytes)*i] << 24;
marcodesilva 17:6f85605f793d 337 position[i] |= (int)data[11+(11+bytes)*i] << 16;
marcodesilva 17:6f85605f793d 338 position[i] |= (int)data[10+(11+bytes)*i] << 8;
marcodesilva 17:6f85605f793d 339 position[i] |= (int)data[9 +(11+bytes)*i];
marcodesilva 17:6f85605f793d 340 }
marcodesilva 17:6f85605f793d 341 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 342 // angle(degree) obtained from position(0 - 4095)
marcodesilva 17:6f85605f793d 343 angle[i] = (float)position[i]*ScaleFactor;
marcodesilva 17:6f85605f793d 344 }
marcodesilva 17:6f85605f793d 345
marcodesilva 17:6f85605f793d 346 #ifdef SYNCGETPOSITION_DEBUG
marcodesilva 17:6f85605f793d 347 for (int i=0 ;i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 348 printf("\nGet RAW position data from ID: %d\n",IDreceived[i]);
marcodesilva 17:6f85605f793d 349 printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 350 printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 351 printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 352 printf(" Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]);
marcodesilva 17:6f85605f793d 353 printf("Converted position (0 - 4095): %d\n",position[i]);
marcodesilva 17:6f85605f793d 354 printf("Converted angle %f\n\n",angle[i]);
marcodesilva 17:6f85605f793d 355 }
marcodesilva 17:6f85605f793d 356 #endif
marcodesilva 17:6f85605f793d 357
marcodesilva 17:6f85605f793d 358 return(angle);
marcodesilva 17:6f85605f793d 359 }
marcodesilva 17:6f85605f793d 360
marcodesilva 17:6f85605f793d 361 int MX::SyncSendPacket(int start, int bytes, char* data, int ID) {
marcodesilva 17:6f85605f793d 362
marcodesilva 17:6f85605f793d 363 int Nmotor ;
marcodesilva 17:6f85605f793d 364 if (ID == -1){
marcodesilva 17:6f85605f793d 365 Nmotor = _Nmotor;
marcodesilva 17:6f85605f793d 366 }
marcodesilva 17:6f85605f793d 367 else{
marcodesilva 17:6f85605f793d 368 Nmotor = 1;
marcodesilva 17:6f85605f793d 369 }
marcodesilva 17:6f85605f793d 370
marcodesilva 17:6f85605f793d 371 char TxBuf[12+((bytes+1)*Nmotor)+2];
marcodesilva 17:6f85605f793d 372 char Status[11];
marcodesilva 17:6f85605f793d 373 int TxBufSize = sizeof(TxBuf);
marcodesilva 17:6f85605f793d 374
marcodesilva 17:6f85605f793d 375 // initialization of vector TxBuf
marcodesilva 17:6f85605f793d 376 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 377 TxBuf[i] = 0x00;
marcodesilva 17:6f85605f793d 378 }
marcodesilva 17:6f85605f793d 379
marcodesilva 17:6f85605f793d 380 // Inizialization
marcodesilva 17:6f85605f793d 381 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 382 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 383 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 384 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 385 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 386 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 387 TxBuf[4] = 0xfe; //Broadcast ID
marcodesilva 17:6f85605f793d 388 // Length
marcodesilva 17:6f85605f793d 389 TxBuf[5] = (7+( (bytes+1)*Nmotor) ) & 0xff; //packet length low 8 = inst(1bytes)+ param(instr.bytes+1)+CRC(2bytes)
marcodesilva 17:6f85605f793d 390 TxBuf[6] = (7+( (bytes+1)*Nmotor) ) >> 8; //packet length high
marcodesilva 17:6f85605f793d 391 // Instruction
marcodesilva 17:6f85605f793d 392 TxBuf[7] = 0x83; //Sync Write
marcodesilva 17:6f85605f793d 393 // Parameters
marcodesilva 17:6f85605f793d 394 TxBuf[8] = start & 0xff; //Low order byte from starting address
marcodesilva 17:6f85605f793d 395 TxBuf[9] = start >> 8; //High order byte from starting address
marcodesilva 17:6f85605f793d 396 // Bytes lenght needed for instruction
marcodesilva 17:6f85605f793d 397 TxBuf[10] = bytes & 0xff; //Low order byte from starting address
marcodesilva 17:6f85605f793d 398 TxBuf[11] = bytes >> 8; //High order byte from starting address
marcodesilva 17:6f85605f793d 399 // data
marcodesilva 17:6f85605f793d 400 // Motor parameters for each motors
marcodesilva 17:6f85605f793d 401
marcodesilva 17:6f85605f793d 402 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 403 printf("\nDEBUG DI SyncSendPacket");
marcodesilva 17:6f85605f793d 404 printf("\nTxBuf length : %d\n",TxBufSize );
marcodesilva 17:6f85605f793d 405 printf("\nTxBuf before data and CRC adding:\n ");
marcodesilva 17:6f85605f793d 406 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 407 printf("\nTxBuf[%d] = %x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 408 }
marcodesilva 17:6f85605f793d 409 printf("\n\nData to add");
marcodesilva 17:6f85605f793d 410 for (int i=0; i<(Nmotor*bytes) ; i++){
marcodesilva 17:6f85605f793d 411 printf("\ndata[%d] = %02x ",i,data[i]);
marcodesilva 17:6f85605f793d 412 }
marcodesilva 17:6f85605f793d 413 #endif
marcodesilva 17:6f85605f793d 414
marcodesilva 17:6f85605f793d 415 for (int i=0; i<Nmotor ; i++){
marcodesilva 17:6f85605f793d 416 // t is a index of TxBuf
marcodesilva 17:6f85605f793d 417 int t = 12 +((bytes+1)*i); // (bytes+1) => instruction dim + 1 (for ID) * i (for each motor)
marcodesilva 17:6f85605f793d 418
marcodesilva 17:6f85605f793d 419 if (Nmotor == 1){
marcodesilva 17:6f85605f793d 420 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 421 printf("\nSINGLE MOTOR ");
marcodesilva 17:6f85605f793d 422 #endif
marcodesilva 17:6f85605f793d 423 TxBuf[t] = ID;
marcodesilva 17:6f85605f793d 424 }else{
marcodesilva 17:6f85605f793d 425 TxBuf[t] = _broadcastID[i];
marcodesilva 17:6f85605f793d 426 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 427 printf("\n\nMULTI MOTOR");
marcodesilva 17:6f85605f793d 428 printf("\nbroadcastID: %d", _broadcastID[i]);
marcodesilva 17:6f85605f793d 429 #endif
marcodesilva 17:6f85605f793d 430 }
marcodesilva 17:6f85605f793d 431
marcodesilva 17:6f85605f793d 432 for (int j = 0; j < bytes ; j++){
marcodesilva 17:6f85605f793d 433 //Little endian
marcodesilva 17:6f85605f793d 434 TxBuf[t+j+1] = data[j+(i*(bytes))]; //(first byte is a low order byte, second byte are High order byte)
marcodesilva 17:6f85605f793d 435 #ifdef SYNC_SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 436 printf("\nTxBuf[%d] = data[%d] = %x ",t+j+1,j+(i*bytes),data[j+(i*bytes)]);
marcodesilva 17:6f85605f793d 437 #endif
marcodesilva 17:6f85605f793d 438 }
marcodesilva 17:6f85605f793d 439 }
marcodesilva 17:6f85605f793d 440 // CRC //
marcodesilva 17:6f85605f793d 441 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 442 int dataSize = TxBufSize-2;
marcodesilva 17:6f85605f793d 443 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 444
marcodesilva 17:6f85605f793d 445 TxBuf[TxBufSize-2] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 446 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 447 //printf("\nCRC 1 : %02x CRC 1 : %02x ",TxBuf[14+((bytes)*Nmotor)],TxBuf[15+((bytes)*Nmotor)]);
marcodesilva 17:6f85605f793d 448
marcodesilva 17:6f85605f793d 449 #ifdef SYNC_SENDPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 450 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
marcodesilva 17:6f85605f793d 451 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 452 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 453 }
marcodesilva 17:6f85605f793d 454 printf("\n SYNC SEND PACKET \n");
marcodesilva 17:6f85605f793d 455 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 456 printf("|%02x",TxBuf[i]);
marcodesilva 17:6f85605f793d 457 }
marcodesilva 17:6f85605f793d 458 printf("\n \n");
marcodesilva 17:6f85605f793d 459 #endif
marcodesilva 17:6f85605f793d 460
marcodesilva 17:6f85605f793d 461 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 462 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 463 _pCommLayer->write(TxBuf,TxBufSize); // UART-write
marcodesilva 17:6f85605f793d 464
marcodesilva 17:6f85605f793d 465 return(Status[8]); // return error code
marcodesilva 17:6f85605f793d 466 }
marcodesilva 17:6f85605f793d 467
marcodesilva 17:6f85605f793d 468
marcodesilva 17:6f85605f793d 469 char* MX::SyncReadPacket(int start, int bytes) {
marcodesilva 17:6f85605f793d 470
marcodesilva 17:6f85605f793d 471 char TxBuf[12+(_Nmotor)+2];
marcodesilva 17:6f85605f793d 472 char Status[(11+bytes)*_Nmotor];
marcodesilva 17:6f85605f793d 473 int TxBufSize = sizeof(TxBuf);
marcodesilva 17:6f85605f793d 474 char* data;
marcodesilva 17:6f85605f793d 475 Status[8] = 0x00; // The error is set to zero
marcodesilva 17:6f85605f793d 476
marcodesilva 17:6f85605f793d 477 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 478 //Header
marcodesilva 17:6f85605f793d 479 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 480 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 481 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 482 // Reserved
marcodesilva 17:6f85605f793d 483 TxBuf[3] = 0x00;
marcodesilva 17:6f85605f793d 484 // ID
marcodesilva 17:6f85605f793d 485 TxBuf[4] = 0xfe; // Broadcast
marcodesilva 17:6f85605f793d 486 // Lenght 7 bytes = inst(1)+ param(4 + numbers of motors)+CRC(2)
marcodesilva 17:6f85605f793d 487 TxBuf[5] = 7+(_Nmotor) & 0xff; // packet length low
marcodesilva 17:6f85605f793d 488 TxBuf[6] = 7+(_Nmotor) >> 8; // packet length high
marcodesilva 17:6f85605f793d 489 // Instruction
marcodesilva 17:6f85605f793d 490 TxBuf[7] = 0x82;
marcodesilva 17:6f85605f793d 491 // Param
marcodesilva 17:6f85605f793d 492 TxBuf[8] = start & 0xFF; // Address
marcodesilva 17:6f85605f793d 493 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 494 TxBuf[10] = bytes & 0xFF;; // Lenght of reply
marcodesilva 17:6f85605f793d 495 TxBuf[11] = bytes >> 8;
marcodesilva 17:6f85605f793d 496 for (int i=0; i<_Nmotor ;i++){
marcodesilva 17:6f85605f793d 497 TxBuf[12+i] = _broadcastID[i];
marcodesilva 17:6f85605f793d 498 }
marcodesilva 17:6f85605f793d 499 // CRC
marcodesilva 17:6f85605f793d 500 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 501 int dataSize = TxBufSize-2;
marcodesilva 17:6f85605f793d 502 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 503 TxBuf[TxBufSize-2] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 504 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 505
marcodesilva 17:6f85605f793d 506 #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 507 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
marcodesilva 17:6f85605f793d 508 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 509 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
marcodesilva 17:6f85605f793d 510 }
marcodesilva 17:6f85605f793d 511 printf("\n SYNC READ PACKET \n");
marcodesilva 17:6f85605f793d 512 for (int i=0; i<TxBufSize ; i++){
marcodesilva 17:6f85605f793d 513 printf("|%02x",TxBuf[i]);
marcodesilva 17:6f85605f793d 514 }
marcodesilva 17:6f85605f793d 515 printf("\n \n");
marcodesilva 17:6f85605f793d 516 #endif
marcodesilva 17:6f85605f793d 517
marcodesilva 17:6f85605f793d 518 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 519 _pCommLayer->write(TxBuf,TxBufSize); // UART-write
marcodesilva 17:6f85605f793d 520 wait(0.0002);
marcodesilva 17:6f85605f793d 521 _pCommLayer->read_timeout(Status, (11+bytes)*_Nmotor, 2.0);
marcodesilva 17:6f85605f793d 522
marcodesilva 17:6f85605f793d 523 // Take data from 8th cell of status array
marcodesilva 17:6f85605f793d 524 // first byte contain error code, for this we use (bytes+1)
marcodesilva 17:6f85605f793d 525 for (int i=0; i<(11+bytes)*_Nmotor ; i++){
marcodesilva 17:6f85605f793d 526 data[i] = Status[i];
marcodesilva 17:6f85605f793d 527 }
marcodesilva 17:6f85605f793d 528
marcodesilva 17:6f85605f793d 529 #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
marcodesilva 17:6f85605f793d 530 printf("\n SYNC-READ (entire buffer RAW values)\n");
marcodesilva 17:6f85605f793d 531 for (int i=0; i<(11+bytes)*_Nmotor ; i++){
marcodesilva 17:6f85605f793d 532 data[i] = Status[i];
marcodesilva 17:6f85605f793d 533 printf("data[%d]:%02x\n",i,data[i]);
marcodesilva 17:6f85605f793d 534 }
marcodesilva 17:6f85605f793d 535 #endif
marcodesilva 17:6f85605f793d 536
marcodesilva 17:6f85605f793d 537 return(data); // return error code
marcodesilva 17:6f85605f793d 538 }
marcodesilva 17:6f85605f793d 539
marcodesilva 17:6f85605f793d 540
marcodesilva 17:6f85605f793d 541 ///////////////////////////////////////////////////////////////////////////////
marcodesilva 17:6f85605f793d 542 ////////////////////////SINGLE OPERATIONS//////////////////////////////////////
marcodesilva 17:6f85605f793d 543 ///////////////////////////////////////////////////////////////////////////////
marcodesilva 17:6f85605f793d 544
marcodesilva 17:6f85605f793d 545
marcodesilva 17:6f85605f793d 546 int MX::SetGoalSpeed(int speed, int flags) {
marcodesilva 17:6f85605f793d 547
marcodesilva 17:6f85605f793d 548 char reg_flag = 0;
marcodesilva 17:6f85605f793d 549 char data[4];
marcodesilva 17:6f85605f793d 550
marcodesilva 17:6f85605f793d 551 // set the flag is only the register bit is set in the flag
marcodesilva 17:6f85605f793d 552 if (flags == 0x2) {
marcodesilva 17:6f85605f793d 553 reg_flag = 1;
marcodesilva 17:6f85605f793d 554 }
marcodesilva 17:6f85605f793d 555
marcodesilva 17:6f85605f793d 556 int goal = (speed) ;
marcodesilva 17:6f85605f793d 557
marcodesilva 17:6f85605f793d 558 data[0] = goal & 0xff; // bottom 8 bits
marcodesilva 17:6f85605f793d 559 data[1] = goal >> 8 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 560 data[2] = goal >> 16 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 561 data[3] = goal >> 24 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 562
marcodesilva 17:6f85605f793d 563 // write the packet, return the error code
marcodesilva 17:6f85605f793d 564 int rVal = sendPacket( MX_REG_GOAL_VELOCITY, 4, data, reg_flag);
marcodesilva 17:6f85605f793d 565
marcodesilva 17:6f85605f793d 566 if (flags == 1) {
marcodesilva 17:6f85605f793d 567 // block until it comes to a halt
marcodesilva 17:6f85605f793d 568 while (isMoving()) {}
marcodesilva 17:6f85605f793d 569 }
marcodesilva 17:6f85605f793d 570
marcodesilva 17:6f85605f793d 571 #ifdef SETGOAL_SPEED_DEBUG
marcodesilva 17:6f85605f793d 572 int16_t sp = speed * float(234.27/1023); //rev/min
marcodesilva 17:6f85605f793d 573 printf("\nSetGoal to: %d Velocity in [rev/min]%d \n" ,speed,sp);
marcodesilva 17:6f85605f793d 574 printf("Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
marcodesilva 17:6f85605f793d 575 #endif
marcodesilva 17:6f85605f793d 576
marcodesilva 17:6f85605f793d 577 return(rVal);
marcodesilva 17:6f85605f793d 578 }
marcodesilva 17:6f85605f793d 579
marcodesilva 17:6f85605f793d 580
marcodesilva 17:6f85605f793d 581 int MX::SetGoal(float degrees, int flags) {
marcodesilva 17:6f85605f793d 582
marcodesilva 17:6f85605f793d 583 char reg_flag = 0;
marcodesilva 17:6f85605f793d 584 char data[4];
marcodesilva 17:6f85605f793d 585
marcodesilva 17:6f85605f793d 586 // set the flag is only the register bit is set in the flag
marcodesilva 17:6f85605f793d 587 if (flags == 0x2) {
marcodesilva 17:6f85605f793d 588 reg_flag = 1;
marcodesilva 17:6f85605f793d 589 }
marcodesilva 17:6f85605f793d 590
marcodesilva 17:6f85605f793d 591 int goal = (degrees) * (float)(4095/360);
marcodesilva 17:6f85605f793d 592
marcodesilva 17:6f85605f793d 593 data[0] = goal & 0xff; // bottom 8 bits
marcodesilva 17:6f85605f793d 594 data[1] = goal >> 8 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 595 data[2] = goal >> 16 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 596 data[3] = goal >> 24 & 0xff; // top 8 bits
marcodesilva 17:6f85605f793d 597
marcodesilva 17:6f85605f793d 598 // write the packet, return the error code
marcodesilva 17:6f85605f793d 599 int rVal = sendPacket( MX_REG_GOAL_POSITION, 4, data, reg_flag );
marcodesilva 17:6f85605f793d 600
marcodesilva 17:6f85605f793d 601 if (flags == 1) {
marcodesilva 17:6f85605f793d 602 // block until it comes to a halt
marcodesilva 17:6f85605f793d 603 while (isMoving()) {}
marcodesilva 17:6f85605f793d 604 }
marcodesilva 17:6f85605f793d 605
marcodesilva 17:6f85605f793d 606 #ifdef SETGOAL_DEBUG
marcodesilva 17:6f85605f793d 607 printf("\n SetGoal to: %d in degree is: %f\n",goal,degrees);
marcodesilva 17:6f85605f793d 608 printf(" Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
marcodesilva 17:6f85605f793d 609 #endif
marcodesilva 17:6f85605f793d 610 return(rVal);
marcodesilva 17:6f85605f793d 611 }
marcodesilva 17:6f85605f793d 612
marcodesilva 17:6f85605f793d 613 // if flag[0] is set, were blocking
marcodesilva 17:6f85605f793d 614 // if flag[1] is set, we're registering
marcodesilva 17:6f85605f793d 615 // they are mutually exclusive operations
marcodesilva 17:6f85605f793d 616
marcodesilva 17:6f85605f793d 617 int MX::OperatingMode(int mode) {
marcodesilva 17:6f85605f793d 618
marcodesilva 17:6f85605f793d 619 char data[1];
marcodesilva 17:6f85605f793d 620 data[0] = mode; // bottom 8 bits
marcodesilva 17:6f85605f793d 621
marcodesilva 17:6f85605f793d 622 #ifdef OPERATING_MODE_DEBUG
marcodesilva 17:6f85605f793d 623 printf("\nOPERATING_MODE_DEBUG\n");
marcodesilva 17:6f85605f793d 624 printf("Set Operating Mode value to: 0x%02x\n",mode);
marcodesilva 17:6f85605f793d 625 printf( "* 0x00 = Current Control Mode\n"
marcodesilva 17:6f85605f793d 626 "* 0x01 = Velocity Control Mode\n"
marcodesilva 17:6f85605f793d 627 "* 0x03 = Position Control Mode (DEFAULT)\n"
marcodesilva 17:6f85605f793d 628 "* 0x04 = Extended Position Control Mode(Multi-turn)\n"
marcodesilva 17:6f85605f793d 629 "* 0x05 = Current-based Position Control Mode\n"
marcodesilva 17:6f85605f793d 630 "* 0x16 = PWM Control Mode (Voltage Control Mode)\n");
marcodesilva 17:6f85605f793d 631 #endif
marcodesilva 17:6f85605f793d 632
marcodesilva 17:6f85605f793d 633 //sendPacket(Motor ID, Address, Lenght, data);
marcodesilva 17:6f85605f793d 634 //return give back a error code of sendPacket function
marcodesilva 17:6f85605f793d 635 return (sendPacket( MX_REG_OPERATING_MODE, 1, data));
marcodesilva 17:6f85605f793d 636 }
marcodesilva 17:6f85605f793d 637
marcodesilva 17:6f85605f793d 638
marcodesilva 17:6f85605f793d 639 // return 1 is the servo is still in flight
marcodesilva 17:6f85605f793d 640 bool MX::isMoving(void) {
marcodesilva 17:6f85605f793d 641
marcodesilva 17:6f85605f793d 642 bool data[1];
marcodesilva 17:6f85605f793d 643 data[0] = 1;
marcodesilva 17:6f85605f793d 644 data[0] = readPacket( MX_REG_MOVING,1 );
marcodesilva 17:6f85605f793d 645 return(data[0]);
marcodesilva 17:6f85605f793d 646 }
marcodesilva 17:6f85605f793d 647
marcodesilva 17:6f85605f793d 648
marcodesilva 17:6f85605f793d 649 int MX::motorSetBaud (int MotorBaud) {
marcodesilva 17:6f85605f793d 650
marcodesilva 17:6f85605f793d 651 char data[1];
marcodesilva 17:6f85605f793d 652 data[0] = MotorBaud;
marcodesilva 17:6f85605f793d 653
marcodesilva 17:6f85605f793d 654 #ifdef SETBAUD_DEBUG
marcodesilva 17:6f85605f793d 655 printf("\nSTATUS Packet - SETBAUD_DEBUG\n");
marcodesilva 17:6f85605f793d 656 printf("Set Baud rate value to: 0x%02x\n",MotorBaud);
marcodesilva 17:6f85605f793d 657 printf( "* 0x00 = 9,600 bps \n"
marcodesilva 17:6f85605f793d 658 "* 0x01 = 57,600 bps (DEFAULT)\n"
marcodesilva 17:6f85605f793d 659 "* 0x02 = 115,200 bps \n"
marcodesilva 17:6f85605f793d 660 "* 0x03 = 1 Mbps \n"
marcodesilva 17:6f85605f793d 661 "* 0x04 = 2 Mbps \n"
marcodesilva 17:6f85605f793d 662 "* 0x05 = 3 Mbps \n"
marcodesilva 17:6f85605f793d 663 "* 0x06 = 4 Mbps \n"
marcodesilva 17:6f85605f793d 664 "* 0x06 = 4,5 Mbps \n");
marcodesilva 17:6f85605f793d 665 #endif
marcodesilva 17:6f85605f793d 666
marcodesilva 17:6f85605f793d 667 //sendPacket(Motor ID, Address, Lenght, data);
marcodesilva 17:6f85605f793d 668 //return give back a error code of sendPacket function
marcodesilva 17:6f85605f793d 669 return (sendPacket( MX_REG_BAUD, 1, data));
marcodesilva 17:6f85605f793d 670 }
marcodesilva 17:6f85605f793d 671
marcodesilva 17:6f85605f793d 672 void MX::Reboot() {
marcodesilva 17:6f85605f793d 673
marcodesilva 17:6f85605f793d 674 char TxBuf[10];
marcodesilva 17:6f85605f793d 675 char Status[11];
marcodesilva 17:6f85605f793d 676 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 677
marcodesilva 17:6f85605f793d 678 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 679 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 680 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 681 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 682 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 683 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 684 // packet Length
marcodesilva 17:6f85605f793d 685 TxBuf[5] = 0x03; //packet length low
marcodesilva 17:6f85605f793d 686 TxBuf[6] = 0x00; //packet length high
marcodesilva 17:6f85605f793d 687 // Instruction
marcodesilva 17:6f85605f793d 688 TxBuf[7] = 0x08; // Instruction to reboot Device
marcodesilva 17:6f85605f793d 689 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 690 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 691 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 692 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/);
marcodesilva 17:6f85605f793d 693 TxBuf[8] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 694 TxBuf[9] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 695 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 696 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 697 _pCommLayer->write(TxBuf,10); // UART-write
marcodesilva 17:6f85605f793d 698 _pCommLayer->read_timeout(Status, 11, 2.0);
marcodesilva 17:6f85605f793d 699
marcodesilva 17:6f85605f793d 700 #ifdef REBOOT_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 701 printf("\n Reboot Motor: (%d)",enableVal);
marcodesilva 17:6f85605f793d 702 #endif
marcodesilva 17:6f85605f793d 703 }
marcodesilva 17:6f85605f793d 704
marcodesilva 17:6f85605f793d 705
marcodesilva 17:6f85605f793d 706 int MX::TorqueEnable(bool enableVal) {
marcodesilva 17:6f85605f793d 707
marcodesilva 17:6f85605f793d 708 char data[1];
marcodesilva 17:6f85605f793d 709 data[0] = enableVal;
marcodesilva 17:6f85605f793d 710
marcodesilva 17:6f85605f793d 711 #ifdef TORQUE_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 712 printf("\n Setting Torque Enable value: (%d)",enableVal);
marcodesilva 17:6f85605f793d 713 #endif
marcodesilva 17:6f85605f793d 714
marcodesilva 17:6f85605f793d 715 return (sendPacket( MX_REG_TORQUE_ENABLE, 1, data));
marcodesilva 17:6f85605f793d 716 }
marcodesilva 17:6f85605f793d 717
marcodesilva 17:6f85605f793d 718
marcodesilva 17:6f85605f793d 719 float MX::GetPosition(void) {
marcodesilva 17:6f85605f793d 720 printf("\nGET POSITION ");
marcodesilva 17:6f85605f793d 721 char* data;
marcodesilva 17:6f85605f793d 722 int32_t position;
marcodesilva 17:6f85605f793d 723 float angle = 0;
marcodesilva 17:6f85605f793d 724 float ScaleFactor = (float)360/4095;
marcodesilva 17:6f85605f793d 725
marcodesilva 17:6f85605f793d 726 data = readPacket( MX_REG_PRESENT_POSITION, 4);
marcodesilva 17:6f85605f793d 727 position = (uint32_t)data[3] << 24;
marcodesilva 17:6f85605f793d 728 position |= (uint32_t)data[2] << 16;
marcodesilva 17:6f85605f793d 729 position |= (uint32_t)data[1] << 8;
marcodesilva 17:6f85605f793d 730 position |= (uint32_t)data[0];
marcodesilva 17:6f85605f793d 731 // angle(degree) obtained from position(0 - 4095)
marcodesilva 17:6f85605f793d 732 angle = (float)position*ScaleFactor;
marcodesilva 17:6f85605f793d 733
marcodesilva 17:6f85605f793d 734 #ifdef GETPOSITION_DEBUG
marcodesilva 17:6f85605f793d 735 printf("\nGetPosition from ID: %d\n",_ID);
marcodesilva 17:6f85605f793d 736 for (uint16_t i=0; i<4 ; i++) {
marcodesilva 17:6f85605f793d 737 printf(" Data[%d] : 0x%02x\n",(i+1),data[i]);
marcodesilva 17:6f85605f793d 738 }
marcodesilva 17:6f85605f793d 739 printf(" Read position (0 - 4095): %d\n",position);
marcodesilva 17:6f85605f793d 740 printf(" Converted angle %f\n",angle);
marcodesilva 17:6f85605f793d 741 #endif
marcodesilva 17:6f85605f793d 742 return (angle);
marcodesilva 17:6f85605f793d 743 }
marcodesilva 17:6f85605f793d 744
marcodesilva 17:6f85605f793d 745
marcodesilva 17:6f85605f793d 746 int MX::sendPacket( int start, int bytes, char* data, int flag) {
marcodesilva 17:6f85605f793d 747
marcodesilva 17:6f85605f793d 748 char TxBuf[12+bytes];
marcodesilva 17:6f85605f793d 749 char Status[11];
marcodesilva 17:6f85605f793d 750 Status[8]=0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 751
marcodesilva 17:6f85605f793d 752 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 753 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 754 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 755 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 756 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 757 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 758 // packet Length
marcodesilva 17:6f85605f793d 759 TxBuf[5] = (5+bytes) & 0xff; //packet length low
marcodesilva 17:6f85605f793d 760 TxBuf[6] = (5+bytes) >> 8; //packet length high
marcodesilva 17:6f85605f793d 761 // Instruction
marcodesilva 17:6f85605f793d 762 if (flag == 1) {
marcodesilva 17:6f85605f793d 763 TxBuf[7]=0x04;
marcodesilva 17:6f85605f793d 764 } else {
marcodesilva 17:6f85605f793d 765 TxBuf[7]=0x03;
marcodesilva 17:6f85605f793d 766 }
marcodesilva 17:6f85605f793d 767 // Start Address
marcodesilva 17:6f85605f793d 768 TxBuf[8] = start & 0xFF;
marcodesilva 17:6f85605f793d 769 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 770 // data
marcodesilva 17:6f85605f793d 771 for (int i = bytes-1; i>=0 ; i--) { //little endian
marcodesilva 17:6f85605f793d 772 TxBuf[10+i] = data[i];
marcodesilva 17:6f85605f793d 773 }
marcodesilva 17:6f85605f793d 774 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 775 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 776 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 777 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/);
marcodesilva 17:6f85605f793d 778
marcodesilva 17:6f85605f793d 779 TxBuf[10+bytes] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 780 TxBuf[11+bytes] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 781 // Build the TxPacket first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 782 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 783 _pCommLayer->write(TxBuf,bytes+12); // UART-write
marcodesilva 17:6f85605f793d 784 _pCommLayer->read_timeout(Status, 11, 2.0);
marcodesilva 17:6f85605f793d 785
marcodesilva 17:6f85605f793d 786
marcodesilva 17:6f85605f793d 787 #ifdef SENDPACKET_DEBUG
marcodesilva 17:6f85605f793d 788 printf("\nWRITE input parameters:\n");
marcodesilva 17:6f85605f793d 789 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
marcodesilva 17:6f85605f793d 790 printf(" Data written:\n");
marcodesilva 17:6f85605f793d 791 for (int i=0; i < bytes ; i++) {
marcodesilva 17:6f85605f793d 792 printf(" Data[%d]: 0x%x\n",i,data[i]);
marcodesilva 17:6f85605f793d 793 }
marcodesilva 17:6f85605f793d 794 #endif
marcodesilva 17:6f85605f793d 795
marcodesilva 17:6f85605f793d 796 #ifdef SENDPACKET_DEBUG_INSTRUCTION_PKT
marcodesilva 17:6f85605f793d 797 printf("\nINSTRUCTIONS Packet - WRITE_DEBUG \n");
marcodesilva 17:6f85605f793d 798 printf(" H1 : 0x%02x\n",TxBuf[0]);
marcodesilva 17:6f85605f793d 799 printf(" H2 : 0x%02x\n",TxBuf[1]);
marcodesilva 17:6f85605f793d 800 printf(" H3 : 0x%02x\n",TxBuf[2]);
marcodesilva 17:6f85605f793d 801 printf(" Reserved : 0x%02x\n",TxBuf[3]);
marcodesilva 17:6f85605f793d 802 printf(" ID : 0x%02x\n",TxBuf[4]);
marcodesilva 17:6f85605f793d 803 printf(" Length L : 0x%02x\n",TxBuf[5]);
marcodesilva 17:6f85605f793d 804 printf(" Length H : 0x%02x\n",TxBuf[6]);
marcodesilva 17:6f85605f793d 805 printf(" Instruction : 0x%02x\n",TxBuf[7]);
marcodesilva 17:6f85605f793d 806 printf(" Cmd L : 0x%02x\n",TxBuf[8]);
marcodesilva 17:6f85605f793d 807 printf(" Cmd H : 0x%02x\n",TxBuf[9]);
marcodesilva 17:6f85605f793d 808 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 809 printf(" Data : 0x%02x\n",TxBuf[10+i]);
marcodesilva 17:6f85605f793d 810 }
marcodesilva 17:6f85605f793d 811 printf(" CRC1 : 0x%02x\n",TxBuf[10+bytes]);
marcodesilva 17:6f85605f793d 812 printf(" CRC2 : 0x%02x\n",TxBuf[11+bytes]);
marcodesilva 17:6f85605f793d 813 printf(" TxBuf = ");
marcodesilva 17:6f85605f793d 814 for (int i = 0; i < sizeof(TxBuf) ; i++) {
marcodesilva 17:6f85605f793d 815 printf(" [0x%02x]",TxBuf[i]);
marcodesilva 17:6f85605f793d 816 }
marcodesilva 17:6f85605f793d 817 printf("\n");
marcodesilva 17:6f85605f793d 818 #endif
marcodesilva 17:6f85605f793d 819
marcodesilva 17:6f85605f793d 820 #ifdef SENDPACKET_DEBUG_STATUS_PKT
marcodesilva 17:6f85605f793d 821 printf("\nSTATUS Packet - WRITE_DEBUG\n");
marcodesilva 17:6f85605f793d 822 printf(" H1 : 0x%02x\n",Status[0]);
marcodesilva 17:6f85605f793d 823 printf(" H2 : 0x%02x\n",Status[1]);
marcodesilva 17:6f85605f793d 824 printf(" H3 : 0x%02x\n",Status[2]);
marcodesilva 17:6f85605f793d 825 printf(" Reserved : 0x%02x\n",Status[3]);
marcodesilva 17:6f85605f793d 826 printf(" ID : 0x%02x\n",Status[4]);
marcodesilva 17:6f85605f793d 827 printf(" Length L : 0x%02x\n",Status[5]);
marcodesilva 17:6f85605f793d 828 printf(" Length H : 0x%02x\n",Status[6]);
marcodesilva 17:6f85605f793d 829 printf(" Instruction : 0x%02x\n",Status[7]);
marcodesilva 17:6f85605f793d 830 printf(" Error : 0x%02x\n",Status[8]);
marcodesilva 17:6f85605f793d 831 printf(" CRC1 : 0x%02x\n",Status[9]);
marcodesilva 17:6f85605f793d 832 printf(" CRC2 : 0x%02x\n",Status[10]);
marcodesilva 17:6f85605f793d 833 printf(" Status = ");
marcodesilva 17:6f85605f793d 834 for (int i = 0; i < sizeof(Status) ; i++) {
marcodesilva 17:6f85605f793d 835 printf(" [0x%02x]",Status[i]);
marcodesilva 17:6f85605f793d 836 }
marcodesilva 17:6f85605f793d 837 printf("\n");
marcodesilva 17:6f85605f793d 838 #endif
marcodesilva 17:6f85605f793d 839
marcodesilva 17:6f85605f793d 840 return(Status[8]); // return error code
marcodesilva 17:6f85605f793d 841 }
marcodesilva 17:6f85605f793d 842
marcodesilva 17:6f85605f793d 843
marcodesilva 17:6f85605f793d 844
marcodesilva 17:6f85605f793d 845 char* MX::readPacket(int start, int bytes, int flag) {
marcodesilva 17:6f85605f793d 846
marcodesilva 17:6f85605f793d 847 char TxBuf[14];
marcodesilva 17:6f85605f793d 848 char Status[11+bytes];
marcodesilva 17:6f85605f793d 849 char* data;
marcodesilva 17:6f85605f793d 850 Status[8] = 0x00; //The error is set to zero
marcodesilva 17:6f85605f793d 851
marcodesilva 17:6f85605f793d 852 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go
marcodesilva 17:6f85605f793d 853 TxBuf[0] = 0xff; //H1
marcodesilva 17:6f85605f793d 854 TxBuf[1] = 0xff; //H2
marcodesilva 17:6f85605f793d 855 TxBuf[2] = 0xfd; //H3
marcodesilva 17:6f85605f793d 856 TxBuf[3] = 0x00; //Reserved
marcodesilva 17:6f85605f793d 857 TxBuf[4] = _ID; //ID
marcodesilva 17:6f85605f793d 858 TxBuf[5] = 0x07; //packet length low
marcodesilva 17:6f85605f793d 859 TxBuf[6] = 0x00; //packet length high
marcodesilva 17:6f85605f793d 860 TxBuf[7] = 0x02; // Instruction
marcodesilva 17:6f85605f793d 861 TxBuf[8] = start & 0xFF; // Start Address
marcodesilva 17:6f85605f793d 862 TxBuf[9] = start >> 8;
marcodesilva 17:6f85605f793d 863 // N param
marcodesilva 17:6f85605f793d 864 TxBuf[10] = 0x04;
marcodesilva 17:6f85605f793d 865 TxBuf[11] = 0x00;
marcodesilva 17:6f85605f793d 866 /*****************CRC***********************/
marcodesilva 17:6f85605f793d 867 uint16_t crc16 ;
marcodesilva 17:6f85605f793d 868 int dataSize = sizeof(TxBuf)-2;
marcodesilva 17:6f85605f793d 869 crc16 = update_crc(0, TxBuf, dataSize);
marcodesilva 17:6f85605f793d 870 TxBuf[12] = (crc16 & 0x00FF);
marcodesilva 17:6f85605f793d 871 TxBuf[13] = (crc16>>8) & 0x00FF;
marcodesilva 17:6f85605f793d 872
marcodesilva 17:6f85605f793d 873 _pCommLayer->flush();
marcodesilva 17:6f85605f793d 874 _pCommLayer->write(TxBuf,sizeof(TxBuf)); // UART-write
marcodesilva 17:6f85605f793d 875 wait(0.0002);
marcodesilva 17:6f85605f793d 876 _pCommLayer->read_timeout(Status, 11+bytes, 2.0);
marcodesilva 17:6f85605f793d 877
marcodesilva 17:6f85605f793d 878
marcodesilva 17:6f85605f793d 879 #ifdef MX_READPACKET_DEBUG
marcodesilva 17:6f85605f793d 880 printf("\nREAD input parameters:\n");
marcodesilva 17:6f85605f793d 881 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
marcodesilva 17:6f85605f793d 882 printf(" Data written:\n");
marcodesilva 17:6f85605f793d 883 for (int i=0; i < bytes ; i++) {
marcodesilva 17:6f85605f793d 884 printf(" Data[%d]: 0x%x\n",i,data[i]);
marcodesilva 17:6f85605f793d 885 }
marcodesilva 17:6f85605f793d 886 #endif
marcodesilva 17:6f85605f793d 887
marcodesilva 17:6f85605f793d 888 #ifdef MX_READPACKET_DEBUG_INSTRUCTION_PKT
marcodesilva 17:6f85605f793d 889 printf("\nINSTRUCTIONS Packet - READ_DEBUG \n");
marcodesilva 17:6f85605f793d 890 printf(" H1 - Header : 0x%02x\n",TxBuf[0]);
marcodesilva 17:6f85605f793d 891 printf(" H2 - Header : 0x%02x\n",TxBuf[1]);
marcodesilva 17:6f85605f793d 892 printf(" H3 - Header : 0x%02x\n",TxBuf[2]);
marcodesilva 17:6f85605f793d 893 printf(" RESERVED : 0x%02x\n",TxBuf[3]);
marcodesilva 17:6f85605f793d 894 printf(" ID : 0x%02x\n",TxBuf[4]);
marcodesilva 17:6f85605f793d 895 printf(" Length L : 0x%02x\n",TxBuf[5]);
marcodesilva 17:6f85605f793d 896 printf(" Length H : 0x%02x\n",TxBuf[6]);
marcodesilva 17:6f85605f793d 897 printf(" Instruction : 0x%02x\n",TxBuf[7]);
marcodesilva 17:6f85605f793d 898 printf(" Param1 - Cmd L : 0x%02x\n",TxBuf[8]);
marcodesilva 17:6f85605f793d 899 printf(" Param2 - Cmd H : 0x%02x\n",TxBuf[9]);
marcodesilva 17:6f85605f793d 900 printf(" Param3 : 0x%02x\n",TxBuf[10]);
marcodesilva 17:6f85605f793d 901 printf(" Param4 : 0x%02x\n",TxBuf[11]);
marcodesilva 17:6f85605f793d 902 printf(" CRC1 : 0x%02x\n",TxBuf[12]);
marcodesilva 17:6f85605f793d 903 printf(" CRC2 : 0x%02x\n",TxBuf[13]);
marcodesilva 17:6f85605f793d 904 printf(" TxBuf = ");
marcodesilva 17:6f85605f793d 905 for (int i = 0; i < 14 ; i++) {
marcodesilva 17:6f85605f793d 906 printf(" [0x%02x]",TxBuf[i]);
marcodesilva 17:6f85605f793d 907 }
marcodesilva 17:6f85605f793d 908 printf("\n\n");
marcodesilva 17:6f85605f793d 909 #endif
marcodesilva 17:6f85605f793d 910
marcodesilva 17:6f85605f793d 911 #ifdef MX_READPACKET_DEBUG_STATUS_PKT
marcodesilva 17:6f85605f793d 912 printf("\nSTATUS Packet - READ_DEBUG \n");
marcodesilva 17:6f85605f793d 913 printf(" H1 - Header : 0x%02x\n",Status[0]);
marcodesilva 17:6f85605f793d 914 printf(" H2 - Header : 0x%02x\n",Status[1]);
marcodesilva 17:6f85605f793d 915 printf(" H3 - Header : 0x%02x\n",Status[2]);
marcodesilva 17:6f85605f793d 916 printf(" Reserved : 0x%02x\n",Status[3]);
marcodesilva 17:6f85605f793d 917 printf(" ID : 0x%02x\n",Status[4]);
marcodesilva 17:6f85605f793d 918 printf(" Length L : 0x%02x\n",Status[5]);
marcodesilva 17:6f85605f793d 919 printf(" Length H : 0x%02x\n",Status[6]);
marcodesilva 17:6f85605f793d 920 printf(" Instruction : 0x%02x\n",Status[7]);
marcodesilva 17:6f85605f793d 921 printf(" Error : 0x%02x\n",Status[8]);
marcodesilva 17:6f85605f793d 922 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 923 printf(" Param%d : 0x%02x\n",(i+1),Status[9+i]);
marcodesilva 17:6f85605f793d 924 }
marcodesilva 17:6f85605f793d 925 printf(" Crc1 : 0x%02x\n",Status[13]);
marcodesilva 17:6f85605f793d 926 printf(" Crc2 : 0x%02x\n",Status[14]);
marcodesilva 17:6f85605f793d 927 printf(" Status = ");
marcodesilva 17:6f85605f793d 928 for (int i = 0; i < sizeof(Status) ; i++) {
marcodesilva 17:6f85605f793d 929 printf(" [0x%02x]",Status[i]);
marcodesilva 17:6f85605f793d 930 }
marcodesilva 17:6f85605f793d 931 printf("\n");
marcodesilva 17:6f85605f793d 932 #endif
marcodesilva 17:6f85605f793d 933 for (uint16_t i=0; i<bytes ; i++) {
marcodesilva 17:6f85605f793d 934 data[i] = Status[9+i];
marcodesilva 17:6f85605f793d 935 }
marcodesilva 17:6f85605f793d 936 return(data); // return error code
marcodesilva 17:6f85605f793d 937 }
marcodesilva 17:6f85605f793d 938
marcodesilva 17:6f85605f793d 939 unsigned short MX::update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size)
marcodesilva 17:6f85605f793d 940 {
marcodesilva 17:6f85605f793d 941 unsigned short i, j;
marcodesilva 17:6f85605f793d 942 unsigned short crc_table[256] = {
marcodesilva 17:6f85605f793d 943 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
marcodesilva 17:6f85605f793d 944 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
marcodesilva 17:6f85605f793d 945 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
marcodesilva 17:6f85605f793d 946 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
marcodesilva 17:6f85605f793d 947 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
marcodesilva 17:6f85605f793d 948 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
marcodesilva 17:6f85605f793d 949 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
marcodesilva 17:6f85605f793d 950 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
marcodesilva 17:6f85605f793d 951 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
marcodesilva 17:6f85605f793d 952 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
marcodesilva 17:6f85605f793d 953 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
marcodesilva 17:6f85605f793d 954 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
marcodesilva 17:6f85605f793d 955 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
marcodesilva 17:6f85605f793d 956 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
marcodesilva 17:6f85605f793d 957 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
marcodesilva 17:6f85605f793d 958 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
marcodesilva 17:6f85605f793d 959 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
marcodesilva 17:6f85605f793d 960 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
marcodesilva 17:6f85605f793d 961 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
marcodesilva 17:6f85605f793d 962 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
marcodesilva 17:6f85605f793d 963 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
marcodesilva 17:6f85605f793d 964 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
marcodesilva 17:6f85605f793d 965 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
marcodesilva 17:6f85605f793d 966 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
marcodesilva 17:6f85605f793d 967 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
marcodesilva 17:6f85605f793d 968 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
marcodesilva 17:6f85605f793d 969 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
marcodesilva 17:6f85605f793d 970 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
marcodesilva 17:6f85605f793d 971 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
marcodesilva 17:6f85605f793d 972 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
marcodesilva 17:6f85605f793d 973 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
marcodesilva 17:6f85605f793d 974 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
marcodesilva 17:6f85605f793d 975 };
marcodesilva 17:6f85605f793d 976 for(j = 0; j < data_blk_size; j++)
marcodesilva 17:6f85605f793d 977 {
marcodesilva 17:6f85605f793d 978 i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
marcodesilva 17:6f85605f793d 979 crc_accum = (crc_accum << 8) ^ crc_table[i];
marcodesilva 17:6f85605f793d 980 }
marcodesilva 17:6f85605f793d 981 return crc_accum;
marcodesilva 17:6f85605f793d 982 }