MX.h

00001 /* mbed MX-64 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_MX_H
00025 #define MBED_MX_H
00026 
00027 #include "mbed.h"
00028 #include "UARTSerial_half.h"
00029 
00030 //#define SENDPACKET_DEBUG 1
00031 //#define SENDPACKET_DEBUG_INSTRUCTION_PKT 1
00032 //#define SENDPACKET_DEBUG_STATUS_PKT 1
00033 //#define READ_DEBUG 1
00034 //#define READ_DEBUG_INSTRUCTION_PKT 1
00035 //#define READ_DEBUG_STATUS_PKT 1
00036 //#define GETPOSITION_DEBUG 1
00037 
00038 //#define TORQUE_ENABLE_DEBUGDEBUG 1
00039 //#define REBOOT_ENABLE_DEBUG
00040 //#define SETGOAL_DEBUG 1 
00041 //#define SETGOAL_SPEED_DEBUG 1
00042 //#define OPERATING_MODE_DEBUG 1
00043 //#define SETBAUD_DEBUG 1
00044 
00045 #define SYNC_TORQUE_ENABLE_DEBUG 0
00046 #define SYNC_SETGOAL_DEBUG 0
00047 #define SYNC_GET_POSITION_DEBUG 0
00048 #define SYNC_SENDPACKET_DEBUG 0
00049 #define SYNC_SENDPACKET_DEBUG_PACKETONLY 0
00050 #define SYNC_READPACKET_DEBUG_PACKETONLY 0
00051 #define SYNC_SET_BAUD_DEBUG 0
00052 #define SYNC_GET_CURRENT_DEBUG 0
00053 #define SYNC_GOAL_CURRENT_DEBUG 0  
00054 #define SYNC_OPERATING_MODE_DEBUG 0
00055 
00056   // Protocol 2.0 
00057 // N.B. all Register values are in HEX, on the on-line manual all are in DEC
00058 #define MX_REG_ID 0x7
00059 #define MX_REG_BAUD 0x8
00060 #define MX_REG_CW_LIMIT 0x30
00061 #define MX_REG_CCW_LIMIT 0x34
00062 #define MX_REG_GOAL_POSITION 0x74
00063 #define MX_REG_GOAL_VELOCITY 0x68
00064 #define MX_REG_PRESENT_INPUT_VOLT 0x90
00065 #define MX_REG_PRESENT_TEMP 0x92
00066 #define MX_REG_MOVING 0x7A
00067 #define MX_REG_PRESENT_POSITION 0x84
00068 #define MX_REG_TORQUE_ENABLE 0x40
00069 #define MX_REG_OPERATING_MODE 0xB
00070 #define MX_REG_PRESENT_CURRENT 0x7E
00071 #define MX_REG_GOAL_CURRENT 0x66
00072 #define MX_REG_CURRENT_LIMIT 0x26
00073 #define MX_REG_GOAL_PWM 0x64
00074 #define MX_REG_PROFILE_ACCELER 0x6C
00075 #define MX_REG_PROFILE_VELOCITY 0x70
00076 #define MX_OFF 0
00077 #define MX_ON 1
00078 
00079 class MX{
00080 
00081 
00082 public:
00083 
00084     //standard constructor
00085     MX();           
00086     //constructor                                  
00087     MX(int*broadcastID, int Nmotor, int MotorBaud, UARTSerial_half* pCommLayer);   
00088     //constructor 2
00089     MX(int ID, int MotorBaud, UARTSerial_half* pCommLayer);    
00090      
00091     /** Set the mode of the servo
00092      * @param mode
00093      *
00094      *
00095      */
00096     int OperatingMode (int mode);
00097 
00098     /** Set baud rate of all attached servos
00099      * @param mode
00100      *    0x00 =     9,600  bps
00101      *    0x01 =    57,600  bps (DEFAULT)
00102      *    0x02 =    115,200 bps
00103      *    0x03 =    1       Mbps
00104      *    0x04 =    2       Mbps
00105      *    0x05 =    3       Mbps
00106      *    0x06 =    4       Mbps
00107      *    0x06 =    4,5       Mbps
00108      */
00109     int motorSetBaud(int MotorBaud);
00110      
00111     /** Set Torque enable 
00112     * enableVal = 1 Torque On  
00113     * enableVal = 0 Torque Off 
00114     **/
00115     int TorqueEnable(bool enable);
00116     
00117     /** Set goal angle in integer degrees, in positional mode
00118      *
00119      * @param degrees 0-300
00120      * @param flags, defaults to 0
00121      *    flags[0] = blocking, return when goal position reached 
00122      *    flags[1] = register, activate with a broadcast trigger
00123      *
00124      */
00125     int SetGoal(float degrees, int flags = 0);
00126     /** Set goal angle in integer degrees, in positional mode
00127      *
00128      * @param degrees 0-360
00129      * @param speed 0-100
00130      * @param flags, defaults to 0
00131      *    flags[0] = blocking, return when goal position reached 
00132      *    flags[1] = register, activate with a broadcast trigger
00133      *
00134      */
00135     int SetGoalSpeed(int speed, int flags = 0);
00136     
00137 
00138     /** Set the speed of the servo in continuous rotation mode
00139      *
00140      * @param speed, -1.0 to 1.0
00141      *   -1.0 = full speed counter clock wise
00142      *    1.0 = full speed clock wise
00143      */
00144     int SetCRSpeed(float speed);
00145 
00146 
00147     /** Set the clockwise limit of the servo
00148      *
00149      * @param degrees, 0-360
00150      */
00151     int SetCWLimit(int degrees);
00152     
00153     /** Set the counter-clockwise limit of the servo
00154      *
00155      * @param degrees, 0-360
00156      */
00157     int SetCCWLimit(int degrees);
00158 
00159     // Change the ID
00160 
00161     /** Change the ID of a servo
00162      *
00163      * @param CurentID 1-255
00164      * @param NewID 1-255
00165      *
00166      * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
00167      * In this situation, only one servo should be connected to the bus
00168      */
00169     int SetID(int CurrentID, int NewID);
00170 
00171 
00172     /** Poll to see if the servo is moving
00173      *
00174      * @returns true is the servo is moving
00175      */
00176     bool isMoving(void);
00177 
00178     /** Send the broadcast "trigger" command, to activate any outstanding registered commands
00179      */
00180     void trigger(void);
00181 
00182     /** Read the current angle of the servo
00183      *
00184      * @returns float in the range 0.0-300.0
00185      */
00186     float GetPosition();
00187 
00188     /** Read the temperature of the servo
00189      *
00190      * @returns float temperature 
00191      */
00192     float GetTemp(void);
00193 
00194     /** Read the supply voltage of the servo
00195      *
00196      * @returns float voltage
00197      */
00198     float GetVolts(void);
00199     /** ENABLE 
00200      *
00201      * @returns float voltage
00202      */
00203     
00204     char* readPacket( int start, int length, int flag=0);
00205     int sendPacket(int start, int length, char* data, int flag=0);    
00206     void Reboot();
00207     
00208     void SyncReadPacket(int start, int bytes, char* data); 
00209     void SyncGetPosition(float* angle);
00210     void SyncGetCurrent(float* current);
00211     int SyncTorqueEnable(bool enableVal[], int Nmotor =-1);
00212     int SyncSetGoal(float degrees[]);
00213     int SyncSetGoal(float degrees, int ID);
00214     int SyncSendPacket(int start, int bytes, char* data, int ID =-1);
00215     int SyncGoalCurrent(float mAmpere, int ID); 
00216     int SyncGoalCurrent(float mAmpere[]);
00217     int SyncCurrentLimit(float mAmpere, int ID);
00218     int SyncCurrentLimit(float mAmpere[]);
00219     int SyncOperatingMode(int mode[], int ID =-1);
00220     int SyncSetBaud(int MotorBaud[], int ID =-1 );
00221     int SyncGoalPWM(float values[], int ID =-1 );
00222     int SyncProfileAccel(float profileValAcc[]);
00223     int SyncProfileVel(float profileValueVel[]);
00224        
00225 private :
00226 
00227     UARTSerial_half* _pCommLayer;
00228     int _ID;
00229     int _MotorBaud;
00230     int* _broadcastID;
00231     int _Nmotor;
00232     
00233     unsigned short update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size);
00234 };
00235 
00236 #endif