PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.cpp@22:5d3f37356915, 2019-11-05 (annotated)

Committer:
marcodesilva
Date:
Tue Nov 05 14:26:34 2019 +0000
Revision:
22:5d3f37356915
Parent:
21:fe5dd48bebc6 
con Corrente e Settings Profili di Acc e Vel

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