File content as of revision 5:4c118a827f11:

/* mbed MX-12 Servo Library
 *
 */

#ifndef MBED_MX12_H
#define MBED_MX12_H

#include "mbed.h"

#define MX12_WRITE_DEBUG 0
#define MX12_READ_DEBUG 0
#define MX12_TRIGGER_DEBUG 0
#define MX12_DEBUG 0

#define MX12_OD_SIZE 34

enum MX12ODIndex {
    MX12_REG_MODEL_NUMBER,
    MX12_REG_VERSION_OF_FIRMWARE,
    MX12_REG_ID,
    MX12_REG_BAUD_RATE,
    MX12_REG_RETURN_DELAY_TIME,
    MX12_REG_CW_ANGLE_LIMIT,
    MX12_REG_CCW_ANGLE_LIMIT,
    MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE,
    MX12_REG_THE_LOWEST_LIMIT_VOLTAGE,
    MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE,
    MX12_REG_MAX_TORQUE,
    MX12_REG_STATUS_RETURN_LEVEL,
    MX12_REG_ALARM_LED,
    MX12_REG_ALARM_SHUTDOWN,
    MX12_REG_MULTI_TURN_OFFSET,
    MX12_REG_RESOLUTION_DIVIDER,
    MX12_REG_TORQUE_ENABLE,
    MX12_REG_LED,
    MX12_REG_D_GAIN,
    MX12_REG_I_GAIN,
    MX12_REG_P_GAIN,
    MX12_REG_GOAL_POSITION,
    MX12_REG_MOVING_SPEED,
    MX12_REG_TORQUE_LIMIT,
    MX12_REG_PRESENT_POSITION,
    MX12_REG_PRESENT_SPEED,
    MX12_REG_PRESENT_LOAD,
    MX12_REG_PRESENT_VOLTAGE,
    MX12_REG_PRESENT_TEMPERATURE,
    MX12_REG_REGISTERED,
    MX12_REG_MOVING,
    MX12_REG_LOCK,
    MX12_REG_PUNCH,
    MX12_REG_GOAL_ACCELERATION
};

struct MX12OD_Object {
    unsigned char Address;
    //unsigned char InitialValue;
    //unsigned char Readable;
    //unsigned char Writeable;
    unsigned char Bytes;
};

//We define these as globals, since they are constant 
//and we only need one.
extern MX12OD_Object MX12_OD[MX12_OD_SIZE];
//Avoid initializing them more than once.
extern bool MX12OD_Object_initalized;

#define MX12_MODE_POSITION  0
#define MX12_MODE_ROTATION  1

// For now we don't have a both option, since no
// one should use that anyways.
enum MX12_Direction {
    MX12_DIR_IN,
    MX12_DIR_OUT,
    MX12_DIR_NONE
};

#define MX12_CW 1
#define MX12_CCW 0

#define MX12_INSTRUCTION_HEADER 0xff

#define MX12_ERROR_RETURN       255
#define MX12_NORMAL_RETURN        1

// The max delay should be 508us according to:
// http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05
// And a max character should be 833 us according to:
// (1 byte) / (9600 (bits per second)) = 833 microseconds
// So 1000 should be a decent value, because 9600 bps is for chumps.
#define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1500

#ifndef M_PI
    #define M_PI    3.14159265358979323846  /* pi */
#endif
#ifndef M_PI_2
    #define M_PI_2  1.57079632679489661923  /* pi/2 */
#endif

#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))

/** Dynamixel servo control class
 *
 * Example:
 * @code
 * #include "mbed.h"
 * #include "MX12.h"
 * 
 * int main() {
 * 
 *   MX12 mymx12 (p9, p10, 1);           // ID=1
 *
 *   while (1) {
 *       mymx12.Set_Goal_Position(0);    // go to 0 degrees
 *       wait (2.0);
 *       mymx12.Set_Goal_Position(300);  // go to 300 degrees
 *       wait (2.0);
 *   }
 * }
 * @endcode
 */
class MX12 {

public:

    /** Create an MX12 servo object connected to the specified serial port, with the specified ID
     *
     * @param pin tx pin
     * @param pin rx pin 
     * @param int ID, the Bus ID of the servo 1-255 
     */
    MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate=1000000);

    void Init(void);
    
    void ChangeDir(MX12_Direction dir);

