MX12.h

00001 /* mbed MX-12 Servo Library
00002  *
00003  */
00004 
00005 #ifndef MBED_MX12_H
00006 #define MBED_MX12_H
00007 
00008 #include "mbed.h"
00009 
00010 #define MX12_WRITE_DEBUG 0
00011 #define MX12_READ_DEBUG 0
00012 #define MX12_TRIGGER_DEBUG 0
00013 #define MX12_DEBUG 0
00014 
00015 #define MX12_OD_SIZE 34
00016 
00017 enum MX12ODIndex {
00018     MX12_REG_MODEL_NUMBER,
00019     MX12_REG_VERSION_OF_FIRMWARE,
00020     MX12_REG_ID,
00021     MX12_REG_BAUD_RATE,
00022     MX12_REG_RETURN_DELAY_TIME,
00023     MX12_REG_CW_ANGLE_LIMIT,
00024     MX12_REG_CCW_ANGLE_LIMIT,
00025     MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE,
00026     MX12_REG_THE_LOWEST_LIMIT_VOLTAGE,
00027     MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE,
00028     MX12_REG_MAX_TORQUE,
00029     MX12_REG_STATUS_RETURN_LEVEL,
00030     MX12_REG_ALARM_LED,
00031     MX12_REG_ALARM_SHUTDOWN,
00032     MX12_REG_MULTI_TURN_OFFSET,
00033     MX12_REG_RESOLUTION_DIVIDER,
00034     MX12_REG_TORQUE_ENABLE,
00035     MX12_REG_LED,
00036     MX12_REG_D_GAIN,
00037     MX12_REG_I_GAIN,
00038     MX12_REG_P_GAIN,
00039     MX12_REG_GOAL_POSITION,
00040     MX12_REG_MOVING_SPEED,
00041     MX12_REG_TORQUE_LIMIT,
00042     MX12_REG_PRESENT_POSITION,
00043     MX12_REG_PRESENT_SPEED,
00044     MX12_REG_PRESENT_LOAD,
00045     MX12_REG_PRESENT_VOLTAGE,
00046     MX12_REG_PRESENT_TEMPERATURE,
00047     MX12_REG_REGISTERED,
00048     MX12_REG_MOVING,
00049     MX12_REG_LOCK,
00050     MX12_REG_PUNCH,
00051     MX12_REG_GOAL_ACCELERATION
00052 };
00053 
00054 struct MX12OD_Object {
00055     unsigned char Address;
00056     //unsigned char InitialValue;
00057     //unsigned char Readable;
00058     //unsigned char Writeable;
00059     unsigned char Bytes;
00060 };
00061 
00062 //We define these as globals, since they are constant 
00063 //and we only need one.
00064 extern MX12OD_Object MX12_OD[MX12_OD_SIZE];
00065 //Avoid initializing them more than once.
00066 extern bool MX12OD_Object_initalized;
00067 
00068 #define MX12_MODE_POSITION  0
00069 #define MX12_MODE_ROTATION  1
00070 
00071 // For now we don't have a both option, since no
00072 // one should use that anyways.
00073 enum MX12_Direction {
00074     MX12_DIR_IN,
00075     MX12_DIR_OUT,
00076     MX12_DIR_NONE
00077 };
00078 
00079 #define MX12_CW 1
00080 #define MX12_CCW 0
00081 
00082 #define MX12_INSTRUCTION_HEADER 0xff
00083 
00084 #define MX12_ERROR_RETURN       255
00085 #define MX12_NORMAL_RETURN        1
00086 
00087 // The max delay should be 508us according to:
00088 // http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05
00089 // And a max character should be 833 us according to:
00090 // (1 byte) / (9600 (bits per second)) = 833 microseconds
00091 // So 1000 should be a decent value, because 9600 bps is for chumps.
00092 #define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1500
00093 
00094 #ifndef M_PI
00095     #define M_PI    3.14159265358979323846  /* pi */
00096 #endif
00097 #ifndef M_PI_2
00098     #define M_PI_2  1.57079632679489661923  /* pi/2 */
00099 #endif
00100 
00101 #define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
00102 
00103 /** Dynamixel servo control class
00104  *
00105  * Example:
00106  * @code
00107  * #include "mbed.h"
00108  * #include "MX12.h"
00109  * 
00110  * int main() {
00111  * 
00112  *   MX12 mymx12 (p9, p10, 1);           // ID=1
00113  *
00114  *   while (1) {
00115  *       mymx12.Set_Goal_Position(0);    // go to 0 degrees
00116  *       wait (2.0);
00117  *       mymx12.Set_Goal_Position(300);  // go to 300 degrees
00118  *       wait (2.0);
00119  *   }
00120  * }
00121  * @endcode
00122  */
00123 class MX12 {
00124 
00125 public:
00126 
00127     /** Create an MX12 servo object connected to the specified serial port, with the specified ID
00128      *
00129      * @param pin tx pin
00130      * @param pin rx pin 
00131      * @param int ID, the Bus ID of the servo 1-255 
00132      */
00133     MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate=1000000);
00134 
00135     void Init(void);
00136     
00137     void ChangeDir(MX12_Direction dir);
00138 
00139 /** 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);}
00140 /** 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);}
00141 /** Max. Torque @retval Max Torque in Percent */    float Get_Max_Torque(void){return 0.097656*read_short(MX12_REG_MAX_TORQUE);}
00142 /** 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);}
00143 /** Goal Position @retval Goal Position in Degrees */   float Get_Goal_Position(void){return 0.087891*read_short(MX12_REG_GOAL_POSITION);}
00144 /** Moving Speed @retval Moving Speed in Degrees/Second */  float Get_Moving_Speed(void){return 0.686628*read_short(MX12_REG_MOVING_SPEED);}
00145 /** Torque Limit @retval Torque Limit in Percent */ float Get_Torque_Limit(void){return 0.097656*read_short(MX12_REG_TORQUE_LIMIT);}
00146 /** Punch @retval Punch in Percent */   float Get_Punch(void){return 0.097656*read_short(MX12_REG_PUNCH);}
00147 /** ID of Dynamixel @retval ID in int */    float Get_ID(void){return 1.000000*read_short(MX12_REG_ID);}
00148 /** Baud Rate of Dynamixel @retval Baud Rate in Lookup */   float Get_Baud_Rate(void){return 1.000000*read_short(MX12_REG_BAUD_RATE);}
00149 /** 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);}
00150 /** 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);}
00151 /** 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);}
00152 /** 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);}
00153 /** 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);}
00154 /** LED for Alarm @retval Alarm LED in Bitmap */    float Get_Alarm_LED(void){return 1.000000*read_short(MX12_REG_ALARM_LED);}
00155 /** Shutdown for Alarm @retval Alarm Shutdown in Bitmap */  float Get_Alarm_Shutdown(void){return 1.000000*read_short(MX12_REG_ALARM_SHUTDOWN);}
00156 /** Resolution divider @retval Resolution Divider in Ratio */   float Get_Resolution_Divider(void){return 1.000000*read_short(MX12_REG_RESOLUTION_DIVIDER);}
00157 /** Torque On/Off @retval Torque Enable in bool */  float Get_Torque_Enable(void){return 1.000000*read_short(MX12_REG_TORQUE_ENABLE);}
00158 /** LED On/Off @retval LED in Bitmap */ float Get_LED(void){return 1.000000*read_short(MX12_REG_LED);}
00159 /** Derivative Gain @retval D Gain in Kd */ float Get_D_Gain(void){return 0.004000*read_short(MX12_REG_D_GAIN);}
00160 /** Integral Gain @retval I Gain in Ki */   float Get_I_Gain(void){return 0.488281*read_short(MX12_REG_I_GAIN);}
00161 /** Proportional Gain @retval P Gain in Kp */   float Get_P_Gain(void){return 0.125000*read_short(MX12_REG_P_GAIN);}
00162 /** Locking EEPROM @retval Lock in bool */  float Get_Lock(void){return 1.000000*read_short(MX12_REG_LOCK);}
00163 /** Goal Acceleration @retval Goal Acceleration in Degrees */   float Get_Goal_Acceleration(void){return 8.582677*read_short(MX12_REG_GOAL_ACCELERATION);}
00164 /** model number @retval Model Number in Bitmap */  float Get_Model_Number(void){return 1.000000*read_short(MX12_REG_MODEL_NUMBER);}
00165 /** Current Position @retval Present Position in Degrees */ float Get_Present_Position(void){return 0.087891*read_short(MX12_REG_PRESENT_POSITION);}
00166 /** Current Speed @retval Present Speed in Degrees/Second */    float Get_Present_Speed(void){return 0.686628*read_short(MX12_REG_PRESENT_SPEED);}
00167 /** Current Load @retval Present Load in Percent */ float Get_Present_Load(void){return 0.097656*read_short(MX12_REG_PRESENT_LOAD);}
00168 /** 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);}
00169 /** Current Voltage @retval Present Voltage in Volts */ float Get_Present_Voltage(void){return 0.100000*read_short(MX12_REG_PRESENT_VOLTAGE);}
00170 /** Current Temperature @retval Present Temperature in Celsius */   float Get_Present_Temperature(void){return 1.000000*read_short(MX12_REG_PRESENT_TEMPERATURE);}
00171 /** Means if Instruction is registered @retval Registered in bool */    float Get_Registered(void){return 1.000000*read_short(MX12_REG_REGISTERED);}
00172 /** Means if there is any movement @retval Moving in bool */    float Get_Moving(void){return 1.000000*read_short(MX12_REG_MOVING);}
00173 
00174 /** 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);}
00175 /** 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);}
00176 /** Max. Torque @param val Max Torque in Percent */ void Set_Max_Torque(float val){write_short(MX12_REG_MAX_TORQUE,val/0.097656);}
00177 /** 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);}
00178 /** Goal Position @param val Goal Position in Degrees */    void Set_Goal_Position(float val){write_short(MX12_REG_GOAL_POSITION,val/0.087891);}
00179 /** Moving Speed @param val Moving Speed in Degrees/Second */   void Set_Moving_Speed(float val){write_short(MX12_REG_MOVING_SPEED,val/0.686628);}
00180 /** Torque Limit @param val Torque Limit in Percent */  void Set_Torque_Limit(float val){write_short(MX12_REG_TORQUE_LIMIT,val/0.097656);}
00181 /** Punch @param val Punch in Percent */    void Set_Punch(float val){write_short(MX12_REG_PUNCH,val/0.097656);}
00182 /** ID of Dynamixel @param val ID in int */ void Set_ID(float val){write_short(MX12_REG_ID,val/1.000000);}
00183 /** 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);}
00184 /** 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);}
00185 /** 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);}
00186 /** 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);}
00187 /** 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);}
00188 /** 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);}
00189 /** LED for Alarm @param val Alarm LED in Bitmap */ void Set_Alarm_LED(float val){write_short(MX12_REG_ALARM_LED,val/1.000000);}
00190 /** Shutdown for Alarm @param val Alarm Shutdown in Bitmap */   void Set_Alarm_Shutdown(float val){write_short(MX12_REG_ALARM_SHUTDOWN,val/1.000000);}
00191 /** Resolution divider @param val Resolution Divider in Ratio */    void Set_Resolution_Divider(float val){write_short(MX12_REG_RESOLUTION_DIVIDER,val/1.000000);}
00192 /** Torque On/Off @param val Torque Enable in bool */   void Set_Torque_Enable(float val){write_short(MX12_REG_TORQUE_ENABLE,val/1.000000);}
00193 /** LED On/Off @param val LED in Bitmap */  void Set_LED(float val){write_short(MX12_REG_LED,val/1.000000);}
00194 /** Derivative Gain @param val D Gain in Kd */  void Set_D_Gain(float val){write_short(MX12_REG_D_GAIN,val/0.004000);}
00195 /** Integral Gain @param val I Gain in Ki */    void Set_I_Gain(float val){write_short(MX12_REG_I_GAIN,val/0.488281);}
00196 /** Proportional Gain @param val P Gain in Kp */    void Set_P_Gain(float val){write_short(MX12_REG_P_GAIN,val/0.125000);}
00197 /** Locking EEPROM @param val Lock in bool */   void Set_Lock(float val){write_short(MX12_REG_LOCK,val/1.000000);}
00198 /** Goal Acceleration @param val Goal Acceleration in Degrees */    void Set_Goal_Acceleration(float val){write_short(MX12_REG_GOAL_ACCELERATION,val/8.582677);}
00199 
00200 
00201     int SetMode(int mode);
00202     
00203     /** Change the Baud Rate of a servo
00204      *
00205      * @param baud_rate The desired rate in bits per second.  
00206      *
00207      * @note Not all baud rates will be met exactly!
00208      *
00209      * The maths for this function are a bit odd, see here:
00210      * @url http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_04
00211      *
00212      *    TABLE OF VALID BAUD RATES:
00213      *   Data   Set BPS  Target  Tolerance
00214      *    1     1000000 1000000  0.000
00215      *    3      500000  500000  0.000
00216      *    4      400000  400000  0.000
00217      *    7      250000  250000  0.000
00218      *    9      200000  200000  0.000
00219      *    16   117647.1  115200 -2.124
00220      *    34    57142.9   57600  0.794
00221      *   103    19230.8   19200 -0.160
00222      *   207     9615.4    9600 -0.160
00223      *   250    2250000 2250000  0.000
00224      *   251    2500000 2500000  0.000
00225      *   252    3000000 3000000  0.000
00226      * 
00227      */
00228     int SetBaud( int target_baud );
00229     
00230     /** Change the Baud Rate of the UART serial port
00231      *
00232      * @param baud_rate The desired rate in bits per second (Default 1Mbps)
00233      *
00234      * @note This doesn't broadcast anything
00235      * 
00236      */
00237     void ChangeUARTBaud( int target_baud=1000000 );
00238 
00239     /** Read the current angle of the servo
00240      *
00241      * @returns short in the range 0-4095
00242      */
00243     short GetRawPosition(void);
00244     
00245     
00246     /** Dump everything we know about the servo to serial.
00247      *  Warning!  This will take a while!  At 1Mbps, it will
00248      *  probably take at least 250 ms, so only do this if you
00249      *  have lots of time to spare!
00250      *  
00251      *  @param serialObject Whichever serial object you want to print to, probably pc but maybe bluetooth
00252      */
00253     void Dump_OD_to_Serial(Serial &serialObject);
00254 
00255     /** Search for all responsive Dynamixels.
00256      *  Warning!  This will take a considerable amount of time if scanning the whole range!
00257      *  I haven't tried it, but off the top of my head it should take at least 3 seconds, probably more.
00258      *  
00259      *  @param scan_all_baud_rates if true, scan all 12 known Dynamixel baud rates. Default false, scans only current UART baud rate.
00260      *  @param max_id scan from zero to this value, defaults to 0xFC which is the max possible for a Dynamixel.
00261      */
00262     void Scan_For_Dynamixels(bool scan_all_baud_rates=false,int max_id=0xFC);
00263     
00264     /** Write a raw value to the servo
00265      *
00266      * @param OD Object Dictionary ID from the Enum
00267      * @param value The raw value to be written.  If the Bytes of the OD is 1, only the lower 8 bits are written.
00268      * @returns status int
00269      */
00270     int write_short(MX12ODIndex OD,short value);
00271     
00272     
00273     /** Read a raw value from the servo
00274      *
00275      * @param OD Object Dictionary ID from the Enum
00276      * @returns The raw value to be read.  If the Bytes of the OD is 1, only the lower 8 bits are used.
00277      */
00278     short read_short(MX12ODIndex OD);
00279     
00280     
00281     /** Perform a coordinated move.  Right now, it commands two servos with position and velocity,
00282      *  but the code should be extendable to more servos/parameters as needed.
00283      *
00284      * @param OD Object Dictionary ID from the Enum
00285      * @param value The raw value to be written.  If the Bytes of the OD is 1, only the lower 8 bits are written.
00286      * @returns status int
00287      */
00288     void coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1);
00289 
00290     /** Send the broadcast "trigger" command, to activate any outstanding registered commands
00291      */
00292     void trigger(void);
00293     
00294     /** Send a ping to a servo.  No matter what it should respond.  Use this to see if a servo exists.
00295      *
00296      * @param ID_Num the ID of the servo in question, 0 to 252 (0xFC) can be used, defaults to current object ID
00297      * @returns true if any response received, otherwise false
00298      */
00299     bool ping(char ID_Num=0xFF);
00300     
00301 private :
00302     //// Variables
00303     Serial mx12_out;
00304     Serial mx12_in;
00305     DigitalOut tx_enable;
00306     DigitalOut rx_enable;
00307     DigitalOut profileOut;
00308     int _ID;
00309     int _baud;
00310 
00311     //// Functions
00312     int read_raw(char* Status, int bytes=0);
00313     int read(int ID, int start, int length, char* data);
00314     int write(int ID, int start, int length, char* data);
00315 
00316 };
00317 
00318 #endif