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@29:0484cbad0770, 2022-01-12 (annotated)
- Committer:
- lorenzodunau
- Date:
- Wed Jan 12 15:38:34 2022 +0000
- Revision:
- 29:0484cbad0770
- Parent:
- 28:c7402e1014b4
- Child:
- 30:4e7cf52ac2dd
last version 12/01
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| lorenzodunau | 28:c7402e1014b4 | 1 | /** |
| lorenzodunau | 28:c7402e1014b4 | 2 | * @file MX12.ccp |
| lorenzodunau | 28:c7402e1014b4 | 3 | * @brief This file contains all the methods of the MX12 class |
| lorenzodunau | 28:c7402e1014b4 | 4 | * whose prototypes are in the MX12.h header file |
| lorenzodunau | 28:c7402e1014b4 | 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 */ |
| lorenzodunau | 28:c7402e1014b4 | 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); |
| lorenzodunau | 28:c7402e1014b4 | 24 | |
| denis2nis | 8:e74ef93ae660 | 25 | // variable used for message reception |
| denis2nis | 13:ccda9a56fef1 | 26 | _status_pck = {.raw = "", |
| denis2nis | 13:ccda9a56fef1 | 27 | .n_byte = 0, |
| lorenzodunau | 28:c7402e1014b4 | 28 | .servo_id = 0, |
| denis2nis | 13:ccda9a56fef1 | 29 | .length = 0, |
| denis2nis | 13:ccda9a56fef1 | 30 | .error = 0, |
| denis2nis | 13:ccda9a56fef1 | 31 | .n_param = 0, |
| lorenzodunau | 28:c7402e1014b4 | 32 | .param = "", |
| denis2nis | 13:ccda9a56fef1 | 33 | .received_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 34 | .calculated_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 35 | .parsed = false, |
| lorenzodunau | 28:c7402e1014b4 | 36 | .valid = false |
| lorenzodunau | 28:c7402e1014b4 | 37 | }; |
| lorenzodunau | 28:c7402e1014b4 | 38 | |
| denis2nis | 13:ccda9a56fef1 | 39 | _parser_state = {.expected_field = PacketField::Header1, |
| lorenzodunau | 28:c7402e1014b4 | 40 | .byte_index = 0, |
| lorenzodunau | 28:c7402e1014b4 | 41 | .param_index = 0 |
| lorenzodunau | 28:c7402e1014b4 | 42 | }; |
| lorenzodunau | 28:c7402e1014b4 | 43 | |
| denis2nis | 8:e74ef93ae660 | 44 | // Internal defaults states |
| denis2nis | 13:ccda9a56fef1 | 45 | _bus_state = SerialState::Idle; |
| lorenzodunau | 28:c7402e1014b4 | 46 | |
| denis2nis | 0:7556356a8bcd | 47 | } |
| denis2nis | 0:7556356a8bcd | 48 | |
| lorenzodunau | 28:c7402e1014b4 | 49 | void MX12::SetSpeed(unsigned char mot_id, float speed) |
| lorenzodunau | 28:c7402e1014b4 | 50 | { |
| denis2nis | 0:7556356a8bcd | 51 | char data[2]; |
| lorenzodunau | 28:c7402e1014b4 | 52 | |
| denis2nis | 0:7556356a8bcd | 53 | // Speed absolute value |
| denis2nis | 0:7556356a8bcd | 54 | int goal = (0x3ff * abs(speed)); |
| lorenzodunau | 28:c7402e1014b4 | 55 | |
| denis2nis | 0:7556356a8bcd | 56 | // Spin direction (CW is negative) |
| denis2nis | 0:7556356a8bcd | 57 | if (speed < 0) { |
| denis2nis | 0:7556356a8bcd | 58 | goal |= (0x1 << 10); |
| denis2nis | 0:7556356a8bcd | 59 | } |
| denis2nis | 0:7556356a8bcd | 60 | |
| denis2nis | 0:7556356a8bcd | 61 | data[0] = goal & 0xff; |
| denis2nis | 0:7556356a8bcd | 62 | data[1] = goal >> 8; |
| lorenzodunau | 28:c7402e1014b4 | 63 | |
| denis2nis | 0:7556356a8bcd | 64 | // Send instruction |
| denis2nis | 13:ccda9a56fef1 | 65 | _bus_state = SerialState::Writing; |
| denis2nis | 5:0cf54586a4be | 66 | rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data); |
| denis2nis | 0:7556356a8bcd | 67 | } |
| denis2nis | 0:7556356a8bcd | 68 | |
| bruno2nis | 24:2ab26ed08c83 | 69 | void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed) |
| bruno2nis | 24:2ab26ed08c83 | 70 | { |
| lorenzodunau | 28:c7402e1014b4 | 71 | if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) { |
| bruno2nis | 24:2ab26ed08c83 | 72 | SetSpeed(mot_id, 1); |
| lorenzodunau | 28:c7402e1014b4 | 73 | } else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) { |
| bruno2nis | 24:2ab26ed08c83 | 74 | SetSpeed(mot_id, -1); |
| lorenzodunau | 28:c7402e1014b4 | 75 | } else { |
| bruno2nis | 24:2ab26ed08c83 | 76 | SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S); |
| bruno2nis | 24:2ab26ed08c83 | 77 | } |
| bruno2nis | 24:2ab26ed08c83 | 78 | } |
| bruno2nis | 24:2ab26ed08c83 | 79 | |
| lorenzodunau | 28:c7402e1014b4 | 80 | char MX12::IsAvailable(void) |
| lorenzodunau | 28:c7402e1014b4 | 81 | { |
| denis2nis | 13:ccda9a56fef1 | 82 | return (_bus_state == SerialState::Idle); |
| denis2nis | 0:7556356a8bcd | 83 | } |
| denis2nis | 0:7556356a8bcd | 84 | |
| lorenzodunau | 28:c7402e1014b4 | 85 | void MX12::rw(unsigned char mot_id, char address, char len, char *data) |
| lorenzodunau | 28:c7402e1014b4 | 86 | { |
| lorenzodunau | 28:c7402e1014b4 | 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, |
| lorenzodunau | 28:c7402e1014b4 | 92 | .servo_id = 0, |
| denis2nis | 13:ccda9a56fef1 | 93 | .length = 0, |
| denis2nis | 13:ccda9a56fef1 | 94 | .error = 0, |
| denis2nis | 13:ccda9a56fef1 | 95 | .n_param = 0, |
| lorenzodunau | 28:c7402e1014b4 | 96 | .param = "", |
| denis2nis | 13:ccda9a56fef1 | 97 | .received_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 98 | .calculated_checksum = 0, |
| denis2nis | 13:ccda9a56fef1 | 99 | .parsed = false, |
| lorenzodunau | 28:c7402e1014b4 | 100 | .valid = false |
| lorenzodunau | 28:c7402e1014b4 | 101 | }; |
| denis2nis | 13:ccda9a56fef1 | 102 | _parser_state = {.expected_field = PacketField::Header1, |
| lorenzodunau | 28:c7402e1014b4 | 103 | .byte_index = 0, |
| lorenzodunau | 28:c7402e1014b4 | 104 | .param_index = 0 |
| lorenzodunau | 28:c7402e1014b4 | 105 | }; |
| denis2nis | 13:ccda9a56fef1 | 106 | |
| lorenzodunau | 28:c7402e1014b4 | 107 | |
| denis2nis | 5:0cf54586a4be | 108 | /* Initialise instruction packet to forge. |
| denis2nis | 5:0cf54586a4be | 109 | * Instruction Packet is the command data sent to the servomotor. |
| denis2nis | 5:0cf54586a4be | 110 | * |
| denis2nis | 5:0cf54586a4be | 111 | * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum| |
| denis2nis | 5:0cf54586a4be | 112 | * |-------|-------|---------|------|-----------|---------------|--------| |
| denis2nis | 5:0cf54586a4be | 113 | * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM | |
| denis2nis | 5:0cf54586a4be | 114 | * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | | |
| denis2nis | 5:0cf54586a4be | 115 | * \__ ___/ \_ _/ \__ __/ |
| denis2nis | 5:0cf54586a4be | 116 | * \/ | \/ \/ | |
| denis2nis | 5:0cf54586a4be | 117 | * mot_id | address data | |
| denis2nis | 5:0cf54586a4be | 118 | * | (len = N-1) | |
| denis2nis | 5:0cf54586a4be | 119 | * \__________________ ________________/ |
| denis2nis | 5:0cf54586a4be | 120 | * \/ |
| denis2nis | 5:0cf54586a4be | 121 | * Length ( cmd[3] ) |
| denis2nis | 5:0cf54586a4be | 122 | */ |
| denis2nis | 5:0cf54586a4be | 123 | char packet[16]; |
| denis2nis | 5:0cf54586a4be | 124 | unsigned char packet_length; |
| lorenzodunau | 28:c7402e1014b4 | 125 | |
| denis2nis | 5:0cf54586a4be | 126 | /* Initialise checksum to calculate |
| lorenzodunau | 28:c7402e1014b4 | 127 | * It is used to check if packet is damaged during communication. |
| denis2nis | 5:0cf54586a4be | 128 | * Status Checksum is calculated according to the following formula: |
| denis2nis | 5:0cf54586a4be | 129 | * |
| denis2nis | 5:0cf54586a4be | 130 | * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N ) |
| denis2nis | 5:0cf54586a4be | 131 | */ |
| denis2nis | 5:0cf54586a4be | 132 | char checksum = 0x00; |
| lorenzodunau | 28:c7402e1014b4 | 133 | |
| denis2nis | 5:0cf54586a4be | 134 | /* header 1 = 0xFF (dynamixel protocol 1.0) */ |
| denis2nis | 5:0cf54586a4be | 135 | packet[0] = 0xff; |
| lorenzodunau | 28:c7402e1014b4 | 136 | |
| denis2nis | 5:0cf54586a4be | 137 | /* header 2 = 0xFF (dynamixel protocol 1.0) */ |
| lorenzodunau | 28:c7402e1014b4 | 138 | packet[1] = 0xff; |
| lorenzodunau | 28:c7402e1014b4 | 139 | |
| lorenzodunau | 28:c7402e1014b4 | 140 | /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */ |
| denis2nis | 5:0cf54586a4be | 141 | packet[2] = mot_id; |
| denis2nis | 5:0cf54586a4be | 142 | checksum += packet[2]; |
| lorenzodunau | 28:c7402e1014b4 | 143 | |
| denis2nis | 5:0cf54586a4be | 144 | /* Guess instruction type. NULL for read, not NULL for write */ |
| lorenzodunau | 28:c7402e1014b4 | 145 | if(data == NULL) { // read instruction |
| denis2nis | 5:0cf54586a4be | 146 | /* byte length of the instruction: parameter and checksum field. */ |
| denis2nis | 5:0cf54586a4be | 147 | /* for read instruction: 1 INSTR + */ |
| denis2nis | 5:0cf54586a4be | 148 | /* 2 PARAM (starting address, length of data) + 1 CHKSUM */ |
| denis2nis | 5:0cf54586a4be | 149 | packet[3] = 4; |
| denis2nis | 5:0cf54586a4be | 150 | checksum += packet[3]; |
| denis2nis | 5:0cf54586a4be | 151 | |
| denis2nis | 5:0cf54586a4be | 152 | /* set write instruction */ |
| denis2nis | 5:0cf54586a4be | 153 | packet[4] = PROTOCOL_INSTRUCTION_READ; |
| denis2nis | 5:0cf54586a4be | 154 | checksum += packet[4]; |
| lorenzodunau | 28:c7402e1014b4 | 155 | |
| denis2nis | 5:0cf54586a4be | 156 | /* Param 1: address to read in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 157 | packet[5] = address; |
| denis2nis | 5:0cf54586a4be | 158 | checksum += packet[5]; |
| lorenzodunau | 28:c7402e1014b4 | 159 | |
| denis2nis | 5:0cf54586a4be | 160 | /* Param 2: number of bytes to read in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 161 | packet[6] = len; |
| denis2nis | 5:0cf54586a4be | 162 | checksum += packet[6]; |
| lorenzodunau | 28:c7402e1014b4 | 163 | |
| denis2nis | 5:0cf54586a4be | 164 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
| denis2nis | 5:0cf54586a4be | 165 | packet[7] = ~checksum; |
| lorenzodunau | 28:c7402e1014b4 | 166 | |
| denis2nis | 5:0cf54586a4be | 167 | packet_length = 8; |
| lorenzodunau | 28:c7402e1014b4 | 168 | } else { // write instruction |
| denis2nis | 5:0cf54586a4be | 169 | /* byte length of the instruction: parameter and checksum field */ |
| denis2nis | 5:0cf54586a4be | 170 | /* For write instruction: 1 INSTR + */ |
| denis2nis | 5:0cf54586a4be | 171 | /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */ |
| lorenzodunau | 28:c7402e1014b4 | 172 | packet[3] = 3 + len; |
| denis2nis | 5:0cf54586a4be | 173 | checksum += packet[3]; |
| denis2nis | 5:0cf54586a4be | 174 | |
| denis2nis | 5:0cf54586a4be | 175 | /* set read instruction */ |
| denis2nis | 5:0cf54586a4be | 176 | packet[4] = PROTOCOL_INSTRUCTION_WRITE; |
| denis2nis | 5:0cf54586a4be | 177 | checksum += packet[4]; |
| denis2nis | 5:0cf54586a4be | 178 | |
| denis2nis | 5:0cf54586a4be | 179 | /* Param 1: address to write in the "Control Table of RAM Area" */ |
| denis2nis | 5:0cf54586a4be | 180 | packet[5] = address; |
| denis2nis | 5:0cf54586a4be | 181 | checksum += packet[5]; |
| lorenzodunau | 28:c7402e1014b4 | 182 | |
| denis2nis | 5:0cf54586a4be | 183 | /* Param 2 to N: data to write in the Control Table of RAM Area */ |
| denis2nis | 5:0cf54586a4be | 184 | for(char i = 0; i < len; i++) { |
| denis2nis | 5:0cf54586a4be | 185 | packet[6 + i] = data[i]; |
| denis2nis | 5:0cf54586a4be | 186 | checksum += data[i]; |
| denis2nis | 5:0cf54586a4be | 187 | } |
| lorenzodunau | 28:c7402e1014b4 | 188 | |
| denis2nis | 5:0cf54586a4be | 189 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
| denis2nis | 5:0cf54586a4be | 190 | packet[6 + len] = ~checksum; |
| lorenzodunau | 28:c7402e1014b4 | 191 | |
| denis2nis | 5:0cf54586a4be | 192 | packet_length = 7 + len; |
| denis2nis | 5:0cf54586a4be | 193 | } |
| lorenzodunau | 28:c7402e1014b4 | 194 | |
| denis2nis | 5:0cf54586a4be | 195 | // Send packet |
| denis2nis | 5:0cf54586a4be | 196 | if(mot_id != 0xFE) { |
| denis2nis | 5:0cf54586a4be | 197 | for(char i = 0; i < packet_length; i++) { |
| denis2nis | 5:0cf54586a4be | 198 | _mx12.write(&packet[i], 1); |
| denis2nis | 5:0cf54586a4be | 199 | } |
| denis2nis | 5:0cf54586a4be | 200 | } |
| denis2nis | 5:0cf54586a4be | 201 | } |
| denis2nis | 5:0cf54586a4be | 202 | |
| denis2nis | 5:0cf54586a4be | 203 | // Debug function to print Serial read |
| lorenzodunau | 28:c7402e1014b4 | 204 | void MX12::PrintSerial() |
| denis2nis | 9:b4a5187fdec6 | 205 | { |
| denis2nis | 13:ccda9a56fef1 | 206 | for(int i = 0; i < _status_pck.n_byte; i++) { |
| denis2nis | 13:ccda9a56fef1 | 207 | printf("%x ", _status_pck.raw[i]); |
| denis2nis | 5:0cf54586a4be | 208 | } |
| denis2nis | 5:0cf54586a4be | 209 | printf("\n"); |
| denis2nis | 5:0cf54586a4be | 210 | } |
| lorenzodunau | 28:c7402e1014b4 | 211 | |
| lorenzodunau | 28:c7402e1014b4 | 212 | MX12::Status MX12::GetStatus() |
| lorenzodunau | 28:c7402e1014b4 | 213 | { |
| 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 | } |
| lorenzodunau | 28:c7402e1014b4 | 244 | |
| lorenzodunau | 28:c7402e1014b4 | 245 | void MX12::_Rx_interrupt() |
| lorenzodunau | 28:c7402e1014b4 | 246 | { |
| denis2nis | 8:e74ef93ae660 | 247 | |
| denis2nis | 5:0cf54586a4be | 248 | char c; |
| lorenzodunau | 28:c7402e1014b4 | 249 | |
| denis2nis | 5:0cf54586a4be | 250 | // Try to read serial |
| denis2nis | 5:0cf54586a4be | 251 | if(_mx12.read(&c, 1)) { |
| lorenzodunau | 28:c7402e1014b4 | 252 | |
| denis2nis | 13:ccda9a56fef1 | 253 | _status_pck.raw[(_parser_state.byte_index)++] = c; |
| denis2nis | 5:0cf54586a4be | 254 | |
| denis2nis | 5:0cf54586a4be | 255 | // State-machine parsing |
| denis2nis | 13:ccda9a56fef1 | 256 | switch(_parser_state.expected_field) { |
| lorenzodunau | 28:c7402e1014b4 | 257 | |
| denis2nis | 13:ccda9a56fef1 | 258 | /* c char is interpreted as a Header1 field */ |
| denis2nis | 13:ccda9a56fef1 | 259 | case PacketField::Header1: |
| lorenzodunau | 28:c7402e1014b4 | 260 | |
| denis2nis | 13:ccda9a56fef1 | 261 | /* do nothing and set next state to Header2 */ |
| denis2nis | 13:ccda9a56fef1 | 262 | _parser_state.expected_field = PacketField::Header2; |
| denis2nis | 13:ccda9a56fef1 | 263 | break; |
| denis2nis | 13:ccda9a56fef1 | 264 | |
| denis2nis | 13:ccda9a56fef1 | 265 | /* c char is interpreted as a Header2 field */ |
| denis2nis | 13:ccda9a56fef1 | 266 | case PacketField::Header2: |
| lorenzodunau | 28:c7402e1014b4 | 267 | |
| denis2nis | 13:ccda9a56fef1 | 268 | /* do nothing and set next state to Id */ |
| denis2nis | 13:ccda9a56fef1 | 269 | _parser_state.expected_field = PacketField::Id; |
| denis2nis | 13:ccda9a56fef1 | 270 | break; |
| lorenzodunau | 28:c7402e1014b4 | 271 | |
| denis2nis | 13:ccda9a56fef1 | 272 | /* c char is interpreted as ID field */ |
| denis2nis | 13:ccda9a56fef1 | 273 | case PacketField::Id: |
| lorenzodunau | 28:c7402e1014b4 | 274 | |
| denis2nis | 13:ccda9a56fef1 | 275 | /* store ID, update checksum and set next state to Length */ |
| denis2nis | 13:ccda9a56fef1 | 276 | _status_pck.servo_id = c; |
| lorenzodunau | 28:c7402e1014b4 | 277 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 278 | _parser_state.expected_field = PacketField::Length; |
| denis2nis | 5:0cf54586a4be | 279 | break; |
| denis2nis | 13:ccda9a56fef1 | 280 | |
| denis2nis | 13:ccda9a56fef1 | 281 | /* c char is interpreted as length of message data field |
| denis2nis | 13:ccda9a56fef1 | 282 | * Length = number of Parameters + 2 |
| denis2nis | 13:ccda9a56fef1 | 283 | * where 2 stands for Length field (1 byte) + Error filed (1 byte) |
| denis2nis | 13:ccda9a56fef1 | 284 | */ |
| denis2nis | 13:ccda9a56fef1 | 285 | case PacketField::Length: |
| lorenzodunau | 28:c7402e1014b4 | 286 | |
| lorenzodunau | 28:c7402e1014b4 | 287 | /* store number of param into _status_pck.n_param, |
| lorenzodunau | 28:c7402e1014b4 | 288 | * update calculated_checksum and set next state to Error |
| denis2nis | 13:ccda9a56fef1 | 289 | */ |
| denis2nis | 13:ccda9a56fef1 | 290 | _status_pck.n_param = c - 2; |
| lorenzodunau | 28:c7402e1014b4 | 291 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 292 | _parser_state.expected_field = PacketField::Error; |
| denis2nis | 5:0cf54586a4be | 293 | break; |
| lorenzodunau | 28:c7402e1014b4 | 294 | |
| denis2nis | 13:ccda9a56fef1 | 295 | /* c char is interpreted as error status field */ |
| denis2nis | 13:ccda9a56fef1 | 296 | case PacketField::Error: |
| lorenzodunau | 28:c7402e1014b4 | 297 | |
| lorenzodunau | 28:c7402e1014b4 | 298 | /* store error status, update checksum |
| lorenzodunau | 28:c7402e1014b4 | 299 | * and set next state to Data |
| denis2nis | 13:ccda9a56fef1 | 300 | */ |
| denis2nis | 13:ccda9a56fef1 | 301 | _status_pck.error = c; |
| lorenzodunau | 28:c7402e1014b4 | 302 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 303 | _parser_state.expected_field = PacketField::Data; |
| denis2nis | 5:0cf54586a4be | 304 | break; |
| lorenzodunau | 28:c7402e1014b4 | 305 | |
| denis2nis | 13:ccda9a56fef1 | 306 | /* c char is interpreted as a param field */ |
| denis2nis | 13:ccda9a56fef1 | 307 | case PacketField::Data: |
| denis2nis | 13:ccda9a56fef1 | 308 | |
| lorenzodunau | 28:c7402e1014b4 | 309 | /* store current param, increase param_index |
| denis2nis | 13:ccda9a56fef1 | 310 | * and update checksum */ |
| denis2nis | 13:ccda9a56fef1 | 311 | _status_pck.param[(_parser_state.param_index)++] = c; |
| lorenzodunau | 28:c7402e1014b4 | 312 | _status_pck.received_checksum += c; |
| lorenzodunau | 28:c7402e1014b4 | 313 | |
| denis2nis | 13:ccda9a56fef1 | 314 | /* increase param index (_parser_state.dataCount) |
| denis2nis | 13:ccda9a56fef1 | 315 | * and test if it is the last param to read |
| denis2nis | 13:ccda9a56fef1 | 316 | */ |
| denis2nis | 13:ccda9a56fef1 | 317 | if(_parser_state.param_index > _status_pck.n_param) { |
| denis2nis | 13:ccda9a56fef1 | 318 | /* reset param index and set next state to Checksum */ |
| denis2nis | 13:ccda9a56fef1 | 319 | _parser_state.param_index = 0; |
| denis2nis | 13:ccda9a56fef1 | 320 | _parser_state.expected_field = PacketField::Checksum; |
| denis2nis | 5:0cf54586a4be | 321 | } |
| denis2nis | 5:0cf54586a4be | 322 | break; |
| lorenzodunau | 28:c7402e1014b4 | 323 | |
| denis2nis | 13:ccda9a56fef1 | 324 | /* c char is interpreted as Checksum field */ |
| denis2nis | 13:ccda9a56fef1 | 325 | case PacketField::Checksum: |
| lorenzodunau | 28:c7402e1014b4 | 326 | |
| denis2nis | 13:ccda9a56fef1 | 327 | /* store received_checksum, set parsed, store n_byte, |
| denis2nis | 13:ccda9a56fef1 | 328 | * evalutate valid and set next state to Header1 */ |
| denis2nis | 13:ccda9a56fef1 | 329 | _status_pck.received_checksum = c; |
| denis2nis | 13:ccda9a56fef1 | 330 | _status_pck.parsed = true; |
| denis2nis | 13:ccda9a56fef1 | 331 | _status_pck.n_byte = _parser_state.byte_index; |
| denis2nis | 13:ccda9a56fef1 | 332 | _status_pck.valid = (_status_pck.received_checksum == c); |
| denis2nis | 13:ccda9a56fef1 | 333 | _parser_state.expected_field = PacketField::Header1; |
| lorenzodunau | 28:c7402e1014b4 | 334 | |
| denis2nis | 13:ccda9a56fef1 | 335 | /* set seriel state to Idle */ |
| denis2nis | 13:ccda9a56fef1 | 336 | _bus_state = SerialState::Idle; |
| denis2nis | 5:0cf54586a4be | 337 | break; |
| lorenzodunau | 28:c7402e1014b4 | 338 | |
| denis2nis | 14:5a0c7b30f8c0 | 339 | default: |
| lorenzodunau | 28:c7402e1014b4 | 340 | |
| lorenzodunau | 28:c7402e1014b4 | 341 | /* unexpected case. If it occurs it would be due to a |
| denis2nis | 14:5a0c7b30f8c0 | 342 | * code error of this class */ |
| denis2nis | 22:8fa806e3ece4 | 343 | break; |
| lorenzodunau | 28:c7402e1014b4 | 344 | } |
| denis2nis | 5:0cf54586a4be | 345 | } |
| denis2nis | 5:0cf54586a4be | 346 | } |
| denis2nis | 5:0cf54586a4be | 347 | |
| denis2nis | 11:9bc7f5e2ccee | 348 | /* Code from previous version of the class */ |
| denis2nis | 11:9bc7f5e2ccee | 349 | |
| denis2nis | 5:0cf54586a4be | 350 | /* |
| denis2nis | 0:7556356a8bcd | 351 | void MX12::ReadPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 352 | // Make a request, interrupt takes care of everything else |
| denis2nis | 0:7556356a8bcd | 353 | _state = State::ReadingPosition; |
| denis2nis | 0:7556356a8bcd | 354 | rw(mot_id, 0x24, 2, NULL); |
| denis2nis | 0:7556356a8bcd | 355 | } |
| denis2nis | 0:7556356a8bcd | 356 | |
| denis2nis | 0:7556356a8bcd | 357 | float MX12::GetPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 358 | return _angle[mot_id]; |
| denis2nis | 0:7556356a8bcd | 359 | } |
| denis2nis | 5:0cf54586a4be | 360 | */ |
| lorenzodunau | 26:4632a02a8ef1 | 361 | |
| lorenzodunau | 29:0484cbad0770 | 362 | |
| lorenzodunau | 29:0484cbad0770 | 363 | |
| lorenzodunau | 29:0484cbad0770 | 364 | |
| lorenzodunau | 29:0484cbad0770 | 365 | |
| lorenzodunau | 29:0484cbad0770 | 366 | |
| lorenzodunau | 28:c7402e1014b4 | 367 | void MX12::cmd_moteur(float Vavance, float Vlat, float Wz) |
| lorenzodunau | 28:c7402e1014b4 | 368 | { |
| lorenzodunau | 29:0484cbad0770 | 369 | // mettre en mode wh |
| lorenzodunau | 29:0484cbad0770 | 370 | char data[2]; |
| lorenzodunau | 29:0484cbad0770 | 371 | data[1] = 0; |
| lorenzodunau | 29:0484cbad0770 | 372 | data[0] = 0; |
| lorenzodunau | 29:0484cbad0770 | 373 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 374 | rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 375 | rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 376 | rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 377 | |
| lorenzodunau | 29:0484cbad0770 | 378 | |
| lorenzodunau | 29:0484cbad0770 | 379 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 380 | rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 381 | rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 382 | rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 383 | |
| lorenzodunau | 26:4632a02a8ef1 | 384 | float W1; |
| lorenzodunau | 26:4632a02a8ef1 | 385 | float W2; |
| lorenzodunau | 26:4632a02a8ef1 | 386 | float W3; |
| lorenzodunau | 26:4632a02a8ef1 | 387 | float Rc; |
| lorenzodunau | 26:4632a02a8ef1 | 388 | float R; |
| lorenzodunau | 26:4632a02a8ef1 | 389 | float x1; |
| lorenzodunau | 26:4632a02a8ef1 | 390 | float x2; |
| lorenzodunau | 26:4632a02a8ef1 | 391 | float x3; |
| lorenzodunau | 26:4632a02a8ef1 | 392 | float y1; |
| lorenzodunau | 26:4632a02a8ef1 | 393 | float y2; |
| lorenzodunau | 26:4632a02a8ef1 | 394 | float y3; |
| lorenzodunau | 26:4632a02a8ef1 | 395 | float a1; |
| lorenzodunau | 26:4632a02a8ef1 | 396 | float a2; |
| lorenzodunau | 26:4632a02a8ef1 | 397 | float a3; |
| lorenzodunau | 26:4632a02a8ef1 | 398 | Rc=0.08; // rayon du chassis*10 |
| lorenzodunau | 26:4632a02a8ef1 | 399 | R=0.019; // rayon de la roue*10 |
| lorenzodunau | 26:4632a02a8ef1 | 400 | W1=0; |
| lorenzodunau | 26:4632a02a8ef1 | 401 | W2=0; |
| lorenzodunau | 26:4632a02a8ef1 | 402 | W3=0; |
| lorenzodunau | 29:0484cbad0770 | 403 | a1 = 4.74 ; |
| lorenzodunau | 29:0484cbad0770 | 404 | a2 = -5.8; |
| lorenzodunau | 29:0484cbad0770 | 405 | a3 = 3.68; |
| lorenzodunau | 29:0484cbad0770 | 406 | x1 = -0.0032; |
| lorenzodunau | 29:0484cbad0770 | 407 | x2 = 0.0616; |
| lorenzodunau | 29:0484cbad0770 | 408 | x3 = -0.0645; |
| lorenzodunau | 29:0484cbad0770 | 409 | y1 = 0.072; |
| lorenzodunau | 29:0484cbad0770 | 410 | y2 = -0.0356; |
| lorenzodunau | 29:0484cbad0770 | 411 | y3 = -0.0376; |
| lorenzodunau | 26:4632a02a8ef1 | 412 | //Vtm_smax=0.8; //sert pour calculer valeurs -999->999 |
| lorenzodunau | 26:4632a02a8ef1 | 413 | //Vnm_smax=0.9; |
| lorenzodunau | 26:4632a02a8ef1 | 414 | //Wcrd_smax=2.9; |
| lorenzodunau | 26:4632a02a8ef1 | 415 | //if (Vtm_s>Vtm_smax) |
| lorenzodunau | 28:c7402e1014b4 | 416 | // {Vtm_s=Vtm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 417 | //if (Vnm_s>Vnm_smax) |
| lorenzodunau | 28:c7402e1014b4 | 418 | // {Vnm_s=Vnm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 419 | //if (Wcrd_s>Wcrd_smax) |
| lorenzodunau | 28:c7402e1014b4 | 420 | // {Wcrd_s=Wcrd_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 421 | //if (Wcrd_s<-Wcrd_smax) |
| lorenzodunau | 28:c7402e1014b4 | 422 | // {Wcrd_s=-Wcrd_smax;} |
| lorenzodunau | 28:c7402e1014b4 | 423 | |
| lorenzodunau | 29:0484cbad0770 | 424 | W1=1/R*(-cosf(a1)*Vavance + sinf(a1)*y1*Wz - sinf(a1)*Vlat + cosf(a1)*x1*Wz); //loi de commande moteur 1 |
| lorenzodunau | 29:0484cbad0770 | 425 | W2=1/R*(-cosf(a2)*Vavance + sinf(a2)*y2*Wz - sinf(a2)*Vlat + cosf(a2)*x2*Wz); |
| lorenzodunau | 29:0484cbad0770 | 426 | W3=1/R*(-cosf(a3)*Vavance + sinf(a3)*y3*Wz - sinf(a3)*Vlat + cosf(a3)*x3*Wz); |
| lorenzodunau | 26:4632a02a8ef1 | 427 | printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3)); |
| lorenzodunau | 28:c7402e1014b4 | 428 | |
| lorenzodunau | 28:c7402e1014b4 | 429 | |
| lorenzodunau | 29:0484cbad0770 | 430 | SetSpeed_rad_s(1,W1); // impose la vitesse au moteur 1 |
| lorenzodunau | 27:06850c65b9c8 | 431 | SetSpeed_rad_s(2,W2); |
| lorenzodunau | 27:06850c65b9c8 | 432 | SetSpeed_rad_s(3,W3); |
| lorenzodunau | 28:c7402e1014b4 | 433 | |
| lorenzodunau | 28:c7402e1014b4 | 434 | |
| lorenzodunau | 26:4632a02a8ef1 | 435 | |
| lorenzodunau | 28:c7402e1014b4 | 436 | } |
| lorenzodunau | 28:c7402e1014b4 | 437 | |
| lorenzodunau | 29:0484cbad0770 | 438 | |
| lorenzodunau | 29:0484cbad0770 | 439 | |
| lorenzodunau | 28:c7402e1014b4 | 440 | void MX12::eteindre_moteurs() |
| lorenzodunau | 28:c7402e1014b4 | 441 | { |
| lorenzodunau | 28:c7402e1014b4 | 442 | char data[1]; |
| lorenzodunau | 27:06850c65b9c8 | 443 | |
| lorenzodunau | 27:06850c65b9c8 | 444 | data[0] = 0; |
| lorenzodunau | 27:06850c65b9c8 | 445 | |
| lorenzodunau | 28:c7402e1014b4 | 446 | |
| lorenzodunau | 27:06850c65b9c8 | 447 | // Send instruction |
| lorenzodunau | 27:06850c65b9c8 | 448 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 449 | rw(1, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 28:c7402e1014b4 | 450 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 451 | rw(2, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 27:06850c65b9c8 | 452 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 453 | rw(3, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 28:c7402e1014b4 | 454 | } |
| lorenzodunau | 28:c7402e1014b4 | 455 | |
| lorenzodunau | 29:0484cbad0770 | 456 | |
| lorenzodunau | 29:0484cbad0770 | 457 | |
| lorenzodunau | 29:0484cbad0770 | 458 | void MX12::cmd_moteur_multiturn(float pos1, float pos2, float pos3) |
| lorenzodunau | 28:c7402e1014b4 | 459 | { |
| lorenzodunau | 29:0484cbad0770 | 460 | // eteindre les moteurs |
| lorenzodunau | 29:0484cbad0770 | 461 | eteindre_moteurs(); |
| lorenzodunau | 29:0484cbad0770 | 462 | // mettre en mode multiturn |
| lorenzodunau | 29:0484cbad0770 | 463 | char data[2]; |
| lorenzodunau | 29:0484cbad0770 | 464 | data[1] = 255; |
| lorenzodunau | 29:0484cbad0770 | 465 | data[0] = 15; |
| lorenzodunau | 29:0484cbad0770 | 466 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 467 | rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 468 | rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 469 | rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 470 | |
| lorenzodunau | 29:0484cbad0770 | 471 | |
| lorenzodunau | 27:06850c65b9c8 | 472 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 473 | rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 474 | rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 475 | rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 476 | |
| lorenzodunau | 29:0484cbad0770 | 477 | //relever la position |
| lorenzodunau | 29:0484cbad0770 | 478 | int pas1 = pos1-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 479 | int pas2 = pos2-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 480 | int pas3 = pos3-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 481 | |
| lorenzodunau | 29:0484cbad0770 | 482 | for (int i = 0; i < 61; i++) { |
| lorenzodunau | 29:0484cbad0770 | 483 | int goal_position1 = i * pas1 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 484 | if (goal_position1 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 485 | goal_position1 = goal_position1 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 486 | } |
| lorenzodunau | 29:0484cbad0770 | 487 | char data1[2]; |
| lorenzodunau | 29:0484cbad0770 | 488 | data1[1] = goal_position1%256; |
| lorenzodunau | 29:0484cbad0770 | 489 | data1[0] = goal_position1/256; |
| lorenzodunau | 29:0484cbad0770 | 490 | |
| lorenzodunau | 29:0484cbad0770 | 491 | int goal_position2 = i * pas2 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 492 | if (goal_position2 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 493 | goal_position2 = goal_position2 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 494 | } |
| lorenzodunau | 29:0484cbad0770 | 495 | char data2[2]; |
| lorenzodunau | 29:0484cbad0770 | 496 | data2[1] = goal_position2%256; |
| lorenzodunau | 29:0484cbad0770 | 497 | data2[0] = goal_position2/256; |
| lorenzodunau | 29:0484cbad0770 | 498 | |
| lorenzodunau | 29:0484cbad0770 | 499 | int goal_position3 = i * pas3 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 500 | if (goal_position3 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 501 | goal_position3 = goal_position3 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 502 | } |
| lorenzodunau | 29:0484cbad0770 | 503 | char data3[2]; |
| lorenzodunau | 29:0484cbad0770 | 504 | data3[1] = goal_position3%256; |
| lorenzodunau | 29:0484cbad0770 | 505 | data3[0] = goal_position3/256; |
| lorenzodunau | 29:0484cbad0770 | 506 | |
| lorenzodunau | 29:0484cbad0770 | 507 | rw(1, CONTROL_TABLE_GOAL_POSITION, 2, data1); |
| lorenzodunau | 29:0484cbad0770 | 508 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 509 | rw(2, CONTROL_TABLE_GOAL_POSITION, 2, data2); |
| lorenzodunau | 29:0484cbad0770 | 510 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 511 | rw(3, CONTROL_TABLE_GOAL_POSITION, 2, data3); |
| lorenzodunau | 29:0484cbad0770 | 512 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 513 | |
| lorenzodunau | 29:0484cbad0770 | 514 | thread_sleep_for(1000); |
| lorenzodunau | 29:0484cbad0770 | 515 | } |
| lorenzodunau | 27:06850c65b9c8 | 516 | } |