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);
 
 };