Diff: MX12.cpp

Revision:

0:7556356a8bcd

Child:

2:02f3323a107d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MX12.cpp	Wed Nov 03 11:17:12 2021 +0000
@@ -0,0 +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;
+}
+
+void MX12::SetSpeed(unsigned char mot_id, float speed) {
+    char data[2];
+ 
+    // Speed absolute value
+    int goal = (0x3ff * abs(speed));
+ 
+    // 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);
+}
+
+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");
+}
+
+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);
+        }
+    }
+}
+
+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;
+                    }
+
+                    _state = State::Available;
+                    break;
+                default:
+                    _status = Status::Unknown;
+            }
+        }
+    }
+}
\ No newline at end of file