File content as of revision 8:e74ef93ae660:

/**  
 * @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"

#define MX12_DATA_MAX_SIZE 256
#define MX12_MAX_MOTOR_COUNT 16

/* 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/
 *
 * @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 (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 
    };
    
    /** 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;
    };

    /** 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);
    int get_counter();

                
    private:
    
    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_ */