Diff: MX12.h

Revision:

5:0cf54586a4be

Parent:

4:277e5a4cba2e

Child:

6:177aead4eb6b

--- a/MX12.h	Thu Nov 04 07:32:20 2021 +0000
+++ b/MX12.h	Thu Nov 04 11:48:53 2021 +0000
@@ -1,24 +1,26 @@
-#ifndef MBED_MX12_H
-#define MBED_MX12_H
-
 /**  
 * @file MX12.h  
 * @brief this header file will contain all required  
 *        definitions and basic utilities functions. 
 * @details comming soon
 *  
-* @author Titouan Soulard 
-* @author Bruno Denis (for comments)
 * 
 */ 
+#ifndef MBED_MX12_H
+#define MBED_MX12_H
 
 #include "mbed.h"
 
-#define MX12_ANSWER_MAX_SIZE 32
-#define MX12_MOTOR_COUNT 16
+#define MX12_DATA_MAX_SIZE 256
+#define MX12_MAX_MOTOR_COUNT 16
+
 
 /* Dynamixel protocol v1.0 : Instructions
  ******************************************/
+/** \def PROTOCOL_INSTRUCTION_PING
+    Instruction that checks whether the Packet has arrived to a device 
+    with the same ID as Packet ID
+*/
 #define PROTOCOL_INSTRUCTION_PING          0x01
 #define PROTOCOL_INSTRUCTION_READ          0x02
 #define PROTOCOL_INSTRUCTION_WRITE         0x03
@@ -29,7 +31,42 @@
 #define PROTOCOL_INSTRUCTION_SYNC_WRITE    0x83
 #define PROTOCOL_INSTRUCTION_BULK_READ     0x92
 
