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@3:add8b050eb86, 2021-11-04 (annotated)

Committer:
denis2nis
Date:
Thu Nov 04 07:21:32 2021 +0000
Revision:
3:add8b050eb86
Parent:
2:02f3323a107d
Child:
5:0cf54586a4be
Arrange and comment (cont.)

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 0:7556356a8bcd 4 _baud = baud;
denis2nis 0:7556356a8bcd 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 0:7556356a8bcd 8 _state = State::Available;
denis2nis 0:7556356a8bcd 9 }
denis2nis 0:7556356a8bcd 10
denis2nis 0:7556356a8bcd 11 void MX12::SetSpeed(unsigned char mot_id, float speed) {
denis2nis 0:7556356a8bcd 12 char data[2];
denis2nis 0:7556356a8bcd 13
denis2nis 0:7556356a8bcd 14 // Speed absolute value
denis2nis 0:7556356a8bcd 15 int goal = (0x3ff * abs(speed));
denis2nis 0:7556356a8bcd 16
denis2nis 0:7556356a8bcd 17 // Spin direction (CW is negative)
denis2nis 0:7556356a8bcd 18 if (speed < 0) {
denis2nis 0:7556356a8bcd 19 goal |= (0x1 << 10);
denis2nis 0:7556356a8bcd 20 }
denis2nis 0:7556356a8bcd 21
denis2nis 0:7556356a8bcd 22 data[0] = goal & 0xff;
denis2nis 0:7556356a8bcd 23 data[1] = goal >> 8;
denis2nis 0:7556356a8bcd 24
denis2nis 0:7556356a8bcd 25 // Enter writing state
denis2nis 0:7556356a8bcd 26 _state = State::Writing;
denis2nis 0:7556356a8bcd 27
denis2nis 0:7556356a8bcd 28 // Send instruction
denis2nis 0:7556356a8bcd 29 rw(mot_id, 0x20, 2, data);
denis2nis 0:7556356a8bcd 30 }
denis2nis 0:7556356a8bcd 31
denis2nis 0:7556356a8bcd 32 char MX12::IsAvailable(void) {
denis2nis 0:7556356a8bcd 33 return _state == State::Available;
denis2nis 0:7556356a8bcd 34 }
denis2nis 0:7556356a8bcd 35
denis2nis 0:7556356a8bcd 36 void MX12::ReadPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 37 // Make a request, interrupt takes care of everything else
denis2nis 0:7556356a8bcd 38 _state = State::ReadingPosition;
denis2nis 0:7556356a8bcd 39 rw(mot_id, 0x24, 2, NULL);
denis2nis 0:7556356a8bcd 40 }
denis2nis 0:7556356a8bcd 41
denis2nis 0:7556356a8bcd 42 float MX12::GetPosition(unsigned char mot_id) {
denis2nis 0:7556356a8bcd 43 return _angle[mot_id];
denis2nis 0:7556356a8bcd 44 }
denis2nis 0:7556356a8bcd 45
denis2nis 0:7556356a8bcd 46 MX12::Status MX12::GetStatus(void) {
denis2nis 0:7556356a8bcd 47 return _status;
denis2nis 0:7556356a8bcd 48 }
denis2nis 0:7556356a8bcd 49
denis2nis 0:7556356a8bcd 50 // Debug function to print Serial read
denis2nis 0:7556356a8bcd 51 void MX12::PrintAnswer() {
denis2nis 0:7556356a8bcd 52 for(char i = 0; i < MX12_ANSWER_MAX_SIZE; i++) {
denis2nis 0:7556356a8bcd 53 printf("%x ", _res[i]);
denis2nis 0:7556356a8bcd 54 }
denis2nis 0:7556356a8bcd 55
denis2nis 0:7556356a8bcd 56 printf("\r\n");
denis2nis 0:7556356a8bcd 57 }
denis2nis 0:7556356a8bcd 58
denis2nis 2:02f3323a107d 59 void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
denis2nis 0:7556356a8bcd 60 _res_count = 0;
denis2nis 0:7556356a8bcd 61 memset(_res, 0, MX12_ANSWER_MAX_SIZE);
denis2nis 0:7556356a8bcd 62
denis2nis 2:02f3323a107d 63 /* Forge instruction packet */
denis2nis 0:7556356a8bcd 64 char cmd[16];
denis2nis 3:add8b050eb86 65 char checksum = 0x00;
denis2nis 0:7556356a8bcd 66
denis2nis 2:02f3323a107d 67 /* Instruction Packet is the command data sent to the Device.
denis2nis 2:02f3323a107d 68 *
denis2nis 2:02f3323a107d 69 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
denis2nis 2:02f3323a107d 70 * |-------|-------|---------|------|-----------|---------------|---------|
denis2nis 2:02f3323a107d 71 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 2:02f3323a107d 72 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... |(2 bytes)|
denis2nis 3:add8b050eb86 73 * \_ _/ \__ __/
denis2nis 3:add8b050eb86 74 * | \/ \/ |
denis2nis 3:add8b050eb86 75 * | address data |
denis2nis 3:add8b050eb86 76 * | (len = N-1) |
denis2nis 3:add8b050eb86 77 * \__________________ _________________/
denis2nis 3:add8b050eb86 78 * \/
denis2nis 3:add8b050eb86 79 * Length ( cmd[3] )
denis2nis 2:02f3323a107d 80 */
denis2nis 2:02f3323a107d 81
denis2nis 2:02f3323a107d 82 /* header 1 = 0xFF (dynamixel protocol 1.0) */
denis2nis 0:7556356a8bcd 83 cmd[0] = 0xff;
denis2nis 0:7556356a8bcd 84
denis2nis 2:02f3323a107d 85 /* header 2 = 0xFF (dynamixel protocol 1.0) */
denis2nis 2:02f3323a107d 86 cmd[1] = 0xff;
denis2nis 2:02f3323a107d 87
denis2nis 2:02f3323a107d 88 /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
denis2nis 0:7556356a8bcd 89 cmd[2] = mot_id;
denis2nis 3:add8b050eb86 90 checksum += cmd[2];
denis2nis 2:02f3323a107d 91
denis2nis 2:02f3323a107d 92 /* Length indicates the byte size of the instruction,
denis2nis 2:02f3323a107d 93 parameter and Checksum field */
denis2nis 2:02f3323a107d 94
denis2nis 0:7556356a8bcd 95
denis2nis 2:02f3323a107d 96 /* Guess instruction type. NULL for read, not NULL for write */
denis2nis 2:02f3323a107d 97 if(data == NULL) // read instruction
denis2nis 2:02f3323a107d 98 {
denis2nis 3:add8b050eb86 99 /* byte length of the instruction: parameter and checksum field. */
denis2nis 3:add8b050eb86 100 /* for read instruction: 1 INSTR + */
denis2nis 3:add8b050eb86 101 /* 2 PARAM (starting address, length of data) + 1 CHKSUM */
denis2nis 3:add8b050eb86 102 cmd[3] = 4;
denis2nis 3:add8b050eb86 103 checksum += cmd[3];
denis2nis 2:02f3323a107d 104
denis2nis 2:02f3323a107d 105 /* set write instruction */
denis2nis 2:02f3323a107d 106 cmd[4] = PROTOCOL_INSTRUCTION_READ;
denis2nis 3:add8b050eb86 107 checksum += cmd[4];
denis2nis 2:02f3323a107d 108
denis2nis 3:add8b050eb86 109 /* Param 1: address to read in the Control Table of RAM Area */
denis2nis 2:02f3323a107d 110 cmd[5] = address;
denis2nis 3:add8b050eb86 111 checksum += cmd[5];
denis2nis 3:add8b050eb86 112
denis2nis 3:add8b050eb86 113 /* Param 2: number of bytes to read in the Control Table of RAM Area */
denis2nis 3:add8b050eb86 114 cmd[6] = len;
denis2nis 3:add8b050eb86 115 checksum += cmd[6];
denis2nis 3:add8b050eb86 116
denis2nis 3:add8b050eb86 117 /* Checksum */
denis2nis 3:add8b050eb86 118 /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
denis2nis 3:add8b050eb86 119 cmd[7] = ~checksum;
denis2nis 3:add8b050eb86 120 cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
denis2nis 3:add8b050eb86 121
denis2nis 2:02f3323a107d 122 }
denis2nis 2:02f3323a107d 123 else // write instruction
denis2nis 2:02f3323a107d 124 {
denis2nis 2:02f3323a107d 125 /* byte length of the instruction: parameter and checksum field */
denis2nis 3:add8b050eb86 126 /* For write instruction: 1 INSTR + */
denis2nis 3:add8b050eb86 127 /* (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
denis2nis 2:02f3323a107d 128 cmd[3] = 3 + len;
denis2nis 3:add8b050eb86 129 checksum += cmd[3];
denis2nis 2:02f3323a107d 130
denis2nis 2:02f3323a107d 131 /* set read instruction */
denis2nis 2:02f3323a107d 132 cmd[4] = PROTOCOL_INSTRUCTION_WRITE;
denis2nis 3:add8b050eb86 133 checksum += cmd[4];
denis2nis 2:02f3323a107d 134
denis2nis 2:02f3323a107d 135 /* Param 1: address to write in the "Control Table of RAM Area" */
denis2nis 2:02f3323a107d 136 cmd[5] = address;
denis2nis 3:add8b050eb86 137 checksum += cmd[5];
denis2nis 2:02f3323a107d 138 }
denis2nis 0:7556356a8bcd 139
denis2nis 2:02f3323a107d 140 //cmd[4] = 0x02 + (data != NULL); // original code from Titouan
denis2nis 2:02f3323a107d 141
denis2nis 0:7556356a8bcd 142
denis2nis 0:7556356a8bcd 143 // Compute checksum
denis2nis 0:7556356a8bcd 144 if(data == NULL) {
denis2nis 0:7556356a8bcd 145 cmd[6] = len;
denis2nis 2:02f3323a107d 146 cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
denis2nis 0:7556356a8bcd 147
denis2nis 0:7556356a8bcd 148 // [XXX] Force length to one to force send
denis2nis 0:7556356a8bcd 149 len = 1;
denis2nis 0:7556356a8bcd 150 } else {
denis2nis 2:02f3323a107d 151 char cs = mot_id + len + address + 6;
denis2nis 0:7556356a8bcd 152
denis2nis 0:7556356a8bcd 153 for(char i = 0; i < len; i++) {
denis2nis 0:7556356a8bcd 154 cmd[6 + i] = data[i];
denis2nis 0:7556356a8bcd 155 cs += data[i];
denis2nis 0:7556356a8bcd 156 }
denis2nis 0:7556356a8bcd 157
denis2nis 0:7556356a8bcd 158 cmd[6 + len] = 0xFF - cs;
denis2nis 0:7556356a8bcd 159 }
denis2nis 0:7556356a8bcd 160
denis2nis 0:7556356a8bcd 161 // Send packet
denis2nis 0:7556356a8bcd 162 if(mot_id != 0xFE) {
denis2nis 0:7556356a8bcd 163 for(char i = 0; i < (7 + len); i++) {
denis2nis 0:7556356a8bcd 164 _mx12.write(&cmd[i], 1);
denis2nis 0:7556356a8bcd 165 }
denis2nis 0:7556356a8bcd 166 }
denis2nis 0:7556356a8bcd 167 }
denis2nis 0:7556356a8bcd 168
denis2nis 0:7556356a8bcd 169 void MX12::_ReadCallback() {
denis2nis 0:7556356a8bcd 170 char c;
denis2nis 0:7556356a8bcd 171
denis2nis 0:7556356a8bcd 172 // Loop on reading serial
denis2nis 0:7556356a8bcd 173 if(_mx12.read(&c, 1)) {
denis2nis 0:7556356a8bcd 174 _res[_res_count] = c;
denis2nis 0:7556356a8bcd 175 _res_count++;
denis2nis 0:7556356a8bcd 176 if(_res_count >= MX12_ANSWER_MAX_SIZE) _res_count = 0;
denis2nis 0:7556356a8bcd 177
denis2nis 0:7556356a8bcd 178 // Find answer in buffer
denis2nis 0:7556356a8bcd 179 char ans_i = 2;
denis2nis 0:7556356a8bcd 180 for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
denis2nis 0:7556356a8bcd 181 if(ans_i >= MX12_ANSWER_MAX_SIZE) return;
denis2nis 0:7556356a8bcd 182
denis2nis 0:7556356a8bcd 183 ans_i += 2;
denis2nis 0:7556356a8bcd 184 char mot_id = _res[ans_i++];
denis2nis 0:7556356a8bcd 185 char len = _res[ans_i++];
denis2nis 0:7556356a8bcd 186 _chksm = _res[ans_i + len - 1];
denis2nis 0:7556356a8bcd 187
denis2nis 0:7556356a8bcd 188 // [TODO] Verify checksum
denis2nis 0:7556356a8bcd 189 if(len != 0 && _chksm != 0) {
denis2nis 0:7556356a8bcd 190 // Interpret answer depending on state
denis2nis 0:7556356a8bcd 191 switch(_state) {
denis2nis 0:7556356a8bcd 192 case State::ReadingPosition:
denis2nis 0:7556356a8bcd 193 _angle[mot_id] = (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
denis2nis 0:7556356a8bcd 194 _state = State::Available;
denis2nis 0:7556356a8bcd 195 break;
denis2nis 0:7556356a8bcd 196 case State::Writing:
denis2nis 0:7556356a8bcd 197 // Return the corresponding status code
denis2nis 0:7556356a8bcd 198 switch(_res[ans_i]) {
denis2nis 0:7556356a8bcd 199 case 0:
denis2nis 0:7556356a8bcd 200 _status = Status::Ok;
denis2nis 0:7556356a8bcd 201 break;
denis2nis 0:7556356a8bcd 202 case 1 << 0:
denis2nis 0:7556356a8bcd 203 _status = Status::InputVoltageError;
denis2nis 0:7556356a8bcd 204 break;
denis2nis 0:7556356a8bcd 205 case 1 << 1:
denis2nis 0:7556356a8bcd 206 _status = Status::AngleLimitError;
denis2nis 0:7556356a8bcd 207 break;
denis2nis 0:7556356a8bcd 208 case 1 << 2:
denis2nis 0:7556356a8bcd 209 _status = Status::OverheatingError;
denis2nis 0:7556356a8bcd 210 break;
denis2nis 0:7556356a8bcd 211 case 1 << 3:
denis2nis 0:7556356a8bcd 212 _status = Status::RangeError;
denis2nis 0:7556356a8bcd 213 break;
denis2nis 0:7556356a8bcd 214 case 1 << 4:
denis2nis 0:7556356a8bcd 215 _status = Status::ChecksumError;
denis2nis 0:7556356a8bcd 216 break;
denis2nis 0:7556356a8bcd 217 case 1 << 5:
denis2nis 0:7556356a8bcd 218 _status = Status::OverloadError;
denis2nis 0:7556356a8bcd 219 break;
denis2nis 0:7556356a8bcd 220 case 1 << 6:
denis2nis 0:7556356a8bcd 221 _status = Status::InstructionError;
denis2nis 0:7556356a8bcd 222 break;
denis2nis 0:7556356a8bcd 223 default:
denis2nis 0:7556356a8bcd 224 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 225 }
denis2nis 0:7556356a8bcd 226
denis2nis 0:7556356a8bcd 227 _state = State::Available;
denis2nis 0:7556356a8bcd 228 break;
denis2nis 0:7556356a8bcd 229 default:
denis2nis 0:7556356a8bcd 230 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 231 }
denis2nis 0:7556356a8bcd 232 }
denis2nis 0:7556356a8bcd 233 }
denis2nis 0:7556356a8bcd 234 }