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@8:e74ef93ae660, 2021-11-06 (annotated)

Committer:
denis2nis
Date:
Sat Nov 06 08:29:28 2021 +0000
Revision:
8:e74ef93ae660
Parent:
5:0cf54586a4be
Child:
9:b4a5187fdec6
Arrange and comment (cont.)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denis2nis 8:e74ef93ae660 1 /**
denis2nis 8:e74ef93ae660 2 * @file MX12.ccp
denis2nis 8:e74ef93ae660 3 * @brief this file will contain methods to manage au bus of servomotor
denis2nis 8:e74ef93ae660 4 * Dynaminel MX12
denis2nis 8:e74ef93ae660 5 *
denis2nis 8:e74ef93ae660 6 */
denis2nis 8:e74ef93ae660 7
denis2nis 0:7556356a8bcd 8 #include "MX12.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 */
denis2nis 8:e74ef93ae660 21 );
denis2nis 8:e74ef93ae660 22 // Register a callback to process a Rx (receive) interrupt.
denis2nis 8:e74ef93ae660 23 //_mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
denis2nis 8:e74ef93ae660 24 _mx12.attach(callback(this, &MX12::_Rx_BD_interrupt), SerialBase::RxIrq);
denis2nis 5:0cf54586a4be 25
denis2nis 8:e74ef93ae660 26 // variable used for message reception
denis2nis 8:e74ef93ae660 27 _frame_pointer = 0;
denis2nis 8:e74ef93ae660 28 _current_frame = { .motorId = 0,
denis2nis 8:e74ef93ae660 29 .length = 0,
denis2nis 8:e74ef93ae660 30 .data="",
denis2nis 8:e74ef93ae660 31 .valid=0};
denis2nis 8:e74ef93ae660 32
denis2nis 8:e74ef93ae660 33 // Internal defaults states
denis2nis 5:0cf54586a4be 34 _sstate = SerialState::Idle;
denis2nis 5:0cf54586a4be 35 _pstate = ParsingState::Header;
denis2nis 8:e74ef93ae660 36
denis2nis 8:e74ef93ae660 37 // for testing
denis2nis 8:e74ef93ae660 38 _BDcount = 0;
denis2nis 8:e74ef93ae660 39 }
denis2nis 8:e74ef93ae660 40
denis2nis 8:e74ef93ae660 41
denis2nis 8:e74ef93ae660 42 // Interupt Routine to read in data from serial port
denis2nis 8:e74ef93ae660 43 void MX12::_Rx_BD_interrupt()
denis2nis 8:e74ef93ae660 44 {
denis2nis 8:e74ef93ae660 45 char c;
denis2nis 8:e74ef93ae660 46 _BDcount++;
denis2nis 8:e74ef93ae660 47 // Read the data to clear the receive interrupt.
denis2nis 8:e74ef93ae660 48 if (_mx12.read(&c, 1)) {
denis2nis 8:e74ef93ae660 49 // Echo the input back to the terminal.
denis2nis 8:e74ef93ae660 50 _BDcount++;
denis2nis 8:e74ef93ae660 51 }
denis2nis 8:e74ef93ae660 52 }
denis2nis 8:e74ef93ae660 53
denis2nis 8:e74ef93ae660 54 int MX12::get_counter()
denis2nis 8:e74ef93ae660 55 {
denis2nis 8:e74ef93ae660 56 return _BDcount;
denis2nis 0:7556356a8bcd 57 }
denis2nis 0:7556356a8bcd 58
denis2nis 0:7556356a8bcd 59 void MX12::SetSpeed(unsigned char mot_id, float speed) {
denis2nis 0:7556356a8bcd 60 char data[2];
denis2nis 8:e74ef93ae660 61
denis2nis 8:e74ef93ae660 62 _BDcount = _BDcount + 100;
denis2nis 0:7556356a8bcd 63
denis2nis 0:7556356a8bcd 64 // Speed absolute value
denis2nis 0:7556356a8bcd 65 int goal = (0x3ff * abs(speed));
denis2nis 0:7556356a8bcd 66
denis2nis 0:7556356a8bcd 67 // Spin direction (CW is negative)
denis2nis 0:7556356a8bcd 68 if (speed < 0) {
denis2nis 0:7556356a8bcd 69 goal |= (0x1 << 10);
denis2nis 0:7556356a8bcd 70 }
denis2nis 0:7556356a8bcd 71
denis2nis 0:7556356a8bcd 72 data[0] = goal & 0xff;
denis2nis 0:7556356a8bcd 73 data[1] = goal >> 8;
denis2nis 0:7556356a8bcd 74
denis2nis 0:7556356a8bcd 75 // Send instruction
denis2nis 5:0cf54586a4be 76 _sstate = SerialState::Writing;
denis2nis 5:0cf54586a4be 77 rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
denis2nis 0:7556356a8bcd 78 }
denis2nis 0:7556356a8bcd 79
denis2nis 0:7556356a8bcd 80 char MX12::IsAvailable(void) {
denis2nis 5:0cf54586a4be 81 return (_sstate == SerialState::Idle);
denis2nis 0:7556356a8bcd 82 }
denis2nis 0:7556356a8bcd 83
denis2nis 5:0cf54586a4be 84 void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
denis2nis 5:0cf54586a4be 85
denis2nis 5:0cf54586a4be 86 /* Set variables for reception from servovotor */
denis2nis 5:0cf54586a4be 87 _answer = 0;
denis2nis 8:e74ef93ae660 88 //memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
denis2nis 8:e74ef93ae660 89 _scontext.checksum = 0;
denis2nis 8:e74ef93ae660 90 _pstate = Header;
denis2nis 8:e74ef93ae660 91 _scontext = {.headingCount = 0, .dataCount = 0, .checksum = 0};
denis2nis 8:e74ef93ae660 92
denis2nis 8:e74ef93ae660 93 printf("Previous value\n");
denis2nis 8:e74ef93ae660 94 printf(" _current_frame.motorId: %d\n", _current_frame.motorId);
denis2nis 8:e74ef93ae660 95 printf(" _current_frame.length: %d\n", _current_frame.length);
denis2nis 8:e74ef93ae660 96 printf(" _current_frame.valid: %d\n", _current_frame.length);
denis2nis 8:e74ef93ae660 97 printf(" _frame_pointer: %d\n", _frame_pointer);
denis2nis 8:e74ef93ae660 98 printf(" _scontext.headingCount: %d\n", _scontext.headingCount);
denis2nis 8:e74ef93ae660 99 printf(" _scontext.dataCount: %d\n", _scontext.dataCount);
denis2nis 8:e74ef93ae660 100 printf(" _scontext.checksum: %d\n", _scontext.checksum);
denis2nis 8:e74ef93ae660 101 printf(" _BDcount: %d\n\n", _BDcount);
denis2nis 8:e74ef93ae660 102
denis2nis 8:e74ef93ae660 103 _current_frame = { .motorId = 0,
denis2nis 8:e74ef93ae660 104 .length = 0,
denis2nis 8:e74ef93ae660 105 .data="",
denis2nis 8:e74ef93ae660 106 .valid=0};
denis2nis 8:e74ef93ae660 107 _frame_pointer = 0;
denis2nis 8:e74ef93ae660 108
denis2nis 8:e74ef93ae660 109 printf("Current value\n");
denis2nis 8:e74ef93ae660 110 printf(" _current_frame.motorId: %d\n", _current_frame.motorId);
denis2nis 8:e74ef93ae660 111 printf(" _current_frame.length: %d\n", _current_frame.length);
denis2nis 8:e74ef93ae660 112 printf(" _current_frame.valid: %d\n", _current_frame.length);
denis2nis 8:e74ef93ae660 113 printf(" _frame_pointer: %d\n", _frame_pointer);
denis2nis 8:e74ef93ae660 114 printf(" _scontext.headingCount: %d\n", _scontext.headingCount);
denis2nis 8:e74ef93ae660 115 printf(" _scontext.dataCount: %d\n", _scontext.dataCount);
denis2nis 8:e74ef93ae660 116 printf(" _scontext.checksum: %d\n", _scontext.checksum);
denis2nis 8:e74ef93ae660 117 printf(" _BDcount: %d\n\n", _BDcount);
denis2nis 5:0cf54586a4be 118
denis2nis 5:0cf54586a4be 119 /* Initialise instruction packet to forge.
denis2nis 5:0cf54586a4be 120 * Instruction Packet is the command data sent to the servomotor.
denis2nis 5:0cf54586a4be 121 *
denis2nis 5:0cf54586a4be 122 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
denis2nis 5:0cf54586a4be 123 * |-------|-------|---------|------|-----------|---------------|--------|
denis2nis 5:0cf54586a4be 124 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 5:0cf54586a4be 125 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
denis2nis 5:0cf54586a4be 126 * \__ ___/ \_ _/ \__ __/
denis2nis 5:0cf54586a4be 127 * \/ | \/ \/ |
denis2nis 5:0cf54586a4be 128 * mot_id | address data |
denis2nis 5:0cf54586a4be 129 * | (len = N-1) |
denis2nis 5:0cf54586a4be 130 * \__________________ ________________/
denis2nis 5:0cf54586a4be 131 * \/
denis2nis 5:0cf54586a4be 132 * Length ( cmd[3] )
denis2nis 5:0cf54586a4be 133 */
denis2nis 5:0cf54586a4be 134 char packet[16];
denis2nis 5:0cf54586a4be 135 unsigned char packet_length;
denis2nis 5:0cf54586a4be 136
denis2nis 5:0cf54586a4be 137 /* Initialise checksum to calculate
denis2nis 5:0cf54586a4be 138 * It is used to check if packet is damaged during communication.
denis2nis 5:0cf54586a4be 139 * Status Checksum is calculated according to the following formula:
denis2nis 5:0cf54586a4be 140 *
denis2nis 5:0cf54586a4be 141 * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
denis2nis 5:0cf54586a4be 142 */
denis2nis 5:0cf54586a4be 143 char checksum = 0x00;
denis2nis 5:0cf54586a4be 144
denis2nis 5:0cf54586a4be 145 /* header 1 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 146 packet[0] = 0xff;
denis2nis 5:0cf54586a4be 147
denis2nis 5:0cf54586a4be 148 /* header 2 = 0xFF (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 149 packet[1] = 0xff;
denis2nis 5:0cf54586a4be 150
denis2nis 5:0cf54586a4be 151 /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
denis2nis 5:0cf54586a4be 152 packet[2] = mot_id;
denis2nis 5:0cf54586a4be 153 checksum += packet[2];
denis2nis 5:0cf54586a4be 154
denis2nis 5:0cf54586a4be 155 /* Guess instruction type. NULL for read, not NULL for write */
denis2nis 5:0cf54586a4be 156 if(data == NULL) // read instruction
denis2nis 5:0cf54586a4be 157 {
denis2nis 5:0cf54586a4be 158 /* byte length of the instruction: parameter and checksum field. */
denis2nis 5:0cf54586a4be 159 /* for read instruction: 1 INSTR + */
denis2nis 5:0cf54586a4be 160 /* 2 PARAM (starting address, length of data) + 1 CHKSUM */
denis2nis 5:0cf54586a4be 161 packet[3] = 4;
denis2nis 5:0cf54586a4be 162 checksum += packet[3];
denis2nis 5:0cf54586a4be 163
denis2nis 5:0cf54586a4be 164 /* set write instruction */
denis2nis 5:0cf54586a4be 165 packet[4] = PROTOCOL_INSTRUCTION_READ;
denis2nis 5:0cf54586a4be 166 checksum += packet[4];
denis2nis 5:0cf54586a4be 167
denis2nis 5:0cf54586a4be 168 /* Param 1: address to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 169 packet[5] = address;
denis2nis 5:0cf54586a4be 170 checksum += packet[5];
denis2nis 5:0cf54586a4be 171
denis2nis 5:0cf54586a4be 172 /* Param 2: number of bytes to read in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 173 packet[6] = len;
denis2nis 5:0cf54586a4be 174 checksum += packet[6];
denis2nis 5:0cf54586a4be 175
denis2nis 5:0cf54586a4be 176 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 177 packet[7] = ~checksum;
denis2nis 5:0cf54586a4be 178
denis2nis 5:0cf54586a4be 179 packet_length = 8;
denis2nis 5:0cf54586a4be 180 }
denis2nis 5:0cf54586a4be 181 else // write instruction
denis2nis 5:0cf54586a4be 182 {
denis2nis 5:0cf54586a4be 183 /* byte length of the instruction: parameter and checksum field */
denis2nis 5:0cf54586a4be 184 /* For write instruction: 1 INSTR + */
denis2nis 5:0cf54586a4be 185 /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
denis2nis 5:0cf54586a4be 186 packet[3] = 3 + len;
denis2nis 5:0cf54586a4be 187 checksum += packet[3];
denis2nis 5:0cf54586a4be 188
denis2nis 5:0cf54586a4be 189 /* set read instruction */
denis2nis 5:0cf54586a4be 190 packet[4] = PROTOCOL_INSTRUCTION_WRITE;
denis2nis 5:0cf54586a4be 191 checksum += packet[4];
denis2nis 5:0cf54586a4be 192
denis2nis 5:0cf54586a4be 193 /* Param 1: address to write in the "Control Table of RAM Area" */
denis2nis 5:0cf54586a4be 194 packet[5] = address;
denis2nis 5:0cf54586a4be 195 checksum += packet[5];
denis2nis 5:0cf54586a4be 196
denis2nis 5:0cf54586a4be 197 /* Param 2 to N: data to write in the Control Table of RAM Area */
denis2nis 5:0cf54586a4be 198 for(char i = 0; i < len; i++) {
denis2nis 5:0cf54586a4be 199 packet[6 + i] = data[i];
denis2nis 5:0cf54586a4be 200 checksum += data[i];
denis2nis 5:0cf54586a4be 201 }
denis2nis 5:0cf54586a4be 202
denis2nis 5:0cf54586a4be 203 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 5:0cf54586a4be 204 packet[6 + len] = ~checksum;
denis2nis 5:0cf54586a4be 205
denis2nis 5:0cf54586a4be 206 packet_length = 7 + len;
denis2nis 5:0cf54586a4be 207 }
denis2nis 5:0cf54586a4be 208
denis2nis 5:0cf54586a4be 209 // Send packet
denis2nis 5:0cf54586a4be 210 if(mot_id != 0xFE) {
denis2nis 5:0cf54586a4be 211 for(char i = 0; i < packet_length; i++) {
denis2nis 5:0cf54586a4be 212 _mx12.write(&packet[i], 1);
denis2nis 5:0cf54586a4be 213 }
denis2nis 5:0cf54586a4be 214 }
denis2nis 5:0cf54586a4be 215 }
denis2nis 5:0cf54586a4be 216
denis2nis 5:0cf54586a4be 217 // Debug function to print Serial read
denis2nis 5:0cf54586a4be 218 void MX12::PrintSerial() {
denis2nis 5:0cf54586a4be 219
denis2nis 8:e74ef93ae660 220 printf("_frame_pointer = %d\n", _frame_pointer);
denis2nis 8:e74ef93ae660 221 printf("frame = .");
denis2nis 5:0cf54586a4be 222 for(int i = 0; i < _frame_pointer; i++) {
denis2nis 8:e74ef93ae660 223 printf("%x.", _stored_frame[i]);
denis2nis 5:0cf54586a4be 224 }
denis2nis 5:0cf54586a4be 225
denis2nis 5:0cf54586a4be 226 printf("\n");
denis2nis 5:0cf54586a4be 227 }
denis2nis 5:0cf54586a4be 228
denis2nis 5:0cf54586a4be 229 MX12::Status MX12::GetStatus() {
denis2nis 5:0cf54586a4be 230 // Return the corresponding status code
denis2nis 5:0cf54586a4be 231 switch(_current_frame.data[0]) {
denis2nis 5:0cf54586a4be 232 case 0:
denis2nis 5:0cf54586a4be 233 return Ok;
denis2nis 5:0cf54586a4be 234 break;
denis2nis 5:0cf54586a4be 235 case 1 << 0:
denis2nis 5:0cf54586a4be 236 return InputVoltageError;
denis2nis 5:0cf54586a4be 237 break;
denis2nis 5:0cf54586a4be 238 case 1 << 1:
denis2nis 5:0cf54586a4be 239 return AngleLimitError;
denis2nis 5:0cf54586a4be 240 break;
denis2nis 5:0cf54586a4be 241 case 1 << 2:
denis2nis 5:0cf54586a4be 242 return OverheatingError;
denis2nis 5:0cf54586a4be 243 break;
denis2nis 5:0cf54586a4be 244 case 1 << 3:
denis2nis 5:0cf54586a4be 245 return RangeError;
denis2nis 5:0cf54586a4be 246 break;
denis2nis 5:0cf54586a4be 247 case 1 << 4:
denis2nis 5:0cf54586a4be 248 return ChecksumError;
denis2nis 5:0cf54586a4be 249 break;
denis2nis 5:0cf54586a4be 250 case 1 << 5:
denis2nis 5:0cf54586a4be 251 return OverloadError;
denis2nis 5:0cf54586a4be 252 break;
denis2nis 5:0cf54586a4be 253 case 1 << 6:
denis2nis 5:0cf54586a4be 254 return InstructionError;
denis2nis 5:0cf54586a4be 255 break;
denis2nis 5:0cf54586a4be 256 default:
denis2nis 5:0cf54586a4be 257 return Unknown;
denis2nis 5:0cf54586a4be 258 }
denis2nis 5:0cf54586a4be 259 }
denis2nis 5:0cf54586a4be 260
denis2nis 5:0cf54586a4be 261 void MX12::_ReadCallback() {
denis2nis 8:e74ef93ae660 262
denis2nis 5:0cf54586a4be 263 char c;
denis2nis 5:0cf54586a4be 264
denis2nis 5:0cf54586a4be 265 // Try to read serial
denis2nis 5:0cf54586a4be 266 if(_mx12.read(&c, 1)) {
denis2nis 8:e74ef93ae660 267
denis2nis 5:0cf54586a4be 268 _stored_frame[_frame_pointer++] = c;
denis2nis 8:e74ef93ae660 269
denis2nis 8:e74ef93ae660 270 /* Include all characters in the checksum calculation,
denis2nis 8:e74ef93ae660 271 * even those that shouldn't, such as header characters.
denis2nis 8:e74ef93ae660 272 * They will be subtracted later (B. Denis remark)
denis2nis 8:e74ef93ae660 273 */
denis2nis 5:0cf54586a4be 274 _scontext.checksum += c;
denis2nis 5:0cf54586a4be 275
denis2nis 5:0cf54586a4be 276 // State-machine parsing
denis2nis 5:0cf54586a4be 277 switch(_pstate) {
denis2nis 5:0cf54586a4be 278
denis2nis 5:0cf54586a4be 279 case Header:
denis2nis 5:0cf54586a4be 280 if(++(_scontext.headingCount) >= 2) {
denis2nis 8:e74ef93ae660 281 //_scontext.headingCount = 0;
denis2nis 5:0cf54586a4be 282 _pstate = Id;
denis2nis 5:0cf54586a4be 283 }
denis2nis 5:0cf54586a4be 284
denis2nis 5:0cf54586a4be 285 _scontext.checksum -= c;
denis2nis 5:0cf54586a4be 286 break;
denis2nis 5:0cf54586a4be 287
denis2nis 5:0cf54586a4be 288 case Id:
denis2nis 5:0cf54586a4be 289 _current_frame.motorId = c;
denis2nis 5:0cf54586a4be 290 _pstate = Length;
denis2nis 5:0cf54586a4be 291 break;
denis2nis 5:0cf54586a4be 292
denis2nis 5:0cf54586a4be 293 case Length:
denis2nis 5:0cf54586a4be 294 _current_frame.length = c - 1;
denis2nis 5:0cf54586a4be 295 _pstate = Data;
denis2nis 5:0cf54586a4be 296 break;
denis2nis 5:0cf54586a4be 297
denis2nis 5:0cf54586a4be 298 case Data:
denis2nis 5:0cf54586a4be 299 _current_frame.data[_scontext.dataCount] = c;
denis2nis 5:0cf54586a4be 300
denis2nis 5:0cf54586a4be 301 if(++(_scontext.dataCount) >= _current_frame.length) {
denis2nis 5:0cf54586a4be 302 _scontext.dataCount = 0;
denis2nis 5:0cf54586a4be 303 _pstate = Checksum;
denis2nis 5:0cf54586a4be 304 }
denis2nis 5:0cf54586a4be 305 break;
denis2nis 5:0cf54586a4be 306
denis2nis 5:0cf54586a4be 307 case Checksum:
denis2nis 5:0cf54586a4be 308 _current_frame.valid = (_scontext.checksum == 0xFF);
denis2nis 5:0cf54586a4be 309 _scontext.checksum = 0;
denis2nis 5:0cf54586a4be 310 _pstate = Header;
denis2nis 5:0cf54586a4be 311 if(_answer) _sstate = Idle;
denis2nis 5:0cf54586a4be 312 _answer = 1;
denis2nis 5:0cf54586a4be 313 break;
denis2nis 5:0cf54586a4be 314 }
denis2nis 5:0cf54586a4be 315 }
denis2nis 5:0cf54586a4be 316 }
denis2nis 5:0cf54586a4be 317
denis2nis 5:0cf54586a4be 318 /*
denis2nis 5:0cf54586a4be 319
denis2nis 0:7556356a8bcd 320 void MX12::ReadPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 321 // Make a request, interrupt takes care of everything else
denis2nis 0:7556356a8bcd 322 _state = State::ReadingPosition;
denis2nis 0:7556356a8bcd 323 rw(mot_id, 0x24, 2, NULL);
denis2nis 0:7556356a8bcd 324 }
denis2nis 0:7556356a8bcd 325
denis2nis 0:7556356a8bcd 326 float MX12::GetPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 327 return _angle[mot_id];
denis2nis 0:7556356a8bcd 328 }
denis2nis 0:7556356a8bcd 329
denis2nis 5:0cf54586a4be 330 */