herkulex.h

00001 //------------------------------------------------------------------------------
00002 /* herkulex servo library for mbed
00003  *
00004  * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
00005  *
00006  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
00007  * and associated documentation files (the "Software"), to deal in the Software without restriction, 
00008  * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
00009  * sublicense, and/or sell 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 all copies or 
00013  * substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
00016  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
00017  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
00018  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00020  */
00021 //------------------------------------------------------------------------------
00022 #ifndef MBED_HERKULEX_H
00023 #define MBED_HERKULEX_H
00024 
00025 //------------------------------------------------------------------------------
00026 #include "mbed.h"
00027 
00028 //------------------------------------------------------------------------------
00029 #define HERKULEX_DEBUG
00030 
00031 //------------------------------------------------------------------------------
00032 // Herkulex ROM Register
00033 #define ROM_MODEL_NO1                       0
00034 #define ROM_MODEL_NO2                       1
00035 #define ROM_VERSION1                        2
00036 #define ROM_VERSION2                        3
00037 #define ROM_BAUD_RATE                       4
00038 #define ROM_RESERVED5                       5
00039 #define ROM_ID                              6
00040 #define ROM_ACK_POLICY                      7
00041 #define ROM_ALARM_LED_POLICY                8
00042 #define ROM_TORQUE_POLICY                   9
00043 #define ROM_RESERVED10                      10
00044 #define ROM_MAX_TEMPERATURE                 11
00045 #define ROM_MIN_VOLTAGE                     12
00046 #define ROM_MAX_VOLTAGE                     13
00047 #define ROM_ACCELERATION_RATIO              14
00048 #define ROM_MAX_ACCELERATION_TIME           15
00049 #define ROM_DEAD_ZONE                       16
00050 #define ROM_SATURATOR_OFFSET                17
00051 #define ROM_SATURATOR_SLOPE                 18  // 2Byte
00052 #define ROM_PWM_OFFSET                      20
00053 #define ROM_MIN_PWM                         21
00054 #define ROM_MAX_PWM                         22  // 2Byte
00055 #define ROM_OVERLOAD_PWM_THRESHOLD          24  // 2Byte
00056 #define ROM_MIN_POSITION                    26  // 2Byte
00057 #define ROM_MAX_POSITION                    28  // 2Byte
00058 #define ROM_POSITION_KP                     30  // 2Byte
00059 #define ROM_POSITION_KD                     32  // 2Byte
00060 #define ROM_POSITION_KI                     34  // 2Byte
00061 #define ROM_POSITION_FEEDFORWARD_1ST_GAIN   36  // 2Byte
00062 #define ROM_POSITION FEEDFORWARD_2ND_GAIN   38  // 2Byte
00063 #define ROM_RESERVED40                      40  // 2Byte
00064 #define ROM_RESERVED42                      42  // 2Byte
00065 #define ROM_LED_BLINK_PERIOD                44
00066 #define ROM_ADC_FAULT_CHECK_PERIOD          45
00067 #define ROM_PACKET_GARBAGE_CHECK_PERIOD     46
00068 #define ROM_STOP_DETECTION_PERIOD           47
00069 #define ROM_OVERLOAD_DETECTION_PERIOD       48   
00070 #define ROM_STOP_THRESHOLD                  49
00071 #define ROM_INPOSITION_MARGIN               50
00072 #define ROM_RESERVED51                      51
00073 #define ROM_RESERVED52                      52
00074 #define ROM_CALIBRATION_DIFFERENCE          53
00075 
00076 //------------------------------------------------------------------------------
00077 // Herkulex RAM Register
00078 #define RAM_ID                              0
00079 #define RAM_ACK_POLICY                      1
00080 #define RAM_ALARM_LED_POLICY                2
00081 #define RAM_TORQUE_POLICY                   3
00082 #define RAM_RESERVED4                       4
00083 #define RAM_MAX_TEMPERATURE                 5
00084 #define RAM_MIN_VOLTAGE                     6
00085 #define RAM_MAX_VOLTAGE                     7
00086 #define RAM_ACCELERATION_RATIO              8
00087 #define RAM_MAX_ACCELERATION                9
00088 #define RAM_DEAD_ZONE                       10
00089 #define RAM_SATURATOR_OFFSET                11
00090 #define RAM_SATURATOR_SLOPE                 12 // 2Byte
00091 #define RAM_PWM_OFFSET                      14
00092 #define RAM_MIN_PWM                         15
00093 #define RAM_MAX_PWM                         16 // 2Byte
00094 #define RAM_OVERLOAD_PWM_THRESHOLD          18 // 2Byte
00095 #define RAM_MIN_POSITION                    20 // 2Byte
00096 #define RAM_MAX_POSITION                    22 // 2Byte
00097 #define RAM_POSITION_KP                     24 // 2Byte
00098 #define RAM_POSITION_KD                     26 // 2Byte
00099 #define RAM_POSITION_KI                     28 // 2Byte
00100 #define RAM_POSITION_FEEDFORWARD_1ST_GAIN   30 // 2Byte
00101 #define RAM_POSITION_FEEDFORWARD 2ND GAIN   32 // 2Byte
00102 #define RAM_RESERVED34                      34 // 2Byte
00103 #define RAM_RESERVED36                      36 // 2Byte
00104 #define RAM_LED_BLINK_PERIOD                38
00105 #define RAM_ADC_FAULT_DETECTION_PERIOD      39
00106 #define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40
00107 #define RAM_STOP_DETECTION_PERIOD           41
00108 #define RAM_OVERLOAD_DETECTION_PERIOD       42
00109 #define RAM_STOP_THRESHOLD                  43
00110 #define RAM_INPOSITION_MARGIN               44
00111 #define RAM_RESERVED45                      45
00112 #define RAM_RESERVED46                      46
00113 #define RAM_CALIBRATION_DIFFERENCE          47
00114 #define RAM_STATUS_ERROR                    48
00115 #define RAM_STATUS_DETAIL                   49
00116 #define RAM_RESERVED50                      50
00117 #define RAM_RESERVED51                      51 
00118 #define RAM_TORQUE_CONTROL                  52
00119 #define RAM_LED_CONTROL                     53
00120 #define RAM_VOLTAGE                         54
00121 #define RAM_TEMPERATURE                     55
00122 #define RAM_CURRENT_CONTROL_MODE            56
00123 #define RAM_TICK                            57
00124 #define RAM_CALIBRATED_POSITION             58 // 2Byte
00125 #define RAM_ABSOLUTE_POSITION               60 // 2Byte
00126 #define RAM_DIFFERENTIAL_POSITION           62 // 2Byte
00127 #define RAM_PWM                             64 // 2Byte
00128 #define RAM_RESERVED66                      66 // 2Byte
00129 #define RAM_ABSOLUTE_GOAL_POSITION          68 // 2Byte
00130 #define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION    70 // 2Byte
00131 #define RAM_DESIRED_VELOCITY                72 // 2Byte
00132 
00133 //------------------------------------------------------------------------------
00134 // Request Packet [To Servo Module] 
00135 #define CMD_ROM_WRITE  0x01    // Write Length number of values to EEP Register Address
00136 #define CMD_ROM_READ   0x02    // Request Length number of values from EEP Register Address
00137 #define CMD_RAM_WRITE  0x03    // Write Length number of values to RAM Register Address
00138 #define CMD_RAM_READ   0x04    // Request Lenght number of values from RAM Register Address
00139 #define CMD_I_JOG      0x05    // Able to send JOG command to maximum 43 servos (operate timing of individual Servo)
00140 #define CMD_S_JOG      0x06    // Able to send JOG command to maximum 53 servos (operate simultaneously at same time)
00141 #define CMD_STAT       0x07    // Status Error, Status Detail request
00142 #define CMD_ROLLBACK   0x08    // Change all EEP Regsters to Factory Default value
00143 #define CMD_REBOOT     0x09    // Request Reboot
00144 
00145 //------------------------------------------------------------------------------
00146 // ACK Packet [To Controller(ACK)]
00147 #define CMD_ACK_MASK   0x40 // ACK Packet CMD is Request Packet CMD + 0x40
00148 #define CMD_EEP_WRITE_ACK   (CMD_EEP_WRITE|CMD_ACK_MASK)
00149 #define CMD_EEP_READ_ACK    (CMD_EEP_READ|CMD_ACK_MASK)
00150 #define CMD_RAM_WRITE_ACK   (CMD_RAM_WRITE|CMD_ACK_MASK)
00151 #define CMD_RAM_READ_ACK    (CMD_RAM_READ|CMD_ACK_MASK)
00152 #define CMD_I_JOG_ACK       (CMD_I_JOG|CMD_ACK_MASK)
00153 #define CMD_S_JOG_ACK       (CMD_S_JOG|CMD_ACK_MASK)
00154 #define CMD_STAT_ACK        (CMD_STAT|CMD_ACK_MASK)
00155 #define CMD_ROLLBACK_ACK    (CMD_ROLLBACK|CMD_ACK_MASK)
00156 #define CMD_REBOOT_ACK      (CMD_REBOOT|CMD_ACK_MASK)
00157 
00158 //------------------------------------------------------------------------------
00159 // Status Error
00160 #define STATUS_OK                       = 0x00;
00161 #define ERROR_EXCEED_INPUT_VOLTAGE      = 0x01;
00162 #define ERROR_EXCEED_POT_LIMIT          = 0x02;
00163 #define ERROR_EXCEED_TEMPERATURE_LIMIT  = 0x04;
00164 #define ERROR_INVALID_PACKET            = 0x08;
00165 #define ERROR_OVERLOAD                  = 0x10;
00166 #define ERROR_DRIVER_FAULT              = 0x20;
00167 #define ERROR_EEP_REG_DISTORT           = 0x40;
00168 
00169 //------------------------------------------------------------------------------
00170 // Status Detail
00171 #define MOVING_FLAG                     = 0x01;
00172 #define INPOSITION_FLAG                 = 0x02;
00173 #define CHECKSUM_ERROR                  = 0x04; // Invalid packet`s detailed information
00174 #define UNKNOWN_COMMAND                 = 0x08; // Invalid packet`s detailed information
00175 #define EXCEED_REG_RANGE                = 0x10; // Invalid packet`s detailed information
00176 #define GARBAGE_DETECTED                = 0x20; // Invalid packet`s detailed information
00177 #define MOTOR_ON_FLAG                   = 0x40;
00178 
00179 //------------------------------------------------------------------------------
00180 // Header
00181 #define HEADER                              0xFF
00182 
00183 // Size
00184 #define MIN_PACKET_SIZE                     7
00185 #define MIN_ACK_PACKET_SIZE                 9
00186 #define WRITE_PACKET_SIZE                   13
00187 #define MAX_PACKET_SIZE                     223
00188 #define MAX_DATA_SIZE                       (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
00189 
00190 // ID
00191 #define MAX_PID                             0xFD
00192 #define DEFAULT_ID                          0xFD
00193 #define MAX_ID                              0xFD
00194 #define BROADCAST_ID                        0xFE
00195 
00196 // Checksum
00197 #define CHKSUM_MASK                         0xFE
00198 
00199 // Torque CMD
00200 #define TORQUE_FREE                         0x00
00201 #define BREAK_ON                            0x40
00202 #define TORQUE_ON                           0x60
00203 
00204 // Register Size
00205 #define BYTE1                               1
00206 #define BYTE2                               2
00207 
00208 // Jog Set CMD
00209 #define STOP                                0x01
00210 #define POS_MODE                            0x00
00211 #define TURN_MODE                           0x02
00212 #define GLED_ON                             0x04
00213 #define BLED_ON                             0x08
00214 #define RLED_ON                             0x10
00215 
00216 //------------------------------------------------------------------------------
00217 /** herkulex Servo control class, based packet protocol on a serial
00218  *
00219  * Example:
00220  * @code
00221  * #include "mbed.h"
00222  * #include "herkulex.h"
00223  *
00224  * Herkulex sv(p9, p10, 115200);
00225  * DigitalOut led(LED1);
00226  *
00227  * int main() 
00228  * {   
00229  *     wait(1);
00230  *     sv.setTorque(0xFD, TORQUE_ON);
00231  *     while(1) 
00232  *     {
00233  *         led=1;
00234  *         sv.movePos(0xFD, 1002, 100, SET_MODE_POS, SET_LED_GREEN_ON);
00235  *         wait(3);
00236  *         
00237  *         led=0;       
00238  *         sv.movePos(0xFD, 21, 100, SET_MODE_POS, SET_LED_BLUE_ON);
00239  *         wait(3);
00240  *     }
00241  * }
00242  * @endcode
00243  */
00244 //------------------------------------------------------------------------------
00245 class Herkulex
00246 {
00247 public:
00248 
00249     /** Create an Herkulex servo object connected to the serial pins and baudrate
00250      *
00251      * @param tx Transmit pin.
00252      * @param rx Receive pin. 
00253      * @param baudRate The serial tx/rx speed. 
00254      */
00255     Herkulex(PinName tx, PinName rx, uint32_t baudRate);
00256     
00257     /** Destroy an Herkulex servo object
00258      */
00259     ~Herkulex(); 
00260 
00261     /** Transmit packet datas
00262      *        
00263      * @param packetSize The packet size.
00264      * @param data The transmit packet data array.
00265      */  
00266     void txPacket(uint8_t packetSize, uint8_t* data);
00267     
00268     /** Receive packet datas
00269      *        
00270      * @param packetSize The packet size.
00271      * @param data The receive packet data array.
00272      */ 
00273     void rxPacket(uint8_t packetSize, uint8_t* data);
00274     
00275     /** Clear error status
00276      *        
00277      * @param id The herkulex servo ID.
00278      */      
00279      void clear(uint8_t id);
00280     
00281     /** Set torque setting
00282      *        
00283      * @param id The herkulex servo ID.
00284      * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
00285      */      
00286     void setTorque(uint8_t id, uint8_t cmdTorue);
00287     
00288     /** Position Control
00289      *        
00290      * @param id The herkulex servo ID.
00291      * @param position The goal position of herkulex servo.
00292      * @param playtime Time to target position.
00293      * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
00294      */          
00295     void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED);
00296 
00297     /** Velocity Control
00298      *        
00299      * @param id The herkulex servo ID.
00300      * @param speed The goal position of herkulex servo.
00301      * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
00302      */             
00303     void velocityControl(uint8_t id, int16_t speed,uint8_t setLED);
00304     
00305     /** Get Status
00306      *        
00307      * @param id The herkulex servo ID.
00308      * @return -1 is getStatus failed. other is servo`s status error value.
00309      */       
00310     int8_t getStatus(uint8_t id);
00311     
00312     /** Get Position
00313      *        
00314      * @param id The herkulex servo ID.
00315      * @return -1 is getPos failed. other is servo's current position.
00316      */         
00317     int16_t getPos(uint8_t id);
00318          
00319 private :
00320 
00321     /** PC serial connection used in debug mode.
00322     */
00323     Serial *pc;
00324     
00325     /** Serial connection (mbed <-> herkulex).
00326     */  
00327     Serial *txd, *rxd;
00328 };
00329 
00330 //------------------------------------------------------------------------------
00331 #endif  // MBED_HERKULEX_H
00332 
00333 //------------------------------------------------------------------------------