PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.cpp@20:029e0fe30b0a, 2019-10-22 (annotated)

Committer:
marcodesilva
Date:
Tue Oct 22 16:42:40 2019 +0000
Revision:
20:029e0fe30b0a
Parent:
19:16d5f8a3311f
Child:
21:fe5dd48bebc6
get Current funzionante

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