Diff: MX12.cpp

Revision:

4:9ffc4009a463

Parent:

3:513956de75d7

Child:

5:4bdd101ce4ec

--- a/MX12.cpp	Fri Dec 11 11:05:33 2020 +0000
+++ b/MX12.cpp	Fri Jan 15 09:15:47 2021 +0000
@@ -5,6 +5,7 @@
     _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) {
@@ -17,73 +18,33 @@
     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);
 }
 
-float MX12::ReadPosition(unsigned char mot_id) {
-    rw(mot_id, 0x24, 2, NULL);
-    // [TODO] Waiting is dirty when using interrupts...
-    wait_us(320);
-    
-    // Find answer in buffer
-    char ans_i = 2;
-    char error_flag = false;
-    
-    for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
-    
-    ans_i += 5;
-    
-    return (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
+char MX12::IsAvailable(void) {
+    return _state == State::Available;
 }
 
-MX12::Status MX12::GetStatus() {
-    // Find answer in buffer
-    char ans_i = 2;
-    char error_flag = false;
-    
-    for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
-    
-    ans_i += 4;
-    
-    // No answer received
-    if(ans_i >= MX12_ANSWER_MAX_SIZE) {
-        return Status::Unknown;
-    }
-    
-    // Return the corresponding status code
-    switch(_res[ans_i]) {
-        case 0:
-            return Status::Ok;
-            break;
-        case 1 << 0:
-            return Status::InputVoltageError;
-            break;
-        case 1 << 1:
-            return Status::AngleLimitError;
-            break;
-        case 1 << 2:
-            return Status::OverheatingError;
-            break;
-        case 1 << 3:
-            return Status::RangeError;
-            break;
-        case 1 << 4:
-            return Status::ChecksumError;
-            break;
-        case 1 << 5:
-            return Status::OverloadError;
-            break;
-        case 1 << 6:
-            return Status::InstructionError;
-            break;
-        default:
-            return Status::Unknown;
-    }
+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
@@ -97,6 +58,7 @@
 
 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];
@@ -146,5 +108,61 @@
         _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