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@1:946a27210553, 2015-01-26 (annotated)
- Committer:
- labmrd
- Date:
- Mon Jan 26 04:02:37 2015 +0000
- Revision:
- 1:946a27210553
- Parent:
- 0:29900c3a4a50
- Child:
- 2:c5236a433f1b
Re-made to include all possible Object Dictionary entries
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
labmrd | 1:946a27210553 | 1 | /* mbed MX-12 Servo Library |
labmrd | 0:29900c3a4a50 | 2 | * |
labmrd | 0:29900c3a4a50 | 3 | */ |
labmrd | 0:29900c3a4a50 | 4 | |
labmrd | 0:29900c3a4a50 | 5 | #ifndef MBED_MX12_H |
labmrd | 0:29900c3a4a50 | 6 | #define MBED_MX12_H |
labmrd | 0:29900c3a4a50 | 7 | |
labmrd | 0:29900c3a4a50 | 8 | #include "mbed.h" |
labmrd | 0:29900c3a4a50 | 9 | |
labmrd | 1:946a27210553 | 10 | #define MX12_WRITE_DEBUG 0 |
labmrd | 1:946a27210553 | 11 | #define MX12_READ_DEBUG 0 |
labmrd | 1:946a27210553 | 12 | #define MX12_TRIGGER_DEBUG 0 |
labmrd | 1:946a27210553 | 13 | #define MX12_DEBUG 0 |
labmrd | 1:946a27210553 | 14 | |
labmrd | 1:946a27210553 | 15 | #define MX12_OD_SIZE 34 |
labmrd | 0:29900c3a4a50 | 16 | |
labmrd | 1:946a27210553 | 17 | enum MX12ODIndex { |
labmrd | 1:946a27210553 | 18 | MX12_REG_MODEL_NUMBER, |
labmrd | 1:946a27210553 | 19 | MX12_REG_VERSION_OF_FIRMWARE, |
labmrd | 1:946a27210553 | 20 | MX12_REG_ID, |
labmrd | 1:946a27210553 | 21 | MX12_REG_BAUD_RATE, |
labmrd | 1:946a27210553 | 22 | MX12_REG_RETURN_DELAY_TIME, |
labmrd | 1:946a27210553 | 23 | MX12_REG_CW_ANGLE_LIMIT, |
labmrd | 1:946a27210553 | 24 | MX12_REG_CCW_ANGLE_LIMIT, |
labmrd | 1:946a27210553 | 25 | MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE, |
labmrd | 1:946a27210553 | 26 | MX12_REG_THE_LOWEST_LIMIT_VOLTAGE, |
labmrd | 1:946a27210553 | 27 | MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE, |
labmrd | 1:946a27210553 | 28 | MX12_REG_MAX_TORQUE, |
labmrd | 1:946a27210553 | 29 | MX12_REG_STATUS_RETURN_LEVEL, |
labmrd | 1:946a27210553 | 30 | MX12_REG_ALARM_LED, |
labmrd | 1:946a27210553 | 31 | MX12_REG_ALARM_SHUTDOWN, |
labmrd | 1:946a27210553 | 32 | MX12_REG_MULTI_TURN_OFFSET, |
labmrd | 1:946a27210553 | 33 | MX12_REG_RESOLUTION_DIVIDER, |
labmrd | 1:946a27210553 | 34 | MX12_REG_TORQUE_ENABLE, |
labmrd | 1:946a27210553 | 35 | MX12_REG_LED, |
labmrd | 1:946a27210553 | 36 | MX12_REG_D_GAIN, |
labmrd | 1:946a27210553 | 37 | MX12_REG_I_GAIN, |
labmrd | 1:946a27210553 | 38 | MX12_REG_P_GAIN, |
labmrd | 1:946a27210553 | 39 | MX12_REG_GOAL_POSITION, |
labmrd | 1:946a27210553 | 40 | MX12_REG_MOVING_SPEED, |
labmrd | 1:946a27210553 | 41 | MX12_REG_TORQUE_LIMIT, |
labmrd | 1:946a27210553 | 42 | MX12_REG_PRESENT_POSITION, |
labmrd | 1:946a27210553 | 43 | MX12_REG_PRESENT_SPEED, |
labmrd | 1:946a27210553 | 44 | MX12_REG_PRESENT_LOAD, |
labmrd | 1:946a27210553 | 45 | MX12_REG_PRESENT_VOLTAGE, |
labmrd | 1:946a27210553 | 46 | MX12_REG_PRESENT_TEMPERATURE, |
labmrd | 1:946a27210553 | 47 | MX12_REG_REGISTERED, |
labmrd | 1:946a27210553 | 48 | MX12_REG_MOVING, |
labmrd | 1:946a27210553 | 49 | MX12_REG_LOCK, |
labmrd | 1:946a27210553 | 50 | MX12_REG_PUNCH, |
labmrd | 1:946a27210553 | 51 | MX12_REG_GOAL_ACCELERATION |
labmrd | 1:946a27210553 | 52 | }; |
labmrd | 1:946a27210553 | 53 | |
labmrd | 1:946a27210553 | 54 | struct MX12OD_Object { |
labmrd | 1:946a27210553 | 55 | unsigned char Address; |
labmrd | 1:946a27210553 | 56 | //unsigned char InitialValue; |
labmrd | 1:946a27210553 | 57 | //unsigned char Readable; |
labmrd | 1:946a27210553 | 58 | //unsigned char Writeable; |
labmrd | 1:946a27210553 | 59 | unsigned char Bytes; |
labmrd | 1:946a27210553 | 60 | }; |
labmrd | 1:946a27210553 | 61 | |
labmrd | 1:946a27210553 | 62 | //We define these as globals, since they are constant |
labmrd | 1:946a27210553 | 63 | //and we only need one. |
labmrd | 1:946a27210553 | 64 | extern MX12OD_Object MX12_OD[MX12_OD_SIZE]; |
labmrd | 1:946a27210553 | 65 | //Avoid initializing them more than once. |
labmrd | 1:946a27210553 | 66 | extern bool MX12OD_Object_initalized; |
labmrd | 0:29900c3a4a50 | 67 | |
labmrd | 0:29900c3a4a50 | 68 | #define MX12_MODE_POSITION 0 |
labmrd | 0:29900c3a4a50 | 69 | #define MX12_MODE_ROTATION 1 |
labmrd | 0:29900c3a4a50 | 70 | |
labmrd | 0:29900c3a4a50 | 71 | #define MX12_CW 1 |
labmrd | 0:29900c3a4a50 | 72 | #define MX12_CCW 0 |
labmrd | 0:29900c3a4a50 | 73 | |
labmrd | 1:946a27210553 | 74 | #define MX12_INSTRUCTION_HEADER 0xff |
labmrd | 1:946a27210553 | 75 | |
labmrd | 1:946a27210553 | 76 | // The max delay should be 508us according to: |
labmrd | 1:946a27210553 | 77 | // http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05 |
labmrd | 1:946a27210553 | 78 | // And a max character should be 833 us according to: |
labmrd | 1:946a27210553 | 79 | // (1 byte) / (9600 (bits per second)) = 833 microseconds |
labmrd | 1:946a27210553 | 80 | // So 1000 should be a decent value, because 9600 bps is for chumps. |
labmrd | 1:946a27210553 | 81 | #define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1000 |
labmrd | 1:946a27210553 | 82 | |
labmrd | 1:946a27210553 | 83 | #ifndef M_PI |
labmrd | 1:946a27210553 | 84 | #define M_PI 3.14159265358979323846 /* pi */ |
labmrd | 1:946a27210553 | 85 | #endif |
labmrd | 1:946a27210553 | 86 | #ifndef M_PI_2 |
labmrd | 1:946a27210553 | 87 | #define M_PI_2 1.57079632679489661923 /* pi/2 */ |
labmrd | 1:946a27210553 | 88 | #endif |
labmrd | 0:29900c3a4a50 | 89 | |
labmrd | 0:29900c3a4a50 | 90 | /** Servo control class, based on a PwmOut |
labmrd | 0:29900c3a4a50 | 91 | * |
labmrd | 0:29900c3a4a50 | 92 | * Example: |
labmrd | 0:29900c3a4a50 | 93 | * @code |
labmrd | 0:29900c3a4a50 | 94 | * #include "mbed.h" |
labmrd | 0:29900c3a4a50 | 95 | * #include "MX12.h" |
labmrd | 0:29900c3a4a50 | 96 | * |
labmrd | 0:29900c3a4a50 | 97 | * int main() { |
labmrd | 0:29900c3a4a50 | 98 | * |
labmrd | 1:946a27210553 | 99 | * MX12 mymx12 (p9, p10, 1); // ID=1 |
labmrd | 0:29900c3a4a50 | 100 | * |
labmrd | 0:29900c3a4a50 | 101 | * while (1) { |
labmrd | 1:946a27210553 | 102 | * mymx12.Set_Goal_Position(0); // go to 0 degrees |
labmrd | 0:29900c3a4a50 | 103 | * wait (2.0); |
labmrd | 1:946a27210553 | 104 | * mymx12.Set_Goal_Position(300); // go to 300 degrees |
labmrd | 0:29900c3a4a50 | 105 | * wait (2.0); |
labmrd | 0:29900c3a4a50 | 106 | * } |
labmrd | 0:29900c3a4a50 | 107 | * } |
labmrd | 0:29900c3a4a50 | 108 | * @endcode |
labmrd | 0:29900c3a4a50 | 109 | */ |
labmrd | 0:29900c3a4a50 | 110 | class MX12 { |
labmrd | 0:29900c3a4a50 | 111 | |
labmrd | 0:29900c3a4a50 | 112 | public: |
labmrd | 0:29900c3a4a50 | 113 | |
labmrd | 0:29900c3a4a50 | 114 | /** Create an MX12 servo object connected to the specified serial port, with the specified ID |
labmrd | 0:29900c3a4a50 | 115 | * |
labmrd | 0:29900c3a4a50 | 116 | * @param pin tx pin |
labmrd | 0:29900c3a4a50 | 117 | * @param pin rx pin |
labmrd | 0:29900c3a4a50 | 118 | * @param int ID, the Bus ID of the servo 1-255 |
labmrd | 0:29900c3a4a50 | 119 | */ |
labmrd | 1:946a27210553 | 120 | MX12(PinName tx, PinName rx, int ID, int baud_rate=1000000); |
labmrd | 0:29900c3a4a50 | 121 | |
labmrd | 1:946a27210553 | 122 | void Init(void); |
labmrd | 0:29900c3a4a50 | 123 | |
labmrd | 1:946a27210553 | 124 | /** 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);} |
labmrd | 1:946a27210553 | 125 | /** 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);} |
labmrd | 1:946a27210553 | 126 | /** Max. Torque @retval Max Torque in Percent */ float Get_Max_Torque(void){return 0.097656*read_short(MX12_REG_MAX_TORQUE);} |
labmrd | 1:946a27210553 | 127 | /** 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);} |
labmrd | 1:946a27210553 | 128 | /** Goal Position @retval Goal Position in Degrees */ float Get_Goal_Position(void){return 0.087891*read_short(MX12_REG_GOAL_POSITION);} |
labmrd | 1:946a27210553 | 129 | /** Moving Speed @retval Moving Speed in Degrees/Second */ float Get_Moving_Speed(void){return 0.686628*read_short(MX12_REG_MOVING_SPEED);} |
labmrd | 1:946a27210553 | 130 | /** Torque Limit @retval Torque Limit in Percent */ float Get_Torque_Limit(void){return 0.097656*read_short(MX12_REG_TORQUE_LIMIT);} |
labmrd | 1:946a27210553 | 131 | /** Punch @retval Punch in Percent */ float Get_Punch(void){return 0.097656*read_short(MX12_REG_PUNCH);} |
labmrd | 1:946a27210553 | 132 | /** ID of Dynamixel @retval ID in int */ float Get_ID(void){return 1.000000*read_short(MX12_REG_ID);} |
labmrd | 1:946a27210553 | 133 | /** Baud Rate of Dynamixel @retval Baud Rate in Lookup */ float Get_Baud_Rate(void){return 1.000000*read_short(MX12_REG_BAUD_RATE);} |
labmrd | 1:946a27210553 | 134 | /** 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);} |
labmrd | 1:946a27210553 | 135 | /** 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);} |
labmrd | 1:946a27210553 | 136 | /** 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);} |
labmrd | 1:946a27210553 | 137 | /** 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);} |
labmrd | 1:946a27210553 | 138 | /** 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);} |
labmrd | 1:946a27210553 | 139 | /** LED for Alarm @retval Alarm LED in Bitmap */ float Get_Alarm_LED(void){return 1.000000*read_short(MX12_REG_ALARM_LED);} |
labmrd | 1:946a27210553 | 140 | /** Shutdown for Alarm @retval Alarm Shutdown in Bitmap */ float Get_Alarm_Shutdown(void){return 1.000000*read_short(MX12_REG_ALARM_SHUTDOWN);} |
labmrd | 1:946a27210553 | 141 | /** Resolution divider @retval Resolution Divider in Ratio */ float Get_Resolution_Divider(void){return 1.000000*read_short(MX12_REG_RESOLUTION_DIVIDER);} |
labmrd | 1:946a27210553 | 142 | /** Torque On/Off @retval Torque Enable in bool */ float Get_Torque_Enable(void){return 1.000000*read_short(MX12_REG_TORQUE_ENABLE);} |
labmrd | 1:946a27210553 | 143 | /** LED On/Off @retval LED in Bitmap */ float Get_LED(void){return 1.000000*read_short(MX12_REG_LED);} |
labmrd | 1:946a27210553 | 144 | /** Derivative Gain @retval D Gain in Kd */ float Get_D_Gain(void){return 0.004000*read_short(MX12_REG_D_GAIN);} |
labmrd | 1:946a27210553 | 145 | /** Integral Gain @retval I Gain in Ki */ float Get_I_Gain(void){return 0.488281*read_short(MX12_REG_I_GAIN);} |
labmrd | 1:946a27210553 | 146 | /** Proportional Gain @retval P Gain in Kp */ float Get_P_Gain(void){return 0.125000*read_short(MX12_REG_P_GAIN);} |
labmrd | 1:946a27210553 | 147 | /** Locking EEPROM @retval Lock in bool */ float Get_Lock(void){return 1.000000*read_short(MX12_REG_LOCK);} |
labmrd | 1:946a27210553 | 148 | /** Goal Acceleration @retval Goal Acceleration in Degrees */ float Get_Goal_Acceleration(void){return 8.582677*read_short(MX12_REG_GOAL_ACCELERATION);} |
labmrd | 1:946a27210553 | 149 | /** model number @retval Model Number in Bitmap */ float Get_Model_Number(void){return 1.000000*read_short(MX12_REG_MODEL_NUMBER);} |
labmrd | 1:946a27210553 | 150 | /** Current Position @retval Present Position in Degrees */ float Get_Present_Position(void){return 0.087891*read_short(MX12_REG_PRESENT_POSITION);} |
labmrd | 1:946a27210553 | 151 | /** Current Speed @retval Present Speed in Degrees/Second */ float Get_Present_Speed(void){return 0.686628*read_short(MX12_REG_PRESENT_SPEED);} |
labmrd | 1:946a27210553 | 152 | /** Current Load @retval Present Load in Percent */ float Get_Present_Load(void){return 0.097656*read_short(MX12_REG_PRESENT_LOAD);} |
labmrd | 1:946a27210553 | 153 | /** 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);} |
labmrd | 1:946a27210553 | 154 | /** Current Voltage @retval Present Voltage in Volts */ float Get_Present_Voltage(void){return 0.100000*read_short(MX12_REG_PRESENT_VOLTAGE);} |
labmrd | 1:946a27210553 | 155 | /** Current Temperature @retval Present Temperature in Celsius */ float Get_Present_Temperature(void){return 1.000000*read_short(MX12_REG_PRESENT_TEMPERATURE);} |
labmrd | 1:946a27210553 | 156 | /** Means if Instruction is registered @retval Registered in bool */ float Get_Registered(void){return 1.000000*read_short(MX12_REG_REGISTERED);} |
labmrd | 1:946a27210553 | 157 | /** Means if there is any movement @retval Moving in bool */ float Get_Moving(void){return 1.000000*read_short(MX12_REG_MOVING);} |
labmrd | 0:29900c3a4a50 | 158 | |
labmrd | 1:946a27210553 | 159 | /** 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);} |
labmrd | 1:946a27210553 | 160 | /** 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);} |
labmrd | 1:946a27210553 | 161 | /** Max. Torque @param val Max Torque in Percent */ void Set_Max_Torque(float val){write_short(MX12_REG_MAX_TORQUE,val/0.097656);} |
labmrd | 1:946a27210553 | 162 | /** 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);} |
labmrd | 1:946a27210553 | 163 | /** Goal Position @param val Goal Position in Degrees */ void Set_Goal_Position(float val){write_short(MX12_REG_GOAL_POSITION,val/0.087891);} |
labmrd | 1:946a27210553 | 164 | /** Moving Speed @param val Moving Speed in Degrees/Second */ void Set_Moving_Speed(float val){write_short(MX12_REG_MOVING_SPEED,val/0.686628);} |
labmrd | 1:946a27210553 | 165 | /** Torque Limit @param val Torque Limit in Percent */ void Set_Torque_Limit(float val){write_short(MX12_REG_TORQUE_LIMIT,val/0.097656);} |
labmrd | 1:946a27210553 | 166 | /** Punch @param val Punch in Percent */ void Set_Punch(float val){write_short(MX12_REG_PUNCH,val/0.097656);} |
labmrd | 1:946a27210553 | 167 | /** ID of Dynamixel @param val ID in int */ void Set_ID(float val){write_short(MX12_REG_ID,val/1.000000);} |
labmrd | 1:946a27210553 | 168 | /** 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);} |
labmrd | 1:946a27210553 | 169 | /** 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);} |
labmrd | 1:946a27210553 | 170 | /** 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);} |
labmrd | 1:946a27210553 | 171 | /** 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);} |
labmrd | 1:946a27210553 | 172 | /** 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);} |
labmrd | 1:946a27210553 | 173 | /** 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);} |
labmrd | 1:946a27210553 | 174 | /** LED for Alarm @param val Alarm LED in Bitmap */ void Set_Alarm_LED(float val){write_short(MX12_REG_ALARM_LED,val/1.000000);} |
labmrd | 1:946a27210553 | 175 | /** Shutdown for Alarm @param val Alarm Shutdown in Bitmap */ void Set_Alarm_Shutdown(float val){write_short(MX12_REG_ALARM_SHUTDOWN,val/1.000000);} |
labmrd | 1:946a27210553 | 176 | /** Resolution divider @param val Resolution Divider in Ratio */ void Set_Resolution_Divider(float val){write_short(MX12_REG_RESOLUTION_DIVIDER,val/1.000000);} |
labmrd | 1:946a27210553 | 177 | /** Torque On/Off @param val Torque Enable in bool */ void Set_Torque_Enable(float val){write_short(MX12_REG_TORQUE_ENABLE,val/1.000000);} |
labmrd | 1:946a27210553 | 178 | /** LED On/Off @param val LED in Bitmap */ void Set_LED(float val){write_short(MX12_REG_LED,val/1.000000);} |
labmrd | 1:946a27210553 | 179 | /** Derivative Gain @param val D Gain in Kd */ void Set_D_Gain(float val){write_short(MX12_REG_D_GAIN,val/0.004000);} |
labmrd | 1:946a27210553 | 180 | /** Integral Gain @param val I Gain in Ki */ void Set_I_Gain(float val){write_short(MX12_REG_I_GAIN,val/0.488281);} |
labmrd | 1:946a27210553 | 181 | /** Proportional Gain @param val P Gain in Kp */ void Set_P_Gain(float val){write_short(MX12_REG_P_GAIN,val/0.125000);} |
labmrd | 1:946a27210553 | 182 | /** Locking EEPROM @param val Lock in bool */ void Set_Lock(float val){write_short(MX12_REG_LOCK,val/1.000000);} |
labmrd | 1:946a27210553 | 183 | /** Goal Acceleration @param val Goal Acceleration in Degrees */ void Set_Goal_Acceleration(float val){write_short(MX12_REG_GOAL_ACCELERATION,val/8.582677);} |
labmrd | 0:29900c3a4a50 | 184 | |
labmrd | 0:29900c3a4a50 | 185 | |
labmrd | 1:946a27210553 | 186 | int SetMode(int mode); |
labmrd | 0:29900c3a4a50 | 187 | |
labmrd | 1:946a27210553 | 188 | /** Change the Baud Rate of a servo |
labmrd | 1:946a27210553 | 189 | * |
labmrd | 1:946a27210553 | 190 | * @param baud_rate The desired rate in bits per second. |
labmrd | 1:946a27210553 | 191 | * |
labmrd | 1:946a27210553 | 192 | * @note Not all baud rates will be met exactly! |
labmrd | 1:946a27210553 | 193 | * |
labmrd | 1:946a27210553 | 194 | * The maths for this function are a bit odd, see here: |
labmrd | 1:946a27210553 | 195 | * @url http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_04 |
labmrd | 0:29900c3a4a50 | 196 | * |
labmrd | 1:946a27210553 | 197 | * TABLE OF VALID BAUD RATES: |
labmrd | 1:946a27210553 | 198 | * Data Set BPS Target Tolerance |
labmrd | 1:946a27210553 | 199 | * 1 1000000 1000000 0.000 |
labmrd | 1:946a27210553 | 200 | * 3 500000 500000 0.000 |
labmrd | 1:946a27210553 | 201 | * 4 400000 400000 0.000 |
labmrd | 1:946a27210553 | 202 | * 7 250000 250000 0.000 |
labmrd | 1:946a27210553 | 203 | * 9 200000 200000 0.000 |
labmrd | 1:946a27210553 | 204 | * 16 117647.1 115200 -2.124 |
labmrd | 1:946a27210553 | 205 | * 34 57142.9 57600 0.794 |
labmrd | 1:946a27210553 | 206 | * 103 19230.8 19200 -0.160 |
labmrd | 1:946a27210553 | 207 | * 207 9615.4 9600 -0.160 |
labmrd | 1:946a27210553 | 208 | * 250 2250000 2250000 0.000 |
labmrd | 1:946a27210553 | 209 | * 251 2500000 2500000 0.000 |
labmrd | 1:946a27210553 | 210 | * 252 3000000 3000000 0.000 |
labmrd | 1:946a27210553 | 211 | * |
labmrd | 0:29900c3a4a50 | 212 | */ |
labmrd | 1:946a27210553 | 213 | int SetBaud( int target_baud ); |
labmrd | 1:946a27210553 | 214 | |
labmrd | 1:946a27210553 | 215 | /** Change the Baud Rate of the UART serial port |
labmrd | 1:946a27210553 | 216 | * |
labmrd | 1:946a27210553 | 217 | * @param baud_rate The desired rate in bits per second (Default 1Mbps) |
labmrd | 1:946a27210553 | 218 | * |
labmrd | 1:946a27210553 | 219 | * @note This doesn't broadcast anything |
labmrd | 1:946a27210553 | 220 | * |
labmrd | 1:946a27210553 | 221 | */ |
labmrd | 1:946a27210553 | 222 | void ChangeUARTBaud( int target_baud=1000000 ); |
labmrd | 0:29900c3a4a50 | 223 | |
labmrd | 1:946a27210553 | 224 | /** Read the current angle of the servo |
labmrd | 0:29900c3a4a50 | 225 | * |
labmrd | 1:946a27210553 | 226 | * @returns short in the range 0-4095 |
labmrd | 1:946a27210553 | 227 | */ |
labmrd | 1:946a27210553 | 228 | short GetRawPosition(void); |
labmrd | 1:946a27210553 | 229 | |
labmrd | 1:946a27210553 | 230 | |
labmrd | 1:946a27210553 | 231 | /** Dump everything we know about the servo to serial. |
labmrd | 1:946a27210553 | 232 | * Warning! This will take a while! At 1Mbps, it will |
labmrd | 1:946a27210553 | 233 | * probably take at least 250 ms, so only do this if you |
labmrd | 1:946a27210553 | 234 | * have lots of time to spare! |
labmrd | 1:946a27210553 | 235 | * |
labmrd | 1:946a27210553 | 236 | * @param serialObject Whichever serial object you want to print to, probably pc but maybe bluetooth |
labmrd | 1:946a27210553 | 237 | */ |
labmrd | 1:946a27210553 | 238 | void Dump_OD_to_Serial(Serial &serialObject); |
labmrd | 1:946a27210553 | 239 | |
labmrd | 1:946a27210553 | 240 | /** Search for all responsive Dynamixels. |
labmrd | 1:946a27210553 | 241 | * Warning! This will take a considerable amount of time if scanning the whole range! |
labmrd | 1:946a27210553 | 242 | * I haven't tried it, but off the top of my head it should take at least 3 seconds, probably more. |
labmrd | 1:946a27210553 | 243 | * |
labmrd | 1:946a27210553 | 244 | * @param scan_all_baud_rates if true, scan all 12 known Dynamixel baud rates. Default false, scans only current UART baud rate. |
labmrd | 1:946a27210553 | 245 | * @param max_id scan from zero to this value, defaults to 0xFC which is the max possible for a Dynamixel. |
labmrd | 0:29900c3a4a50 | 246 | */ |
labmrd | 1:946a27210553 | 247 | void Scan_For_Dynamixels(bool scan_all_baud_rates=false,int max_id=0xFC); |
labmrd | 1:946a27210553 | 248 | |
labmrd | 1:946a27210553 | 249 | /** Write a raw value to the servo |
labmrd | 1:946a27210553 | 250 | * |
labmrd | 1:946a27210553 | 251 | * @param OD Object Dictionary ID from the Enum |
labmrd | 1:946a27210553 | 252 | * @param value The raw value to be written. If the Bytes of the OD is 1, only the lower 8 bits are written. |
labmrd | 1:946a27210553 | 253 | * @returns status int |
labmrd | 1:946a27210553 | 254 | */ |
labmrd | 1:946a27210553 | 255 | int write_short(MX12ODIndex OD,short value); |
labmrd | 1:946a27210553 | 256 | |
labmrd | 1:946a27210553 | 257 | |
labmrd | 1:946a27210553 | 258 | /** Read a raw value from the servo |
labmrd | 0:29900c3a4a50 | 259 | * |
labmrd | 1:946a27210553 | 260 | * @param OD Object Dictionary ID from the Enum |
labmrd | 1:946a27210553 | 261 | * @returns The raw value to be read. If the Bytes of the OD is 1, only the lower 8 bits are used. |
labmrd | 0:29900c3a4a50 | 262 | */ |
labmrd | 1:946a27210553 | 263 | short read_short(MX12ODIndex OD); |
labmrd | 1:946a27210553 | 264 | |
labmrd | 1:946a27210553 | 265 | |
labmrd | 1:946a27210553 | 266 | /** Perform a coordinated move. Right now, it commands two servos with position and velocity, |
labmrd | 1:946a27210553 | 267 | * but the code should be extendable to more servos/parameters as needed. |
labmrd | 1:946a27210553 | 268 | * |
labmrd | 1:946a27210553 | 269 | * @param OD Object Dictionary ID from the Enum |
labmrd | 1:946a27210553 | 270 | * @param value The raw value to be written. If the Bytes of the OD is 1, only the lower 8 bits are written. |
labmrd | 1:946a27210553 | 271 | * @returns status int |
labmrd | 1:946a27210553 | 272 | */ |
labmrd | 1:946a27210553 | 273 | void coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1); |
labmrd | 0:29900c3a4a50 | 274 | |
labmrd | 0:29900c3a4a50 | 275 | /** Send the broadcast "trigger" command, to activate any outstanding registered commands |
labmrd | 0:29900c3a4a50 | 276 | */ |
labmrd | 0:29900c3a4a50 | 277 | void trigger(void); |
labmrd | 1:946a27210553 | 278 | |
labmrd | 1:946a27210553 | 279 | /** Send a ping to a servo. No matter what it should respond. Use this to see if a servo exists. |
labmrd | 0:29900c3a4a50 | 280 | * |
labmrd | 1:946a27210553 | 281 | * @param ID_Num the ID of the servo in question, 0 to 252 (0xFC) can be used, defaults to current object ID |
labmrd | 1:946a27210553 | 282 | * @returns true if any response received, otherwise false |
labmrd | 0:29900c3a4a50 | 283 | */ |
labmrd | 1:946a27210553 | 284 | bool ping(char ID_Num=0xFF); |
labmrd | 1:946a27210553 | 285 | |
labmrd | 0:29900c3a4a50 | 286 | private : |
labmrd | 1:946a27210553 | 287 | //// Variables |
labmrd | 1:946a27210553 | 288 | Serial mx12_out; |
labmrd | 1:946a27210553 | 289 | Serial mx12_in; |
labmrd | 1:946a27210553 | 290 | DigitalOut profileOut; |
labmrd | 0:29900c3a4a50 | 291 | int _ID; |
labmrd | 0:29900c3a4a50 | 292 | |
labmrd | 1:946a27210553 | 293 | //// Functions |
labmrd | 0:29900c3a4a50 | 294 | int read(int ID, int start, int length, char* data); |
labmrd | 1:946a27210553 | 295 | int write(int ID, int start, int length, char* data); |
labmrd | 0:29900c3a4a50 | 296 | |
labmrd | 0:29900c3a4a50 | 297 | }; |
labmrd | 0:29900c3a4a50 | 298 | |
labmrd | 0:29900c3a4a50 | 299 | #endif |