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@5:0cf54586a4be, 2021-11-04 (annotated)
- Committer:
- denis2nis
- Date:
- Thu Nov 04 11:48:53 2021 +0000
- Revision:
- 5:0cf54586a4be
- Parent:
- 3:add8b050eb86
- Child:
- 8:e74ef93ae660
Include last change of Titouan (Switch to state machine for more reliable parsing); https://os.mbed.com/users/tsoul/code/MX12//rev/4bdd101ce4ec
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
denis2nis | 0:7556356a8bcd | 1 | #include "MX12.h" |
denis2nis | 0:7556356a8bcd | 2 | |
denis2nis | 0:7556356a8bcd | 3 | MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) { |
denis2nis | 5:0cf54586a4be | 4 | // Serial configuration |
denis2nis | 5:0cf54586a4be | 5 | _mx12.baud(baud); |
denis2nis | 0:7556356a8bcd | 6 | _mx12.format(8, SerialBase::None, 1); |
denis2nis | 0:7556356a8bcd | 7 | _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq); |
denis2nis | 5:0cf54586a4be | 8 | |
denis2nis | 5:0cf54586a4be | 9 | // Internal defaults |
denis2nis | 5:0cf54586a4be | 10 | _sstate = SerialState::Idle; |
denis2nis | 5:0cf54586a4be | 11 | _pstate = ParsingState::Header; |
denis2nis | 0:7556356a8bcd | 12 | } |
denis2nis | 0:7556356a8bcd | 13 | |
denis2nis | 0:7556356a8bcd | 14 | void MX12::SetSpeed(unsigned char mot_id, float speed) { |
denis2nis | 0:7556356a8bcd | 15 | char data[2]; |
denis2nis | 0:7556356a8bcd | 16 | |
denis2nis | 0:7556356a8bcd | 17 | // Speed absolute value |
denis2nis | 0:7556356a8bcd | 18 | int goal = (0x3ff * abs(speed)); |
denis2nis | 0:7556356a8bcd | 19 | |
denis2nis | 0:7556356a8bcd | 20 | // Spin direction (CW is negative) |
denis2nis | 0:7556356a8bcd | 21 | if (speed < 0) { |
denis2nis | 0:7556356a8bcd | 22 | goal |= (0x1 << 10); |
denis2nis | 0:7556356a8bcd | 23 | } |
denis2nis | 0:7556356a8bcd | 24 | |
denis2nis | 0:7556356a8bcd | 25 | data[0] = goal & 0xff; |
denis2nis | 0:7556356a8bcd | 26 | data[1] = goal >> 8; |
denis2nis | 0:7556356a8bcd | 27 | |
denis2nis | 0:7556356a8bcd | 28 | // Send instruction |
denis2nis | 5:0cf54586a4be | 29 | _sstate = SerialState::Writing; |
denis2nis | 5:0cf54586a4be | 30 | rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data); |
denis2nis | 0:7556356a8bcd | 31 | } |
denis2nis | 0:7556356a8bcd | 32 | |
denis2nis | 0:7556356a8bcd | 33 | char MX12::IsAvailable(void) { |
denis2nis | 5:0cf54586a4be | 34 | return (_sstate == SerialState::Idle); |
denis2nis | 0:7556356a8bcd | 35 | } |
denis2nis | 0:7556356a8bcd | 36 | |
denis2nis | 5:0cf54586a4be | 37 | void MX12::rw(unsigned char mot_id, char address, char len, char *data) { |
denis2nis | 5:0cf54586a4be | 38 | |
denis2nis | 5:0cf54586a4be | 39 | /* Set variables for reception from servovotor */ |
denis2nis | 5:0cf54586a4be | 40 | _answer = 0; |
denis2nis | 5:0cf54586a4be | 41 | memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE); |
denis2nis | 5:0cf54586a4be | 42 | |
denis2nis | 5:0cf54586a4be | 43 | /* Initialise instruction packet to forge. |
denis2nis | 5:0cf54586a4be | 44 | * Instruction Packet is the command data sent to the servomotor. |
denis2nis | 5:0cf54586a4be | 45 | * |
denis2nis | 5:0cf54586a4be | 46 | * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum| |
denis2nis | 5:0cf54586a4be | 47 | * |-------|-------|---------|------|-----------|---------------|--------| |
denis2nis | 5:0cf54586a4be | 48 | * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM | |
denis2nis | 5:0cf54586a4be | 49 | * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | | |
denis2nis | 5:0cf54586a4be | 50 | * \__ ___/ \_ _/ \__ __/ |
denis2nis | 5:0cf54586a4be | 51 | * \/ | \/ \/ | |
denis2nis | 5:0cf54586a4be | 52 | * mot_id | address data | |
denis2nis | 5:0cf54586a4be | 53 | * | (len = N-1) | |
denis2nis | 5:0cf54586a4be | 54 | * \__________________ ________________/ |
denis2nis | 5:0cf54586a4be | 55 | * \/ |
denis2nis | 5:0cf54586a4be | 56 | * Length ( cmd[3] ) |
denis2nis | 5:0cf54586a4be | 57 | */ |
denis2nis | 5:0cf54586a4be | 58 | char packet[16]; |
denis2nis | 5:0cf54586a4be | 59 | unsigned char packet_length; |
denis2nis | 5:0cf54586a4be | 60 | |
denis2nis | 5:0cf54586a4be | 61 | /* Initialise checksum to calculate |
denis2nis | 5:0cf54586a4be | 62 | * It is used to check if packet is damaged during communication. |
denis2nis | 5:0cf54586a4be | 63 | * Status Checksum is calculated according to the following formula: |
denis2nis | 5:0cf54586a4be | 64 | * |
denis2nis | 5:0cf54586a4be | 65 | * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N ) |
denis2nis | 5:0cf54586a4be | 66 | */ |
denis2nis | 5:0cf54586a4be | 67 | char checksum = 0x00; |
denis2nis | 5:0cf54586a4be | 68 | |
denis2nis | 5:0cf54586a4be | 69 | /* header 1 = 0xFF (dynamixel protocol 1.0) */ |
denis2nis | 5:0cf54586a4be | 70 | packet[0] = 0xff; |
denis2nis | 5:0cf54586a4be | 71 | |
denis2nis | 5:0cf54586a4be | 72 | /* header 2 = 0xFF (dynamixel protocol 1.0) */ |
denis2nis | 5:0cf54586a4be | 73 | packet[1] = 0xff; |
denis2nis | 5:0cf54586a4be | 74 | |
denis2nis | 5:0cf54586a4be | 75 | /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */ |
denis2nis | 5:0cf54586a4be | 76 | packet[2] = mot_id; |
denis2nis | 5:0cf54586a4be | 77 | checksum += packet[2]; |
denis2nis | 5:0cf54586a4be | 78 | |
denis2nis | 5:0cf54586a4be | 79 | /* Guess instruction type. NULL for read, not NULL for write */ |
denis2nis | 5:0cf54586a4be | 80 | if(data == NULL) // read instruction |
denis2nis | 5:0cf54586a4be | 81 | { |
denis2nis | 5:0cf54586a4be | 82 | /* byte length of the instruction: parameter and checksum field. */ |
denis2nis | 5:0cf54586a4be | 83 | /* for read instruction: 1 INSTR + */ |
denis2nis | 5:0cf54586a4be | 84 | /* 2 PARAM (starting address, length of data) + 1 CHKSUM */ |
denis2nis | 5:0cf54586a4be | 85 | packet[3] = 4; |
denis2nis | 5:0cf54586a4be | 86 | checksum += packet[3]; |
denis2nis | 5:0cf54586a4be | 87 | |
denis2nis | 5:0cf54586a4be | 88 | /* set write instruction */ |
denis2nis | 5:0cf54586a4be | 89 | packet[4] = PROTOCOL_INSTRUCTION_READ; |
denis2nis | 5:0cf54586a4be | 90 | checksum += packet[4]; |
denis2nis | 5:0cf54586a4be | 91 | |
denis2nis | 5:0cf54586a4be | 92 | /* Param 1: address to read in the Control Table of RAM Area */ |
denis2nis | 5:0cf54586a4be | 93 | packet[5] = address; |
denis2nis | 5:0cf54586a4be | 94 | checksum += packet[5]; |
denis2nis | 5:0cf54586a4be | 95 | |
denis2nis | 5:0cf54586a4be | 96 | /* Param 2: number of bytes to read in the Control Table of RAM Area */ |
denis2nis | 5:0cf54586a4be | 97 | packet[6] = len; |
denis2nis | 5:0cf54586a4be | 98 | checksum += packet[6]; |
denis2nis | 5:0cf54586a4be | 99 | |
denis2nis | 5:0cf54586a4be | 100 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
denis2nis | 5:0cf54586a4be | 101 | packet[7] = ~checksum; |
denis2nis | 5:0cf54586a4be | 102 | |
denis2nis | 5:0cf54586a4be | 103 | packet_length = 8; |
denis2nis | 5:0cf54586a4be | 104 | } |
denis2nis | 5:0cf54586a4be | 105 | else // write instruction |
denis2nis | 5:0cf54586a4be | 106 | { |
denis2nis | 5:0cf54586a4be | 107 | /* byte length of the instruction: parameter and checksum field */ |
denis2nis | 5:0cf54586a4be | 108 | /* For write instruction: 1 INSTR + */ |
denis2nis | 5:0cf54586a4be | 109 | /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */ |
denis2nis | 5:0cf54586a4be | 110 | packet[3] = 3 + len; |
denis2nis | 5:0cf54586a4be | 111 | checksum += packet[3]; |
denis2nis | 5:0cf54586a4be | 112 | |
denis2nis | 5:0cf54586a4be | 113 | /* set read instruction */ |
denis2nis | 5:0cf54586a4be | 114 | packet[4] = PROTOCOL_INSTRUCTION_WRITE; |
denis2nis | 5:0cf54586a4be | 115 | checksum += packet[4]; |
denis2nis | 5:0cf54586a4be | 116 | |
denis2nis | 5:0cf54586a4be | 117 | /* Param 1: address to write in the "Control Table of RAM Area" */ |
denis2nis | 5:0cf54586a4be | 118 | packet[5] = address; |
denis2nis | 5:0cf54586a4be | 119 | checksum += packet[5]; |
denis2nis | 5:0cf54586a4be | 120 | |
denis2nis | 5:0cf54586a4be | 121 | /* Param 2 to N: data to write in the Control Table of RAM Area */ |
denis2nis | 5:0cf54586a4be | 122 | for(char i = 0; i < len; i++) { |
denis2nis | 5:0cf54586a4be | 123 | packet[6 + i] = data[i]; |
denis2nis | 5:0cf54586a4be | 124 | checksum += data[i]; |
denis2nis | 5:0cf54586a4be | 125 | } |
denis2nis | 5:0cf54586a4be | 126 | |
denis2nis | 5:0cf54586a4be | 127 | /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */ |
denis2nis | 5:0cf54586a4be | 128 | packet[6 + len] = ~checksum; |
denis2nis | 5:0cf54586a4be | 129 | |
denis2nis | 5:0cf54586a4be | 130 | packet_length = 7 + len; |
denis2nis | 5:0cf54586a4be | 131 | } |
denis2nis | 5:0cf54586a4be | 132 | |
denis2nis | 5:0cf54586a4be | 133 | // Send packet |
denis2nis | 5:0cf54586a4be | 134 | if(mot_id != 0xFE) { |
denis2nis | 5:0cf54586a4be | 135 | for(char i = 0; i < packet_length; i++) { |
denis2nis | 5:0cf54586a4be | 136 | _mx12.write(&packet[i], 1); |
denis2nis | 5:0cf54586a4be | 137 | } |
denis2nis | 5:0cf54586a4be | 138 | } |
denis2nis | 5:0cf54586a4be | 139 | } |
denis2nis | 5:0cf54586a4be | 140 | |
denis2nis | 5:0cf54586a4be | 141 | // Debug function to print Serial read |
denis2nis | 5:0cf54586a4be | 142 | void MX12::PrintSerial() { |
denis2nis | 5:0cf54586a4be | 143 | |
denis2nis | 5:0cf54586a4be | 144 | for(int i = 0; i < _frame_pointer; i++) { |
denis2nis | 5:0cf54586a4be | 145 | printf("%x ", _stored_frame[i]); |
denis2nis | 5:0cf54586a4be | 146 | } |
denis2nis | 5:0cf54586a4be | 147 | |
denis2nis | 5:0cf54586a4be | 148 | printf("\n"); |
denis2nis | 5:0cf54586a4be | 149 | } |
denis2nis | 5:0cf54586a4be | 150 | |
denis2nis | 5:0cf54586a4be | 151 | MX12::Status MX12::GetStatus() { |
denis2nis | 5:0cf54586a4be | 152 | // Return the corresponding status code |
denis2nis | 5:0cf54586a4be | 153 | switch(_current_frame.data[0]) { |
denis2nis | 5:0cf54586a4be | 154 | case 0: |
denis2nis | 5:0cf54586a4be | 155 | return Ok; |
denis2nis | 5:0cf54586a4be | 156 | break; |
denis2nis | 5:0cf54586a4be | 157 | case 1 << 0: |
denis2nis | 5:0cf54586a4be | 158 | return InputVoltageError; |
denis2nis | 5:0cf54586a4be | 159 | break; |
denis2nis | 5:0cf54586a4be | 160 | case 1 << 1: |
denis2nis | 5:0cf54586a4be | 161 | return AngleLimitError; |
denis2nis | 5:0cf54586a4be | 162 | break; |
denis2nis | 5:0cf54586a4be | 163 | case 1 << 2: |
denis2nis | 5:0cf54586a4be | 164 | return OverheatingError; |
denis2nis | 5:0cf54586a4be | 165 | break; |
denis2nis | 5:0cf54586a4be | 166 | case 1 << 3: |
denis2nis | 5:0cf54586a4be | 167 | return RangeError; |
denis2nis | 5:0cf54586a4be | 168 | break; |
denis2nis | 5:0cf54586a4be | 169 | case 1 << 4: |
denis2nis | 5:0cf54586a4be | 170 | return ChecksumError; |
denis2nis | 5:0cf54586a4be | 171 | break; |
denis2nis | 5:0cf54586a4be | 172 | case 1 << 5: |
denis2nis | 5:0cf54586a4be | 173 | return OverloadError; |
denis2nis | 5:0cf54586a4be | 174 | break; |
denis2nis | 5:0cf54586a4be | 175 | case 1 << 6: |
denis2nis | 5:0cf54586a4be | 176 | return InstructionError; |
denis2nis | 5:0cf54586a4be | 177 | break; |
denis2nis | 5:0cf54586a4be | 178 | default: |
denis2nis | 5:0cf54586a4be | 179 | return Unknown; |
denis2nis | 5:0cf54586a4be | 180 | } |
denis2nis | 5:0cf54586a4be | 181 | } |
denis2nis | 5:0cf54586a4be | 182 | |
denis2nis | 5:0cf54586a4be | 183 | void MX12::_ReadCallback() { |
denis2nis | 5:0cf54586a4be | 184 | char c; |
denis2nis | 5:0cf54586a4be | 185 | |
denis2nis | 5:0cf54586a4be | 186 | // Try to read serial |
denis2nis | 5:0cf54586a4be | 187 | if(_mx12.read(&c, 1)) { |
denis2nis | 5:0cf54586a4be | 188 | _stored_frame[_frame_pointer++] = c; |
denis2nis | 5:0cf54586a4be | 189 | //_res[_res_count] = c; |
denis2nis | 5:0cf54586a4be | 190 | //_res_count++; |
denis2nis | 5:0cf54586a4be | 191 | _scontext.checksum += c; |
denis2nis | 5:0cf54586a4be | 192 | |
denis2nis | 5:0cf54586a4be | 193 | // State-machine parsing |
denis2nis | 5:0cf54586a4be | 194 | switch(_pstate) { |
denis2nis | 5:0cf54586a4be | 195 | |
denis2nis | 5:0cf54586a4be | 196 | case Header: |
denis2nis | 5:0cf54586a4be | 197 | if(++(_scontext.headingCount) >= 2) { |
denis2nis | 5:0cf54586a4be | 198 | _scontext.headingCount = 0; |
denis2nis | 5:0cf54586a4be | 199 | _pstate = Id; |
denis2nis | 5:0cf54586a4be | 200 | } |
denis2nis | 5:0cf54586a4be | 201 | |
denis2nis | 5:0cf54586a4be | 202 | _scontext.checksum -= c; |
denis2nis | 5:0cf54586a4be | 203 | break; |
denis2nis | 5:0cf54586a4be | 204 | |
denis2nis | 5:0cf54586a4be | 205 | case Id: |
denis2nis | 5:0cf54586a4be | 206 | _current_frame.motorId = c; |
denis2nis | 5:0cf54586a4be | 207 | _pstate = Length; |
denis2nis | 5:0cf54586a4be | 208 | break; |
denis2nis | 5:0cf54586a4be | 209 | |
denis2nis | 5:0cf54586a4be | 210 | case Length: |
denis2nis | 5:0cf54586a4be | 211 | _current_frame.length = c - 1; |
denis2nis | 5:0cf54586a4be | 212 | _pstate = Data; |
denis2nis | 5:0cf54586a4be | 213 | break; |
denis2nis | 5:0cf54586a4be | 214 | |
denis2nis | 5:0cf54586a4be | 215 | case Data: |
denis2nis | 5:0cf54586a4be | 216 | _current_frame.data[_scontext.dataCount] = c; |
denis2nis | 5:0cf54586a4be | 217 | |
denis2nis | 5:0cf54586a4be | 218 | if(++(_scontext.dataCount) >= _current_frame.length) { |
denis2nis | 5:0cf54586a4be | 219 | _scontext.dataCount = 0; |
denis2nis | 5:0cf54586a4be | 220 | _pstate = Checksum; |
denis2nis | 5:0cf54586a4be | 221 | } |
denis2nis | 5:0cf54586a4be | 222 | break; |
denis2nis | 5:0cf54586a4be | 223 | |
denis2nis | 5:0cf54586a4be | 224 | case Checksum: |
denis2nis | 5:0cf54586a4be | 225 | _current_frame.valid = (_scontext.checksum == 0xFF); |
denis2nis | 5:0cf54586a4be | 226 | _scontext.checksum = 0; |
denis2nis | 5:0cf54586a4be | 227 | _pstate = Header; |
denis2nis | 5:0cf54586a4be | 228 | if(_answer) _sstate = Idle; |
denis2nis | 5:0cf54586a4be | 229 | _answer = 1; |
denis2nis | 5:0cf54586a4be | 230 | break; |
denis2nis | 5:0cf54586a4be | 231 | } |
denis2nis | 5:0cf54586a4be | 232 | } |
denis2nis | 5:0cf54586a4be | 233 | } |
denis2nis | 5:0cf54586a4be | 234 | |
denis2nis | 5:0cf54586a4be | 235 | /* |
denis2nis | 5:0cf54586a4be | 236 | |
denis2nis | 0:7556356a8bcd | 237 | void MX12::ReadPosition(unsigned char mot_id) { |
denis2nis | 0:7556356a8bcd | 238 | // Make a request, interrupt takes care of everything else |
denis2nis | 0:7556356a8bcd | 239 | _state = State::ReadingPosition; |
denis2nis | 0:7556356a8bcd | 240 | rw(mot_id, 0x24, 2, NULL); |
denis2nis | 0:7556356a8bcd | 241 | } |
denis2nis | 0:7556356a8bcd | 242 | |
denis2nis | 0:7556356a8bcd | 243 | float MX12::GetPosition(unsigned char mot_id) { |
denis2nis | 0:7556356a8bcd | 244 | return _angle[mot_id]; |
denis2nis | 0:7556356a8bcd | 245 | } |
denis2nis | 0:7556356a8bcd | 246 | |
denis2nis | 5:0cf54586a4be | 247 | */ |