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@33:67e5482f83dd, 2022-06-15 (annotated)
- Committer:
- martinr
- Date:
- Wed Jun 15 13:04:37 2022 +0000
- Revision:
- 33:67e5482f83dd
- Parent:
- 31:e48d21e28023
zfev
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 | 30:4e7cf52ac2dd | 212 | char MX12::ReadSerial() |
| lorenzodunau | 30:4e7cf52ac2dd | 213 | { |
| lorenzodunau | 30:4e7cf52ac2dd | 214 | char data; |
| lorenzodunau | 30:4e7cf52ac2dd | 215 | for(int i = 0; i < _status_pck.n_byte; i++) { |
| lorenzodunau | 30:4e7cf52ac2dd | 216 | } |
| lorenzodunau | 30:4e7cf52ac2dd | 217 | return data; |
| lorenzodunau | 30:4e7cf52ac2dd | 218 | } |
| lorenzodunau | 30:4e7cf52ac2dd | 219 | |
| lorenzodunau | 28:c7402e1014b4 | 220 | MX12::Status MX12::GetStatus() |
| lorenzodunau | 28:c7402e1014b4 | 221 | { |
| denis2nis | 5:0cf54586a4be | 222 | // Return the corresponding status code |
| denis2nis | 13:ccda9a56fef1 | 223 | switch(_status_pck.error) { |
| denis2nis | 5:0cf54586a4be | 224 | case 0: |
| denis2nis | 5:0cf54586a4be | 225 | return Ok; |
| denis2nis | 5:0cf54586a4be | 226 | break; |
| denis2nis | 5:0cf54586a4be | 227 | case 1 << 0: |
| denis2nis | 5:0cf54586a4be | 228 | return InputVoltageError; |
| denis2nis | 5:0cf54586a4be | 229 | break; |
| denis2nis | 5:0cf54586a4be | 230 | case 1 << 1: |
| denis2nis | 5:0cf54586a4be | 231 | return AngleLimitError; |
| denis2nis | 5:0cf54586a4be | 232 | break; |
| denis2nis | 5:0cf54586a4be | 233 | case 1 << 2: |
| denis2nis | 5:0cf54586a4be | 234 | return OverheatingError; |
| denis2nis | 5:0cf54586a4be | 235 | break; |
| denis2nis | 5:0cf54586a4be | 236 | case 1 << 3: |
| denis2nis | 5:0cf54586a4be | 237 | return RangeError; |
| denis2nis | 5:0cf54586a4be | 238 | break; |
| denis2nis | 5:0cf54586a4be | 239 | case 1 << 4: |
| denis2nis | 5:0cf54586a4be | 240 | return ChecksumError; |
| denis2nis | 5:0cf54586a4be | 241 | break; |
| denis2nis | 5:0cf54586a4be | 242 | case 1 << 5: |
| denis2nis | 5:0cf54586a4be | 243 | return OverloadError; |
| denis2nis | 5:0cf54586a4be | 244 | break; |
| denis2nis | 5:0cf54586a4be | 245 | case 1 << 6: |
| denis2nis | 5:0cf54586a4be | 246 | return InstructionError; |
| denis2nis | 5:0cf54586a4be | 247 | break; |
| denis2nis | 5:0cf54586a4be | 248 | default: |
| denis2nis | 5:0cf54586a4be | 249 | return Unknown; |
| denis2nis | 5:0cf54586a4be | 250 | } |
| denis2nis | 5:0cf54586a4be | 251 | } |
| lorenzodunau | 28:c7402e1014b4 | 252 | |
| lorenzodunau | 28:c7402e1014b4 | 253 | void MX12::_Rx_interrupt() |
| lorenzodunau | 28:c7402e1014b4 | 254 | { |
| denis2nis | 8:e74ef93ae660 | 255 | |
| denis2nis | 5:0cf54586a4be | 256 | char c; |
| lorenzodunau | 28:c7402e1014b4 | 257 | |
| denis2nis | 5:0cf54586a4be | 258 | // Try to read serial |
| denis2nis | 5:0cf54586a4be | 259 | if(_mx12.read(&c, 1)) { |
| lorenzodunau | 28:c7402e1014b4 | 260 | |
| denis2nis | 13:ccda9a56fef1 | 261 | _status_pck.raw[(_parser_state.byte_index)++] = c; |
| denis2nis | 5:0cf54586a4be | 262 | |
| denis2nis | 5:0cf54586a4be | 263 | // State-machine parsing |
| denis2nis | 13:ccda9a56fef1 | 264 | switch(_parser_state.expected_field) { |
| lorenzodunau | 28:c7402e1014b4 | 265 | |
| denis2nis | 13:ccda9a56fef1 | 266 | /* c char is interpreted as a Header1 field */ |
| denis2nis | 13:ccda9a56fef1 | 267 | case PacketField::Header1: |
| lorenzodunau | 28:c7402e1014b4 | 268 | |
| denis2nis | 13:ccda9a56fef1 | 269 | /* do nothing and set next state to Header2 */ |
| denis2nis | 13:ccda9a56fef1 | 270 | _parser_state.expected_field = PacketField::Header2; |
| denis2nis | 13:ccda9a56fef1 | 271 | break; |
| denis2nis | 13:ccda9a56fef1 | 272 | |
| denis2nis | 13:ccda9a56fef1 | 273 | /* c char is interpreted as a Header2 field */ |
| denis2nis | 13:ccda9a56fef1 | 274 | case PacketField::Header2: |
| lorenzodunau | 28:c7402e1014b4 | 275 | |
| denis2nis | 13:ccda9a56fef1 | 276 | /* do nothing and set next state to Id */ |
| denis2nis | 13:ccda9a56fef1 | 277 | _parser_state.expected_field = PacketField::Id; |
| denis2nis | 13:ccda9a56fef1 | 278 | break; |
| lorenzodunau | 28:c7402e1014b4 | 279 | |
| denis2nis | 13:ccda9a56fef1 | 280 | /* c char is interpreted as ID field */ |
| denis2nis | 13:ccda9a56fef1 | 281 | case PacketField::Id: |
| lorenzodunau | 28:c7402e1014b4 | 282 | |
| denis2nis | 13:ccda9a56fef1 | 283 | /* store ID, update checksum and set next state to Length */ |
| denis2nis | 13:ccda9a56fef1 | 284 | _status_pck.servo_id = c; |
| lorenzodunau | 28:c7402e1014b4 | 285 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 286 | _parser_state.expected_field = PacketField::Length; |
| denis2nis | 5:0cf54586a4be | 287 | break; |
| denis2nis | 13:ccda9a56fef1 | 288 | |
| denis2nis | 13:ccda9a56fef1 | 289 | /* c char is interpreted as length of message data field |
| denis2nis | 13:ccda9a56fef1 | 290 | * Length = number of Parameters + 2 |
| denis2nis | 13:ccda9a56fef1 | 291 | * where 2 stands for Length field (1 byte) + Error filed (1 byte) |
| denis2nis | 13:ccda9a56fef1 | 292 | */ |
| denis2nis | 13:ccda9a56fef1 | 293 | case PacketField::Length: |
| lorenzodunau | 28:c7402e1014b4 | 294 | |
| lorenzodunau | 28:c7402e1014b4 | 295 | /* store number of param into _status_pck.n_param, |
| lorenzodunau | 28:c7402e1014b4 | 296 | * update calculated_checksum and set next state to Error |
| denis2nis | 13:ccda9a56fef1 | 297 | */ |
| denis2nis | 13:ccda9a56fef1 | 298 | _status_pck.n_param = c - 2; |
| lorenzodunau | 28:c7402e1014b4 | 299 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 300 | _parser_state.expected_field = PacketField::Error; |
| denis2nis | 5:0cf54586a4be | 301 | break; |
| lorenzodunau | 28:c7402e1014b4 | 302 | |
| denis2nis | 13:ccda9a56fef1 | 303 | /* c char is interpreted as error status field */ |
| denis2nis | 13:ccda9a56fef1 | 304 | case PacketField::Error: |
| lorenzodunau | 28:c7402e1014b4 | 305 | |
| lorenzodunau | 28:c7402e1014b4 | 306 | /* store error status, update checksum |
| lorenzodunau | 28:c7402e1014b4 | 307 | * and set next state to Data |
| denis2nis | 13:ccda9a56fef1 | 308 | */ |
| denis2nis | 13:ccda9a56fef1 | 309 | _status_pck.error = c; |
| lorenzodunau | 28:c7402e1014b4 | 310 | _status_pck.calculated_checksum += c; |
| denis2nis | 13:ccda9a56fef1 | 311 | _parser_state.expected_field = PacketField::Data; |
| denis2nis | 5:0cf54586a4be | 312 | break; |
| lorenzodunau | 28:c7402e1014b4 | 313 | |
| denis2nis | 13:ccda9a56fef1 | 314 | /* c char is interpreted as a param field */ |
| denis2nis | 13:ccda9a56fef1 | 315 | case PacketField::Data: |
| denis2nis | 13:ccda9a56fef1 | 316 | |
| lorenzodunau | 28:c7402e1014b4 | 317 | /* store current param, increase param_index |
| denis2nis | 13:ccda9a56fef1 | 318 | * and update checksum */ |
| denis2nis | 13:ccda9a56fef1 | 319 | _status_pck.param[(_parser_state.param_index)++] = c; |
| lorenzodunau | 28:c7402e1014b4 | 320 | _status_pck.received_checksum += c; |
| lorenzodunau | 28:c7402e1014b4 | 321 | |
| denis2nis | 13:ccda9a56fef1 | 322 | /* increase param index (_parser_state.dataCount) |
| denis2nis | 13:ccda9a56fef1 | 323 | * and test if it is the last param to read |
| denis2nis | 13:ccda9a56fef1 | 324 | */ |
| denis2nis | 13:ccda9a56fef1 | 325 | if(_parser_state.param_index > _status_pck.n_param) { |
| denis2nis | 13:ccda9a56fef1 | 326 | /* reset param index and set next state to Checksum */ |
| denis2nis | 13:ccda9a56fef1 | 327 | _parser_state.param_index = 0; |
| denis2nis | 13:ccda9a56fef1 | 328 | _parser_state.expected_field = PacketField::Checksum; |
| denis2nis | 5:0cf54586a4be | 329 | } |
| denis2nis | 5:0cf54586a4be | 330 | break; |
| lorenzodunau | 28:c7402e1014b4 | 331 | |
| denis2nis | 13:ccda9a56fef1 | 332 | /* c char is interpreted as Checksum field */ |
| denis2nis | 13:ccda9a56fef1 | 333 | case PacketField::Checksum: |
| lorenzodunau | 28:c7402e1014b4 | 334 | |
| denis2nis | 13:ccda9a56fef1 | 335 | /* store received_checksum, set parsed, store n_byte, |
| denis2nis | 13:ccda9a56fef1 | 336 | * evalutate valid and set next state to Header1 */ |
| denis2nis | 13:ccda9a56fef1 | 337 | _status_pck.received_checksum = c; |
| denis2nis | 13:ccda9a56fef1 | 338 | _status_pck.parsed = true; |
| denis2nis | 13:ccda9a56fef1 | 339 | _status_pck.n_byte = _parser_state.byte_index; |
| denis2nis | 13:ccda9a56fef1 | 340 | _status_pck.valid = (_status_pck.received_checksum == c); |
| denis2nis | 13:ccda9a56fef1 | 341 | _parser_state.expected_field = PacketField::Header1; |
| lorenzodunau | 28:c7402e1014b4 | 342 | |
| denis2nis | 13:ccda9a56fef1 | 343 | /* set seriel state to Idle */ |
| denis2nis | 13:ccda9a56fef1 | 344 | _bus_state = SerialState::Idle; |
| denis2nis | 5:0cf54586a4be | 345 | break; |
| lorenzodunau | 28:c7402e1014b4 | 346 | |
| denis2nis | 14:5a0c7b30f8c0 | 347 | default: |
| lorenzodunau | 28:c7402e1014b4 | 348 | |
| lorenzodunau | 28:c7402e1014b4 | 349 | /* unexpected case. If it occurs it would be due to a |
| denis2nis | 14:5a0c7b30f8c0 | 350 | * code error of this class */ |
| denis2nis | 22:8fa806e3ece4 | 351 | break; |
| lorenzodunau | 28:c7402e1014b4 | 352 | } |
| denis2nis | 5:0cf54586a4be | 353 | } |
| denis2nis | 5:0cf54586a4be | 354 | } |
| denis2nis | 5:0cf54586a4be | 355 | |
| denis2nis | 11:9bc7f5e2ccee | 356 | /* Code from previous version of the class */ |
| denis2nis | 11:9bc7f5e2ccee | 357 | |
| denis2nis | 5:0cf54586a4be | 358 | /* |
| denis2nis | 0:7556356a8bcd | 359 | void MX12::ReadPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 360 | // Make a request, interrupt takes care of everything else |
| denis2nis | 0:7556356a8bcd | 361 | _state = State::ReadingPosition; |
| denis2nis | 0:7556356a8bcd | 362 | rw(mot_id, 0x24, 2, NULL); |
| denis2nis | 0:7556356a8bcd | 363 | } |
| denis2nis | 0:7556356a8bcd | 364 | |
| denis2nis | 0:7556356a8bcd | 365 | float MX12::GetPosition(unsigned char mot_id) { |
| denis2nis | 0:7556356a8bcd | 366 | return _angle[mot_id]; |
| denis2nis | 0:7556356a8bcd | 367 | } |
| denis2nis | 5:0cf54586a4be | 368 | */ |
| lorenzodunau | 26:4632a02a8ef1 | 369 | |
| lorenzodunau | 29:0484cbad0770 | 370 | |
| lorenzodunau | 29:0484cbad0770 | 371 | |
| lorenzodunau | 29:0484cbad0770 | 372 | |
| lorenzodunau | 29:0484cbad0770 | 373 | |
| lorenzodunau | 29:0484cbad0770 | 374 | |
| lorenzodunau | 28:c7402e1014b4 | 375 | void MX12::cmd_moteur(float Vavance, float Vlat, float Wz) |
| lorenzodunau | 28:c7402e1014b4 | 376 | { |
| lorenzodunau | 29:0484cbad0770 | 377 | // mettre en mode wh |
| lorenzodunau | 29:0484cbad0770 | 378 | char data[2]; |
| lorenzodunau | 29:0484cbad0770 | 379 | data[1] = 0; |
| lorenzodunau | 29:0484cbad0770 | 380 | data[0] = 0; |
| lorenzodunau | 29:0484cbad0770 | 381 | |
| lorenzodunau | 29:0484cbad0770 | 382 | |
| lorenzodunau | 26:4632a02a8ef1 | 383 | float W1; |
| lorenzodunau | 26:4632a02a8ef1 | 384 | float W2; |
| lorenzodunau | 26:4632a02a8ef1 | 385 | float W3; |
| lorenzodunau | 26:4632a02a8ef1 | 386 | float Rc; |
| lorenzodunau | 26:4632a02a8ef1 | 387 | float R; |
| lorenzodunau | 26:4632a02a8ef1 | 388 | float x1; |
| lorenzodunau | 26:4632a02a8ef1 | 389 | float x2; |
| lorenzodunau | 26:4632a02a8ef1 | 390 | float x3; |
| lorenzodunau | 26:4632a02a8ef1 | 391 | float y1; |
| lorenzodunau | 26:4632a02a8ef1 | 392 | float y2; |
| lorenzodunau | 26:4632a02a8ef1 | 393 | float y3; |
| lorenzodunau | 26:4632a02a8ef1 | 394 | float a1; |
| lorenzodunau | 26:4632a02a8ef1 | 395 | float a2; |
| lorenzodunau | 26:4632a02a8ef1 | 396 | float a3; |
| lorenzodunau | 26:4632a02a8ef1 | 397 | Rc=0.08; // rayon du chassis*10 |
| lorenzodunau | 26:4632a02a8ef1 | 398 | R=0.019; // rayon de la roue*10 |
| lorenzodunau | 26:4632a02a8ef1 | 399 | W1=0; |
| lorenzodunau | 26:4632a02a8ef1 | 400 | W2=0; |
| lorenzodunau | 26:4632a02a8ef1 | 401 | W3=0; |
| martinr | 31:e48d21e28023 | 402 | a1 = 3.74 ; |
| martinr | 31:e48d21e28023 | 403 | a2 = 1.57; |
| martinr | 31:e48d21e28023 | 404 | a3 = 5.86; |
| martinr | 31:e48d21e28023 | 405 | x1 = -6.87; |
| martinr | 31:e48d21e28023 | 406 | x2 = -0.58; |
| martinr | 31:e48d21e28023 | 407 | x3 = 7.78; |
| martinr | 31:e48d21e28023 | 408 | y1 = -5.09; |
| martinr | 31:e48d21e28023 | 409 | y2 = 8.22; |
| martinr | 31:e48d21e28023 | 410 | y3 = -4.2; |
| lorenzodunau | 26:4632a02a8ef1 | 411 | //Vtm_smax=0.8; //sert pour calculer valeurs -999->999 |
| lorenzodunau | 26:4632a02a8ef1 | 412 | //Vnm_smax=0.9; |
| lorenzodunau | 26:4632a02a8ef1 | 413 | //Wcrd_smax=2.9; |
| lorenzodunau | 26:4632a02a8ef1 | 414 | //if (Vtm_s>Vtm_smax) |
| lorenzodunau | 28:c7402e1014b4 | 415 | // {Vtm_s=Vtm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 416 | //if (Vnm_s>Vnm_smax) |
| lorenzodunau | 28:c7402e1014b4 | 417 | // {Vnm_s=Vnm_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 418 | //if (Wcrd_s>Wcrd_smax) |
| lorenzodunau | 28:c7402e1014b4 | 419 | // {Wcrd_s=Wcrd_smax;} |
| lorenzodunau | 26:4632a02a8ef1 | 420 | //if (Wcrd_s<-Wcrd_smax) |
| lorenzodunau | 28:c7402e1014b4 | 421 | // {Wcrd_s=-Wcrd_smax;} |
| lorenzodunau | 28:c7402e1014b4 | 422 | |
| lorenzodunau | 29:0484cbad0770 | 423 | 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 | 424 | W2=1/R*(-cosf(a2)*Vavance + sinf(a2)*y2*Wz - sinf(a2)*Vlat + cosf(a2)*x2*Wz); |
| lorenzodunau | 29:0484cbad0770 | 425 | W3=1/R*(-cosf(a3)*Vavance + sinf(a3)*y3*Wz - sinf(a3)*Vlat + cosf(a3)*x3*Wz); |
| lorenzodunau | 26:4632a02a8ef1 | 426 | printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3)); |
| lorenzodunau | 28:c7402e1014b4 | 427 | |
| lorenzodunau | 28:c7402e1014b4 | 428 | |
| lorenzodunau | 29:0484cbad0770 | 429 | SetSpeed_rad_s(1,W1); // impose la vitesse au moteur 1 |
| lorenzodunau | 27:06850c65b9c8 | 430 | SetSpeed_rad_s(2,W2); |
| lorenzodunau | 27:06850c65b9c8 | 431 | SetSpeed_rad_s(3,W3); |
| lorenzodunau | 28:c7402e1014b4 | 432 | |
| lorenzodunau | 28:c7402e1014b4 | 433 | |
| lorenzodunau | 26:4632a02a8ef1 | 434 | |
| lorenzodunau | 28:c7402e1014b4 | 435 | } |
| lorenzodunau | 28:c7402e1014b4 | 436 | |
| lorenzodunau | 30:4e7cf52ac2dd | 437 | void MX12::cmd_moteur_rampe_unitaire(int id_moteur, float w_moteur, int step, float time) |
| lorenzodunau | 30:4e7cf52ac2dd | 438 | { |
| lorenzodunau | 29:0484cbad0770 | 439 | |
| lorenzodunau | 30:4e7cf52ac2dd | 440 | } |
| lorenzodunau | 29:0484cbad0770 | 441 | |
| lorenzodunau | 28:c7402e1014b4 | 442 | void MX12::eteindre_moteurs() |
| lorenzodunau | 28:c7402e1014b4 | 443 | { |
| lorenzodunau | 28:c7402e1014b4 | 444 | char data[1]; |
| lorenzodunau | 27:06850c65b9c8 | 445 | |
| lorenzodunau | 27:06850c65b9c8 | 446 | data[0] = 0; |
| lorenzodunau | 27:06850c65b9c8 | 447 | |
| lorenzodunau | 28:c7402e1014b4 | 448 | |
| lorenzodunau | 27:06850c65b9c8 | 449 | // Send instruction |
| lorenzodunau | 27:06850c65b9c8 | 450 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 451 | rw(1, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 28:c7402e1014b4 | 452 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 453 | rw(2, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 27:06850c65b9c8 | 454 | _bus_state = SerialState::Writing; |
| lorenzodunau | 28:c7402e1014b4 | 455 | rw(3, CONTROL_TABLE_TORQUE_ENABLE, 1, data); |
| lorenzodunau | 28:c7402e1014b4 | 456 | } |
| lorenzodunau | 28:c7402e1014b4 | 457 | |
| lorenzodunau | 29:0484cbad0770 | 458 | |
| lorenzodunau | 29:0484cbad0770 | 459 | |
| lorenzodunau | 29:0484cbad0770 | 460 | void MX12::cmd_moteur_multiturn(float pos1, float pos2, float pos3) |
| lorenzodunau | 28:c7402e1014b4 | 461 | { |
| lorenzodunau | 29:0484cbad0770 | 462 | // eteindre les moteurs |
| lorenzodunau | 29:0484cbad0770 | 463 | eteindre_moteurs(); |
| lorenzodunau | 29:0484cbad0770 | 464 | // mettre en mode multiturn |
| lorenzodunau | 29:0484cbad0770 | 465 | char data[2]; |
| lorenzodunau | 29:0484cbad0770 | 466 | data[1] = 255; |
| lorenzodunau | 29:0484cbad0770 | 467 | data[0] = 15; |
| lorenzodunau | 29:0484cbad0770 | 468 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 469 | rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 470 | rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 471 | rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 472 | |
| lorenzodunau | 29:0484cbad0770 | 473 | |
| lorenzodunau | 27:06850c65b9c8 | 474 | _bus_state = SerialState::Writing; |
| lorenzodunau | 29:0484cbad0770 | 475 | rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 476 | rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 477 | rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data); |
| lorenzodunau | 29:0484cbad0770 | 478 | |
| lorenzodunau | 29:0484cbad0770 | 479 | //relever la position |
| lorenzodunau | 29:0484cbad0770 | 480 | int pas1 = pos1-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 481 | int pas2 = pos2-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 482 | int pas3 = pos3-28672/60; |
| lorenzodunau | 29:0484cbad0770 | 483 | |
| lorenzodunau | 29:0484cbad0770 | 484 | for (int i = 0; i < 61; i++) { |
| lorenzodunau | 29:0484cbad0770 | 485 | int goal_position1 = i * pas1 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 486 | if (goal_position1 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 487 | goal_position1 = goal_position1 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 488 | } |
| lorenzodunau | 29:0484cbad0770 | 489 | char data1[2]; |
| lorenzodunau | 29:0484cbad0770 | 490 | data1[1] = goal_position1%256; |
| lorenzodunau | 29:0484cbad0770 | 491 | data1[0] = goal_position1/256; |
| lorenzodunau | 29:0484cbad0770 | 492 | |
| lorenzodunau | 29:0484cbad0770 | 493 | int goal_position2 = i * pas2 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 494 | if (goal_position2 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 495 | goal_position2 = goal_position2 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 496 | } |
| lorenzodunau | 29:0484cbad0770 | 497 | char data2[2]; |
| lorenzodunau | 29:0484cbad0770 | 498 | data2[1] = goal_position2%256; |
| lorenzodunau | 29:0484cbad0770 | 499 | data2[0] = goal_position2/256; |
| lorenzodunau | 29:0484cbad0770 | 500 | |
| lorenzodunau | 29:0484cbad0770 | 501 | int goal_position3 = i * pas3 - 28672; |
| lorenzodunau | 29:0484cbad0770 | 502 | if (goal_position3 < 0) { |
| lorenzodunau | 29:0484cbad0770 | 503 | goal_position3 = goal_position3 + 28627 + 36864; |
| lorenzodunau | 29:0484cbad0770 | 504 | } |
| lorenzodunau | 29:0484cbad0770 | 505 | char data3[2]; |
| lorenzodunau | 29:0484cbad0770 | 506 | data3[1] = goal_position3%256; |
| lorenzodunau | 29:0484cbad0770 | 507 | data3[0] = goal_position3/256; |
| lorenzodunau | 29:0484cbad0770 | 508 | |
| lorenzodunau | 29:0484cbad0770 | 509 | rw(1, CONTROL_TABLE_GOAL_POSITION, 2, data1); |
| lorenzodunau | 29:0484cbad0770 | 510 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 511 | rw(2, CONTROL_TABLE_GOAL_POSITION, 2, data2); |
| lorenzodunau | 29:0484cbad0770 | 512 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 513 | rw(3, CONTROL_TABLE_GOAL_POSITION, 2, data3); |
| lorenzodunau | 29:0484cbad0770 | 514 | thread_sleep_for(60); |
| lorenzodunau | 29:0484cbad0770 | 515 | |
| lorenzodunau | 29:0484cbad0770 | 516 | thread_sleep_for(1000); |
| lorenzodunau | 29:0484cbad0770 | 517 | } |
| lorenzodunau | 27:06850c65b9c8 | 518 | } |