Titouan Soulard / MX12

MX-12W Servo Library

Dependents:   DISCO_L475VG_IOT01-Sensors-BSP

MX12.cpp@5:4bdd101ce4ec, 2021-04-16 (annotated)

Committer:
tsoul
Date:
Fri Apr 16 14:52:24 2021 +0000
Revision:
5:4bdd101ce4ec
Parent:
4:9ffc4009a463 
Switch to state machine for more reliable parsing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tsoul 0:5e5d94ac9c80 1 #include "MX12.h"
tsoul 0:5e5d94ac9c80 2
tsoul 0:5e5d94ac9c80 3 MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
tsoul 5:4bdd101ce4ec 4 // Serial configuration
tsoul 5:4bdd101ce4ec 5 _mx12.baud(baud);
tsoul 5:4bdd101ce4ec 6 _mx12.format(8, SerialBase::None, 1);
tsoul 5:4bdd101ce4ec 7 _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
tsoul 5:4bdd101ce4ec 8
tsoul 5:4bdd101ce4ec 9 // Internal defaults
tsoul 5:4bdd101ce4ec 10 _sstate = SerialState::Idling;
tsoul 5:4bdd101ce4ec 11 _pstate = ParsingState::Header;
tsoul 0:5e5d94ac9c80 12 }
tsoul 0:5e5d94ac9c80 13
tsoul 0:5e5d94ac9c80 14 void MX12::SetSpeed(unsigned char mot_id, float speed) {
tsoul 5:4bdd101ce4ec 15 char data[2];
tsoul 5:4bdd101ce4ec 16
tsoul 5:4bdd101ce4ec 17 // Speed absolute value
tsoul 5:4bdd101ce4ec 18 int goal = (0x3ff * abs(speed));
tsoul 0:5e5d94ac9c80 19
tsoul 5:4bdd101ce4ec 20 // Spin direction (CW is negative)
tsoul 5:4bdd101ce4ec 21 if (speed < 0) {
tsoul 5:4bdd101ce4ec 22 goal |= (0x1 << 10);
tsoul 5:4bdd101ce4ec 23 }
tsoul 4:9ffc4009a463 24
tsoul 5:4bdd101ce4ec 25 data[0] = goal & 0xff;
tsoul 5:4bdd101ce4ec 26 data[1] = goal >> 8;
tsoul 5:4bdd101ce4ec 27
tsoul 5:4bdd101ce4ec 28 // Send instruction
tsoul 5:4bdd101ce4ec 29 _sstate = SerialState::Writing;
tsoul 5:4bdd101ce4ec 30 rw(mot_id, 0x20, 2, data);
tsoul 0:5e5d94ac9c80 31 }
tsoul 0:5e5d94ac9c80 32
tsoul 4:9ffc4009a463 33 char MX12::IsAvailable(void) {
tsoul 5:4bdd101ce4ec 34 return (_sstate == SerialState::Idling);
tsoul 3:513956de75d7 35 }
tsoul 3:513956de75d7 36
tsoul 3:513956de75d7 37 void MX12::rw(unsigned char mot_id, char adress, char len, char *data) {
tsoul 5:4bdd101ce4ec 38 _answer = 0;
tsoul 5:4bdd101ce4ec 39 memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
tsoul 5:4bdd101ce4ec 40
tsoul 5:4bdd101ce4ec 41 // Forge packet
tsoul 5:4bdd101ce4ec 42 char cmd[16];
tsoul 5:4bdd101ce4ec 43
tsoul 5:4bdd101ce4ec 44 cmd[0] = 0xff;
tsoul 5:4bdd101ce4ec 45 cmd[1] = 0xff;
tsoul 5:4bdd101ce4ec 46 cmd[4] = 0x02 + (data != NULL);
tsoul 5:4bdd101ce4ec 47
tsoul 5:4bdd101ce4ec 48 cmd[2] = mot_id;
tsoul 5:4bdd101ce4ec 49
tsoul 5:4bdd101ce4ec 50 if(data == NULL) cmd[3] = 4;
tsoul 5:4bdd101ce4ec 51 else cmd[3] = 3 + len;
tsoul 5:4bdd101ce4ec 52
tsoul 5:4bdd101ce4ec 53 cmd[5] = adress;
tsoul 5:4bdd101ce4ec 54
tsoul 5:4bdd101ce4ec 55 // Compute checksum
tsoul 5:4bdd101ce4ec 56 if(data == NULL) {
tsoul 5:4bdd101ce4ec 57 cmd[6] = len;
tsoul 5:4bdd101ce4ec 58 cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
tsoul 5:4bdd101ce4ec 59
tsoul 5:4bdd101ce4ec 60 // Force length to one to force send
tsoul 5:4bdd101ce4ec 61 len = 1;
tsoul 5:4bdd101ce4ec 62 } else {
tsoul 5:4bdd101ce4ec 63 char cs = mot_id + len + adress + 6;
tsoul 5:4bdd101ce4ec 64
tsoul 5:4bdd101ce4ec 65 for(char i = 0; i < len; i++) {
tsoul 5:4bdd101ce4ec 66 cmd[6 + i] = data[i];
tsoul 5:4bdd101ce4ec 67 cs += data[i];
tsoul 5:4bdd101ce4ec 68 }
tsoul 5:4bdd101ce4ec 69
tsoul 5:4bdd101ce4ec 70 cmd[6 + len] = 0xFF - cs;
tsoul 5:4bdd101ce4ec 71 }
tsoul 5:4bdd101ce4ec 72
tsoul 5:4bdd101ce4ec 73 // Send packet
tsoul 5:4bdd101ce4ec 74 if(mot_id != 0xFE) {
tsoul 5:4bdd101ce4ec 75 for(char i = 0; i < (7 + len); i++) {
tsoul 5:4bdd101ce4ec 76 _mx12.write(&cmd[i], 1);
tsoul 5:4bdd101ce4ec 77 }
tsoul 5:4bdd101ce4ec 78 }
tsoul 5:4bdd101ce4ec 79 }
tsoul 5:4bdd101ce4ec 80
tsoul 5:4bdd101ce4ec 81 void MX12::PrintSerial() {
tsoul 5:4bdd101ce4ec 82 for(int i = 0; i < _frame_pointer; i++) {
tsoul 5:4bdd101ce4ec 83 printf("%x ", _stored_frame[i]);
tsoul 5:4bdd101ce4ec 84 }
tsoul 5:4bdd101ce4ec 85
tsoul 5:4bdd101ce4ec 86 printf("\n");
tsoul 5:4bdd101ce4ec 87 }
tsoul 5:4bdd101ce4ec 88
tsoul 5:4bdd101ce4ec 89 MX12::Status MX12::GetStatus() {
tsoul 5:4bdd101ce4ec 90 // Return the corresponding status code
tsoul 5:4bdd101ce4ec 91 switch(_current_frame.data[0]) {
tsoul 5:4bdd101ce4ec 92 case 0:
tsoul 5:4bdd101ce4ec 93 return Ok;
tsoul 5:4bdd101ce4ec 94 break;
tsoul 5:4bdd101ce4ec 95 case 1 << 0:
tsoul 5:4bdd101ce4ec 96 return InputVoltageError;
tsoul 5:4bdd101ce4ec 97 break;
tsoul 5:4bdd101ce4ec 98 case 1 << 1:
tsoul 5:4bdd101ce4ec 99 return AngleLimitError;
tsoul 5:4bdd101ce4ec 100 break;
tsoul 5:4bdd101ce4ec 101 case 1 << 2:
tsoul 5:4bdd101ce4ec 102 return OverheatingError;
tsoul 5:4bdd101ce4ec 103 break;
tsoul 5:4bdd101ce4ec 104 case 1 << 3:
tsoul 5:4bdd101ce4ec 105 return RangeError;
tsoul 5:4bdd101ce4ec 106 break;
tsoul 5:4bdd101ce4ec 107 case 1 << 4:
tsoul 5:4bdd101ce4ec 108 return ChecksumError;
tsoul 5:4bdd101ce4ec 109 break;
tsoul 5:4bdd101ce4ec 110 case 1 << 5:
tsoul 5:4bdd101ce4ec 111 return OverloadError;
tsoul 5:4bdd101ce4ec 112 break;
tsoul 5:4bdd101ce4ec 113 case 1 << 6:
tsoul 5:4bdd101ce4ec 114 return InstructionError;
tsoul 5:4bdd101ce4ec 115 break;
tsoul 5:4bdd101ce4ec 116 default:
tsoul 5:4bdd101ce4ec 117 return Unknown;
tsoul 5:4bdd101ce4ec 118 }
tsoul 3:513956de75d7 119 }
tsoul 3:513956de75d7 120
tsoul 3:513956de75d7 121 void MX12::_ReadCallback() {
tsoul 5:4bdd101ce4ec 122 char c;
tsoul 5:4bdd101ce4ec 123
tsoul 5:4bdd101ce4ec 124 // Try to read serial
tsoul 5:4bdd101ce4ec 125 if(_mx12.read(&c, 1)) {
tsoul 5:4bdd101ce4ec 126 _stored_frame[_frame_pointer++] = c;
tsoul 5:4bdd101ce4ec 127 _scontext.checksum += c;
tsoul 5:4bdd101ce4ec 128
tsoul 5:4bdd101ce4ec 129 // State-machine parsing
tsoul 5:4bdd101ce4ec 130 switch(_pstate) {
tsoul 5:4bdd101ce4ec 131 case Header:
tsoul 5:4bdd101ce4ec 132 if(++(_scontext.headingCount) >= 2) {
tsoul 5:4bdd101ce4ec 133 _scontext.headingCount = 0;
tsoul 5:4bdd101ce4ec 134 _pstate = Id;
tsoul 5:4bdd101ce4ec 135 }
tsoul 5:4bdd101ce4ec 136
tsoul 5:4bdd101ce4ec 137 _scontext.checksum -= c;
tsoul 5:4bdd101ce4ec 138 break;
tsoul 5:4bdd101ce4ec 139
tsoul 5:4bdd101ce4ec 140 case Id:
tsoul 5:4bdd101ce4ec 141 _current_frame.motorId = c;
tsoul 5:4bdd101ce4ec 142 _pstate = Length;
tsoul 5:4bdd101ce4ec 143 break;
tsoul 4:9ffc4009a463 144
tsoul 5:4bdd101ce4ec 145 case Length:
tsoul 5:4bdd101ce4ec 146 _current_frame.length = c - 1;
tsoul 5:4bdd101ce4ec 147 _pstate = Data;
tsoul 5:4bdd101ce4ec 148 break;
tsoul 5:4bdd101ce4ec 149
tsoul 5:4bdd101ce4ec 150 case Data:
tsoul 5:4bdd101ce4ec 151 _current_frame.data[_scontext.dataCount] = c;
tsoul 5:4bdd101ce4ec 152
tsoul 5:4bdd101ce4ec 153 if(++(_scontext.dataCount) >= _current_frame.length) {
tsoul 5:4bdd101ce4ec 154 _scontext.dataCount = 0;
tsoul 5:4bdd101ce4ec 155 _pstate = Checksum;
tsoul 5:4bdd101ce4ec 156 }
tsoul 5:4bdd101ce4ec 157 break;
tsoul 5:4bdd101ce4ec 158
tsoul 5:4bdd101ce4ec 159 case Checksum:
tsoul 5:4bdd101ce4ec 160 _current_frame.valid = (_scontext.checksum == 0xFF);
tsoul 5:4bdd101ce4ec 161 _scontext.checksum = 0;
tsoul 5:4bdd101ce4ec 162 _pstate = Header;
tsoul 5:4bdd101ce4ec 163 if(_answer) _sstate = Idling;
tsoul 5:4bdd101ce4ec 164 _answer = 1;
tsoul 5:4bdd101ce4ec 165 break;
tsoul 5:4bdd101ce4ec 166 }
tsoul 5:4bdd101ce4ec 167 }
tsoul 5:4bdd101ce4ec 168 }