PSL_2021 / servomotor_MX12_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.

Dependents:   PSL_ROBOT_lorenzo robot_lorenzo recepteur_mbed_os_6

MX12.cpp@0:7556356a8bcd, 2021-11-03 (annotated)

Committer:
denis2nis
Date:
Wed Nov 03 11:17:12 2021 +0000
Revision:
0:7556356a8bcd
Child:
2:02f3323a107d
Initial commit

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 0:7556356a8bcd 59 void MX12::rw(unsigned char mot_id, char adress, 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 0:7556356a8bcd 63 // Forge packet
denis2nis 0:7556356a8bcd 64 char cmd[16];
denis2nis 0:7556356a8bcd 65
denis2nis 0:7556356a8bcd 66 cmd[0] = 0xff;
denis2nis 0:7556356a8bcd 67 cmd[1] = 0xff;
denis2nis 0:7556356a8bcd 68 cmd[4] = 0x02 + (data != NULL);
denis2nis 0:7556356a8bcd 69
denis2nis 0:7556356a8bcd 70 cmd[2] = mot_id;
denis2nis 0:7556356a8bcd 71
denis2nis 0:7556356a8bcd 72 if(data == NULL) cmd[3] = 4;
denis2nis 0:7556356a8bcd 73 else cmd[3] = 3 + len;
denis2nis 0:7556356a8bcd 74
denis2nis 0:7556356a8bcd 75 cmd[5] = adress;
denis2nis 0:7556356a8bcd 76
denis2nis 0:7556356a8bcd 77 // Compute checksum
denis2nis 0:7556356a8bcd 78 if(data == NULL) {
denis2nis 0:7556356a8bcd 79 cmd[6] = len;
denis2nis 0:7556356a8bcd 80 cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
denis2nis 0:7556356a8bcd 81
denis2nis 0:7556356a8bcd 82 // [XXX] Force length to one to force send
denis2nis 0:7556356a8bcd 83 len = 1;
denis2nis 0:7556356a8bcd 84 } else {
denis2nis 0:7556356a8bcd 85 char cs = mot_id + len + adress + 6;
denis2nis 0:7556356a8bcd 86
denis2nis 0:7556356a8bcd 87 for(char i = 0; i < len; i++) {
denis2nis 0:7556356a8bcd 88 cmd[6 + i] = data[i];
denis2nis 0:7556356a8bcd 89 cs += data[i];
denis2nis 0:7556356a8bcd 90 }
denis2nis 0:7556356a8bcd 91
denis2nis 0:7556356a8bcd 92 cmd[6 + len] = 0xFF - cs;
denis2nis 0:7556356a8bcd 93 }
denis2nis 0:7556356a8bcd 94
denis2nis 0:7556356a8bcd 95 // Send packet
denis2nis 0:7556356a8bcd 96 if(mot_id != 0xFE) {
denis2nis 0:7556356a8bcd 97 for(char i = 0; i < (7 + len); i++) {
denis2nis 0:7556356a8bcd 98 _mx12.write(&cmd[i], 1);
denis2nis 0:7556356a8bcd 99 }
denis2nis 0:7556356a8bcd 100 }
denis2nis 0:7556356a8bcd 101 }
denis2nis 0:7556356a8bcd 102
denis2nis 0:7556356a8bcd 103 void MX12::_ReadCallback() {
denis2nis 0:7556356a8bcd 104 char c;
denis2nis 0:7556356a8bcd 105
denis2nis 0:7556356a8bcd 106 // Loop on reading serial
denis2nis 0:7556356a8bcd 107 if(_mx12.read(&c, 1)) {
denis2nis 0:7556356a8bcd 108 _res[_res_count] = c;
denis2nis 0:7556356a8bcd 109 _res_count++;
denis2nis 0:7556356a8bcd 110 if(_res_count >= MX12_ANSWER_MAX_SIZE) _res_count = 0;
denis2nis 0:7556356a8bcd 111
denis2nis 0:7556356a8bcd 112 // Find answer in buffer
denis2nis 0:7556356a8bcd 113 char ans_i = 2;
denis2nis 0:7556356a8bcd 114 for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
denis2nis 0:7556356a8bcd 115 if(ans_i >= MX12_ANSWER_MAX_SIZE) return;
denis2nis 0:7556356a8bcd 116
denis2nis 0:7556356a8bcd 117 ans_i += 2;
denis2nis 0:7556356a8bcd 118 char mot_id = _res[ans_i++];
denis2nis 0:7556356a8bcd 119 char len = _res[ans_i++];
denis2nis 0:7556356a8bcd 120 _chksm = _res[ans_i + len - 1];
denis2nis 0:7556356a8bcd 121
denis2nis 0:7556356a8bcd 122 // [TODO] Verify checksum
denis2nis 0:7556356a8bcd 123 if(len != 0 && _chksm != 0) {
denis2nis 0:7556356a8bcd 124 // Interpret answer depending on state
denis2nis 0:7556356a8bcd 125 switch(_state) {
denis2nis 0:7556356a8bcd 126 case State::ReadingPosition:
denis2nis 0:7556356a8bcd 127 _angle[mot_id] = (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
denis2nis 0:7556356a8bcd 128 _state = State::Available;
denis2nis 0:7556356a8bcd 129 break;
denis2nis 0:7556356a8bcd 130 case State::Writing:
denis2nis 0:7556356a8bcd 131 // Return the corresponding status code
denis2nis 0:7556356a8bcd 132 switch(_res[ans_i]) {
denis2nis 0:7556356a8bcd 133 case 0:
denis2nis 0:7556356a8bcd 134 _status = Status::Ok;
denis2nis 0:7556356a8bcd 135 break;
denis2nis 0:7556356a8bcd 136 case 1 << 0:
denis2nis 0:7556356a8bcd 137 _status = Status::InputVoltageError;
denis2nis 0:7556356a8bcd 138 break;
denis2nis 0:7556356a8bcd 139 case 1 << 1:
denis2nis 0:7556356a8bcd 140 _status = Status::AngleLimitError;
denis2nis 0:7556356a8bcd 141 break;
denis2nis 0:7556356a8bcd 142 case 1 << 2:
denis2nis 0:7556356a8bcd 143 _status = Status::OverheatingError;
denis2nis 0:7556356a8bcd 144 break;
denis2nis 0:7556356a8bcd 145 case 1 << 3:
denis2nis 0:7556356a8bcd 146 _status = Status::RangeError;
denis2nis 0:7556356a8bcd 147 break;
denis2nis 0:7556356a8bcd 148 case 1 << 4:
denis2nis 0:7556356a8bcd 149 _status = Status::ChecksumError;
denis2nis 0:7556356a8bcd 150 break;
denis2nis 0:7556356a8bcd 151 case 1 << 5:
denis2nis 0:7556356a8bcd 152 _status = Status::OverloadError;
denis2nis 0:7556356a8bcd 153 break;
denis2nis 0:7556356a8bcd 154 case 1 << 6:
denis2nis 0:7556356a8bcd 155 _status = Status::InstructionError;
denis2nis 0:7556356a8bcd 156 break;
denis2nis 0:7556356a8bcd 157 default:
denis2nis 0:7556356a8bcd 158 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 159 }
denis2nis 0:7556356a8bcd 160
denis2nis 0:7556356a8bcd 161 _state = State::Available;
denis2nis 0:7556356a8bcd 162 break;
denis2nis 0:7556356a8bcd 163 default:
denis2nis 0:7556356a8bcd 164 _status = Status::Unknown;
denis2nis 0:7556356a8bcd 165 }
denis2nis 0:7556356a8bcd 166 }
denis2nis 0:7556356a8bcd 167 }
denis2nis 0:7556356a8bcd 168 }