PSL_2021 / servomotor_MX12_Lorenzo

Dependents:   PSL_ROBOT_lorenzo robot_lorenzo recepteur_mbed_os_6

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?

UserRevisionLine numberNew 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 */