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@2:02f3323a107d, 2021-11-04 (annotated)

Committer:
denis2nis
Date:
Thu Nov 04 06:12:21 2021 +0000
Revision:
2:02f3323a107d
Parent:
0:7556356a8bcd
Child:
3:add8b050eb86
Arrange and comment MX12::rw function

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 0:7556356a8bcd 65
denis2nis 2:02f3323a107d 66 /* Instruction Packet is the command data sent to the Device.
denis2nis 2:02f3323a107d 67 *
denis2nis 2:02f3323a107d 68 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
denis2nis 2:02f3323a107d 69 * |-------|-------|---------|------|-----------|---------------|---------|
denis2nis 2:02f3323a107d 70 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 2:02f3323a107d 71 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... |(2 bytes)|
denis2nis 2:02f3323a107d 72 * \__ ___/
denis2nis 2:02f3323a107d 73 * | \/ |
denis2nis 2:02f3323a107d 74 * | len = N-1 |
denis2nis 2:02f3323a107d 75 * \____________ ___________/
denis2nis 2:02f3323a107d 76 * \/
denis2nis 2:02f3323a107d 77 * Length (cmd[4])
denis2nis 2:02f3323a107d 78 */
denis2nis 2:02f3323a107d 79
denis2nis 2:02f3323a107d 80 /* header 1 = 0xFF (dynamixel protocol 1.0) */
denis2nis 0:7556356a8bcd 81 cmd[0] = 0xff;
denis2nis 0:7556356a8bcd 82
denis2nis 2:02f3323a107d 83 /* header 2 = 0xFF (dynamixel protocol 1.0) */
denis2nis 2:02f3323a107d 84 cmd[1] = 0xff;
denis2nis 2:02f3323a107d 85
denis2nis 2:02f3323a107d 86 /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
denis2nis 0:7556356a8bcd 87 cmd[2] = mot_id;
denis2nis 2:02f3323a107d 88
denis2nis 2:02f3323a107d 89 /* Length indicates the byte size of the instruction,
denis2nis 2:02f3323a107d 90 parameter and Checksum field */
denis2nis 2:02f3323a107d 91
denis2nis 0:7556356a8bcd 92
denis2nis 2:02f3323a107d 93 /* Guess instruction type. NULL for read, not NULL for write */
denis2nis 2:02f3323a107d 94 if(data == NULL) // read instruction
denis2nis 2:02f3323a107d 95 {
denis2nis 2:02f3323a107d 96 /* byte length of the instruction: parameter and checksum field. */
denis2nis 2:02f3323a107d 97 /* for read instruction: */
denis2nis 2:02f3323a107d 98 /* 2 PARAM (starting address, length of data) + 2 CHKSUM */
denis2nis 2:02f3323a107d 99 cmd[3] = 4; //
denis2nis 2:02f3323a107d 100
denis2nis 2:02f3323a107d 101 /* set write instruction */
denis2nis 2:02f3323a107d 102 cmd[4] = PROTOCOL_INSTRUCTION_READ;
denis2nis 2:02f3323a107d 103
denis2nis 2:02f3323a107d 104 /* Param 1: address to read in the "Control Table of RAM Area" */
denis2nis 2:02f3323a107d 105 cmd[5] = address;
denis2nis 2:02f3323a107d 106 }
denis2nis 2:02f3323a107d 107 else // write instruction
denis2nis 2:02f3323a107d 108 {
denis2nis 2:02f3323a107d 109 /* byte length of the instruction: parameter and checksum field */
denis2nis 2:02f3323a107d 110 /* For write instruction: */
denis2nis 2:02f3323a107d 111 /* (1 + len) PARAM (starting Address, bytes to write) + 2 CHKSUM */
denis2nis 2:02f3323a107d 112 cmd[3] = 3 + len;
denis2nis 2:02f3323a107d 113
denis2nis 2:02f3323a107d 114 /* set read instruction */
denis2nis 2:02f3323a107d 115 cmd[4] = PROTOCOL_INSTRUCTION_WRITE;
denis2nis 2:02f3323a107d 116
denis2nis 2:02f3323a107d 117 /* Param 1: address to write in the "Control Table of RAM Area" */
denis2nis 2:02f3323a107d 118 cmd[5] = address;
denis2nis 2:02f3323a107d 119 }
denis2nis 0:7556356a8bcd 120
denis2nis 2:02f3323a107d 121 //cmd[4] = 0x02 + (data != NULL); // original code from Titouan
denis2nis 2:02f3323a107d 122
denis2nis 0:7556356a8bcd 123
denis2nis 0:7556356a8bcd 124 // Compute checksum
denis2nis 0:7556356a8bcd 125 if(data == NULL) {
denis2nis 0:7556356a8bcd 126 cmd[6] = len;
denis2nis 2:02f3323a107d 127 cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
denis2nis 0:7556356a8bcd 128
denis2nis 0:7556356a8bcd 129 // [XXX] Force length to one to force send
denis2nis 0:7556356a8bcd 130 len = 1;
denis2nis 0:7556356a8bcd 131 } else {
denis2nis 2:02f3323a107d 132 char cs = mot_id + len + address + 6;
denis2nis 0:7556356a8bcd 133
denis2nis 0:7556356a8bcd 134 for(char i = 0; i < len; i++) {
denis2nis 0:7556356a8bcd 135 cmd[6 + i] = data[i];
denis2nis 0:7556356a8bcd 136 cs += data[i];
denis2nis 0:7556356a8bcd 137 }
denis2nis 0:7556356a8bcd 138
denis2nis 0:7556356a8bcd 139 cmd[6 + len] = 0xFF - cs;
denis2nis 0:7556356a8bcd 140 }
denis2nis 0:7556356a8bcd 141
denis2nis 0:7556356a8bcd 142 // Send packet
denis2nis 0:7556356a8bcd 143 if(mot_id != 0xFE) {
denis2nis 0:7556356a8bcd 144 for(char i = 0; i < (7 + len); i++) {
denis2nis 0:7556356a8bcd 145 _mx12.write(&cmd[i], 1);
denis2nis 0:7556356a8bcd 146 }
denis2nis 0:7556356a8bcd 147 }
denis2nis 0:7556356a8bcd 148 }
denis2nis 0:7556356a8bcd 149
denis2nis 0:7556356a8bcd 150 void MX12::_ReadCallback() {
denis2nis 0:7556356a8bcd 151 char c;
denis2nis 0:7556356a8bcd 152
denis2nis 0:7556356a8bcd 153 // Loop on reading serial
denis2nis 0:7556356a8bcd 154 if(_mx12.read(&c, 1)) {
denis2nis 0:7556356a8bcd 155 _res[_res_count] = c;
denis2nis 0:7556356a8bcd 156 _res_count++;
denis2nis 0:7556356a8bcd 157 if(_res_count >= MX12_ANSWER_MAX_SIZE) _res_count = 0;
denis2nis 0:7556356a8bcd 158
denis2nis 0:7556356a8bcd 159 // Find answer in buffer
denis2nis 0:7556356a8bcd 160 char ans_i = 2;
denis2nis 0:7556356a8bcd 161 for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
denis2nis 0:7556356a8bcd 162 if(ans_i >= MX12_ANSWER_MAX_SIZE) return;
denis2nis 0:7556356a8bcd 163
denis2nis 0:7556356a8bcd 164 ans_i += 2;
denis2nis 0:7556356a8bcd 165 char mot_id = _res[ans_i++];
denis2nis 0:7556356a8bcd 166 char len = _res[ans_i++];
denis2nis 0:7556356a8bcd 167 _chksm = _res[ans_i + len - 1];
denis2nis 0:7556356a8bcd 168
denis2nis 0:7556356a8bcd 169 // [TODO] Verify checksum
denis2nis 0:7556356a8bcd 170 if(len != 0 && _chksm != 0) {
denis2nis 0:7556356a8bcd 171 // Interpret answer depending on state
denis2nis 0:7556356a8bcd 172 switch(_state) {
denis2nis 0:7556356a8bcd 173 case State::ReadingPosition:
denis2nis 0:7556356a8bcd 174 _angle[mot_id] = (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
denis2nis 0:7556356a8bcd 175 _state = State::Available;
denis2nis 0:7556356a8bcd 176 break;
denis2nis 0:7556356a8bcd 177 case State::Writing:
denis2nis 0:7556356a8bcd 178 // Return the corresponding status code
denis2nis 0:7556356a8bcd 179 switch(_res[ans_i]) {
denis2nis 0:7556356a8bcd 180 case 0:
denis2nis 0:7556356a8bcd 181 _status = Status::Ok;
denis2nis 0:7556356a8bcd 182 break;
denis2nis 0:7556356a8bcd 183 case 1 << 0:
denis2nis 0:7556356a8bcd 184 _status = Status::InputVoltageError;
denis2nis 0:7556356a8bcd 185 break;
denis2nis 0:7556356a8bcd 186 case 1 << 1:
denis2nis 0:7556356a8bcd 187 _status = Status::AngleLimitError;
denis2nis 0:7556356a8bcd 188 break;
denis2nis 0:7556356a8bcd 189 case 1 << 2:
denis2nis 0:7556356a8bcd 190 _status = Status::OverheatingError;
denis2nis 0:7556356a8bcd 191 break;
denis2nis 0:7556356a8bcd 192 case 1 << 3:
denis2nis 0:7556356a8bcd 193 _status = Status::RangeError;
denis2nis 0:7556356a8bcd 194 break;
denis2nis 0:7556356a8bcd 195 case 1 << 4:
denis2nis 0:7556356a8bcd 196 _status = Status::ChecksumError;
denis2nis 0:7556356a8bcd 197 break;
denis2nis 0:7556356a8bcd 198 case 1 << 5:
denis2nis 0:7556356a8bcd 199 _status = Status::OverloadError;
denis2nis 0:7556356a8bcd 200 break;
denis2nis 0:7556356a8bcd 201 case 1 << 6:
denis2nis 0:7556356a8bcd 202 _status = Status::InstructionError;
denis2nis 0:7556356a8bcd 203 break;
denis2nis 0:7556356a8bcd 204 default:
denis2nis 0:7556356a8bcd 205 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 206 }
denis2nis 0:7556356a8bcd 207
denis2nis 0:7556356a8bcd 208 _state = State::Available;
denis2nis 0:7556356a8bcd 209 break;
denis2nis 0:7556356a8bcd 210 default:
denis2nis 0:7556356a8bcd 211 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 212 }
denis2nis 0:7556356a8bcd 213 }
denis2nis 0:7556356a8bcd 214 }
denis2nis 0:7556356a8bcd 215 }