PSL_2021 / dribble

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.

MX12.cpp@11:9bc7f5e2ccee, 2021-11-06 (annotated)

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