PSL_2021 / robot_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.

Dependencies:   servomotor_MX12_Lorenzo

MX12.cpp@24:2ab26ed08c83, 2021-11-13 (annotated)

Committer:
bruno2nis
Date:
Sat Nov 13 10:12:09 2021 +0100
Revision:
24:2ab26ed08c83
Parent:
22:8fa806e3ece4
Child:
26:4632a02a8ef1
Add MX12::SetSpeed_rad_s() method

Who changed what in which revision?

UserRevisionLine numberNew 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"
denis2nis 0:7556356a8bcd 8
denis2nis 8:e74ef93ae660 9 MX12::MX12(PinName tx, PinName rx, int baud)
denis2nis 8:e74ef93ae660 10 : _mx12(tx, rx) // initializes UnbufferedSerial object
denis2nis 8:e74ef93ae660 11 {
denis2nis 8:e74ef93ae660 12 /* Serial bus setup
denis2nis 8:e74ef93ae660 13 */
denis2nis 8:e74ef93ae660 14 // // Set desired properties (baud-8-N-1)
denis2nis 8:e74ef93ae660 15 _mx12.baud(baud); /* modulation speed */
denis2nis 8:e74ef93ae660 16 _mx12.format(
denis2nis 8:e74ef93ae660 17 8, /* bits */
denis2nis 8:e74ef93ae660 18 SerialBase::None, /* parity */
denis2nis 8:e74ef93ae660 19 1 /* stop bit */
denis2nis 8:e74ef93ae660 20 );
denis2nis 8:e74ef93ae660 21 // Register a callback to process a Rx (receive) interrupt.
denis2nis 13:ccda9a56fef1 22 _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq);
denis2nis 5:0cf54586a4be 23
denis2nis 8:e74ef93ae660 24 // variable used for message reception
denis2nis 13:ccda9a56fef1 25 _status_pck = {.raw = "",
denis2nis 13:ccda9a56fef1 26 .n_byte = 0,
denis2nis 13:ccda9a56fef1 27 .servo_id = 0,
denis2nis 13:ccda9a56fef1 28 .length = 0,
denis2nis 13:ccda9a56fef1 29 .error = 0,
denis2nis 13:ccda9a56fef1 30 .n_param = 0,
denis2nis 13:ccda9a56fef1 31 .param = "",
denis2nis 13:ccda9a56fef1 32 .received_checksum = 0,
denis2nis 13:ccda9a56fef1 33 .calculated_checksum = 0,
denis2nis 13:ccda9a56fef1 34 .parsed = false,
denis2nis 13:ccda9a56fef1 35 .valid = false};
denis2nis 13:ccda9a56fef1 36
denis2nis 13:ccda9a56fef1 37 _parser_state = {.expected_field = PacketField::Header1,
denis2nis 13:ccda9a56fef1 38 .byte_index = 0,
denis2nis 13:ccda9a56fef1 39 .param_index = 0};
denis2nis 13:ccda9a56fef1 40
denis2nis 8:e74ef93ae660 41 // Internal defaults states
denis2nis 13:ccda9a56fef1 42 _bus_state = SerialState::Idle;
denis2nis 8:e74ef93ae660 43
denis2nis 0:7556356a8bcd 44 }
denis2nis 0:7556356a8bcd 45
denis2nis 0:7556356a8bcd 46 void MX12::SetSpeed(unsigned char mot_id, float speed) {
denis2nis 0:7556356a8bcd 47 char data[2];
denis2nis 8:e74ef93ae660 48
denis2nis 0:7556356a8bcd 49 // Speed absolute value
denis2nis 0:7556356a8bcd 50 int goal = (0x3ff * abs(speed));
denis2nis 0:7556356a8bcd 51
denis2nis 0:7556356a8bcd 52 // Spin direction (CW is negative)
denis2nis 0:7556356a8bcd 53 if (speed < 0) {
denis2nis 0:7556356a8bcd 54 goal |= (0x1 << 10);
denis2nis 0:7556356a8bcd 55 }
denis2nis 0:7556356a8bcd 56
denis2nis 0:7556356a8bcd 57 data[0] = goal & 0xff;
denis2nis 0:7556356a8bcd 58 data[1] = goal >> 8;
denis2nis 0:7556356a8bcd 59
denis2nis 0:7556356a8bcd 60 // Send instruction
denis2nis 13:ccda9a56fef1 61 _bus_state = SerialState::Writing;
denis2nis 5:0cf54586a4be 62 rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
denis2nis 0:7556356a8bcd 63 }
denis2nis 0:7556356a8bcd 64
bruno2nis 24:2ab26ed08c83 65 void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed)
bruno2nis 24:2ab26ed08c83 66 {
bruno2nis 24:2ab26ed08c83 67 if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S)
bruno2nis 24:2ab26ed08c83 68 {
bruno2nis 24:2ab26ed08c83 69 SetSpeed(mot_id, 1);
bruno2nis 24:2ab26ed08c83 70 }
bruno2nis 24:2ab26ed08c83 71 else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S)
bruno2nis 24:2ab26ed08c83 72 {
bruno2nis 24:2ab26ed08c83 73 SetSpeed(mot_id, -1);
bruno2nis 24:2ab26ed08c83 74 }
bruno2nis 24:2ab26ed08c83 75 else
bruno2nis 24:2ab26ed08c83 76 {
bruno2nis 24:2ab26ed08c83 77 SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S);
bruno2nis 24:2ab26ed08c83 78 }
bruno2nis 24:2ab26ed08c83 79 }
bruno2nis 24:2ab26ed08c83 80
denis2nis 0:7556356a8bcd 81 char MX12::IsAvailable(void) {
denis2nis 13:ccda9a56fef1 82 return (_bus_state == SerialState::Idle);
denis2nis 0:7556356a8bcd 83 }
denis2nis 0:7556356a8bcd 84
denis2nis 5:0cf54586a4be 85 void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
denis2nis 5:0cf54586a4be 86
denis2nis 5:0cf54586a4be 87 /* Set variables for reception from servovotor */
denis2nis 5:0cf54586a4be 88 _answer = 0;
denis2nis 13:ccda9a56fef1 89 _status_pck = {.raw = "",
denis2nis 13:ccda9a56fef1 90 .n_byte = 0,
denis2nis 13:ccda9a56fef1 91 .servo_id = 0,
denis2nis 13:ccda9a56fef1 92 .length = 0,
denis2nis 13:ccda9a56fef1 93 .error = 0,
denis2nis 13:ccda9a56fef1 94 .n_param = 0,
denis2nis 13:ccda9a56fef1 95 .param = "",
denis2nis 13:ccda9a56fef1 96 .received_checksum = 0,
denis2nis 13:ccda9a56fef1 97 .calculated_checksum = 0,
denis2nis 13:ccda9a56fef1 98 .parsed = false,
denis2nis 13:ccda9a56fef1 99 .valid = false};
denis2nis 13:ccda9a56fef1 100 _parser_state = {.expected_field = PacketField::Header1,
denis2nis 13:ccda9a56fef1 101 .byte_index = 0,
denis2nis 13:ccda9a56fef1 102 .param_index = 0};
denis2nis 13:ccda9a56fef1 103
denis2nis 5:0cf54586a4be 104
denis2nis 5:0cf54586a4be 105 /* Initialise instruction packet to forge.
denis2nis 5:0cf54586a4be 106 * Instruction Packet is the command data sent to the servomotor.
denis2nis 5:0cf54586a4be 107 *
denis2nis 5:0cf54586a4be 108 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
denis2nis 5:0cf54586a4be 109 * |-------|-------|---------|------|-----------|---------------|--------|
denis2nis 5:0cf54586a4be 110 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 5:0cf54586a4be 111 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
denis2nis 5:0cf54586a4be 112 * \__ ___/ \_ _/ \__ __/
denis2nis 5:0cf54586a4be 113 * \/ | \/ \/ |
denis2nis 5:0cf54586a4be 114 * mot_id | address data |
denis2nis 5:0cf54586a4be 115 * | (len = N-1) |
denis2nis 5:0cf54586a4be 116 * \__________________ ________________/
denis2nis 5:0cf54586a4be 117 * \/
denis2nis 5:0cf54586a4be 118 * Length ( cmd[3] )
denis2nis 5:0cf54586a4be 119 */
denis2nis 5:0cf54586a4be 120 char packet[16];
denis2nis 5:0cf54586a4be 121 unsigned char packet_length;
denis2nis 5:0cf54586a4be 122
denis2nis 5:0cf54586a4be 123 /* Initialise checksum to calculate
denis2nis 5:0cf54586a4be 124 * It is used to check if packet is damaged during communication.
denis2nis 5:0cf54586a4be 125 * Status Checksum is calculated according to the following formula:
denis2nis 5:0cf54586a4be 126 *
denis2nis 5:0cf54586a4be 127 * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
denis2nis 5:0cf54586a4be 128 */
denis2nis 5:0cf54586a4be 129 char checksum = 0x00;
denis2nis 5:0cf54586a4be 130
denis2nis 5:0cf54586a4be 131 /* header 1 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 132 packet[0] = 0xff;
denis2nis 5:0cf54586a4be 133
denis2nis 5:0cf54586a4be 134 /* header 2 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 135 packet[1] = 0xff;
denis2nis 5:0cf54586a4be 136
denis2nis 5:0cf54586a4be 137 /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 138 packet[2] = mot_id;
denis2nis 5:0cf54586a4be 139 checksum += packet[2];
denis2nis 5:0cf54586a4be 140
denis2nis 5:0cf54586a4be 141 /* Guess instruction type. NULL for read, not NULL for write */
denis2nis 5:0cf54586a4be 142 if(data == NULL) // read instruction
denis2nis 5:0cf54586a4be 143 {
denis2nis 5:0cf54586a4be 144 /* byte length of the instruction: parameter and checksum field. */
denis2nis 5:0cf54586a4be 145 /* for read instruction: 1 INSTR + */
denis2nis 5:0cf54586a4be 146 /* 2 PARAM (starting address, length of data) + 1 CHKSUM */
denis2nis 5:0cf54586a4be 147 packet[3] = 4;
denis2nis 5:0cf54586a4be 148 checksum += packet[3];
denis2nis 5:0cf54586a4be 149
denis2nis 5:0cf54586a4be 150 /* set write instruction */
denis2nis 5:0cf54586a4be 151 packet[4] = PROTOCOL_INSTRUCTION_READ;
denis2nis 5:0cf54586a4be 152 checksum += packet[4];
denis2nis 5:0cf54586a4be 153
denis2nis 5:0cf54586a4be 154 /* Param 1: address to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 155 packet[5] = address;
denis2nis 5:0cf54586a4be 156 checksum += packet[5];
denis2nis 5:0cf54586a4be 157
denis2nis 5:0cf54586a4be 158 /* Param 2: number of bytes to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 159 packet[6] = len;
denis2nis 5:0cf54586a4be 160 checksum += packet[6];
denis2nis 5:0cf54586a4be 161
denis2nis 5:0cf54586a4be 162 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 163 packet[7] = ~checksum;
denis2nis 5:0cf54586a4be 164
denis2nis 5:0cf54586a4be 165 packet_length = 8;
denis2nis 5:0cf54586a4be 166 }
denis2nis 5:0cf54586a4be 167 else // write instruction
denis2nis 5:0cf54586a4be 168 {
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 */
denis2nis 5:0cf54586a4be 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];
denis2nis 5:0cf54586a4be 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 }
denis2nis 5:0cf54586a4be 188
denis2nis 5:0cf54586a4be 189 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 190 packet[6 + len] = ~checksum;
denis2nis 5:0cf54586a4be 191
denis2nis 5:0cf54586a4be 192 packet_length = 7 + len;
denis2nis 5:0cf54586a4be 193 }
denis2nis 5:0cf54586a4be 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
denis2nis 9:b4a5187fdec6 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 }
denis2nis 5:0cf54586a4be 211
denis2nis 5:0cf54586a4be 212 MX12::Status MX12::GetStatus() {
denis2nis 5:0cf54586a4be 213 // Return the corresponding status code
denis2nis 13:ccda9a56fef1 214 switch(_status_pck.error) {
denis2nis 5:0cf54586a4be 215 case 0:
denis2nis 5:0cf54586a4be 216 return Ok;
denis2nis 5:0cf54586a4be 217 break;
denis2nis 5:0cf54586a4be 218 case 1 << 0:
denis2nis 5:0cf54586a4be 219 return InputVoltageError;
denis2nis 5:0cf54586a4be 220 break;
denis2nis 5:0cf54586a4be 221 case 1 << 1:
denis2nis 5:0cf54586a4be 222 return AngleLimitError;
denis2nis 5:0cf54586a4be 223 break;
denis2nis 5:0cf54586a4be 224 case 1 << 2:
denis2nis 5:0cf54586a4be 225 return OverheatingError;
denis2nis 5:0cf54586a4be 226 break;
denis2nis 5:0cf54586a4be 227 case 1 << 3:
denis2nis 5:0cf54586a4be 228 return RangeError;
denis2nis 5:0cf54586a4be 229 break;
denis2nis 5:0cf54586a4be 230 case 1 << 4:
denis2nis 5:0cf54586a4be 231 return ChecksumError;
denis2nis 5:0cf54586a4be 232 break;
denis2nis 5:0cf54586a4be 233 case 1 << 5:
denis2nis 5:0cf54586a4be 234 return OverloadError;
denis2nis 5:0cf54586a4be 235 break;
denis2nis 5:0cf54586a4be 236 case 1 << 6:
denis2nis 5:0cf54586a4be 237 return InstructionError;
denis2nis 5:0cf54586a4be 238 break;
denis2nis 5:0cf54586a4be 239 default:
denis2nis 5:0cf54586a4be 240 return Unknown;
denis2nis 5:0cf54586a4be 241 }
denis2nis 5:0cf54586a4be 242 }
denis2nis 5:0cf54586a4be 243
denis2nis 13:ccda9a56fef1 244 void MX12::_Rx_interrupt() {
denis2nis 8:e74ef93ae660 245
denis2nis 5:0cf54586a4be 246 char c;
denis2nis 5:0cf54586a4be 247
denis2nis 5:0cf54586a4be 248 // Try to read serial
denis2nis 5:0cf54586a4be 249 if(_mx12.read(&c, 1)) {
denis2nis 8:e74ef93ae660 250
denis2nis 13:ccda9a56fef1 251 _status_pck.raw[(_parser_state.byte_index)++] = c;
denis2nis 5:0cf54586a4be 252
denis2nis 5:0cf54586a4be 253 // State-machine parsing
denis2nis 13:ccda9a56fef1 254 switch(_parser_state.expected_field) {
denis2nis 13:ccda9a56fef1 255
denis2nis 13:ccda9a56fef1 256 /* c char is interpreted as a Header1 field */
denis2nis 13:ccda9a56fef1 257 case PacketField::Header1:
denis2nis 13:ccda9a56fef1 258
denis2nis 13:ccda9a56fef1 259 /* do nothing and set next state to Header2 */
denis2nis 13:ccda9a56fef1 260 _parser_state.expected_field = PacketField::Header2;
denis2nis 13:ccda9a56fef1 261 break;
denis2nis 13:ccda9a56fef1 262
denis2nis 13:ccda9a56fef1 263 /* c char is interpreted as a Header2 field */
denis2nis 13:ccda9a56fef1 264 case PacketField::Header2:
denis2nis 13:ccda9a56fef1 265
denis2nis 13:ccda9a56fef1 266 /* do nothing and set next state to Id */
denis2nis 13:ccda9a56fef1 267 _parser_state.expected_field = PacketField::Id;
denis2nis 13:ccda9a56fef1 268 break;
denis2nis 13:ccda9a56fef1 269
denis2nis 13:ccda9a56fef1 270 /* c char is interpreted as ID field */
denis2nis 13:ccda9a56fef1 271 case PacketField::Id:
denis2nis 5:0cf54586a4be 272
denis2nis 13:ccda9a56fef1 273 /* store ID, update checksum and set next state to Length */
denis2nis 13:ccda9a56fef1 274 _status_pck.servo_id = c;
denis2nis 13:ccda9a56fef1 275 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 276 _parser_state.expected_field = PacketField::Length;
denis2nis 5:0cf54586a4be 277 break;
denis2nis 13:ccda9a56fef1 278
denis2nis 13:ccda9a56fef1 279 /* c char is interpreted as length of message data field
denis2nis 13:ccda9a56fef1 280 * Length = number of Parameters + 2
denis2nis 13:ccda9a56fef1 281 * where 2 stands for Length field (1 byte) + Error filed (1 byte)
denis2nis 13:ccda9a56fef1 282 */
denis2nis 13:ccda9a56fef1 283 case PacketField::Length:
denis2nis 5:0cf54586a4be 284
denis2nis 13:ccda9a56fef1 285 /* store number of param into _status_pck.n_param,
denis2nis 13:ccda9a56fef1 286 * update calculated_checksum and set next state to Error
denis2nis 13:ccda9a56fef1 287 */
denis2nis 13:ccda9a56fef1 288 _status_pck.n_param = c - 2;
denis2nis 13:ccda9a56fef1 289 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 290 _parser_state.expected_field = PacketField::Error;
denis2nis 5:0cf54586a4be 291 break;
denis2nis 5:0cf54586a4be 292
denis2nis 13:ccda9a56fef1 293 /* c char is interpreted as error status field */
denis2nis 13:ccda9a56fef1 294 case PacketField::Error:
denis2nis 13:ccda9a56fef1 295
denis2nis 13:ccda9a56fef1 296 /* store error status, update checksum
denis2nis 13:ccda9a56fef1 297 * and set next state to Data
denis2nis 13:ccda9a56fef1 298 */
denis2nis 13:ccda9a56fef1 299 _status_pck.error = c;
denis2nis 13:ccda9a56fef1 300 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 301 _parser_state.expected_field = PacketField::Data;
denis2nis 5:0cf54586a4be 302 break;
denis2nis 5:0cf54586a4be 303
denis2nis 13:ccda9a56fef1 304 /* c char is interpreted as a param field */
denis2nis 13:ccda9a56fef1 305 case PacketField::Data:
denis2nis 13:ccda9a56fef1 306
denis2nis 13:ccda9a56fef1 307 /* store current param, increase param_index
denis2nis 13:ccda9a56fef1 308 * and update checksum */
denis2nis 13:ccda9a56fef1 309 _status_pck.param[(_parser_state.param_index)++] = c;
denis2nis 13:ccda9a56fef1 310 _status_pck.received_checksum += c;
denis2nis 5:0cf54586a4be 311
denis2nis 13:ccda9a56fef1 312 /* increase param index (_parser_state.dataCount)
denis2nis 13:ccda9a56fef1 313 * and test if it is the last param to read
denis2nis 13:ccda9a56fef1 314 */
denis2nis 13:ccda9a56fef1 315 if(_parser_state.param_index > _status_pck.n_param) {
denis2nis 13:ccda9a56fef1 316 /* reset param index and set next state to Checksum */
denis2nis 13:ccda9a56fef1 317 _parser_state.param_index = 0;
denis2nis 13:ccda9a56fef1 318 _parser_state.expected_field = PacketField::Checksum;
denis2nis 5:0cf54586a4be 319 }
denis2nis 5:0cf54586a4be 320 break;
denis2nis 5:0cf54586a4be 321
denis2nis 13:ccda9a56fef1 322 /* c char is interpreted as Checksum field */
denis2nis 13:ccda9a56fef1 323 case PacketField::Checksum:
denis2nis 13:ccda9a56fef1 324
denis2nis 13:ccda9a56fef1 325 /* store received_checksum, set parsed, store n_byte,
denis2nis 13:ccda9a56fef1 326 * evalutate valid and set next state to Header1 */
denis2nis 13:ccda9a56fef1 327 _status_pck.received_checksum = c;
denis2nis 13:ccda9a56fef1 328 _status_pck.parsed = true;
denis2nis 13:ccda9a56fef1 329 _status_pck.n_byte = _parser_state.byte_index;
denis2nis 13:ccda9a56fef1 330 _status_pck.valid = (_status_pck.received_checksum == c);
denis2nis 13:ccda9a56fef1 331 _parser_state.expected_field = PacketField::Header1;
denis2nis 13:ccda9a56fef1 332
denis2nis 13:ccda9a56fef1 333 /* set seriel state to Idle */
denis2nis 13:ccda9a56fef1 334 _bus_state = SerialState::Idle;
denis2nis 5:0cf54586a4be 335 break;
denis2nis 14:5a0c7b30f8c0 336
denis2nis 14:5a0c7b30f8c0 337 default:
denis2nis 14:5a0c7b30f8c0 338
denis2nis 14:5a0c7b30f8c0 339 /* unexpected case. If it occurs it would be due to a
denis2nis 14:5a0c7b30f8c0 340 * code error of this class */
denis2nis 22:8fa806e3ece4 341 break;
denis2nis 5:0cf54586a4be 342 }
denis2nis 5:0cf54586a4be 343 }
denis2nis 5:0cf54586a4be 344 }
denis2nis 5:0cf54586a4be 345
denis2nis 11:9bc7f5e2ccee 346 /* Code from previous version of the class */
denis2nis 11:9bc7f5e2ccee 347
denis2nis 5:0cf54586a4be 348 /*
denis2nis 0:7556356a8bcd 349 void MX12::ReadPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 350 // Make a request, interrupt takes care of everything else
denis2nis 0:7556356a8bcd 351 _state = State::ReadingPosition;
denis2nis 0:7556356a8bcd 352 rw(mot_id, 0x24, 2, NULL);
denis2nis 0:7556356a8bcd 353 }
denis2nis 0:7556356a8bcd 354
denis2nis 0:7556356a8bcd 355 float MX12::GetPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 356 return _angle[mot_id];
denis2nis 0:7556356a8bcd 357 }
denis2nis 5:0cf54586a4be 358 */