Robotis Dynamixel MX-12W Servo Library
Dependents: SpindleBot_1_5b Utilisatio_MX12_V4
This is my attempt to adapt Chris Styles's AX12 library to work with my Dynamixel MX12 servos. This library is still very much a work in progress, and it may have some/many errors in it, but hopefully I will keep improving it to bring it up to snuff.
Dynamixel aficionados should also check out This MX28 library for a completely separate library that provides very similar functionality, and I wish I had known it existed before I started my work...
minimal example
#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); } }
MX12.h
- Committer:
- labmrd
- Date:
- 2015-01-26
- Revision:
- 1:946a27210553
- Parent:
- 0:29900c3a4a50
- Child:
- 2:c5236a433f1b
File content as of revision 1:946a27210553:
/* 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 #define MX12_CW 1 #define MX12_CCW 0 #define MX12_INSTRUCTION_HEADER 0xff // 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 1000 #ifndef M_PI #define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 #define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif /** Servo control class, based on a PwmOut * * 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, int ID, int baud_rate=1000000); void Init(void); /** 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 profileOut; int _ID; //// Functions int read(int ID, int start, int length, char* data); int write(int ID, int start, int length, char* data); }; #endif