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