/** clockwise Angle Limit @retval CW Angle Limit in Degrees */  float Get_CW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CW_ANGLE_LIMIT);}
/** counterclockwise Angle Limit @retval CCW Angle Limit in Degrees */  float Get_CCW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CCW_ANGLE_LIMIT);}
/** Max. Torque @retval Max Torque in Percent */    float Get_Max_Torque(void){return 0.097656*read_short(MX12_REG_MAX_TORQUE);}
/** et least significant byte (LSB) @retval Multi Turn Offset in Degrees */ float Get_Multi_Turn_Offset(void){return 0.087891*read_short(MX12_REG_MULTI_TURN_OFFSET);}
/** Goal Position @retval Goal Position in Degrees */   float Get_Goal_Position(void){return 0.087891*read_short(MX12_REG_GOAL_POSITION);}
/** Moving Speed @retval Moving Speed in Degrees/Second */  float Get_Moving_Speed(void){return 0.686628*read_short(MX12_REG_MOVING_SPEED);}
/** Torque Limit @retval Torque Limit in Percent */ float Get_Torque_Limit(void){return 0.097656*read_short(MX12_REG_TORQUE_LIMIT);}
/** Punch @retval Punch in Percent */   float Get_Punch(void){return 0.097656*read_short(MX12_REG_PUNCH);}
/** ID of Dynamixel @retval ID in int */    float Get_ID(void){return 1.000000*read_short(MX12_REG_ID);}
/** Baud Rate of Dynamixel @retval Baud Rate in Lookup */   float Get_Baud_Rate(void){return 1.000000*read_short(MX12_REG_BAUD_RATE);}
/** Return Delay Time @retval Return Delay Time in milliseconds */  float Get_Return_Delay_Time(void){return 0.002000*read_short(MX12_REG_RETURN_DELAY_TIME);}
/** Internal Limit Temperature @retval the Highest Limit Temperature in Celsius */  float Get_the_Highest_Limit_Temperature(void){return 1.000000*read_short(MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE);}
/** Lowest Limit Voltage @retval the Lowest Limit Voltage in Volts */   float Get_the_Lowest_Limit_Voltage(void){return 0.100000*read_short(MX12_REG_THE_LOWEST_LIMIT_VOLTAGE);}
/** Highest Limit Voltage @retval the Highest Limit Voltage in Volts */ float Get_the_Highest_Limit_Voltage(void){return 0.100000*read_short(MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE);}
/** Status Return Level @retval Status Return Level in int */   float Get_Status_Return_Level(void){return 1.000000*read_short(MX12_REG_STATUS_RETURN_LEVEL);}
/** LED for Alarm @retval Alarm LED in Bitmap */    float Get_Alarm_LED(void){return 1.000000*read_short(MX12_REG_ALARM_LED);}
/** Shutdown for Alarm @retval Alarm Shutdown in Bitmap */  float Get_Alarm_Shutdown(void){return 1.000000*read_short(MX12_REG_ALARM_SHUTDOWN);}
/** Resolution divider @retval Resolution Divider in Ratio */   float Get_Resolution_Divider(void){return 1.000000*read_short(MX12_REG_RESOLUTION_DIVIDER);}
/** Torque On/Off @retval Torque Enable in bool */  float Get_Torque_Enable(void){return 1.000000*read_short(MX12_REG_TORQUE_ENABLE);}
/** LED On/Off @retval LED in Bitmap */ float Get_LED(void){return 1.000000*read_short(MX12_REG_LED);}
/** Derivative Gain @retval D Gain in Kd */ float Get_D_Gain(void){return 0.004000*read_short(MX12_REG_D_GAIN);}
/** Integral Gain @retval I Gain in Ki */   float Get_I_Gain(void){return 0.488281*read_short(MX12_REG_I_GAIN);}
/** Proportional Gain @retval P Gain in Kp */   float Get_P_Gain(void){return 0.125000*read_short(MX12_REG_P_GAIN);}
/** Locking EEPROM @retval Lock in bool */  float Get_Lock(void){return 1.000000*read_short(MX12_REG_LOCK);}
/** Goal Acceleration @retval Goal Acceleration in Degrees */   float Get_Goal_Acceleration(void){return 8.582677*read_short(MX12_REG_GOAL_ACCELERATION);}
/** model number @retval Model Number in Bitmap */  float Get_Model_Number(void){return 1.000000*read_short(MX12_REG_MODEL_NUMBER);}
/** Current Position @retval Present Position in Degrees */ float Get_Present_Position(void){return 0.087891*read_short(MX12_REG_PRESENT_POSITION);}
/** Current Speed @retval Present Speed in Degrees/Second */    float Get_Present_Speed(void){return 0.686628*read_short(MX12_REG_PRESENT_SPEED);}
/** Current Load @retval Present Load in Percent */ float Get_Present_Load(void){return 0.097656*read_short(MX12_REG_PRESENT_LOAD);}
/** Information on the version of firmware @retval Version of Firmware in int */    float Get_Version_of_Firmware(void){return 1.000000*read_short(MX12_REG_VERSION_OF_FIRMWARE);}
/** Current Voltage @retval Present Voltage in Volts */ float Get_Present_Voltage(void){return 0.100000*read_short(MX12_REG_PRESENT_VOLTAGE);}
/** Current Temperature @retval Present Temperature in Celsius */   float Get_Present_Temperature(void){return 1.000000*read_short(MX12_REG_PRESENT_TEMPERATURE);}
/** Means if Instruction is registered @retval Registered in bool */    float Get_Registered(void){return 1.000000*read_short(MX12_REG_REGISTERED);}
/** Means if there is any movement @retval Moving in bool */    float Get_Moving(void){return 1.000000*read_short(MX12_REG_MOVING);}

