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@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?

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 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