Diff: MX12.h

Revision:

9:67d737d8a349

diff -r 04ab02bb2262 -r 67d737d8a349 MX12.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MX12.h	Fri Feb 25 10:13:06 2022 +0000
@@ -0,0 +1,403 @@
+/**  
+ * @file MX12.h
+ * @brief this header file contains 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"
+
+#define MX12_PACKET_MAX_SIZE 32
+#define MX12_MAX_MOTOR_COUNT 16
+
+#define MX12_SPEED_QUANTA_RPM 0.916 /* Data from MX12 e-manual */
+#define MX12_ABSOLUTE_MAX_SPEED_RAD_S 98.13
+
+
+
+/* Dynamixel protocol v1.0 : Instructions
+ ******************************************/
+#define PROTOCOL_INSTRUCTION_PING          0x01
+#define PROTOCOL_INSTRUCTION_READ          0x02
+#define PROTOCOL_INSTRUCTION_WRITE         0x03
+#define PROTOCOL_INSTRUCTION_REG_WRITE     0x04
+#define PROTOCOL_INSTRUCTION_ACTION        0x05
+#define PROTOCOL_INSTRUCTION_FACTORY_RESET 0x06
+#define PROTOCOL_INSTRUCTION_REBOOT        0x08
+#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
+ *
+ * @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 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)
+ *  
+ * @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/
+ *
+ * Example of class usage to send two instruction packets to servomotor #1
+ * with one second gap. First ask to move at 10% of maximum speed and the 
+ * stop the movement.
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "MX12.h"
+ * 
+ * #define SERVO_TX_PIN PC_4
+ * #define SERVO_RX_PIN PC_5
+ * #define SERVO_BAUD 115200
+ * #define SERVO_ID 1
+ * 
+ * #define DELAY_1000ms 1000
+ * #define DELAY_1ms       1
+ * 
+ * MX12 servo_bus(SERVO_TX_PIN, SERVO_RX_PIN, SERVO_BAUD);
+ * float relative_speed;
+ * 
+ * int main()
+ * {
+ *     // Set speed of SERVO_ID servomotor to 10% 
+ *     // Send on servo_bus to SERVO_ID motor the instruction "moving speed"
+ *     // with parameter 0.1 (10% of maximum speed)
+ *     relative_speed = 0.1; 
+ *     servo_bus.SetSpeed(SERVO_ID, relative_speed);
+ *         
+ *     // wait for one second
+ *     thread_sleep_for(DELAY_1000ms);
+ *     
+ *     // set speed of SERVO_ID servomotor to 0% 
+ *     relative_speed = 0.0; 
+ *     servo_bus.SetSpeed(SERVO_ID, relative_speed);
+ * 
+ *     // infinite loop 
+ *     while (true) thread_sleep_for(DELAY_1000ms);
+ * }
+ * @endcode
+ *
+ * @warning
+ *   Warning: receipt of packets from servomotors has not been tested 
+ *   (hardwre problem). However, sending move speed instruction is operational.
+ *
+ * @warning
+ *   Warning: Error field of status packet if decoded by GetStatus() returns
+ *   an enumation type "Status". As several errors can be reported
+ *   simultaneously the type enum is not suitable (B. Denis 11/2021).
+ *
+ * @bug
+ *   Bug: GetStatus() method has a hazardous behavior because it provides 
+ *   a result by reading the content of the variable _current_frame 
+ *   which can be modified at any time by the private ISR _Rx_interrupt().
+ *    
+ */
+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
+    };
+    
+    /** Enumeration of states of the medium acces control (MAC) 
+     *  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 Status_packet {
+        unsigned char raw[MX12_PACKET_MAX_SIZE]; /**< bytes received */
+        unsigned char n_byte;   /**< Number of received byte */
+        unsigned char servo_id; /**< Identifier field denoted servomotor  
+                                 *   involved in the message */
+        unsigned char length;   /**< Length field of status packet */
+        unsigned char error;    /**< Error field of status packet */
+        unsigned char n_param;  /**< Number of received parameter*/
+        unsigned char param[MX12_PACKET_MAX_SIZE]; /**< array of received
+                                                    *   parameters */
+        unsigned char received_checksum; /**< Received checksum field */
+        unsigned char calculated_checksum; /**< Calculated checksum  */
+        bool parsed;  /**< status packet is correctly parsed */
+        bool valid;   /**< received_checksum == calculated_checksum */
+    };
+
+    /** 
+     * @brief  Discrete states of the packet parser.
+     *
+     * @detail 
+     *    Each value corresponding to a field that the parser 
+     *    can identify according the Dynamixel protocol v1.0.
+     *    Enumeration include field of instruction packets
+     *    (packets sent to servomotors) and field of status packets
+     *    (packets received from servomotors).
+     */
+    enum PacketField {
+        Header1,     ///< Heading1 field of instruction or status packet
+        Header2,     ///< Heading2 field of instruction or status packet
+        Id,          ///< ID field of instruction or status packet
+        Length,      ///< Length field of status packet
+        Error,       ///< Error status field of status packet
+        Instruction, ///< Instruction field of instruction packet
+        Data,        ///< expect to read parameter fields of status packet
+        Checksum     ///< expect to read Checksum field of status packet
+    };
+
+    /** Structure to store the current state of packet parserComplement to the ParsingState enumeration to store the reading 
+     *  progress of each section of the status packet.
+     */
+    struct ParserState {
+        MX12::PacketField expected_field; ///< next expected field
+        unsigned char byte_index;  ///< index of byte already parsed
+        unsigned char param_index; ///< index of parameter already parsed
+    };
+
+    /** Create an instance of MX12 and attach it to a serial link
+     *
+     * @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 normalized 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);
+    
+    /** Send desired ablosute speed to a specific servomotor
+     *
+     * @param mot_id a unique value in the daisy chain network to identify
+     *               each servomotor
+     * @param speed desired speed in radians per second  
+     */
+    void SetSpeed_rad_s(unsigned char mot_id, float speed);
+    
+    /** Informs about the availability of the bus for a new transmission 
+     *  of instruction packet (Method Access Control) 
+     *
+     * @returns true if bus ready for a new transmission, otherwise false
+     */
+    char IsAvailable(void);
+    
+    /**
+     * @brief     Build and send an instruction packet to a servomotor 
+     *            according Dynamixel Protocol 1.0
+     *
+     * @detail    This method is limited to only two kind of instruction :
+     *            read data from a servomoteur et write data on servomotor.
+     *            Ping, Reg Write, Action, Factory Reset, Rebbot, Sync Write
+     *            and Bulk Read are not supported.
+     *
+     *            For the method to issue a read instruction, set the data 
+     *            parameter to NULL, otherwise the method issue a 
+     *            write instruction 
+     *
+     * @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.
+     *            Set to NULL for read instruction, otherwise the method issue 
+     *            a write instruction 
+     *  
+     * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
+     *
+     * Structure of instruction Packet
+     *
+     * <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);
+    
+    /** 
+     * Display status packet in hexadecimal format
+     * 
+     * @warning
+     *    Use console without any previous declaration. Assume console is 
+     *    setup by calling program
+     */
+    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 
+     *    Warning: if a combination of several errors is reported the function
+     *    returns "Unknown".
+     *
+     * @bug
+     *    Bug: this method has a hazardous behavior because it provides 
+     *    a result by reading the content of the variable _current_frame 
+     *    which can be modified at any time by the private ISR _Rx_interrupt()  
+     */
+    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);
+    
+    void cmd_moteur(float Vavance, float Vlat, float Wz);
+    // envoie la commande moteur de manière à avancer d'une vitesse Vavance en 
+    // mètres par seconde, d'une vitesse Vlat en mètre par seconde et d'une 
+    // vitesse de rotation Wz en rad/s
+    
+    void eteindre_moteurs();
+    // permet d'enlever le couple dans les moteurs
+    
+    void cmd_moteur_multiturn(float pos1, float pos2, float pos3);
+    
+                
+    protected:
+    
+    /** Serial port where servomotor bus is connected
+     */
+    UnbufferedSerial _mx12;
+    
+    
+    /** Servomotor bus state to managed medium acces control (MAC)
+     */
+    MX12::SerialState _bus_state;
+    
+    /** Structure filled by ISR (Interrupt Service Routine) ReadCallback()
+     *  parser to store message received from servomotor byte after byte 
+     */
+    MX12::Status_packet _status_pck;
+    
+    /** Structure update by ISR (Interrupt Service Routine) ReadCallback()
+     *  parser to model its current state
+     */
+    MX12::ParserState _parser_state;
+
+            
+    unsigned char _answer;
+                
+    /** 
+     * @brief  
+     *    Interupt service routine (ISR) to read and parse received message
+     *
+     * @detail
+     *    This interupt routine is attached to reception event on servomoteur 
+     *    bus and is called for each character received on bus. 
+     *
+     *    As the characters are received, the progress of the parser 
+     *    is stored in _parser_state and the result of the analysis 
+     *    is stored in _status_pck.
+     */
+    void _Rx_interrupt();
+    
+};
+
+#endif /* MBED_MX12_H_ */
\ No newline at end of file