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); } }
Diff: MX12.h
- Revision:
- 1:946a27210553
- Parent:
- 0:29900c3a4a50
- Child:
- 2:c5236a433f1b
--- a/MX12.h Tue Nov 25 03:07:36 2014 +0000 +++ b/MX12.h Mon Jan 26 04:02:37 2015 +0000 @@ -1,24 +1,5 @@ -/* mbed AX-12+ Servo Library - * - * Copyright (c) 2010, cstyles (http://mbed.org) - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: +/* mbed MX-12 Servo Library * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. */ #ifndef MBED_MX12_H @@ -26,20 +7,63 @@ #include "mbed.h" -#define MX12_WRITE_DEBUG 1 -#define MX12_READ_DEBUG 1 -#define MX12_TRIGGER_DEBUG 1 -#define MX12_DEBUG 1 +#define MX12_WRITE_DEBUG 0 +#define MX12_READ_DEBUG 0 +#define MX12_TRIGGER_DEBUG 0 +#define MX12_DEBUG 0 + +#define MX12_OD_SIZE 34 -#define MX12_REG_ID 0x3 -#define MX12_REG_CW_LIMIT 0x06 -#define MX12_REG_CCW_LIMIT 0x08 -#define MX12_REG_GOAL_POSITION 0x1E -#define MX12_REG_MOVING_SPEED 0x20 -#define MX12_REG_VOLTS 0x2A -#define MX12_REG_TEMP 0x2B -#define MX12_REG_MOVING 0x2E -#define MX12_REG_POSITION 0x24 +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 @@ -47,7 +71,21 @@ #define MX12_CW 1 #define MX12_CCW 0 -#define MAX_DELAY_BETWEEN_CHARCTERS_IN_MS 10 +#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 * @@ -58,12 +96,12 @@ * * int main() { * - * MX12 mymx12 (p9, p10, 1); + * MX12 mymx12 (p9, p10, 1); // ID=1 * * while (1) { - * mymx12.SetGoal(0); // go to 0 degrees + * mymx12.Set_Goal_Position(0); // go to 0 degrees * wait (2.0); - * mymx12.SetGoal(300); // go to 300 degrees + * mymx12.Set_Goal_Position(300); // go to 300 degrees * wait (2.0); * } * } @@ -79,95 +117,182 @@ * @param pin rx pin * @param int ID, the Bus ID of the servo 1-255 */ - MX12(PinName tx, PinName rx, int ID); + MX12(PinName tx, PinName rx, int ID, int baud_rate=1000000); - /** Set the mode of the servo - * @param mode - * 0 = Positional, default - * 1 = Continuous rotation - */ - int SetMode(int mode); + void Init(void); - /** Set goal angle in integer degrees, in positional mode - * - * @param degrees 0-300 - * @param flags, defaults to 0 - * flags[0] = blocking, return when goal position reached - * flags[1] = register, activate with a broadcast trigger - * - */ - int SetGoal(float radians, int flags = 0); +/** 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);} - - /** Set the speed of the servo in continuous rotation mode - * - * @param speed, -1.0 to 1.0 - * -1.0 = full speed counter clock wise - * 1.0 = full speed clock wise - */ - int SetCRSpeed(float speed); +/** 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);} - /** Set the clockwise limit of the servo - * - * @param degrees, 0-300 - */ - int SetCWLimit(int degrees); + int SetMode(int mode); - /** Set the counter-clockwise limit of the servo + /** 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 * - * @param degrees, 0-300 + * 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 SetCCWLimit(int degrees); - - // Change the ID + 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 ); - /** Change the ID of a servo + /** Read the current angle of the servo * - * @param CurentID 1-255 - * @param NewID 1-255 - * - * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID. - * In this situation, only one servo should be connected to the bus + * @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. */ - int SetID(int CurrentID, int NewID); - - - /** Poll to see if the servo is moving + 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 * - * @returns true is the servo is moving + * @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. */ - int isMoving(void); + 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); - - /** Read the current angle of the servo + + /** Send a ping to a servo. No matter what it should respond. Use this to see if a servo exists. * - * @returns float in the range 0.0-300.0 - */ - float GetPosition(); - - /** Read the temperature of the servo - * - * @returns float temperature + * @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 */ - float GetTemp(void); - - /** Read the supply voltage of the servo - * - * @returns float voltage - */ - float GetVolts(void); - + bool ping(char ID_Num=0xFF); + private : - - SerialHalfDuplex _mx12; + //// 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, int flag=0); + int write(int ID, int start, int length, char* data); };