Diff: MX12.h

Revision:

8:e74ef93ae660

Parent:

7:68983a9b0531

Child:

9:b4a5187fdec6

--- a/MX12.h	Thu Nov 04 15:11:58 2021 +0000
+++ b/MX12.h	Sat Nov 06 08:29:28 2021 +0000
@@ -1,13 +1,12 @@
 /**  
-* @file MX12.h  
-* @brief this header file will contain all required  
-*        definitions and basic utilities functions. 
-* @details comming soon
-*  
-* 
-*/ 
-#ifndef MBED_MX12_H
-#define MBED_MX12_H
+ * @file MX12.h  
+ * @brief this header file will contain all required definitions and 
+ *        basic utilities functions to manage au bus of servomotor 
+ *        Dynaminel MX12
+ * 
+ */ 
+#ifndef MBED_MX12_H_
+#define MBED_MX12_H_
 
 #include "mbed.h"
 
@@ -64,15 +63,17 @@
 #define CONTROL_TABLE_GOAL_ACCELERATION      73
 
 /** 
- * @brief Class to communicate with Dynamixel MX12 servomotors. 
+ * @brief Class to communicate with Dynamixel MX12 servomotors
  *
  * @details
+ *
  *  The servomotors are daisy chained to a serial link of the target 
  *  microcontroller. The class ensures the initialization of serial link 
- *  and the management of communications. 
+ *  and the management of communications mainly to control rotational
+ *  speed of servomotors. 
+ *
  *  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)
@@ -82,184 +83,222 @@
  * @see Dynamixel protocol v1.0 manual
  * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
  *
- * @warning 
- *    Error field of status packet if decoded by GetStatus(() function which
- *    returns an enumation type "Status". As several errors can be reported 
- *    simultaneously the type enum is not suitable (B. Denis 11/2021)
+ * @warning
+ *
+ *   Error field of status packet if decoded by GetStatus(() function which
+ *   returns an enumation type "Status". As several errors can be reported 
+ *   simultaneously the type enum is not suitable (B. Denis 11/2021)
+ *
+ *   _frame_pointer variable is used by _ReadCallback() ISR to store the
+ *   current size for message (status packet) received from servomotor.
+ *   This variable is NOT initialised and NEVER reset to zero at each new
+ *   message.
+ *    
  */
 class MX12 
 {
     public:
     
-        /** Error status occurred during the operation of servomotor.
-         *
-         * @warning 
-         *    Enumaration type is not suitable for all error status because
-         *    it can denote only one error at a time (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,           /**<  Combination of several errors (Caution: 
-                                     limit of enum approach to code errors) */
-            Ok                 ///<  no error
-        };
-        
-        /** 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 
-        };
+    /** Error status occurred during the operation of servomotor.
+     *
+     * @warning 
+     *    Enumaration type is not suitable for all error status because
+     *    it can denote only one error at a time (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,           /**<  Combination of several errors (Caution: 
+                                 limit of enum approach to code errors) */
+        Ok                 ///<  no error
+    };
+    
+    /** State of packet parser to determine the meaning of reading byte
+     *  into the received packet (status 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 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;
-        };
+    /** 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 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 
+    };
+    
+    /** Structure of store status packet (also known as return packet)
+     *  according tne Dynamixel protocol v1.0, which are messages sent
+     *  by servomotors to contriller in response of an instruction packet
+     */
+    struct Frame {
+        unsigned char motorId; /**< Identifier of the servomotor involved 
+                                    in this exchange */
+        unsigned char length;  /**< */
+        unsigned char data[MX12_DATA_MAX_SIZE]; /**< */
+        unsigned char valid; /**< */
+    };
+
+    /** 
+     */
+    struct StateContext {
+        unsigned char headingCount;
+        unsigned char dataCount;
+        unsigned char checksum;
+    };
 
