PSL_2021 / Mbed OS prog_robot_Eve_v1

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:   mbed-os nRF24L01P

MX12.cpp@9:67d737d8a349, 2022-02-25 (annotated)

Committer:
evedelegue
Date:
Fri Feb 25 10:13:06 2022 +0000
Revision:
9:67d737d8a349
Child:
11:5f2289e0d0e8
premiere version juste pour la com (mais %f bug)

Who changed what in which revision?

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