-
+/* Dynamixel protocol v1.0 : Contro table content
+ ************************************************/
+#define CONTROL_TABLE_MODEL_NUMBER            0
+#define CONTROL_TABLE_FIRMWARE_VERSION        2 
+#define CONTROL_TABLE_ID                      3
+#define CONTROL_TABLE_BAUD_RATE               4
+#define CONTROL_TABLE_RETURN_DELAY_TIME       5
+#define CONTROL_TABLE_CW_ANGLE_LIMIT          6
+#define CONTROL_TABLE_CCW_ANGLE_LIMIT         8
+#define CONTROL_TABLE_TEMPERATURE_LIMIT      11
+#define CONTROL_TABLE_MIN_VOLTAGE_LIMIT      12
+#define CONTROL_TABLE_MAX_VOLTAGE_LIMIT      13
+#define CONTROL_TABLE_MAX_TORQUE             14
+#define CONTROL_TABLE_STATUS_RETURN_LEVEL    16
+#define CONTROL_TABLE_ALARM_LED              17
+#define CONTROL_TABLE_SHUTDOWN               18
+#define CONTROL_TABLE_MULTITURN_OFFSET       20
+#define CONTROL_TABLE_RESOLUTION_DIVIDER     22
+#define CONTROL_TABLE_TORQUE_ENABLE          24
+#define CONTROL_TABLE_LED                    25
+#define CONTROL_TABLE_P_GAIN                 26
+#define CONTROL_TABLE_I_GAIN                 27
+#define CONTROL_TABLE_D_GAIN                 28
+#define CONTROL_TABLE_GOAL_POSITION          30
+#define CONTROL_TABLE_MOVING_SPEED           32
+#define CONTROL_TABLE_TORQUE_LIMIT           34
+#define CONTROL_TABLE_PRESENT_POSITION       36
+#define CONTROL_TABLE_PRESENT_SPEED          38
+#define CONTROL_TABLE_PRESENT_LOAD           40
+#define CONTROL_TABLE_PRESENT_VOLTAGE        42
+#define CONTROL_TABLE_PRESENT_TEMPERATURE    43
+#define CONTROL_TABLE_REGISTRED_INSTRUCTION  44
+#define CONTROL_TABLE_MOVING                 46
+#define CONTROL_TABLE_LOCK                   47
+#define CONTROL_TABLE_PUNCH                  48
+#define CONTROL_TABLE_GOAL_ACCELERATION      73
 
 /** 
  * @brief Class to communicate with Dynamixel MX12 servomotors. 
@@ -40,7 +77,19 @@
  *  and the management of communications. 
  *  Transmission of messages to the servomotors is blocking while 
  *  reception is non-blocking thanks to the use of an interrupt routine. 
+ *
+ * 
+ * @author Titouan Soulard (creator and maintainer until April 2021)
+ * @author Bruno Denis (Doxygen documentation and code rearrangement)
  *  
+ * @see Control table of Dynamixel MX12 servomotor 
+ * @see https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/
+ * @see Dynamixel protocol v1.0 manual
+ * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
+ *
+ * @warning 
+ *    enum type is not suitable for the errors status because
+ *    several errors can be repported simultaneously (B. Denis 11/2021)
  */
 class MX12 
 {
@@ -48,30 +97,75 @@
     
         /** Error status occurred during the operation of servomotor.
          *
-         * BDenis remarj: enum type is not suitable for the errors status 
-         *                because several errors can be repported simultaneously 
+         * @warning 
+         *    enum type is not suitable for the errors status because
+         *    several errors can be repported simultaneously (B. Denis 11/2021)
          */
         enum Status {
-            InstructionError,  ///<  In case of sending an undefined instruction or delivering the action instruction without the Reg Write instruction
-            OverloadError,     ///<  When the current load cannot be controlled by the set Torque
-            ChecksumError,     ///<  When the Checksum of the transmitted Instruction Packet is incorrect
-            RangeError,        ///<  When an instruction is out of the range for use 
-            OverheatingError,  ///<  When internal temperature of servomotor is out of the range of operating temperature set in the Control table
-            AngleLimitError,   ///<  When Goal Position is written out of the range from CW Angle Limit to CCW Angle Limit
-            InputVoltageError, ///<  When the applied voltage is out of the range of operating voltage set in the Control table
-            Unknown,           ///<  ???
+            InstructionError,  /**<  In case of sending an undefined instruction 
+                                     or delivering the action instruction 
+                                     without the Reg Write instruction */
+            OverloadError,     /**<  When the current load cannot be controlled 
+                                     by the set Torque */
+            ChecksumError,     /**<  When the Checksum of the transmitted 
+                                     Instruction Packet is incorrect */
+            RangeError,        /**<  When an instruction is out of the range 
+                                     for use */
+            OverheatingError,  /**<  When internal temperature of servomotor 
+                                     is out of the range of operating 
+                                     temperature set in the Control table */
+            AngleLimitError,   /**<  When Goal Position is written out of the 
+                                     range from CW Angle Limit to CCW Angle 
+                                     Limit */
+            InputVoltageError, /**<  When the applied voltage is out of the 
+                                     range of operating voltage set in the 
+                                     Control table */
+            Unknown,           /**<  Combination of several errors (Caution: 
+                                     limit of enum approach to code errors) */
             Ok                 ///<  no error
         };
         
-        /** State enum, possible state of communication automaton
-         *
+        /** State of packet parser to determine the meaning of reading byte
+         *  into the received packet (satuts packet) according the Dynamixel
+         *  protocol v1.0
+         */
+        enum ParsingState {
+            Header,    ///< reading the two heading bytes
+            Id,        ///< reading the servomotor ID byte
+            Length,    ///< reading byte length of the instruction
+            Data,      ///< reading parameters
+            Checksum,  ///< reading one bytes checksum 
+        };
+
+        /** Enumeration of states of the acces methode of the master-slave 
+         *  protocol of the communication between the robot controller (master) 
+         *  and the servomotors (slaves).
          */
-         enum State {
-            ReadingPosition,   ///<  ReadingPosition
-            Writing,           ///<  Writing
-            Available,         ///<  Available
+         enum SerialState {
+            Writing, /**< Robot controller send an "instruction packet"
+                          (request) to a servomotor */
+            Reading, /**< Robot controller receive a "status packet" from a 
+                          requested servomotor */
+            Idle,    ///< Robot controller ready for a new cycle request-answer 
         };
-    
+        
+        /**  type
+         */
+        struct Frame {
+            unsigned char motorId;
+            unsigned char length;
+            unsigned char data[MX12_DATA_MAX_SIZE];
+            unsigned char valid;
+        };
+
+        /** 
+         */
+        struct StateContext {
+            unsigned char headingCount;
+            unsigned char dataCount;
+            unsigned char checksum;
+        };
+  
         /** Create MX12 instance
          *
          * @param tx board pin used for transmission in UART daisy chain link 
@@ -94,15 +188,6 @@
         
         char IsAvailable(void);
         
-        MX12::Status GetStatus(void);
-        
-        
-        void ReadPosition(unsigned char mot_id);
-        
-        float GetPosition(unsigned char mot_id);
-        
-        void PrintAnswer();
-        
         /**
          * Build and send an instruction packet to a servomotor according
          * DYNAMIXEL Protocol 1.0 (online protocol documentation 
@@ -121,36 +206,63 @@
          *
          * Packet sent
          * <PRE>
-         * |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
+         * |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)
          * </PRE>
          */
         void rw(unsigned char mot_id, char address, char len, char *data);
         
+        /**
+         * 
+         * @warning
+         *    
+         */
+        void PrintSerial();
+        
+        /**
+         * Get information from de Error byte of the last status packet
+         * received from a servomotor
+         *
+         * @return 
+         *    One of enum MX12::Status value to describe errors. 'Ok' is
+         *    returns if no error occurred during the last operation of
+         *    servomotor.
+         * 
+         * @warning 
+         *    If a combination of several errors is reported the function
+         *    returns 'Unknown' (B. Denis 11/2021)
+         */
+        MX12::Status GetStatus(void);
+
+        /* function aivailaible in a previous version of the class 
+         */
+        // MX12::Status GetStatus(void);
+        // void ReadPosition(unsigned char mot_id);
+        // float GetPosition(unsigned char mot_id);
+        
+    private:
+    
+        UnbufferedSerial _mx12;
+        MX12::ParsingState _pstate;
+        MX12::SerialState _sstate;
+        MX12::StateContext _scontext;
+        MX12::Frame _current_frame;
+                
+        unsigned char _answer;
+        unsigned char _stored_frame[MX12_DATA_MAX_SIZE];
+        unsigned char _frame_pointer;
+        
         /** Interupt Routine to read in data from UART daisy chain link
          *  (serial port)
          *
          */
         void _ReadCallback();
-        
-        char _chksm;
-    
-    private:
-    
-        UnbufferedSerial _mx12;
-        MX12::Status _status;
-        char _res[MX12_ANSWER_MAX_SIZE];
-        char _res_count;
-        char _len;
-        MX12::State _state;
-        float _angle[MX12_MOTOR_COUNT];
-        int _baud;
 };
 
 #endif /* MBED_MX12_H */
\ No newline at end of file