-        /** 
-         */
-        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 
-         *           to servomotors 
-         * @param rx board pin used for reception in UART daisy chain link 
-         *           to servomotors 
-         * @param baud modulation rate of UART signal, unit: Baud
-         *           to servomotors 
-         */
-        MX12(PinName tx, PinName rx, int baud=115200);
-        
-        /** Send desired speed to a specifc servomotor
-         *
-         * @param mot_id a unique value in the daisy chain network to identify
-         *               each servomotor
-         * @param speed a float between -1 and 1 that represents the percentage
-         *              of maximum requested speed
-         */
-        void SetSpeed(unsigned char mot_id, float speed);
-        
-        char IsAvailable(void);
-        
-        /**
-         * Build and send an instruction packet to a servomotor according
-         * DYNAMIXEL Protocol 1.0 (online protocol documentation 
-         * https://emanual.robotis.com/docs/en/dxl/protocol1/)
-         *
-         * @param[in] mot_id indicates the ID of the device (servomotor) that
-         *            should receive the Instruction Packet and process it
-         * @param[in] address data address in the "Control Table of RAM Area"
-         *            of a servomotor MX12 
-         *            (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
-         * @param[in] len if it is a write instruction, size in bytes 
-         *            of the data to write in the "Control Table of RAM Area"
-         * @param[in] data array of char containing parameter of Danamixel  
-         *            protocol that are the instruction’s auxiliary data field
-         *  
-         * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
-         *
-         * 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]...      |        |
-         *                  \\__  ___/                    \\_  _/ \\__  __/
-         *                     \/                          \/      \/
-         *                   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);
+    /** Create MX12 instance
+     *
+     * @param tx board pin used for transmission in UART daisy chain link 
+     *           to servomotors 
+     * @param rx board pin used for reception in UART daisy chain link 
+     *           to servomotors 
+     * @param baud modulation rate of UART signal, unit: Baud
+     *           to servomotors 
+     */
+    MX12(PinName tx, PinName rx, int baud=115200);
+    
+    /** Send desired speed to a specific servomotor
+     *
+     * @param mot_id a unique value in the daisy chain network to identify
+     *               each servomotor
+     * @param speed a float between -1 and 1 that represents the percentage
+     *              of maximum requested speed
+     */
+    void SetSpeed(unsigned char mot_id, float speed);
+    
+    char IsAvailable(void);
+    
+    /**
+     * Build and send an instruction packet to a servomotor according
+     * DYNAMIXEL Protocol 1.0 (online protocol documentation 
+     * https://emanual.robotis.com/docs/en/dxl/protocol1/)
+     *
+     * @param[in] mot_id indicates the ID of the device (servomotor) that
+     *            should receive the Instruction Packet and process it
+     * @param[in] address data address in the "Control Table of RAM Area"
+     *            of a servomotor MX12 
+     *            (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
+     * @param[in] len if it is a write instruction, size in bytes 
+     *            of the data to write in the "Control Table of RAM Area"
+     * @param[in] data array of char containing parameter of Danamixel  
+     *            protocol that are the instruction’s auxiliary data field
+     *  
+     * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
+     *
+     * 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]...      |        |
+     *                  \\__  ___/                    \\_  _/ \\__  __/
+     *                     \/                          \/      \/
+     *                   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);
-        
+    /* 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);
+    int get_counter();
+
+                
     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();
+    UnbufferedSerial _mx12;
+    MX12::ParsingState _pstate;
+    MX12::SerialState _sstate;
+    
+    /** Structure used only by ISR (Interrupt Service Routine) ReadCallback() 
+     *
+     */
+    MX12::StateContext _scontext;
+    MX12::Frame _current_frame;
+            
+    unsigned char _answer;
+    
+    /** Character array to store status message received from servomotors.
+     *  _ReadCallback() ISR fill this array character by character and 
+     *  update the size of the message currently received in the variable
+     *  _frame_pointer.
+     */
+    unsigned char _stored_frame[MX12_DATA_MAX_SIZE];
+    
+    /** Size of the message currently received from servomotor. This variable
+     *  is update at each call of _ReadCallback() ISR. The content of the
+     *  message is store into _stored_frame array.
+     *
+     *  @warning
+     */
+    unsigned char _frame_pointer;
+    
+    volatile int _BDcount;
+            
+    /** Interupt service routine (ISR) to read and parse the response message
+     *  comming from servomotor on UART (serial port). This routine is called 
+     *  when each character is received on the servomotor bus. 
+     *
+     *  As the characters are received, the progress of the parser 
+     *  is stored in the two state variables _pstate and _scontext,
+     *  and the result of the analysis is stored in _current_frame. 
+     */
+    void _ReadCallback();
+    
+    void _Rx_BD_interrupt();
+
 };
 
-#endif /* MBED_MX12_H */
\ No newline at end of file
+#endif /* MBED_MX12_H_ */
\ No newline at end of file