/** clockwise Angle Limit @param val CW Angle Limit in Degrees */   void Set_CW_Angle_Limit(float val){write_short(MX12_REG_CW_ANGLE_LIMIT,val/0.087891);}
/** counterclockwise Angle Limit @param val CCW Angle Limit in Degrees */   void Set_CCW_Angle_Limit(float val){write_short(MX12_REG_CCW_ANGLE_LIMIT,val/0.087891);}
/** Max. Torque @param val Max Torque in Percent */ void Set_Max_Torque(float val){write_short(MX12_REG_MAX_TORQUE,val/0.097656);}
/** et least significant byte (LSB) @param val Multi Turn Offset in Degrees */  void Set_Multi_Turn_Offset(float val){write_short(MX12_REG_MULTI_TURN_OFFSET,val/0.087891);}
/** Goal Position @param val Goal Position in Degrees */    void Set_Goal_Position(float val){write_short(MX12_REG_GOAL_POSITION,val/0.087891);}
/** Moving Speed @param val Moving Speed in Degrees/Second */   void Set_Moving_Speed(float val){write_short(MX12_REG_MOVING_SPEED,val/0.686628);}
/** Torque Limit @param val Torque Limit in Percent */  void Set_Torque_Limit(float val){write_short(MX12_REG_TORQUE_LIMIT,val/0.097656);}
/** Punch @param val Punch in Percent */    void Set_Punch(float val){write_short(MX12_REG_PUNCH,val/0.097656);}
/** ID of Dynamixel @param val ID in int */ void Set_ID(float val){write_short(MX12_REG_ID,val/1.000000);}
/** Baud Rate of Dynamixel @param val Baud Rate in Lookup */    void Set_Baud_Rate(float val){write_short(MX12_REG_BAUD_RATE,val/1.000000);}
/** Return Delay Time @param val Return Delay Time in milliseconds */   void Set_Return_Delay_Time(float val){write_short(MX12_REG_RETURN_DELAY_TIME,val/0.002000);}
/** Internal Limit Temperature @param val the Highest Limit Temperature in Celsius */   void Set_the_Highest_Limit_Temperature(float val){write_short(MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE,val/1.000000);}
/** Lowest Limit Voltage @param val the Lowest Limit Voltage in Volts */    void Set_the_Lowest_Limit_Voltage(float val){write_short(MX12_REG_THE_LOWEST_LIMIT_VOLTAGE,val/0.100000);}
/** Highest Limit Voltage @param val the Highest Limit Voltage in Volts */  void Set_the_Highest_Limit_Voltage(float val){write_short(MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE,val/0.100000);}
/** Status Return Level @param val Status Return Level in int */    void Set_Status_Return_Level(float val){write_short(MX12_REG_STATUS_RETURN_LEVEL,val/1.000000);}
/** LED for Alarm @param val Alarm LED in Bitmap */ void Set_Alarm_LED(float val){write_short(MX12_REG_ALARM_LED,val/1.000000);}
/** Shutdown for Alarm @param val Alarm Shutdown in Bitmap */   void Set_Alarm_Shutdown(float val){write_short(MX12_REG_ALARM_SHUTDOWN,val/1.000000);}
/** Resolution divider @param val Resolution Divider in Ratio */    void Set_Resolution_Divider(float val){write_short(MX12_REG_RESOLUTION_DIVIDER,val/1.000000);}
/** Torque On/Off @param val Torque Enable in bool */   void Set_Torque_Enable(float val){write_short(MX12_REG_TORQUE_ENABLE,val/1.000000);}
/** LED On/Off @param val LED in Bitmap */  void Set_LED(float val){write_short(MX12_REG_LED,val/1.000000);}
/** Derivative Gain @param val D Gain in Kd */  void Set_D_Gain(float val){write_short(MX12_REG_D_GAIN,val/0.004000);}
/** Integral Gain @param val I Gain in Ki */    void Set_I_Gain(float val){write_short(MX12_REG_I_GAIN,val/0.488281);}
/** Proportional Gain @param val P Gain in Kp */    void Set_P_Gain(float val){write_short(MX12_REG_P_GAIN,val/0.125000);}
/** Locking EEPROM @param val Lock in bool */   void Set_Lock(float val){write_short(MX12_REG_LOCK,val/1.000000);}
/** Goal Acceleration @param val Goal Acceleration in Degrees */    void Set_Goal_Acceleration(float val){write_short(MX12_REG_GOAL_ACCELERATION,val/8.582677);}


    int SetMode(int mode);
    
    /** Change the Baud Rate of a servo
     *
     * @param baud_rate The desired rate in bits per second.  
     *
     * @note Not all baud rates will be met exactly!
     *
     * The maths for this function are a bit odd, see here:
     * @url http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_04
     *
     *    TABLE OF VALID BAUD RATES:
     *   Data   Set BPS  Target  Tolerance
     *    1     1000000 1000000  0.000
     *    3      500000  500000  0.000
     *    4      400000  400000  0.000
     *    7      250000  250000  0.000
     *    9      200000  200000  0.000
     *    16   117647.1  115200 -2.124
     *    34    57142.9   57600  0.794
     *   103    19230.8   19200 -0.160
     *   207     9615.4    9600 -0.160
     *   250    2250000 2250000  0.000
     *   251    2500000 2500000  0.000
     *   252    3000000 3000000  0.000
     * 
     */
    int SetBaud( int target_baud );
    
    /** Change the Baud Rate of the UART serial port
     *
     * @param baud_rate The desired rate in bits per second (Default 1Mbps)
     *
     * @note This doesn't broadcast anything
     * 
     */
    void ChangeUARTBaud( int target_baud=1000000 );

    /** Read the current angle of the servo
     *
     * @returns short in the range 0-4095
     */
    short GetRawPosition(void);
    
    
    /** Dump everything we know about the servo to serial.
     *  Warning!  This will take a while!  At 1Mbps, it will
     *  probably take at least 250 ms, so only do this if you
     *  have lots of time to spare!
     *  
     *  @param serialObject Whichever serial object you want to print to, probably pc but maybe bluetooth
     */
    void Dump_OD_to_Serial(Serial &serialObject);

    /** Search for all responsive Dynamixels.
     *  Warning!  This will take a considerable amount of time if scanning the whole range!
     *  I haven't tried it, but off the top of my head it should take at least 3 seconds, probably more.
     *  
     *  @param scan_all_baud_rates if true, scan all 12 known Dynamixel baud rates. Default false, scans only current UART baud rate.
     *  @param max_id scan from zero to this value, defaults to 0xFC which is the max possible for a Dynamixel.
     */
    void Scan_For_Dynamixels(bool scan_all_baud_rates=false,int max_id=0xFC);
    
    /** Write a raw value to the servo
     *
     * @param OD Object Dictionary ID from the Enum
     * @param value The raw value to be written.  If the Bytes of the OD is 1, only the lower 8 bits are written.
     * @returns status int
     */
    int write_short(MX12ODIndex OD,short value);
    
    
    /** Read a raw value from the servo
     *
     * @param OD Object Dictionary ID from the Enum
     * @returns The raw value to be read.  If the Bytes of the OD is 1, only the lower 8 bits are used.
     */
    short read_short(MX12ODIndex OD);
    
    
    /** Perform a coordinated move.  Right now, it commands two servos with position and velocity,
     *  but the code should be extendable to more servos/parameters as needed.
     *
     * @param OD Object Dictionary ID from the Enum
     * @param value The raw value to be written.  If the Bytes of the OD is 1, only the lower 8 bits are written.
     * @returns status int
     */
    void coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1);

    /** Send the broadcast "trigger" command, to activate any outstanding registered commands
     */
    void trigger(void);
    
    /** Send a ping to a servo.  No matter what it should respond.  Use this to see if a servo exists.
     *
     * @param ID_Num the ID of the servo in question, 0 to 252 (0xFC) can be used, defaults to current object ID
     * @returns true if any response received, otherwise false
     */
    bool ping(char ID_Num=0xFF);
    
private :
    //// Variables
    Serial mx12_out;
    Serial mx12_in;
    DigitalOut tx_enable;
    DigitalOut rx_enable;
    DigitalOut profileOut;
    int _ID;
    int _baud;

    //// Functions
    int read_raw(char* Status, int bytes=0);
    int read(int ID, int start, int length, char* data);
    int write(int ID, int start, int length, char* data);

};

#endif