PSL_2021
/
servomotor_MX12_Lorenzo
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: PSL_ROBOT_lorenzo robot_lorenzo recepteur_mbed_os_6
MX12.cpp@27:06850c65b9c8, 2021-11-26 (annotated)
- Committer:
- lorenzodunau
- Date:
- Fri Nov 26 08:55:31 2021 +0000
- Revision:
- 27:06850c65b9c8
- Parent:
- 26:4632a02a8ef1
- Child:
- 28:c7402e1014b4
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| denis2nis | 8:e74ef93ae660 | 1 | /** |
| denis2nis | 8:e74ef93ae660 | 2 | * @file MX12.ccp |
| denis2nis | 13:ccda9a56fef1 | 3 | * @brief This file contains all the methods of the MX12 class |
| denis2nis | 13:ccda9a56fef1 | 4 | * whose prototypes are in the MX12.h header file |
| denis2nis | 8:e74ef93ae660 | 5 | */ |
| denis2nis | 8:e74ef93ae660 | 6 | |
| denis2nis | 0:7556356a8bcd | 7 | #include "MX12.h" |
| lorenzodunau | 26:4632a02a8ef1 | 8 | #include "math.h" |
| denis2nis | 0:7556356a8bcd | 9 | |
| denis2nis | 8:e74ef93ae660 | 10 | MX12::MX12(PinName tx, PinName rx, int baud) |
| denis2nis | 8:e74ef93ae660 | 11 | : _mx12(tx, rx) // initializes UnbufferedSerial object |
| denis2nis | 8:e74ef93ae660 | 12 | { |
| denis2nis | 8:e74ef93ae660 | 13 | /* Serial bus setup |
| denis2nis | 8:e74ef93ae660 | 14 | */ |
| denis2nis | 8:e74ef93ae660 | 15 | // // Set desired properties (baud-8-N-1) |
| denis2nis | 8:e74ef93ae660 | 16 | _mx12.baud(baud); /* modulation speed */ |
| denis2nis | 8:e74ef93ae660 | 17 | _mx12.format( |
| denis2nis | 8:e74ef93ae660 | 18 | 8, /* bits */ |
| denis2nis | 8:e74ef93ae660 | 19 | SerialBase::None, /* parity */ |
| denis2nis | 8:e74ef93ae660 | 20 | 1 /* stop bit */ |
| denis2nis | 8:e74ef93ae660 | 21 | ); |
| denis2nis | 8:e74ef93ae660 | 22 | // Register a callback to process a Rx (receive) interrupt. |
| denis2nis | 13:ccda9a56fef1 | 23 | _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq); |
| denis2nis | 5:0cf54586a4be | 24 | |
| denis2nis | 8:e74ef93ae660 | 25 | // variable used for message reception |
| denis2nis | 13:ccda9a56fef1 | 26 | _status_pck = {.raw = "", |
| denis2nis | 13:ccda9a56fef1 | 27 | .n_byte = 0, |
| denis2nis | 13:ccda9a56fef1 | 28 | .servo_id = 0, |
| denis2nis | 13:ccda9a56fef1 | 29 | .length = 0, |
| denis2nis | 13:ccda9a56fef1 | 30 | .error = 0, |
| denis2nis | 13:ccda9a56fef1 | 31 | .n_param = 0, |
| denis2nis | 13:ccda9a56fef1 | 32 | .param = "", |
| denis2nis | 13:ccda9a56fef1 | 33 | .received_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 34 | .calculated_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 35 | .parsed = false, |
| denis2nis | 13:ccda9a56fef1 | 36 | .valid = false}; |
| denis2nis | 13:ccda9a56fef1 | 37 | |
| denis2nis | 13:ccda9a56fef1 | 38 | _parser_state = {.expected_field = PacketField::Header1, |
| denis2nis | 13:ccda9a56fef1 | 39 | .byte_index = 0, |
| denis2nis | 13:ccda9a56fef1 | 40 | .param_index = 0}; |
| denis2nis | 13:ccda9a56fef1 | 41 | |
| denis2nis | 8:e74ef93ae660 | 42 | // Internal defaults states |
| denis2nis | 13:ccda9a56fef1 | 43 | _bus_state = SerialState::Idle; |
| denis2nis | 8:e74ef93ae660 | 44 | |
| denis2nis | 0:7556356a8bcd | 45 | } |
| denis2nis | 0:7556356a8bcd | 46 | |
| denis2nis | 0:7556356a8bcd | 47 | void MX12::SetSpeed(unsigned char mot_id, float speed) { |
| denis2nis | 0:7556356a8bcd | 48 | char data[2]; |
| denis2nis | 8:e74ef93ae660 | 49 | |
| denis2nis | 0:7556356a8bcd | 50 | // Speed absolute value |
| denis2nis | 0:7556356a8bcd | 51 | int goal = (0x3ff * abs(speed)); |
| denis2nis | 0:7556356a8bcd | 52 | |
| denis2nis | 0:7556356a8bcd | 53 | // Spin direction (CW is negative) |
| denis2nis | 0:7556356a8bcd | 54 | if (speed < 0) { |
| denis2nis | 0:7556356a8bcd | 55 | goal |= (0x1 << 10); |
| denis2nis | 0:7556356a8bcd | 56 | } |
| denis2nis | 0:7556356a8bcd | 57 | |
| denis2nis | 0:7556356a8bcd | 58 | data[0] = goal & 0xff; |
| denis2nis | 0:7556356a8bcd | 59 | data[1] = goal >> 8; |
| denis2nis | 0:7556356a8bcd | 60 | |
| denis2nis | 0:7556356a8bcd | 61 | // Send instruction |
| denis2nis | 13:ccda9a56fef1 | 62 | _bus_state = SerialState::Writing; |
| denis2nis | 5:0cf54586a4be | 63 | rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data); |
| denis2nis | 0:7556356a8bcd | 64 | } |
| denis2nis | 0:7556356a8bcd | 65 | |
| bruno2nis | 24:2ab26ed08c83 | 66 | void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed) |
| bruno2nis | 24:2ab26ed08c83 | 67 | { |
| bruno2nis | 24:2ab26ed08c83 | 68 | if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) |
| bruno2nis | 24:2ab26ed08c83 | 69 | { |
| bruno2nis | 24:2ab26ed08c83 | 70 | SetSpeed(mot_id, 1); |
| bruno2nis | 24:2ab26ed08c83 | 71 | } |
| bruno2nis | 24:2ab26ed08c83 | 72 | else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) |
| bruno2nis | 24:2ab26ed08c83 | 73 | { |
| bruno2nis | 24:2ab26ed08c83 | 74 | SetSpeed(mot_id, -1); |
| bruno2nis | 24:2ab26ed08c83 | 75 | } |
| bruno2nis | 24:2ab26ed08c83 | 76 | else |
| bruno2nis | 24:2ab26ed08c83 | 77 | { |
| bruno2nis | 24:2ab26ed08c83 | 78 | SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S); |
| bruno2nis | 24:2ab26ed08c83 | 79 | } |
| bruno2nis | 24:2ab26ed08c83 | 80 | } |
| bruno2nis | 24:2ab26ed08c83 | 81 | |
| denis2nis | 0:7556356a8bcd | 82 | char MX12::IsAvailable(void) { |
| denis2nis | 13:ccda9a56fef1 | 83 | return (_bus_state == SerialState::Idle); |
| denis2nis | 0:7556356a8bcd | 84 | } |
| denis2nis | 0:7556356a8bcd | 85 | |
| denis2nis | 5:0cf54586a4be | 86 | void MX12::rw(unsigned char mot_id, char address, char len, char *data) { |
| denis2nis | 5:0cf54586a4be | 87 | |
| denis2nis | 5:0cf54586a4be | 88 | /* Set variables for reception from servovotor */ |
| denis2nis | 5:0cf54586a4be | 89 | _answer = 0; |
| denis2nis | 13:ccda9a56fef1 | 90 | _status_pck = {.raw = "", |
| denis2nis | 13:ccda9a56fef1 | 91 | .n_byte = 0, |
| denis2nis | 13:ccda9a56fef1 | 92 | .servo_id = 0, |
| denis2nis | 13:ccda9a56fef1 | 93 | .length = 0, |
| denis2nis | 13:ccda9a56fef1 | 94 | .error = 0, |
| denis2nis | 13:ccda9a56fef1 | 95 | .n_param = 0, |
| denis2nis | 13:ccda9a56fef1 | 96 | .param = "", |
| denis2nis | 13:ccda9a56fef1 | 97 | .received_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 98 | .calculated_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 99 | .parsed = false, |
| denis2nis | 13:ccda9a56fef1 | 100 | .valid = false}; |
| denis2nis | 13:ccda9a56fef1 | 101 | _parser_state = {.expected_field = PacketField::Header1, |
| denis2nis | 13:ccda9a56fef1 | 102 | .byte_index = 0, |
| denis2nis | 13:ccda9a56fef1 | 103 | .param_index = 0}; |
| denis2nis | 13:ccda9a56fef1 | 104 | |
| denis2nis | 5:0cf54586a4be | 105 | |
| denis2nis | 5:0cf54586a4be | 106 | /* Initialise instruction packet to forge. |
| denis2nis | 5:0cf54586a4be | 107 | * Instruction Packet is the command data sent to the servomotor. |
| denis2nis | 5:0cf54586a4be | 108 | * |
| denis2nis | 5:0cf54586a4be | 109 | * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum| |
| denis2nis | 5:0cf54586a4be | 110 | * |-------|-------|---------|------|-----------|---------------|--------| |
| denis2nis | 5:0cf54586a4be | 111 | * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM | |
| denis2nis | 5:0cf54586a4be | 112 | * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | | |
| denis2nis | 5:0cf54586a4be | 113 | * \__ ___/ \_ _/ \__ __/ |
| denis2nis | 5:0cf54586a4be | 114 | * \/ | \/ \/ | |
| denis2nis | 5:0cf54586a4be | 115 | * mot_id | address data | |
| denis2nis | 5:0cf54586a4be | 116 | * | (len = N-1) | |
| denis2nis | 5:0cf54586a4be | 117 | * \__________________ ________________/ |
| denis2nis | 5:0cf54586a4be | 118 | * \/ |
| denis2nis | 5:0cf54586a4be | 119 | * Length ( cmd[3] ) |
| denis2nis | 5:0cf54586a4be | 120 | */ |
| denis2nis | 5:0cf54586a4be | 121 | char packet[16]; |
| denis2nis | 5:0cf54586a4be | 122 | unsigned char packet_length; |
| denis2nis | 5:0cf54586a4be | 123 | |
| denis2nis | 5:0cf54586a4be | 124 | /* Initialise checksum to calculate |
| denis2nis | 5:0cf54586a4be | 125 | * It is used to check if packet is damaged during communication. |
| denis2nis | 5:0cf54586a4be | 126 | * Status Checksum is calculated according to the following formula: |
| denis2nis | 5:0cf54586a4be | 127 | * |
| denis2nis | 5:0cf54586a4be | 128 | * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N ) |
| denis2nis | 5:0cf54586a4be | 129 | */ |
| denis2nis | 5:0cf54586a4be | 130 | char checksum = 0x00; |
| denis2nis | 5:0cf54586a4be | 131 | |
| denis2nis | 5:0cf54586a4be | 132 | /* header 1 = 0xFF (dynamixel protocol 1.0) */ |
| denis2nis | 5:0cf54586a4be | 133 | packet[0] = 0xff; |
| denis2nis | 5:0cf54586a4be | 134 | |
| denis2nis | 5:0cf54586a4be | 135 | /* header 2 = 0xFF (dynamixel protocol 1.0) */ |
| denis2nis | 5:0cf54586a4be | 136 | packet[1] = 0xff; |
| denis2nis | 5:0cf54586a4be | 137 | |
| denis2nis | 5:0cf54586a4be | 138 | /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */ |
| denis2nis | 5:0cf54586a4be | 139 | packet[2] = mot_id; |
| denis2nis | 5:0cf54586a4be | 140 | checksum += packet[2]; |
| denis2nis | 5:0cf54586a4be | 141 | |
| denis2nis | 5:0cf54586a4be | 142 | /* Guess instruction type. NULL for read, not NULL for write */ |
| denis2nis | 5:0cf54586a4be | 143 | if(data == NULL) // read instruction |
| denis2nis | 5:0cf54586a4be | 144 | { |
| denis2nis | 5:0cf54586a4be | 145 | /* byte length of the instruction: parameter and checksum field. */ |
| denis2nis | 5:0cf54586a4be | 146 | /* for read instruction: 1 INSTR + */ |
| denis2nis | 5:0cf54586a4be | 147 | /* 2 PARAM (starting address, length of data) + 1 CHKSUM */ |
| denis2nis | 5:0cf54586a4be | 148 | packet[3] = 4; |
| denis2nis | 5:0cf54586a4be | 149 | checksum += packet[3]; |
| denis2nis | 5:0cf54586a4be | 150 | |
| denis2nis | 5:0cf54586a4be | 151 | /* set write instruction */ |
| denis2nis | 5:0cf54586a4be | 152 | packet[4] = PROTOCOL_INSTRUCTION_READ; |
| denis2nis | 5:0cf54586a4be | 153 | checksum += packet[4]; |
| denis2nis | 5:0cf54586a4be | 154 | |
| denis2nis | 5:0cf54586a4be | 155 | /* Param 1: address to read in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 156 | packet[5] = address; |
| denis2nis | 5:0cf54586a4be | 157 | checksum += packet[5]; |
| denis2nis | 5:0cf54586a4be | 158 | |
| denis2nis | 5:0cf54586a4be | 159 | /* Param 2: number of bytes to read in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 160 | packet[6] = len; |
| denis2nis | 5:0cf54586a4be | 161 | checksum += packet[6]; |
| denis2nis | 5:0cf54586a4be | 162 | |
| denis2nis | 5:0cf54586a4be | 163 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
| denis2nis | 5:0cf54586a4be | 164 | packet[7] = ~checksum; |
| denis2nis | 5:0cf54586a4be | 165 | |
| denis2nis | 5:0cf54586a4be | 166 | packet_length = 8; |
| denis2nis | 5:0cf54586a4be | 167 | } |
| denis2nis | 5:0cf54586a4be | 168 | else // write instruction |
| denis2nis | 5:0cf54586a4be | 169 | { |
| denis2nis | 5:0cf54586a4be | 170 | /* byte length of the instruction: parameter and checksum field */ |
| denis2nis | 5:0cf54586a4be | 171 | /* For write instruction: 1 INSTR + */ |
| denis2nis | 5:0cf54586a4be | 172 | /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */ |
| denis2nis | 5:0cf54586a4be | 173 | packet[3] = 3 + len; |
| denis2nis | 5:0cf54586a4be | 174 | checksum += packet[3]; |
| denis2nis | 5:0cf54586a4be | 175 | |
| denis2nis | 5:0cf54586a4be | 176 | /* set read instruction */ |
| denis2nis | 5:0cf54586a4be | 177 | packet[4] = PROTOCOL_INSTRUCTION_WRITE; |
| denis2nis | 5:0cf54586a4be | 178 | checksum += packet[4]; |
| denis2nis | 5:0cf54586a4be | 179 | |
| denis2nis | 5:0cf54586a4be | 180 | /* Param 1: address to write in the "Control Table of RAM Area" */ |
| denis2nis | 5:0cf54586a4be | 181 | packet[5] = address; |
| denis2nis | 5:0cf54586a4be | 182 | checksum += packet[5]; |
| denis2nis | 5:0cf54586a4be | 183 | |
| denis2nis | 5:0cf54586a4be | 184 | /* Param 2 to N: data to write in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 185 | for(char i = 0; i < len; i++) { |
| denis2nis | 5:0cf54586a4be | 186 | packet[6 + i] = data[i]; |
| denis2nis | 5:0cf54586a4be | 187 | checksum += data[i]; |
| denis2nis | 5:0cf54586a4be | 188 | } |
| denis2nis | 5:0cf54586a4be | 189 | |
| denis2nis | 5:0cf54586a4be | 190 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
| denis2nis | 5:0cf54586a4be | 191 | packet[6 + len] = ~checksum; |
| denis2nis | 5:0cf54586a4be | 192 | |
| denis2nis | 5:0cf54586a4be | 193 | packet_length = 7 + len; |
| denis2nis | 5:0cf54586a4be | 194 | } |
| denis2nis | 5:0cf54586a4be | 195 | |
| denis2nis | 5:0cf54586a4be | 196 | // Send packet |
| denis2nis | 5:0cf54586a4be | 197 | if(mot_id != 0xFE) { |
| denis2nis | 5:0cf54586a4be | 198 | for(char i = 0; i < packet_length; i++) { |
| denis2nis | 5:0cf54586a4be | 199 | _mx12.write(&packet[i], 1); |
| denis2nis | 5:0cf54586a4be | 200 | } |
| denis2nis | 5:0cf54586a4be | 201 | } |
| denis2nis | 5:0cf54586a4be | 202 | } |
| denis2nis | 5:0cf54586a4be | 203 | |
| denis2nis | 5:0cf54586a4be | 204 | // Debug function to print Serial read |
| denis2nis | 9:b4a5187fdec6 | 205 | void MX12::PrintSerial() |
| denis2nis | 9:b4a5187fdec6 | 206 | { |
| denis2nis | 13:ccda9a56fef1 | 207 | for(int i = 0; i < _status_pck.n_byte; i++) { |
| denis2nis | 13:ccda9a56fef1 | 208 | printf("%x ", _status_pck.raw[i]); |
| denis2nis | 5:0cf54586a4be | 209 | } |
| denis2nis | 5:0cf54586a4be | 210 | printf("\n"); |
| denis2nis | 5:0cf54586a4be | 211 | } |
| denis2nis | 5:0cf54586a4be | 212 | |
| denis2nis | 5:0cf54586a4be | 213 | MX12::Status MX12::GetStatus() { |
| denis2nis | 5:0cf54586a4be | 214 | // Return the corresponding status code |
| denis2nis | 13:ccda9a56fef1 | 215 | switch(_status_pck.error) { |
| denis2nis | 5:0cf54586a4be | 216 | case 0: |
| denis2nis | 5:0cf54586a4be | 217 | return Ok; |
| denis2nis | 5:0cf54586a4be | 218 | break; |
| denis2nis | 5:0cf54586a4be | 219 | case 1 << 0: |
| denis2nis | 5:0cf54586a4be | 220 | return InputVoltageError; |
| denis2nis | 5:0cf54586a4be | 221 | break; |
| denis2nis | 5:0cf54586a4be | 222 | case 1 << 1: |
| denis2nis | 5:0cf54586a4be | 223 | return AngleLimitError; |
| denis2nis | 5:0cf54586a4be | 224 | break; |
| denis2nis | 5:0cf54586a4be | 225 | case 1 << 2: |
| denis2nis | 5:0cf54586a4be | 226 | return OverheatingError; |
| denis2nis | 5:0cf54586a4be | 227 | break; |
| denis2nis | 5:0cf54586a4be | 228 | case 1 << 3: |
| denis2nis | 5:0cf54586a4be | 229 | return RangeError; |
| denis2nis | 5:0cf54586a4be | 230 | break; |
| denis2nis | 5:0cf54586a4be | 231 | case 1 << 4: |
| denis2nis | 5:0cf54586a4be | 232 | return ChecksumError; |
| denis2nis | 5:0cf54586a4be | 233 | break; |
| denis2nis | 5:0cf54586a4be | 234 | case 1 << 5: |
| denis2nis | 5:0cf54586a4be | 235 | return OverloadError; |
| denis2nis | 5:0cf54586a4be | 236 | break; |
| denis2nis | 5:0cf54586a4be | 237 | case 1 << 6: |
| denis2nis | 5:0cf54586a4be | 238 | return InstructionError; |
| denis2nis | 5:0cf54586a4be | 239 | break; |
| denis2nis | 5:0cf54586a4be | 240 | default: |
| denis2nis | 5:0cf54586a4be | 241 | return Unknown; |
| denis2nis | 5:0cf54586a4be | 242 | } |
| denis2nis | 5:0cf54586a4be | 243 | } |
| denis2nis | 5:0cf54586a4be | 244 | |
| denis2nis | 13:ccda9a56fef1 | 245 | void MX12::_Rx_interrupt() { |
| denis2nis | 8:e74ef93ae660 | 246 | |
| denis2nis | 5:0cf54586a4be | 247 | char c; |
| denis2nis | 5:0cf54586a4be | 248 | |
| denis2nis | 5:0cf54586a4be | 249 | // Try to read serial |
| denis2nis | 5:0cf54586a4be | 250 | if(_mx12.read(&c, 1)) { |
| denis2nis | 8:e74ef93ae660 | 251 | |
| denis2nis | 13:ccda9a56fef1 | 252 | _status_pck.raw[(_parser_state.byte_index)++] = c; |
| denis2nis | 5:0cf54586a4be | 253 | |
| denis2nis | 5:0cf54586a4be | 254 | // State-machine parsing |
| denis2nis | 13:ccda9a56fef1 | 255 | switch(_parser_state.expected_field) { |
| denis2nis | 13:ccda9a56fef1 | 256 | |
| denis2nis | 13:ccda9a56fef1 | 257 | /* c char is interpreted as a Header1 field */ |
| denis2nis | 13:ccda9a56fef1 | 258 | case PacketField::Header1: |
| denis2nis | 13:ccda9a56fef1 | 259 | |
| denis2nis | 13:ccda9a56fef1 | 260 | /* do nothing and set next state to Header2 */ |
| denis2nis | 13:ccda9a56fef1 | 261 | _parser_state.expected_field = PacketField::Header2; |
| denis2nis | 13:ccda9a56fef1 | 262 | break; |
| denis2nis | 13:ccda9a56fef1 | 263 | |
| denis2nis | 13:ccda9a56fef1 | 264 | /* c char is interpreted as a Header2 field */ |
| denis2nis | 13:ccda9a56fef1 | 265 | case PacketField::Header2: |
| denis2nis | 13:ccda9a56fef1 | 266 | |
| denis2nis | 13:ccda9a56fef1 | 267 | /* do nothing and set next state to Id */ |
| denis2nis | 13:ccda9a56fef1 | 268 | _parser_state.expected_field = PacketField::Id; |
| denis2nis | 13:ccda9a56fef1 | 269 | break; |
| denis2nis | 13:ccda9a56fef1 | 270 | |
| denis2nis | 13:ccda9a56fef1 | 271 | /* c char is interpreted as ID field */ |
| denis2nis | 13:ccda9a56fef1 | 272 | case PacketField::Id: |
| denis2nis | 5:0cf54586a4be | 273 | |
| denis2nis | 13:ccda9a56fef1 | 274 | /* store ID, update checksum and set next state to Length */ |
| denis2nis | 13:ccda9a56fef1 | 275 | _status_pck.servo_id = c; |
| denis2nis | 13:ccda9a56fef1 | 276 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 277 | _parser_state.expected_field = PacketField::Length; |
| denis2nis | 5:0cf54586a4be | 278 | break; |
| denis2nis | 13:ccda9a56fef1 | 279 | |
| denis2nis | 13:ccda9a56fef1 | 280 | /* c char is interpreted as length of message data field |
| denis2nis | 13:ccda9a56fef1 | 281 | * Length = number of Parameters + 2 |
| denis2nis | 13:ccda9a56fef1 | 282 | * where 2 stands for Length field (1 byte) + Error filed (1 byte) |
| denis2nis | 13:ccda9a56fef1 | 283 | */ |
| denis2nis | 13:ccda9a56fef1 | 284 | case PacketField::Length: |
| denis2nis | 5:0cf54586a4be | 285 | |
| denis2nis | 13:ccda9a56fef1 | 286 | /* store number of param into _status_pck.n_param, |
| denis2nis | 13:ccda9a56fef1 | 287 | * update calculated_checksum and set next state to Error |
| denis2nis | 13:ccda9a56fef1 | 288 | */ |
| denis2nis | 13:ccda9a56fef1 | 289 | _status_pck.n_param = c - 2; |
| denis2nis | 13:ccda9a56fef1 | 290 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 291 | _parser_state.expected_field = PacketField::Error; |
| denis2nis | 5:0cf54586a4be | 292 | break; |
| denis2nis | 5:0cf54586a4be | 293 | |
| denis2nis | 13:ccda9a56fef1 | 294 | /* c char is interpreted as error status field */ |
| denis2nis | 13:ccda9a56fef1 | 295 | case PacketField::Error: |
| denis2nis | 13:ccda9a56fef1 | 296 | |
| denis2nis | 13:ccda9a56fef1 | 297 | /* store error status, update checksum |
| denis2nis | 13:ccda9a56fef1 | 298 | * and set next state to Data |
| denis2nis | 13:ccda9a56fef1 | 299 | */ |
| denis2nis | 13:ccda9a56fef1 | 300 | _status_pck.error = c; |
| denis2nis | 13:ccda9a56fef1 | 301 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 302 | _parser_state.expected_field = PacketField::Data; |
| denis2nis | 5:0cf54586a4be | 303 | break; |
| denis2nis | 5:0cf54586a4be | 304 | |
| denis2nis | 13:ccda9a56fef1 | 305 | /* c char is interpreted as a param field */ |
| denis2nis | 13:ccda9a56fef1 | 306 | case PacketField::Data: |
| denis2nis | 13:ccda9a56fef1 | 307 | |
| denis2nis | 13:ccda9a56fef1 | 308 | /* store current param, increase param_index |
| denis2nis | 13:ccda9a56fef1 | 309 | * and update checksum */ |
| denis2nis | 13:ccda9a56fef1 | 310 | _status_pck.param[(_parser_state.param_index)++] = c; |
| denis2nis | 13:ccda9a56fef1 | 311 | _status_pck.received_checksum += c; |
| denis2nis | 5:0cf54586a4be | 312 | |
| denis2nis | 13:ccda9a56fef1 | 313 | /* increase param index (_parser_state.dataCount) |
| denis2nis | 13:ccda9a56fef1 | 314 | * and test if it is the last param to read |
| denis2nis | 13:ccda9a56fef1 | 315 | */ |
| denis2nis | 13:ccda9a56fef1 | 316 | if(_parser_state.param_index > _status_pck.n_param) { |
| denis2nis | 13:ccda9a56fef1 | 317 | /* reset param index and set next state to Checksum */ |
| denis2nis | 13:ccda9a56fef1 | 318 | _parser_state.param_index = 0; |
| denis2nis | 13:ccda9a56fef1 | 319 | _parser_state.expected_field = PacketField::Checksum; |
| denis2nis | 5:0cf54586a4be | 320 | } |
| denis2nis | 5:0cf54586a4be | 321 | break; |
| denis2nis | 5:0cf54586a4be | 322 | |
| denis2nis | 13:ccda9a56fef1 | 323 | /* c char is interpreted as Checksum field */ |
| denis2nis | 13:ccda9a56fef1 | 324 | case PacketField::Checksum: |
| denis2nis | 13:ccda9a56fef1 | 325 | |
| denis2nis | 13:ccda9a56fef1 | 326 | /* store received_checksum, set parsed, store n_byte, |
| denis2nis | 13:ccda9a56fef1 | 327 | * evalutate valid and set next state to Header1 */ |
| denis2nis | 13:ccda9a56fef1 | 328 | _status_pck.received_checksum = c; |
| denis2nis | 13:ccda9a56fef1 | 329 | _status_pck.parsed = true; |
| denis2nis | 13:ccda9a56fef1 | 330 | _status_pck.n_byte = _parser_state.byte_index; |
| denis2nis | 13:ccda9a56fef1 | 331 | _status_pck.valid = (_status_pck.received_checksum == c); |
| denis2nis | 13:ccda9a56fef1 | 332 | _parser_state.expected_field = PacketField::Header1; |
| denis2nis | 13:ccda9a56fef1 | 333 | |
| denis2nis | 13:ccda9a56fef1 | 334 | /* set seriel state to Idle */ |
| denis2nis | 13:ccda9a56fef1 | 335 | _bus_state = SerialState::Idle; |
| denis2nis | 5:0cf54586a4be | 336 | break; |
| denis2nis | 14:5a0c7b30f8c0 | 337 | |
| denis2nis | 14:5a0c7b30f8c0 | 338 | default: |
| denis2nis | 14:5a0c7b30f8c0 | 339 | |
| denis2nis | 14:5a0c7b30f8c0 | 340 | /* unexpected case. If it occurs it would be due to a |
| denis2nis | 14:5a0c7b30f8c0 | 341 | * code error of this class */ |
| denis2nis | 22:8fa806e3ece4 | 342 | break; |
| denis2nis | 5:0cf54586a4be | 343 | } |
| denis2nis | 5:0cf54586a4be | 344 | } |
| denis2nis | 5:0cf54586a4be | 345 | } |
| denis2nis | 5:0cf54586a4be | 346 | |
| denis2nis | 11:9bc7f5e2ccee | 347 | /* Code from previous version of the class */ |
| denis2nis | 11:9bc7f5e2ccee | 348 | |
| denis2nis | 5:0cf54586a4be | 349 | /* |
| denis2nis | 0:7556356a8bcd | 350 | void MX12::ReadPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 351 | // Make a request, interrupt takes care of everything else |
| denis2nis | 0:7556356a8bcd | 352 | _state = State::ReadingPosition; |
| denis2nis | 0:7556356a8bcd | 353 | rw(mot_id, 0x24, 2, NULL); |
| denis2nis | 0:7556356a8bcd | 354 | } |
| denis2nis | 0:7556356a8bcd | 355 | |
| denis2nis | 0:7556356a8bcd | 356 | float MX12::GetPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 357 | return _angle[mot_id]; |
| denis2nis | 0:7556356a8bcd | 358 | } |
| denis2nis | 5:0cf54586a4be | 359 | */ |
| lorenzodunau | 26:4632a02a8ef1 | 360 | |
| lorenzodunau | 26:4632a02a8ef1 | 361 | void MX12::cmd_moteur(float Vavance, float Vlat, float Wz){ |
| lorenzodunau | 26:4632a02a8ef1 | 362 | float coeff=578.7/1023; |
| lorenzodunau | 26:4632a02a8ef1 | 363 | float W1; |
| lorenzodunau | 26:4632a02a8ef1 | 364 | float W2; |
| lorenzodunau | 26:4632a02a8ef1 | 365 | float W3; |
| lorenzodunau | 26:4632a02a8ef1 | 366 | float Rc; |
| lorenzodunau | 26:4632a02a8ef1 | 367 | float R; |
| lorenzodunau | 26:4632a02a8ef1 | 368 | float x1; |
| lorenzodunau | 26:4632a02a8ef1 | 369 | float x2; |
| lorenzodunau | 26:4632a02a8ef1 | 370 | float x3; |
| lorenzodunau | 26:4632a02a8ef1 | 371 | float y1; |
| lorenzodunau | 26:4632a02a8ef1 | 372 | float y2; |
| lorenzodunau | 26:4632a02a8ef1 | 373 | float y3; |
| lorenzodunau | 26:4632a02a8ef1 | 374 | float a1; |
| lorenzodunau | 26:4632a02a8ef1 | 375 | float a2; |
| lorenzodunau | 26:4632a02a8ef1 | 376 | float a3; |
| lorenzodunau | 26:4632a02a8ef1 | 377 | Rc=0.08; // rayon du chassis*10 |
| lorenzodunau | 26:4632a02a8ef1 | 378 | R=0.019; // rayon de la roue*10 |
| lorenzodunau | 26:4632a02a8ef1 | 379 | W1=0; |
| lorenzodunau | 26:4632a02a8ef1 | 380 | W2=0; |
| lorenzodunau | 26:4632a02a8ef1 | 381 | W3=0; |
| lorenzodunau | 26:4632a02a8ef1 | 382 | a1 = 0 ; |
| lorenzodunau | 26:4632a02a8ef1 | 383 | a2 = 2*1.047197551; |
| lorenzodunau | 26:4632a02a8ef1 | 384 | a3 = -2*1.047197551; |
| lorenzodunau | 26:4632a02a8ef1 | 385 | x1 = (Rc - 0.007)*cosf(a1); |
| lorenzodunau | 26:4632a02a8ef1 | 386 | x2 = (Rc - 0.007)*cosf(a2); |
| lorenzodunau | 26:4632a02a8ef1 | 387 | x3 = (Rc - 0.007)*cosf(a3); |
| lorenzodunau | 26:4632a02a8ef1 | 388 | y1 = (Rc - 0.007)*sinf(a1); |
| lorenzodunau | 26:4632a02a8ef1 | 389 | y2 = (Rc - 0.007)*sinf(a2); |
| lorenzodunau | 26:4632a02a8ef1 | 390 | y3 = (Rc - 0.007)*sinf(a3); |
| lorenzodunau | 26:4632a02a8ef1 | 391 | //Vtm_smax=0.8; //sert pour calculer valeurs -999->999 |
| lorenzodunau | 26:4632a02a8ef1 | 392 | //Vnm_smax=0.9; |
| lorenzodunau | 26:4632a02a8ef1 | 393 | //Wcrd_smax=2.9; |
| lorenzodunau | 26:4632a02a8ef1 | 394 | //if (Vtm_s>Vtm_smax) |
| lorenzodunau | 26:4632a02a8ef1 | 395 | // {Vtm_s=Vtm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 396 | //if (Vnm_s>Vnm_smax) |
| lorenzodunau | 26:4632a02a8ef1 | 397 | // {Vnm_s=Vnm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 398 | //if (Wcrd_s>Wcrd_smax) |
| lorenzodunau | 26:4632a02a8ef1 | 399 | // {Wcrd_s=Wcrd_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 400 | //if (Wcrd_s<-Wcrd_smax) |
| lorenzodunau | 26:4632a02a8ef1 | 401 | // {Wcrd_s=-Wcrd_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 402 | |
| lorenzodunau | 26:4632a02a8ef1 | 403 | W1=1/Rc*(sinf(a1)*Vavance- sinf(a1)*y1*Wz - cosf(a1)*Vlat + cosf(a1)*x1*Wz); //loi de commande moteur 1 |
| lorenzodunau | 26:4632a02a8ef1 | 404 | W2=1/Rc*(sinf(a2)*Vavance- sinf(a2)*y2*Wz - cosf(a2)*Vlat + cosf(a2)*x2*Wz); |
| lorenzodunau | 26:4632a02a8ef1 | 405 | W3=1/Rc*(sinf(a3)*Vavance- sinf(a3)*y3*Wz - cosf(a3)*Vlat + cosf(a3)*x3*Wz); |
| lorenzodunau | 26:4632a02a8ef1 | 406 | printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3)); |
| lorenzodunau | 26:4632a02a8ef1 | 407 | |
| lorenzodunau | 26:4632a02a8ef1 | 408 | |
| lorenzodunau | 27:06850c65b9c8 | 409 | SetSpeed_rad_s(1,W1); // impose la vitesse au moteur 1 |
| lorenzodunau | 27:06850c65b9c8 | 410 | SetSpeed_rad_s(2,W2); |
| lorenzodunau | 27:06850c65b9c8 | 411 | SetSpeed_rad_s(3,W3); |
| lorenzodunau | 26:4632a02a8ef1 | 412 | |
| lorenzodunau | 26:4632a02a8ef1 | 413 | |
| lorenzodunau | 27:06850c65b9c8 | 414 | |
| lorenzodunau | 27:06850c65b9c8 | 415 | } |
| lorenzodunau | 27:06850c65b9c8 | 416 | |
| lorenzodunau | 27:06850c65b9c8 | 417 | void MX12::eteindre_moteurs(){ |
| lorenzodunau | 27:06850c65b9c8 | 418 | char data[1]; |
| lorenzodunau | 27:06850c65b9c8 | 419 | |
| lorenzodunau | 27:06850c65b9c8 | 420 | data[0] = 0; |
| lorenzodunau | 27:06850c65b9c8 | 421 | |
| lorenzodunau | 26:4632a02a8ef1 | 422 | |
| lorenzodunau | 27:06850c65b9c8 | 423 | // Send instruction |
| lorenzodunau | 27:06850c65b9c8 | 424 | _bus_state = SerialState::Writing; |
| lorenzodunau | 27:06850c65b9c8 | 425 | rw(1, CONTROL_TABLE_TORQUE_ENABLE , 1, data); |
| lorenzodunau | 27:06850c65b9c8 | 426 | _bus_state = SerialState::Writing; |
| lorenzodunau | 27:06850c65b9c8 | 427 | rw(2, CONTROL_TABLE_TORQUE_ENABLE , 1, data); |
| lorenzodunau | 27:06850c65b9c8 | 428 | _bus_state = SerialState::Writing; |
| lorenzodunau | 27:06850c65b9c8 | 429 | rw(3, CONTROL_TABLE_TORQUE_ENABLE , 1, data); |
| lorenzodunau | 27:06850c65b9c8 | 430 | } |