AX12.h

00001 /* mbed AX-12+ Servo Library
00002  *
00003  * Copyright (c) 2010, cstyles (http://mbed.org)
00004  *
00005  * Permission is hereby granted, free of charge, to any person obtaining a copy
00006  * of this software and associated documentation files (the "Software"), to deal
00007  * in the Software without restriction, including without limitation the rights
00008  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009  * copies of the Software, and to permit persons to whom the Software is
00010  * furnished to do so, subject to the following conditions:
00011  *
00012  * The above copyright notice and this permission notice shall be included in
00013  * all copies or substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021  * THE SOFTWARE.
00022  */
00023 
00024 #ifndef MBED_AX12_H
00025 #define MBED_AX12_H
00026 
00027 #include "mbed.h"
00028 
00029 #define AX12_REG_ID 0x3
00030 #define AX12_REG_BAUD 0x4
00031 #define AX12_REG_CW_LIMIT 0x06
00032 #define AX12_REG_CCW_LIMIT 0x08
00033 #define AX12_REG_GOAL_POSITION 0x1E
00034 #define AX12_REG_MOVING_SPEED 0x20
00035 #define AX12_REG_VOLTS 0x2A
00036 #define AX12_REG_TEMP 0x2B
00037 #define AX12_REG_MOVING 0x2E
00038 #define AX12_REG_POSITION 0x24
00039 #define AX12_TORQUE_ENABLE 0x18
00040 
00041 #define AX12_MODE_POSITION  0
00042 #define AX12_MODE_ROTATION  1
00043 
00044 #define AX12_CW 1
00045 #define AX12_CCW 0
00046 
00047 /** Servo control class for AX12 and other compatible Dynamixel servo
00048  *
00049  * Example:
00050  * @code
00051  * #include "mbed.h"
00052  * #include "AX12.h"
00053  * 
00054  * int main() {
00055  *   // The controlled servo has ID = 1
00056  *   // Connected to pin p9 as TX and p10 as RX
00057  *   // p11 used as toggle between TX and RX mode
00058  *   // The baudrate used to communicate with servo is 1 000 000
00059  *   AX12 myax12 (p9, p10, p11, 1, 1000000);
00060  *
00061  *   while (1) {
00062  *       myax12.SetGoal(0);    // go to 0 degrees
00063  *       wait (2.0);
00064  *       myax12.SetGoal(300);  // go to 300 degrees
00065  *       wait (2.0);
00066  *   }
00067  * }
00068  * @endcode
00069  */
00070 class AX12 {
00071 
00072 public:
00073 
00074     /** Create an AX12 servo object connected to the specified serial port, with the specified ID
00075      *
00076      * @param pin tx pin
00077      * @param pin rx pin
00078      * @param pin tx enable, to switch between tx and rx
00079      * @param int ID, the Bus ID of the servo 1-255
00080      * @param int baud, the baudrate of the servo
00081      */
00082     AX12(PinName tx, PinName rx, PinName tx_ena, int ID, int baud);
00083 
00084     /** Set the mode of the servo
00085      * @param mode
00086      *    0 = Positional, default
00087      *    1 = Continuous rotation
00088      */
00089     int SetMode(int mode);
00090 
00091     /** Set baud rate of all attached servos
00092      * @param mode
00093      *    0x01 = 1,000,000 bps
00094      *    0x03 =   500,000 bps
00095      *    0x04 =   400,000 bps
00096      *    0x07 =   250,000 bps
00097      *    0x09 =   200,000 bps
00098      *    0x10 =   115,200 bps
00099      *    0x22 =    57,600 bps
00100      *    0x67 =    19,200 bps
00101      *    0xCF =     9,600 bp
00102      */
00103     int SetBaud(int baud);
00104 
00105     /** Set goal angle in integer degrees, in positional mode
00106      *
00107      * @param degrees 0-300
00108      * @param flags, defaults to 0
00109      *    flags[0] = blocking, return when goal position reached 
00110      *    flags[1] = register, activate with a broadcast trigger
00111      *
00112      */
00113     int SetGoal(int degrees, int flags = 0);
00114 
00115     /** Set the position and speed of 18 servo from ID 1-18 at the same time
00116      *
00117      * @param degrees1..18 The position of servo with ID 1..18
00118      * @param speed1..18 The speed of servo with ID 1..18
00119      *
00120      */
00121     void MultSetGoal(int degrees1, float speed1, 
00122                         int degrees2, float speed2, 
00123                         int degrees3, float speed3, 
00124                         int degrees4, float speed4, 
00125                         int degrees5, float speed5, 
00126                         int degrees6, float speed6, 
00127                         int degrees7, float speed7, 
00128                         int degrees8, float speed8, 
00129                         int degrees9, float speed9, 
00130                         int degrees10, float speed10, 
00131                         int degrees11, float speed11,
00132                         int degrees12, float speed12, 
00133                         int degrees13, float speed13, 
00134                         int degrees14, float speed14, 
00135                         int degrees15, float speed15, 
00136                         int degrees16, float speed16, 
00137                         int degrees17, float speed17, 
00138                         int degrees18, float speed18);
00139 
00140     /** Set the speed of the servo in continuous rotation mode
00141      *
00142      * @param speed, -1.0 to 1.0
00143      *   -1.0 = full speed counter clock wise
00144      *    1.0 = full speed clock wise
00145      */
00146     int SetCRSpeed(float speed);
00147 
00148     /** Set the clockwise limit of the servo
00149      *
00150      * @param degrees, 0-300
00151      */
00152     int SetCWLimit(int degrees);
00153     
00154     /** Set the counter-clockwise limit of the servo
00155      *
00156      * @param degrees, 0-300
00157      */
00158     int SetCCWLimit(int degrees);
00159     
00160     /** Change the ID of a servo controlled
00161      *
00162      * @param ID 1-255
00163      *
00164      * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
00165      * In this situation, only one servo should be connected to the bus
00166      */
00167     void ControlID (int ID); 
00168 
00169     /** Change the ID of a servo
00170      *
00171      * @param CurentID 0-253
00172      * @param NewID 0-253
00173      *
00174      * If a servo ID is not know, the broadcast address of 254 (0xFE) can be used for CurrentID.
00175      * In this situation, only one servo should be connected to the bus
00176      */
00177     int SetID(int CurrentID, int NewID);
00178 
00179 
00180     /** Poll to see if the servo is moving
00181      *
00182      * @returns true is the servo is moving
00183      */
00184     int isMoving(void);
00185 
00186 
00187     /** Send the broadcast "trigger" command, to activate any outstanding registered commands
00188      */
00189     void trigger(void);
00190 
00191 
00192     /** Read the current angle of the servo
00193      *
00194      * @returns float in the range 0.0-300.0
00195      */
00196     float GetPosition();
00197 
00198 
00199     /** Read the temperature of the servo
00200      *
00201      * @returns float temperature 
00202      */
00203     float GetTemp(void);
00204 
00205 
00206     /** Read the supply voltage of the servo
00207      *
00208      * @returns float voltage
00209      */
00210     float GetVolts(void);
00211     
00212     /** Set torque enable of the servo
00213     */
00214     void SetTorqueEnable(char val);
00215 
00216     int read(int ID, int start, int length, char* data);
00217 
00218     int write(int ID, int start, int length, char* data, int flag=0);
00219 
00220     int sync_write(char* data); 
00221 
00222 private :
00223 
00224     Serial _ax12;
00225     DigitalOut _ena_tx;
00226     char TxBuf[10];
00227     char Status[10];
00228     static const char start_reg = 0x1E;
00229     static const char length_data = 4;
00230     static const char total_ID = 18;
00231     int _ID;
00232     int _baud_bit;
00233     int _wait_sent;
00234     int _timeout_tick;
00235     int _error_code;
00236     char _data[2];
00237     char _reg_flag;
00238 };
00239 
00240 #endif