Diff: MX12.cpp

Revision:

5:4bdd101ce4ec

Parent:

4:9ffc4009a463

--- a/MX12.cpp	Fri Jan 15 09:15:47 2021 +0000
+++ b/MX12.cpp	Fri Apr 16 14:52:24 2021 +0000
@@ -1,168 +1,168 @@
 #include "MX12.h"
 
 MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
-    _baud = baud;
-    _mx12.baud(_baud);
-    _mx12.format(8, SerialBase::None, 1);
-    _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
-    _state = State::Available;
+	// Serial configuration
+	_mx12.baud(baud);
+	_mx12.format(8, SerialBase::None, 1);
+	_mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
+
+	// Internal defaults
+	_sstate = SerialState::Idling;
+	_pstate = ParsingState::Header;
 }
 
 void MX12::SetSpeed(unsigned char mot_id, float speed) {
-    char data[2];
- 
-    // Speed absolute value
-    int goal = (0x3ff * abs(speed));
+	char data[2];
+
+	// Speed absolute value
+	int goal = (0x3ff * abs(speed));
  
-    // Spin direction (CW is negative)
-    if (speed < 0) {
-        goal |= (0x1 << 10);
-    }
+	// Spin direction (CW is negative)
+	if (speed < 0) {
+		goal |= (0x1 << 10);
+	}
 
-    data[0] = goal & 0xff;
-    data[1] = goal >> 8;
-    
-    // Enter writing state
-    _state = State::Writing;
- 
-    // Send instruction
-    rw(mot_id, 0x20, 2, data);
+	data[0] = goal & 0xff;
+	data[1] = goal >> 8;
+	
+	// Send instruction
+	_sstate = SerialState::Writing;
+	rw(mot_id, 0x20, 2, data);
 }
 
 char MX12::IsAvailable(void) {
-    return _state == State::Available;
-}
-
-void MX12::ReadPosition(unsigned char mot_id) {
-    // Make a request, interrupt takes care of everything else
-    _state = State::ReadingPosition;
-    rw(mot_id, 0x24, 2, NULL);
-}
-
-float MX12::GetPosition(unsigned char mot_id) {
-    return _angle[mot_id];
-}
-
-MX12::Status MX12::GetStatus(void) {
-    return _status;
-}
-
-// Debug function to print Serial read
-void MX12::PrintAnswer() {
-    for(char i = 0; i < MX12_ANSWER_MAX_SIZE; i++) {
-        printf("%x ", _res[i]);
-    }
-    
-    printf("\r\n");
+	return (_sstate == SerialState::Idling);
 }
 
 void MX12::rw(unsigned char mot_id, char adress, char len, char *data) {
-    _res_count = 0;
-    memset(_res, 0, MX12_ANSWER_MAX_SIZE);
-    
-    // Forge packet
-    char cmd[16];
-    
-    cmd[0] = 0xff;
-    cmd[1] = 0xff;
-    cmd[4] = 0x02 + (data != NULL);
-    
-    cmd[2] = mot_id;
-    
-    if(data == NULL) cmd[3] = 4;
-    else cmd[3] = 3 + len;
-    
-    cmd[5] = adress;
-    
-    // Compute checksum
-    if(data == NULL) {
-        cmd[6] = len;
-        cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
-        
-        // [XXX] Force length to one to force send
-        len = 1;
-    } else {
-        char cs = mot_id + len + adress + 6;
-        
-        for(char i = 0; i < len; i++) {
-            cmd[6 + i] = data[i];
-            cs += data[i];
-        }
-        
-        cmd[6 + len] = 0xFF - cs;
-    }
-    
-    // Send packet
-    if(mot_id != 0xFE) {
-        for(char i = 0; i < (7 + len); i++) {
-            _mx12.write(&cmd[i], 1);
-        }
-    }
+	_answer = 0;
+	memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
+
+	// Forge packet
+	char cmd[16];
+	
+	cmd[0] = 0xff;
+	cmd[1] = 0xff;
+	cmd[4] = 0x02 + (data != NULL);
+	
+	cmd[2] = mot_id;
+	
+	if(data == NULL) cmd[3] = 4;
+	else cmd[3] = 3 + len;
+	
+	cmd[5] = adress;
+	
+	// Compute checksum
+	if(data == NULL) {
+		cmd[6] = len;
+		cmd[7] = 0xFF - (mot_id + 4 + 2 + adress + len);
+		
+		// Force length to one to force send
+		len = 1;
+	} else {
+		char cs = mot_id + len + adress + 6;
+		
+		for(char i = 0; i < len; i++) {
+			cmd[6 + i] = data[i];
+			cs += data[i];
+		}
+		
+		cmd[6 + len] = 0xFF - cs;
+	}
+	
+	// Send packet
+	if(mot_id != 0xFE) {
+		for(char i = 0; i < (7 + len); i++) {
+			_mx12.write(&cmd[i], 1);
+		}
+	}
+}
+
+void MX12::PrintSerial() {
+	for(int i = 0; i < _frame_pointer; i++) {
+		printf("%x ", _stored_frame[i]);
+	}
+
+	printf("\n");
+}
+
+MX12::Status MX12::GetStatus() {
+	// Return the corresponding status code
+	switch(_current_frame.data[0]) {
+		case 0:
+			return Ok;
+			break;
+		case 1 << 0:
+			return InputVoltageError;
+			break;
+		case 1 << 1:
+			return AngleLimitError;
+			break;
+		case 1 << 2:
+			return OverheatingError;
+			break;
+		case 1 << 3:
+			return RangeError;
+			break;
+		case 1 << 4:
+			return ChecksumError;
+			break;
+		case 1 << 5:
+			return OverloadError;
+			break;
+		case 1 << 6:
+			return InstructionError;
+			break;
+		default:
+			return Unknown;
+	}
 }
 
 void MX12::_ReadCallback() {
-    char c;
-    
-    // Loop on reading serial
-    if(_mx12.read(&c, 1)) {
-        _res[_res_count] = c;
-        _res_count++;
-        if(_res_count >= MX12_ANSWER_MAX_SIZE) _res_count = 0;
-        
-        // Find answer in buffer
-        char ans_i = 2;
-        for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
-        if(ans_i >= MX12_ANSWER_MAX_SIZE) return;
-        
-        ans_i += 2;
-        char mot_id = _res[ans_i++];
-        char len = _res[ans_i++];
-        _chksm = _res[ans_i + len - 1];
-        
-        // [TODO] Verify checksum
-        if(len != 0 && _chksm != 0) {
-            // Interpret answer depending on state
-            switch(_state) {
-                case State::ReadingPosition:
-                    _angle[mot_id] = (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
-                    _state = State::Available;
-                    break;
-                case State::Writing:
-                    // Return the corresponding status code
-                    switch(_res[ans_i]) {
-                        case 0:
-                            _status = Status::Ok;
-                            break;
-                        case 1 << 0:
-                            _status = Status::InputVoltageError;
-                            break;
-                        case 1 << 1:
-                            _status = Status::AngleLimitError;
-                            break;
-                        case 1 << 2:
-                            _status = Status::OverheatingError;
-                            break;
-                        case 1 << 3:
-                            _status = Status::RangeError;
-                            break;
-                        case 1 << 4:
-                            _status = Status::ChecksumError;
-                            break;
-                        case 1 << 5:
-                            _status = Status::OverloadError;
-                            break;
-                        case 1 << 6:
-                            _status = Status::InstructionError;
-                            break;
-                        default:
-                            _status = Status::Unknown;
-                    }
+	char c;
+	
+	// Try to read serial
+	if(_mx12.read(&c, 1)) {
+		_stored_frame[_frame_pointer++] = c;
+		_scontext.checksum += c;
+
+		// State-machine parsing
+		switch(_pstate) {
+			case Header:
+				if(++(_scontext.headingCount) >= 2) {
+					_scontext.headingCount = 0;
+					_pstate = Id;
+				}
+
+				_scontext.checksum -= c;
+				break;
+			
+			case Id:
+				_current_frame.motorId = c;
+				_pstate = Length;
+				break;
 
-                    _state = State::Available;
-                    break;
-                default:
-                    _status = Status::Unknown;
-            }
-        }
-    }
-}
\ No newline at end of file
+			case Length:
+				_current_frame.length = c - 1;
+				_pstate = Data;
+				break;
+
+			case Data:
+				_current_frame.data[_scontext.dataCount] = c;
+				
+				if(++(_scontext.dataCount) >= _current_frame.length) {
+					_scontext.dataCount = 0;
+					_pstate = Checksum;
+				}
+				break;
+			
+			case Checksum:
+				_current_frame.valid = (_scontext.checksum == 0xFF);
+				_scontext.checksum = 0;
+				_pstate = Header;
+				if(_answer) _sstate = Idling;
+				_answer = 1;
+				break;
+		}
+	}
+}