Diff: MX12.cpp

Revision:

5:0cf54586a4be

Parent:

3:add8b050eb86

Child:

8:e74ef93ae660

--- a/MX12.cpp	Thu Nov 04 07:32:20 2021 +0000
+++ b/MX12.cpp	Thu Nov 04 11:48:53 2021 +0000
@@ -1,11 +1,14 @@
 #include "MX12.h"
 
 MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
-    _baud = baud;
-    _mx12.baud(_baud);
+    // Serial configuration
+    _mx12.baud(baud);
     _mx12.format(8, SerialBase::None, 1);
     _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
-    _state = State::Available;
+    
+    // Internal defaults
+    _sstate = SerialState::Idle;
+    _pstate = ParsingState::Header;
 }
 
 void MX12::SetSpeed(unsigned char mot_id, float speed) {
@@ -22,17 +25,215 @@
     data[0] = goal & 0xff;
     data[1] = goal >> 8;
     
-    // Enter writing state
-    _state = State::Writing;
- 
     // Send instruction
-    rw(mot_id, 0x20, 2, data);
+    _sstate = SerialState::Writing;
+    rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
 }
 
 char MX12::IsAvailable(void) {
-    return _state == State::Available;
+    return (_sstate == SerialState::Idle);
 }
 
+void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
+    
+    /* Set variables for reception from servovotor */
+    _answer  = 0;
+    memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
+    
+    /* Initialise instruction packet to forge.
+     * Instruction Packet is the command data sent to the servomotor.
+     *
+     * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
+     * |-------|-------|---------|------|-----------|---------------|--------|
+     * |  0xFF |  0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
+     * | cmd[0]| cmd[1]|  cmd[2] |cmd[3]|  cmd[4]   |cmd[5]...      |        |
+     *                  \__  ___/                    \_  _/ \__  __/
+     *                     \/           |              \/      \/            |
+     *                   mot_id         |           address   data           |
+     *                                  |                     (len = N-1)    |
+     *                                  \__________________  ________________/
+     *                                                     \/
+     *                                                  Length ( cmd[3] )
+     */
+    char packet[16];
+    unsigned char packet_length;
+    
+    /* Initialise checksum to calculate
+     * It is used to check if packet is damaged during communication. 
+     * Status Checksum is calculated according to the following formula:
+     *
+     * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
+     */
+    char checksum = 0x00;
+        
+    /* header 1 = 0xFF (dynamixel protocol 1.0) */
+    packet[0] = 0xff;
+    
+    /* header 2 = 0xFF (dynamixel protocol 1.0) */
+    packet[1] = 0xff; 
+    
+     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
+    packet[2] = mot_id;
+    checksum += packet[2];
+    
+    /* Guess instruction type. NULL for read, not NULL for write */
+    if(data == NULL) // read instruction
+    {
+        /* byte length of the instruction: parameter and checksum field.   */
+        /* for read instruction:   1 INSTR +                               */
+        /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
+        packet[3] = 4;
+        checksum += packet[3];
+
+        /* set write instruction */
+        packet[4] = PROTOCOL_INSTRUCTION_READ;
+        checksum += packet[4];
+        
+        /* Param 1: address to read in the Control Table of RAM Area */
+        packet[5] = address;
+        checksum += packet[5];
+        
+        /* Param 2: number of bytes to read in the Control Table of RAM Area */
+        packet[6] = len;
+        checksum += packet[6];
+        
+        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
+        packet[7] = ~checksum;
+        
+        packet_length = 8;
+    }
+    else // write instruction
+    {
+        /* byte length of the instruction: parameter and checksum field     */
+        /* For write instruction:   1 INSTR +                               */
+        /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
+        packet[3] = 3 + len; 
+        checksum += packet[3];
+
+        /* set read instruction */
+        packet[4] = PROTOCOL_INSTRUCTION_WRITE;
+        checksum += packet[4];
+
+        /* Param 1: address to write in the "Control Table of RAM Area" */
+        packet[5] = address;
+        checksum += packet[5];
+        
+        /* Param 2 to N: data to write in the Control Table of RAM Area */
+        for(char i = 0; i < len; i++) {
+            packet[6 + i] = data[i];
+            checksum += data[i];
+        }
+        
+        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
+        packet[6 + len] = ~checksum;
+        
+        packet_length = 7 + len;
+    }
+    
+    // Send packet
+    if(mot_id != 0xFE) {
+        for(char i = 0; i < packet_length; i++) {
+            _mx12.write(&packet[i], 1);
+        }
+    }
+}
+
+// Debug function to print Serial read
+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;
+    
+    // Try to read serial
+    if(_mx12.read(&c, 1)) {
+        _stored_frame[_frame_pointer++] = c;
+        //_res[_res_count] = c;
+        //_res_count++;
+        _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;
+ 
+            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 = Idle;
+                _answer = 1;
+                break;
+        }        
+    }
+}
+
+/*
+
 void MX12::ReadPosition(unsigned char mot_id) {
     // Make a request, interrupt takes care of everything else
     _state = State::ReadingPosition;
@@ -43,192 +244,4 @@
     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 address, char len, char *data) {
-    _res_count = 0;
-    memset(_res, 0, MX12_ANSWER_MAX_SIZE);
-    
-    /* Forge instruction packet */
-    char cmd[16];
-    char checksum = 0x00;
-    
-    /* Instruction Packet is the command data sent to the Device.
-     *
-     * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
-     * |-------|-------|---------|------|-----------|---------------|---------|
-     * |  0xFF |  0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM  |
-     * | cmd[0]| cmd[1]|  cmd[2] |cmd[3]|  cmd[4]   |cmd[5]...      |(2 bytes)|
-     *                                               \_  _/ \__  __/
-     *                                  |              \/      \/             |
-     *                                  |           address   data            |
-     *                                  |                     (len = N-1)     |
-     *                                  \__________________  _________________/
-     *                                                     \/
-     *                                                  Length ( cmd[3] )
-     */
-    
-    /* header 1 = 0xFF (dynamixel protocol 1.0) */
-    cmd[0] = 0xff;
-    
-    /* header 2 = 0xFF (dynamixel protocol 1.0) */
-    cmd[1] = 0xff; 
-    
-     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
-    cmd[2] = mot_id;
-    checksum += cmd[2];
-
-    /* Length indicates the byte size of the instruction, 
-       parameter and Checksum field */
-       
-    
-    /* Guess instruction type. NULL for read, not NULL for write */
-    if(data == NULL) // read instruction
-    {
-        /* byte length of the instruction: parameter and checksum field.   */
-        /* for read instruction:   1 INSTR +                               */
-        /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
-        cmd[3] = 4;
-        checksum += cmd[3];
-
-        /* set write instruction */
-        cmd[4] = PROTOCOL_INSTRUCTION_READ;
-        checksum += cmd[4];
-        
-        /* Param 1: address to read in the Control Table of RAM Area */
-        cmd[5] = address;
-        checksum += cmd[5];
-        
-        /* Param 2: number of bytes to read in the Control Table of RAM Area */
-        cmd[6] = len;
-        checksum += cmd[6];
-        
-        /* Checksum */
-        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
-        cmd[7] = ~checksum;
-        cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
-
-    }
-    else // write instruction
-    {
-        /* byte length of the instruction: parameter and checksum field     */
-        /* For write instruction:   1 INSTR +                               */
-        /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
-        cmd[3] = 3 + len; 
-        checksum += cmd[3];
-
-        /* set read instruction */
-        cmd[4] = PROTOCOL_INSTRUCTION_WRITE;
-        checksum += cmd[4];
-
-        /* Param 1: address to write in the "Control Table of RAM Area" */
-        cmd[5] = address;
-        checksum += cmd[5];
-    }
-    
-    //cmd[4] = 0x02 + (data != NULL); // original code from Titouan
-    
-    
-    // Compute checksum
-    if(data == NULL) {
-        cmd[6] = len;
-        cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
-        
-        // [XXX] Force length to one to force send
-        len = 1;
-    } else {
-        char cs = mot_id + len + address + 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
+*/