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@22:8fa806e3ece4, 2021-11-07 (annotated)

Committer:
denis2nis
Date:
Sun Nov 07 11:34:38 2021 +0000
Revision:
22:8fa806e3ece4
Parent:
14:5a0c7b30f8c0
Child:
24:2ab26ed08c83
Version 1.0 (ok for SetSpeed)

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
denis2nis 0:7556356a8bcd 65 char MX12::IsAvailable(void) {
denis2nis 13:ccda9a56fef1 66 return (_bus_state == SerialState::Idle);
denis2nis 0:7556356a8bcd 67 }
denis2nis 0:7556356a8bcd 68
denis2nis 5:0cf54586a4be 69 void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
denis2nis 5:0cf54586a4be 70
denis2nis 5:0cf54586a4be 71 /* Set variables for reception from servovotor */
denis2nis 5:0cf54586a4be 72 _answer = 0;
denis2nis 13:ccda9a56fef1 73 _status_pck = {.raw = "",
denis2nis 13:ccda9a56fef1 74 .n_byte = 0,
denis2nis 13:ccda9a56fef1 75 .servo_id = 0,
denis2nis 13:ccda9a56fef1 76 .length = 0,
denis2nis 13:ccda9a56fef1 77 .error = 0,
denis2nis 13:ccda9a56fef1 78 .n_param = 0,
denis2nis 13:ccda9a56fef1 79 .param = "",
denis2nis 13:ccda9a56fef1 80 .received_checksum = 0,
denis2nis 13:ccda9a56fef1 81 .calculated_checksum = 0,
denis2nis 13:ccda9a56fef1 82 .parsed = false,
denis2nis 13:ccda9a56fef1 83 .valid = false};
denis2nis 13:ccda9a56fef1 84 _parser_state = {.expected_field = PacketField::Header1,
denis2nis 13:ccda9a56fef1 85 .byte_index = 0,
denis2nis 13:ccda9a56fef1 86 .param_index = 0};
denis2nis 13:ccda9a56fef1 87
denis2nis 5:0cf54586a4be 88
denis2nis 5:0cf54586a4be 89 /* Initialise instruction packet to forge.
denis2nis 5:0cf54586a4be 90 * Instruction Packet is the command data sent to the servomotor.
denis2nis 5:0cf54586a4be 91 *
denis2nis 5:0cf54586a4be 92 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
denis2nis 5:0cf54586a4be 93 * |-------|-------|---------|------|-----------|---------------|--------|
denis2nis 5:0cf54586a4be 94 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 5:0cf54586a4be 95 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
denis2nis 5:0cf54586a4be 96 * \__ ___/ \_ _/ \__ __/
denis2nis 5:0cf54586a4be 97 * \/ | \/ \/ |
denis2nis 5:0cf54586a4be 98 * mot_id | address data |
denis2nis 5:0cf54586a4be 99 * | (len = N-1) |
denis2nis 5:0cf54586a4be 100 * \__________________ ________________/
denis2nis 5:0cf54586a4be 101 * \/
denis2nis 5:0cf54586a4be 102 * Length ( cmd[3] )
denis2nis 5:0cf54586a4be 103 */
denis2nis 5:0cf54586a4be 104 char packet[16];
denis2nis 5:0cf54586a4be 105 unsigned char packet_length;
denis2nis 5:0cf54586a4be 106
denis2nis 5:0cf54586a4be 107 /* Initialise checksum to calculate
denis2nis 5:0cf54586a4be 108 * It is used to check if packet is damaged during communication.
denis2nis 5:0cf54586a4be 109 * Status Checksum is calculated according to the following formula:
denis2nis 5:0cf54586a4be 110 *
denis2nis 5:0cf54586a4be 111 * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
denis2nis 5:0cf54586a4be 112 */
denis2nis 5:0cf54586a4be 113 char checksum = 0x00;
denis2nis 5:0cf54586a4be 114
denis2nis 5:0cf54586a4be 115 /* header 1 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 116 packet[0] = 0xff;
denis2nis 5:0cf54586a4be 117
denis2nis 5:0cf54586a4be 118 /* header 2 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 119 packet[1] = 0xff;
denis2nis 5:0cf54586a4be 120
denis2nis 5:0cf54586a4be 121 /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 122 packet[2] = mot_id;
denis2nis 5:0cf54586a4be 123 checksum += packet[2];
denis2nis 5:0cf54586a4be 124
denis2nis 5:0cf54586a4be 125 /* Guess instruction type. NULL for read, not NULL for write */
denis2nis 5:0cf54586a4be 126 if(data == NULL) // read instruction
denis2nis 5:0cf54586a4be 127 {
denis2nis 5:0cf54586a4be 128 /* byte length of the instruction: parameter and checksum field. */
denis2nis 5:0cf54586a4be 129 /* for read instruction: 1 INSTR + */
denis2nis 5:0cf54586a4be 130 /* 2 PARAM (starting address, length of data) + 1 CHKSUM */
denis2nis 5:0cf54586a4be 131 packet[3] = 4;
denis2nis 5:0cf54586a4be 132 checksum += packet[3];
denis2nis 5:0cf54586a4be 133
denis2nis 5:0cf54586a4be 134 /* set write instruction */
denis2nis 5:0cf54586a4be 135 packet[4] = PROTOCOL_INSTRUCTION_READ;
denis2nis 5:0cf54586a4be 136 checksum += packet[4];
denis2nis 5:0cf54586a4be 137
denis2nis 5:0cf54586a4be 138 /* Param 1: address to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 139 packet[5] = address;
denis2nis 5:0cf54586a4be 140 checksum += packet[5];
denis2nis 5:0cf54586a4be 141
denis2nis 5:0cf54586a4be 142 /* Param 2: number of bytes to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 143 packet[6] = len;
denis2nis 5:0cf54586a4be 144 checksum += packet[6];
denis2nis 5:0cf54586a4be 145
denis2nis 5:0cf54586a4be 146 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 147 packet[7] = ~checksum;
denis2nis 5:0cf54586a4be 148
denis2nis 5:0cf54586a4be 149 packet_length = 8;
denis2nis 5:0cf54586a4be 150 }
denis2nis 5:0cf54586a4be 151 else // write instruction
denis2nis 5:0cf54586a4be 152 {
denis2nis 5:0cf54586a4be 153 /* byte length of the instruction: parameter and checksum field */
denis2nis 5:0cf54586a4be 154 /* For write instruction: 1 INSTR + */
denis2nis 5:0cf54586a4be 155 /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
denis2nis 5:0cf54586a4be 156 packet[3] = 3 + len;
denis2nis 5:0cf54586a4be 157 checksum += packet[3];
denis2nis 5:0cf54586a4be 158
denis2nis 5:0cf54586a4be 159 /* set read instruction */
denis2nis 5:0cf54586a4be 160 packet[4] = PROTOCOL_INSTRUCTION_WRITE;
denis2nis 5:0cf54586a4be 161 checksum += packet[4];
denis2nis 5:0cf54586a4be 162
denis2nis 5:0cf54586a4be 163 /* Param 1: address to write in the "Control Table of RAM Area" */
denis2nis 5:0cf54586a4be 164 packet[5] = address;
denis2nis 5:0cf54586a4be 165 checksum += packet[5];
denis2nis 5:0cf54586a4be 166
denis2nis 5:0cf54586a4be 167 /* Param 2 to N: data to write in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 168 for(char i = 0; i < len; i++) {
denis2nis 5:0cf54586a4be 169 packet[6 + i] = data[i];
denis2nis 5:0cf54586a4be 170 checksum += data[i];
denis2nis 5:0cf54586a4be 171 }
denis2nis 5:0cf54586a4be 172
denis2nis 5:0cf54586a4be 173 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 174 packet[6 + len] = ~checksum;
denis2nis 5:0cf54586a4be 175
denis2nis 5:0cf54586a4be 176 packet_length = 7 + len;
denis2nis 5:0cf54586a4be 177 }
denis2nis 5:0cf54586a4be 178
denis2nis 5:0cf54586a4be 179 // Send packet
denis2nis 5:0cf54586a4be 180 if(mot_id != 0xFE) {
denis2nis 5:0cf54586a4be 181 for(char i = 0; i < packet_length; i++) {
denis2nis 5:0cf54586a4be 182 _mx12.write(&packet[i], 1);
denis2nis 5:0cf54586a4be 183 }
denis2nis 5:0cf54586a4be 184 }
denis2nis 5:0cf54586a4be 185 }
denis2nis 5:0cf54586a4be 186
denis2nis 5:0cf54586a4be 187 // Debug function to print Serial read
denis2nis 9:b4a5187fdec6 188 void MX12::PrintSerial()
denis2nis 9:b4a5187fdec6 189 {
denis2nis 13:ccda9a56fef1 190 for(int i = 0; i < _status_pck.n_byte; i++) {
denis2nis 13:ccda9a56fef1 191 printf("%x ", _status_pck.raw[i]);
denis2nis 5:0cf54586a4be 192 }
denis2nis 5:0cf54586a4be 193 printf("\n");
denis2nis 5:0cf54586a4be 194 }
denis2nis 5:0cf54586a4be 195
denis2nis 5:0cf54586a4be 196 MX12::Status MX12::GetStatus() {
denis2nis 5:0cf54586a4be 197 // Return the corresponding status code
denis2nis 13:ccda9a56fef1 198 switch(_status_pck.error) {
denis2nis 5:0cf54586a4be 199 case 0:
denis2nis 5:0cf54586a4be 200 return Ok;
denis2nis 5:0cf54586a4be 201 break;
denis2nis 5:0cf54586a4be 202 case 1 << 0:
denis2nis 5:0cf54586a4be 203 return InputVoltageError;
denis2nis 5:0cf54586a4be 204 break;
denis2nis 5:0cf54586a4be 205 case 1 << 1:
denis2nis 5:0cf54586a4be 206 return AngleLimitError;
denis2nis 5:0cf54586a4be 207 break;
denis2nis 5:0cf54586a4be 208 case 1 << 2:
denis2nis 5:0cf54586a4be 209 return OverheatingError;
denis2nis 5:0cf54586a4be 210 break;
denis2nis 5:0cf54586a4be 211 case 1 << 3:
denis2nis 5:0cf54586a4be 212 return RangeError;
denis2nis 5:0cf54586a4be 213 break;
denis2nis 5:0cf54586a4be 214 case 1 << 4:
denis2nis 5:0cf54586a4be 215 return ChecksumError;
denis2nis 5:0cf54586a4be 216 break;
denis2nis 5:0cf54586a4be 217 case 1 << 5:
denis2nis 5:0cf54586a4be 218 return OverloadError;
denis2nis 5:0cf54586a4be 219 break;
denis2nis 5:0cf54586a4be 220 case 1 << 6:
denis2nis 5:0cf54586a4be 221 return InstructionError;
denis2nis 5:0cf54586a4be 222 break;
denis2nis 5:0cf54586a4be 223 default:
denis2nis 5:0cf54586a4be 224 return Unknown;
denis2nis 5:0cf54586a4be 225 }
denis2nis 5:0cf54586a4be 226 }
denis2nis 5:0cf54586a4be 227
denis2nis 13:ccda9a56fef1 228 void MX12::_Rx_interrupt() {
denis2nis 8:e74ef93ae660 229
denis2nis 5:0cf54586a4be 230 char c;
denis2nis 5:0cf54586a4be 231
denis2nis 5:0cf54586a4be 232 // Try to read serial
denis2nis 5:0cf54586a4be 233 if(_mx12.read(&c, 1)) {
denis2nis 8:e74ef93ae660 234
denis2nis 13:ccda9a56fef1 235 _status_pck.raw[(_parser_state.byte_index)++] = c;
denis2nis 5:0cf54586a4be 236
denis2nis 5:0cf54586a4be 237 // State-machine parsing
denis2nis 13:ccda9a56fef1 238 switch(_parser_state.expected_field) {
denis2nis 13:ccda9a56fef1 239
denis2nis 13:ccda9a56fef1 240 /* c char is interpreted as a Header1 field */
denis2nis 13:ccda9a56fef1 241 case PacketField::Header1:
denis2nis 13:ccda9a56fef1 242
denis2nis 13:ccda9a56fef1 243 /* do nothing and set next state to Header2 */
denis2nis 13:ccda9a56fef1 244 _parser_state.expected_field = PacketField::Header2;
denis2nis 13:ccda9a56fef1 245 break;
denis2nis 13:ccda9a56fef1 246
denis2nis 13:ccda9a56fef1 247 /* c char is interpreted as a Header2 field */
denis2nis 13:ccda9a56fef1 248 case PacketField::Header2:
denis2nis 13:ccda9a56fef1 249
denis2nis 13:ccda9a56fef1 250 /* do nothing and set next state to Id */
denis2nis 13:ccda9a56fef1 251 _parser_state.expected_field = PacketField::Id;
denis2nis 13:ccda9a56fef1 252 break;
denis2nis 13:ccda9a56fef1 253
denis2nis 13:ccda9a56fef1 254 /* c char is interpreted as ID field */
denis2nis 13:ccda9a56fef1 255 case PacketField::Id:
denis2nis 5:0cf54586a4be 256
denis2nis 13:ccda9a56fef1 257 /* store ID, update checksum and set next state to Length */
denis2nis 13:ccda9a56fef1 258 _status_pck.servo_id = c;
denis2nis 13:ccda9a56fef1 259 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 260 _parser_state.expected_field = PacketField::Length;
denis2nis 5:0cf54586a4be 261 break;
denis2nis 13:ccda9a56fef1 262
denis2nis 13:ccda9a56fef1 263 /* c char is interpreted as length of message data field
denis2nis 13:ccda9a56fef1 264 * Length = number of Parameters + 2
denis2nis 13:ccda9a56fef1 265 * where 2 stands for Length field (1 byte) + Error filed (1 byte)
denis2nis 13:ccda9a56fef1 266 */
denis2nis 13:ccda9a56fef1 267 case PacketField::Length:
denis2nis 5:0cf54586a4be 268
denis2nis 13:ccda9a56fef1 269 /* store number of param into _status_pck.n_param,
denis2nis 13:ccda9a56fef1 270 * update calculated_checksum and set next state to Error
denis2nis 13:ccda9a56fef1 271 */
denis2nis 13:ccda9a56fef1 272 _status_pck.n_param = c - 2;
denis2nis 13:ccda9a56fef1 273 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 274 _parser_state.expected_field = PacketField::Error;
denis2nis 5:0cf54586a4be 275 break;
denis2nis 5:0cf54586a4be 276
denis2nis 13:ccda9a56fef1 277 /* c char is interpreted as error status field */
denis2nis 13:ccda9a56fef1 278 case PacketField::Error:
denis2nis 13:ccda9a56fef1 279
denis2nis 13:ccda9a56fef1 280 /* store error status, update checksum
denis2nis 13:ccda9a56fef1 281 * and set next state to Data
denis2nis 13:ccda9a56fef1 282 */
denis2nis 13:ccda9a56fef1 283 _status_pck.error = c;
denis2nis 13:ccda9a56fef1 284 _status_pck.calculated_checksum += c;
denis2nis 13:ccda9a56fef1 285 _parser_state.expected_field = PacketField::Data;
denis2nis 5:0cf54586a4be 286 break;
denis2nis 5:0cf54586a4be 287
denis2nis 13:ccda9a56fef1 288 /* c char is interpreted as a param field */
denis2nis 13:ccda9a56fef1 289 case PacketField::Data:
denis2nis 13:ccda9a56fef1 290
denis2nis 13:ccda9a56fef1 291 /* store current param, increase param_index
denis2nis 13:ccda9a56fef1 292 * and update checksum */
denis2nis 13:ccda9a56fef1 293 _status_pck.param[(_parser_state.param_index)++] = c;
denis2nis 13:ccda9a56fef1 294 _status_pck.received_checksum += c;
denis2nis 5:0cf54586a4be 295
denis2nis 13:ccda9a56fef1 296 /* increase param index (_parser_state.dataCount)
denis2nis 13:ccda9a56fef1 297 * and test if it is the last param to read
denis2nis 13:ccda9a56fef1 298 */
denis2nis 13:ccda9a56fef1 299 if(_parser_state.param_index > _status_pck.n_param) {
denis2nis 13:ccda9a56fef1 300 /* reset param index and set next state to Checksum */
denis2nis 13:ccda9a56fef1 301 _parser_state.param_index = 0;
denis2nis 13:ccda9a56fef1 302 _parser_state.expected_field = PacketField::Checksum;
denis2nis 5:0cf54586a4be 303 }
denis2nis 5:0cf54586a4be 304 break;
denis2nis 5:0cf54586a4be 305
denis2nis 13:ccda9a56fef1 306 /* c char is interpreted as Checksum field */
denis2nis 13:ccda9a56fef1 307 case PacketField::Checksum:
denis2nis 13:ccda9a56fef1 308
denis2nis 13:ccda9a56fef1 309 /* store received_checksum, set parsed, store n_byte,
denis2nis 13:ccda9a56fef1 310 * evalutate valid and set next state to Header1 */
denis2nis 13:ccda9a56fef1 311 _status_pck.received_checksum = c;
denis2nis 13:ccda9a56fef1 312 _status_pck.parsed = true;
denis2nis 13:ccda9a56fef1 313 _status_pck.n_byte = _parser_state.byte_index;
denis2nis 13:ccda9a56fef1 314 _status_pck.valid = (_status_pck.received_checksum == c);
denis2nis 13:ccda9a56fef1 315 _parser_state.expected_field = PacketField::Header1;
denis2nis 13:ccda9a56fef1 316
denis2nis 13:ccda9a56fef1 317 /* set seriel state to Idle */
denis2nis 13:ccda9a56fef1 318 _bus_state = SerialState::Idle;
denis2nis 5:0cf54586a4be 319 break;
denis2nis 14:5a0c7b30f8c0 320
denis2nis 14:5a0c7b30f8c0 321 default:
denis2nis 14:5a0c7b30f8c0 322
denis2nis 14:5a0c7b30f8c0 323 /* unexpected case. If it occurs it would be due to a
denis2nis 14:5a0c7b30f8c0 324 * code error of this class */
denis2nis 22:8fa806e3ece4 325 break;
denis2nis 5:0cf54586a4be 326 }
denis2nis 5:0cf54586a4be 327 }
denis2nis 5:0cf54586a4be 328 }
denis2nis 5:0cf54586a4be 329
denis2nis 11:9bc7f5e2ccee 330 /* Code from previous version of the class */
denis2nis 11:9bc7f5e2ccee 331
denis2nis 5:0cf54586a4be 332 /*
denis2nis 0:7556356a8bcd 333 void MX12::ReadPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 334 // Make a request, interrupt takes care of everything else
denis2nis 0:7556356a8bcd 335 _state = State::ReadingPosition;
denis2nis 0:7556356a8bcd 336 rw(mot_id, 0x24, 2, NULL);
denis2nis 0:7556356a8bcd 337 }
denis2nis 0:7556356a8bcd 338
denis2nis 0:7556356a8bcd 339 float MX12::GetPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 340 return _angle[mot_id];
denis2nis 0:7556356a8bcd 341 }
denis2nis 5:0cf54586a4be 342 */