PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.cpp@21:fe5dd48bebc6, 2019-11-05 (annotated)

Committer:
anfontanelli
Date:
Tue Nov 05 08:57:38 2019 +0000
Revision:
21:fe5dd48bebc6
Parent:
20:029e0fe30b0a
Child:
22:5d3f37356915
mbedos senza lettura corrente

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