MRD Lab / MX12

Robotis Dynamixel MX-12W Servo Library

Dependents:   SpindleBot_1_5b Utilisatio_MX12_V4

/media/uploads/labmrd/mx12.jpg

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@5:4c118a827f11, 2015-02-10 (annotated)

Committer:
labmrd
Date:
Tue Feb 10 21:52:40 2015 +0000
Revision:
5:4c118a827f11
Parent:
3:624d04c390b8 
Now with buffer chip!

Who changed what in which revision?

UserRevisionLine numberNew 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 5:4c118a827f11 71 // For now we don't have a both option, since no
labmrd 5:4c118a827f11 72 // one should use that anyways.
labmrd 5:4c118a827f11 73 enum MX12_Direction {
labmrd 5:4c118a827f11 74 MX12_DIR_IN,
labmrd 5:4c118a827f11 75 MX12_DIR_OUT,
labmrd 5:4c118a827f11 76 MX12_DIR_NONE
labmrd 5:4c118a827f11 77 };
labmrd 5:4c118a827f11 78
labmrd 0:29900c3a4a50 79 #define MX12_CW 1
labmrd 0:29900c3a4a50 80 #define MX12_CCW 0
labmrd 0:29900c3a4a50 81
labmrd 1:946a27210553 82 #define MX12_INSTRUCTION_HEADER 0xff
labmrd 1:946a27210553 83
labmrd 5:4c118a827f11 84 #define MX12_ERROR_RETURN 255
labmrd 5:4c118a827f11 85 #define MX12_NORMAL_RETURN 1
labmrd 5:4c118a827f11 86
labmrd 1:946a27210553 87 // The max delay should be 508us according to:
labmrd 1:946a27210553 88 // http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05
labmrd 1:946a27210553 89 // And a max character should be 833 us according to:
labmrd 1:946a27210553 90 // (1 byte) / (9600 (bits per second)) = 833 microseconds
labmrd 1:946a27210553 91 // So 1000 should be a decent value, because 9600 bps is for chumps.
labmrd 5:4c118a827f11 92 #define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1500
labmrd 1:946a27210553 93
labmrd 1:946a27210553 94 #ifndef M_PI
labmrd 1:946a27210553 95 #define M_PI 3.14159265358979323846 /* pi */
labmrd 1:946a27210553 96 #endif
labmrd 1:946a27210553 97 #ifndef M_PI_2
labmrd 1:946a27210553 98 #define M_PI_2 1.57079632679489661923 /* pi/2 */
labmrd 1:946a27210553 99 #endif
labmrd 0:29900c3a4a50 100
labmrd 3:624d04c390b8 101 #define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
labmrd 3:624d04c390b8 102
labmrd 2:c5236a433f1b 103 /** Dynamixel servo control class
labmrd 0:29900c3a4a50 104 *
labmrd 0:29900c3a4a50 105 * Example:
labmrd 0:29900c3a4a50 106 * @code
labmrd 0:29900c3a4a50 107 * #include "mbed.h"
labmrd 0:29900c3a4a50 108 * #include "MX12.h"
labmrd 0:29900c3a4a50 109 *
labmrd 0:29900c3a4a50 110 * int main() {
labmrd 0:29900c3a4a50 111 *
labmrd 1:946a27210553 112 * MX12 mymx12 (p9, p10, 1); // ID=1
labmrd 0:29900c3a4a50 113 *
labmrd 0:29900c3a4a50 114 * while (1) {
labmrd 1:946a27210553 115 * mymx12.Set_Goal_Position(0); // go to 0 degrees
labmrd 0:29900c3a4a50 116 * wait (2.0);
labmrd 1:946a27210553 117 * mymx12.Set_Goal_Position(300); // go to 300 degrees
labmrd 0:29900c3a4a50 118 * wait (2.0);
labmrd 0:29900c3a4a50 119 * }
labmrd 0:29900c3a4a50 120 * }
labmrd 0:29900c3a4a50 121 * @endcode
labmrd 0:29900c3a4a50 122 */
labmrd 0:29900c3a4a50 123 class MX12 {
labmrd 0:29900c3a4a50 124
labmrd 0:29900c3a4a50 125 public:
labmrd 0:29900c3a4a50 126
labmrd 0:29900c3a4a50 127 /** Create an MX12 servo object connected to the specified serial port, with the specified ID
labmrd 0:29900c3a4a50 128 *
labmrd 0:29900c3a4a50 129 * @param pin tx pin
labmrd 0:29900c3a4a50 130 * @param pin rx pin
labmrd 0:29900c3a4a50 131 * @param int ID, the Bus ID of the servo 1-255
labmrd 0:29900c3a4a50 132 */
labmrd 5:4c118a827f11 133 MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate=1000000);
labmrd 0:29900c3a4a50 134
labmrd 1:946a27210553 135 void Init(void);
labmrd 5:4c118a827f11 136
labmrd 5:4c118a827f11 137 void ChangeDir(MX12_Direction dir);
labmrd 0:29900c3a4a50 138
labmrd 1:946a27210553 139 /** 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 140 /** 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 141 /** Max. Torque @retval Max Torque in Percent */ float Get_Max_Torque(void){return 0.097656*read_short(MX12_REG_MAX_TORQUE);}
labmrd 1:946a27210553 142 /** 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 143 /** Goal Position @retval Goal Position in Degrees */ float Get_Goal_Position(void){return 0.087891*read_short(MX12_REG_GOAL_POSITION);}
labmrd 1:946a27210553 144 /** 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 145 /** Torque Limit @retval Torque Limit in Percent */ float Get_Torque_Limit(void){return 0.097656*read_short(MX12_REG_TORQUE_LIMIT);}
labmrd 1:946a27210553 146 /** Punch @retval Punch in Percent */ float Get_Punch(void){return 0.097656*read_short(MX12_REG_PUNCH);}
labmrd 1:946a27210553 147 /** ID of Dynamixel @retval ID in int */ float Get_ID(void){return 1.000000*read_short(MX12_REG_ID);}
labmrd 1:946a27210553 148 /** 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 149 /** 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 150 /** 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 151 /** 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 152 /** 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 153 /** 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 154 /** 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 155 /** 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 156 /** Resolution divider @retval Resolution Divider in Ratio */ float Get_Resolution_Divider(void){return 1.000000*read_short(MX12_REG_RESOLUTION_DIVIDER);}
labmrd 1:946a27210553 157 /** 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 158 /** LED On/Off @retval LED in Bitmap */ float Get_LED(void){return 1.000000*read_short(MX12_REG_LED);}
labmrd 1:946a27210553 159 /** Derivative Gain @retval D Gain in Kd */ float Get_D_Gain(void){return 0.004000*read_short(MX12_REG_D_GAIN);}
labmrd 1:946a27210553 160 /** Integral Gain @retval I Gain in Ki */ float Get_I_Gain(void){return 0.488281*read_short(MX12_REG_I_GAIN);}
labmrd 1:946a27210553 161 /** Proportional Gain @retval P Gain in Kp */ float Get_P_Gain(void){return 0.125000*read_short(MX12_REG_P_GAIN);}
labmrd 1:946a27210553 162 /** Locking EEPROM @retval Lock in bool */ float Get_Lock(void){return 1.000000*read_short(MX12_REG_LOCK);}
labmrd 1:946a27210553 163 /** Goal Acceleration @retval Goal Acceleration in Degrees */ float Get_Goal_Acceleration(void){return 8.582677*read_short(MX12_REG_GOAL_ACCELERATION);}
labmrd 1:946a27210553 164 /** model number @retval Model Number in Bitmap */ float Get_Model_Number(void){return 1.000000*read_short(MX12_REG_MODEL_NUMBER);}
labmrd 1:946a27210553 165 /** Current Position @retval Present Position in Degrees */ float Get_Present_Position(void){return 0.087891*read_short(MX12_REG_PRESENT_POSITION);}
labmrd 1:946a27210553 166 /** 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 167 /** Current Load @retval Present Load in Percent */ float Get_Present_Load(void){return 0.097656*read_short(MX12_REG_PRESENT_LOAD);}
labmrd 1:946a27210553 168 /** 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 169 /** Current Voltage @retval Present Voltage in Volts */ float Get_Present_Voltage(void){return 0.100000*read_short(MX12_REG_PRESENT_VOLTAGE);}
labmrd 1:946a27210553 170 /** Current Temperature @retval Present Temperature in Celsius */ float Get_Present_Temperature(void){return 1.000000*read_short(MX12_REG_PRESENT_TEMPERATURE);}
labmrd 1:946a27210553 171 /** Means if Instruction is registered @retval Registered in bool */ float Get_Registered(void){return 1.000000*read_short(MX12_REG_REGISTERED);}
labmrd 1:946a27210553 172 /** 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 173
labmrd 1:946a27210553 174 /** 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 175 /** 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 176 /** 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 177 /** 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 178 /** 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 179 /** 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 180 /** 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 181 /** Punch @param val Punch in Percent */ void Set_Punch(float val){write_short(MX12_REG_PUNCH,val/0.097656);}
labmrd 1:946a27210553 182 /** ID of Dynamixel @param val ID in int */ void Set_ID(float val){write_short(MX12_REG_ID,val/1.000000);}
labmrd 1:946a27210553 183 /** 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 184 /** 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 185 /** 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 186 /** 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 187 /** 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 188 /** 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 189 /** 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 190 /** 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 191 /** 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 192 /** 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 193 /** LED On/Off @param val LED in Bitmap */ void Set_LED(float val){write_short(MX12_REG_LED,val/1.000000);}
labmrd 1:946a27210553 194 /** 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 195 /** 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 196 /** 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 197 /** Locking EEPROM @param val Lock in bool */ void Set_Lock(float val){write_short(MX12_REG_LOCK,val/1.000000);}
labmrd 1:946a27210553 198 /** 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 199
labmrd 0:29900c3a4a50 200
labmrd 1:946a27210553 201 int SetMode(int mode);
labmrd 0:29900c3a4a50 202
labmrd 1:946a27210553 203 /** Change the Baud Rate of a servo
labmrd 1:946a27210553 204 *
labmrd 1:946a27210553 205 * @param baud_rate The desired rate in bits per second.
labmrd 1:946a27210553 206 *
labmrd 1:946a27210553 207 * @note Not all baud rates will be met exactly!
labmrd 1:946a27210553 208 *
labmrd 1:946a27210553 209 * The maths for this function are a bit odd, see here:
labmrd 1:946a27210553 210 * @url http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_04
labmrd 0:29900c3a4a50 211 *
labmrd 1:946a27210553 212 * TABLE OF VALID BAUD RATES:
labmrd 1:946a27210553 213 * Data Set BPS Target Tolerance
labmrd 1:946a27210553 214 * 1 1000000 1000000  0.000
labmrd 1:946a27210553 215 * 3 500000 500000  0.000
labmrd 1:946a27210553 216 * 4 400000 400000  0.000
labmrd 1:946a27210553 217 * 7 250000 250000  0.000
labmrd 1:946a27210553 218 * 9 200000 200000  0.000
labmrd 1:946a27210553 219 * 16 117647.1 115200 -2.124
labmrd 1:946a27210553 220 * 34 57142.9 57600  0.794
labmrd 1:946a27210553 221 * 103 19230.8 19200 -0.160
labmrd 1:946a27210553 222 * 207 9615.4 9600 -0.160
labmrd 1:946a27210553 223 * 250 2250000 2250000  0.000
labmrd 1:946a27210553 224 * 251 2500000 2500000  0.000
labmrd 1:946a27210553 225 * 252 3000000 3000000  0.000
labmrd 1:946a27210553 226 *
labmrd 0:29900c3a4a50 227 */
labmrd 1:946a27210553 228 int SetBaud( int target_baud );
labmrd 1:946a27210553 229
labmrd 1:946a27210553 230 /** Change the Baud Rate of the UART serial port
labmrd 1:946a27210553 231 *
labmrd 1:946a27210553 232 * @param baud_rate The desired rate in bits per second (Default 1Mbps)
labmrd 1:946a27210553 233 *
labmrd 1:946a27210553 234 * @note This doesn't broadcast anything
labmrd 1:946a27210553 235 *
labmrd 1:946a27210553 236 */
labmrd 1:946a27210553 237 void ChangeUARTBaud( int target_baud=1000000 );
labmrd 0:29900c3a4a50 238
labmrd 1:946a27210553 239 /** Read the current angle of the servo
labmrd 0:29900c3a4a50 240 *
labmrd 1:946a27210553 241 * @returns short in the range 0-4095
labmrd 1:946a27210553 242 */
labmrd 1:946a27210553 243 short GetRawPosition(void);
labmrd 1:946a27210553 244
labmrd 1:946a27210553 245
labmrd 1:946a27210553 246 /** Dump everything we know about the servo to serial.
labmrd 1:946a27210553 247 * Warning! This will take a while! At 1Mbps, it will
labmrd 1:946a27210553 248 * probably take at least 250 ms, so only do this if you
labmrd 1:946a27210553 249 * have lots of time to spare!
labmrd 1:946a27210553 250 *
labmrd 1:946a27210553 251 * @param serialObject Whichever serial object you want to print to, probably pc but maybe bluetooth
labmrd 1:946a27210553 252 */
labmrd 1:946a27210553 253 void Dump_OD_to_Serial(Serial &serialObject);
labmrd 1:946a27210553 254
labmrd 1:946a27210553 255 /** Search for all responsive Dynamixels.
labmrd 1:946a27210553 256 * Warning! This will take a considerable amount of time if scanning the whole range!
labmrd 1:946a27210553 257 * I haven't tried it, but off the top of my head it should take at least 3 seconds, probably more.
labmrd 1:946a27210553 258 *
labmrd 1:946a27210553 259 * @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 260 * @param max_id scan from zero to this value, defaults to 0xFC which is the max possible for a Dynamixel.
labmrd 0:29900c3a4a50 261 */
labmrd 1:946a27210553 262 void Scan_For_Dynamixels(bool scan_all_baud_rates=false,int max_id=0xFC);
labmrd 1:946a27210553 263
labmrd 1:946a27210553 264 /** Write a raw value to the servo
labmrd 1:946a27210553 265 *
labmrd 1:946a27210553 266 * @param OD Object Dictionary ID from the Enum
labmrd 1:946a27210553 267 * @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 268 * @returns status int
labmrd 1:946a27210553 269 */
labmrd 1:946a27210553 270 int write_short(MX12ODIndex OD,short value);
labmrd 1:946a27210553 271
labmrd 1:946a27210553 272
labmrd 1:946a27210553 273 /** Read a raw value from the servo
labmrd 0:29900c3a4a50 274 *
labmrd 1:946a27210553 275 * @param OD Object Dictionary ID from the Enum
labmrd 1:946a27210553 276 * @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 277 */
labmrd 1:946a27210553 278 short read_short(MX12ODIndex OD);
labmrd 1:946a27210553 279
labmrd 1:946a27210553 280
labmrd 1:946a27210553 281 /** Perform a coordinated move. Right now, it commands two servos with position and velocity,
labmrd 1:946a27210553 282 * but the code should be extendable to more servos/parameters as needed.
labmrd 1:946a27210553 283 *
labmrd 1:946a27210553 284 * @param OD Object Dictionary ID from the Enum
labmrd 1:946a27210553 285 * @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 286 * @returns status int
labmrd 1:946a27210553 287 */
labmrd 1:946a27210553 288 void coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1);
labmrd 0:29900c3a4a50 289
labmrd 0:29900c3a4a50 290 /** Send the broadcast "trigger" command, to activate any outstanding registered commands
labmrd 0:29900c3a4a50 291 */
labmrd 0:29900c3a4a50 292 void trigger(void);
labmrd 1:946a27210553 293
labmrd 1:946a27210553 294 /** Send a ping to a servo. No matter what it should respond. Use this to see if a servo exists.
labmrd 0:29900c3a4a50 295 *
labmrd 1:946a27210553 296 * @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 297 * @returns true if any response received, otherwise false
labmrd 0:29900c3a4a50 298 */
labmrd 1:946a27210553 299 bool ping(char ID_Num=0xFF);
labmrd 1:946a27210553 300
labmrd 0:29900c3a4a50 301 private :
labmrd 1:946a27210553 302 //// Variables
labmrd 1:946a27210553 303 Serial mx12_out;
labmrd 1:946a27210553 304 Serial mx12_in;
labmrd 5:4c118a827f11 305 DigitalOut tx_enable;
labmrd 5:4c118a827f11 306 DigitalOut rx_enable;
labmrd 1:946a27210553 307 DigitalOut profileOut;
labmrd 0:29900c3a4a50 308 int _ID;
labmrd 5:4c118a827f11 309 int _baud;
labmrd 0:29900c3a4a50 310
labmrd 1:946a27210553 311 //// Functions
labmrd 5:4c118a827f11 312 int read_raw(char* Status, int bytes=0);
labmrd 0:29900c3a4a50 313 int read(int ID, int start, int length, char* data);
labmrd 1:946a27210553 314 int write(int ID, int start, int length, char* data);
labmrd 0:29900c3a4a50 315
labmrd 0:29900c3a4a50 316 };
labmrd 0:29900c3a4a50 317
labmrd 0:29900c3a4a50 318 #endif