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?
User | Revision | Line number | New 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 | } |