File content as of revision 11:9bc7f5e2ccee:

/**  
 * @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/
 
@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
 *
 *   Error field of status packet if decoded by GetStatus() 
 *   Doxygen Release 1.7.2 returns an enumation type "Status". As several 
 *   errors can be reported simultaneously the type enum is not suitable 
 *   (B. Denis 11/2021).
 *
 * @bug
 *
 *   Bug: _frame_pointer variable is used by private _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.
 *
 *   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 _ReadCallback().
 *    
 */
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 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;   /**< */
    };

    /** State of packet parser store which section of a status packet 
     *  (according the Dynamixel according the Dynamixel) is currently reading
     *  from a servomotor return message.
     */
    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 
    };

    /** Complement to the ParsingState enumeration to store the reading 
     *  progress of each section of the status packet.
     */
    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);
    
    /** 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);
    
    /**
     * 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>
     *
     * Code simple
     *
     @code
     * |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)
     @endcode
     *
     * Code simple {.unparsed}
     *
     @code{.unparsed}
     * |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)
     @endcode
     */
    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 
     *
     *    Warning: if a combination of several errors is reported the function
     *    returns 'Unknown' (B. Denis 11/2021).
     *
     * @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 _ReadCallback()  
     */
    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);
                
    protected:
    
    /*
     */
    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;
                
    /** 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();
    
};

#endif /* MBED_MX12_H_ */