fonctions_herkulex.h

00001 #ifndef LIB_HERKULEX_H
00002 #define LIB_HERKULEX_H
00003 #include "mbed.h"
00004 
00005 //#define ID 0xFD     //=253
00006 
00007 // Herkulex ROM Register
00008 #define ROM_MODEL_NO1                       0
00009 #define ROM_MODEL_NO2                       1
00010 #define ROM_VERSION1                        2
00011 #define ROM_VERSION2                        3
00012 #define ROM_BAUD_RATE                       4
00013 #define ROM_RESERVED5                       5
00014 #define ROM_ID                              6
00015 #define ROM_ACK_POLICY                      7
00016 #define ROM_ALARM_LED_POLICY                8
00017 #define ROM_TORQUE_POLICY                   9
00018 #define ROM_RESERVED10                      10
00019 #define ROM_MAX_TEMPERATURE                 11
00020 #define ROM_MIN_VOLTAGE                     12
00021 #define ROM_MAX_VOLTAGE                     13
00022 #define ROM_ACCELERATION_RATIO              14
00023 #define ROM_MAX_ACCELERATION_TIME           15
00024 #define ROM_DEAD_ZONE                       16
00025 #define ROM_SATURATOR_OFFSET                17
00026 #define ROM_SATURATOR_SLOPE                 18  // 2Byte
00027 #define ROM_PWM_OFFSET                      20
00028 #define ROM_MIN_PWM                         21
00029 #define ROM_MAX_PWM                         22  // 2Byte
00030 #define ROM_OVERLOAD_PWM_THRESHOLD          24  // 2Byte
00031 #define ROM_MIN_POSITION                    26  // 2Byte
00032 #define ROM_MAX_POSITION                    28  // 2Byte
00033 #define ROM_POSITION_KP                     30  // 2Byte
00034 #define ROM_POSITION_KD                     32  // 2Byte
00035 #define ROM_POSITION_KI                     34  // 2Byte
00036 #define ROM_POSITION_FEEDFORWARD_1ST_GAIN   36  // 2Byte
00037 #define ROM_POSITION FEEDFORWARD_2ND_GAIN   38  // 2Byte
00038 #define ROM_RESERVED40                      40  // 2Byte
00039 #define ROM_RESERVED42                      42  // 2Byte
00040 #define ROM_LED_BLINK_PERIOD                44
00041 #define ROM_ADC_FAULT_CHECK_PERIOD          45
00042 #define ROM_PACKET_GARBAGE_CHECK_PERIOD     46
00043 #define ROM_STOP_DETECTION_PERIOD           47
00044 #define ROM_OVERLOAD_DETECTION_PERIOD       48
00045 #define ROM_STOP_THRESHOLD                  49
00046 #define ROM_INPOSITION_MARGIN               50
00047 #define ROM_RESERVED51                      51
00048 #define ROM_RESERVED52                      52
00049 #define ROM_CALIBRATION_DIFFERENCE          53
00050 
00051 //------------------------------------------------------------------------------
00052 // Herkulex RAM Register
00053 #define RAM_ID                              0
00054 #define RAM_ACK_POLICY                      1
00055 #define RAM_ALARM_LED_POLICY                2
00056 #define RAM_TORQUE_POLICY                   3
00057 #define RAM_RESERVED4                       4
00058 #define RAM_MAX_TEMPERATURE                 5
00059 #define RAM_MIN_VOLTAGE                     6
00060 #define RAM_MAX_VOLTAGE                     7
00061 #define RAM_ACCELERATION_RATIO              8
00062 #define RAM_MAX_ACCELERATION                9
00063 #define RAM_DEAD_ZONE                       10
00064 #define RAM_SATURATOR_OFFSET                11
00065 #define RAM_SATURATOR_SLOPE                 12 // 2Byte
00066 #define RAM_PWM_OFFSET                      14
00067 #define RAM_MIN_PWM                         15
00068 #define RAM_MAX_PWM                         16 // 2Byte
00069 #define RAM_OVERLOAD_PWM_THRESHOLD          18 // 2Byte
00070 #define RAM_MIN_POSITION                    20 // 2Byte
00071 #define RAM_MAX_POSITION                    22 // 2Byte
00072 #define RAM_POSITION_KP                     24 // 2Byte
00073 #define RAM_POSITION_KD                     26 // 2Byte
00074 #define RAM_POSITION_KI                     28 // 2Byte
00075 #define RAM_POSITION_FEEDFORWARD_1ST_GAIN   30 // 2Byte
00076 #define RAM_POSITION_FEEDFORWARD 2ND GAIN   32 // 2Byte
00077 #define RAM_RESERVED34                      34 // 2Byte
00078 #define RAM_RESERVED36                      36 // 2Byte
00079 #define RAM_LED_BLINK_PERIOD                38
00080 #define RAM_ADC_FAULT_DETECTION_PERIOD      39
00081 #define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40
00082 #define RAM_STOP_DETECTION_PERIOD           41
00083 #define RAM_OVERLOAD_DETECTION_PERIOD       42
00084 #define RAM_STOP_THRESHOLD                  43
00085 #define RAM_INPOSITION_MARGIN               44
00086 #define RAM_RESERVED45                      45
00087 #define RAM_RESERVED46                      46
00088 #define RAM_CALIBRATION_DIFFERENCE          47
00089 #define RAM_STATUS_ERROR                    48
00090 #define RAM_STATUS_DETAIL                   49
00091 #define RAM_RESERVED50                      50
00092 #define RAM_RESERVED51                      51
00093 #define RAM_TORQUE_CONTROL                  52
00094 #define RAM_LED_CONTROL                     53
00095 #define RAM_VOLTAGE                         54
00096 #define RAM_TEMPERATURE                     55
00097 #define RAM_CURRENT_CONTROL_MODE            56
00098 #define RAM_TICK                            57
00099 #define RAM_CALIBRATED_POSITION             58 // 2Byte
00100 #define RAM_ABSOLUTE_POSITION               60 // 2Byte
00101 #define RAM_DIFFERENTIAL_POSITION           62 // 2Byte
00102 #define RAM_PWM                             64 // 2Byte
00103 #define RAM_RESERVED66                      66 // 2Byte
00104 #define RAM_ABSOLUTE_GOAL_POSITION          68 // 2Byte
00105 #define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION    70 // 2Byte
00106 #define RAM_DESIRED_VELOCITY                72 // 2Byte
00107 
00108 //------------------------------------------------------------------------------
00109 // Request Packet [To Servo Module]
00110 #define CMD_ROM_WRITE  0x01    // Write Length number of values to EEP Register Address
00111 #define CMD_ROM_READ   0x02    // Request Length number of values from EEP Register Address
00112 #define CMD_RAM_WRITE  0x03    // Write Length number of values to RAM Register Address
00113 #define CMD_RAM_READ   0x04    // Request Lenght number of values from RAM Register Address
00114 #define CMD_I_JOG      0x05    // Able to send JOG command to maximum 43 servos (operate timing of individual Servo)
00115 #define CMD_S_JOG      0x06    // Able to send JOG command to maximum 53 servos (operate simultaneously at same time)
00116 #define CMD_STAT       0x07    // Status Error, Status Detail request
00117 #define CMD_ROLLBACK   0x08    // Change all EEP Regsters to Factory Default value
00118 #define CMD_REBOOT     0x09    // Request Reboot
00119 
00120 //------------------------------------------------------------------------------
00121 // ACK Packet [To Controller(ACK)]
00122 #define CMD_ACK_MASK   0x40 // ACK Packet CMD is Request Packet CMD + 0x40
00123 #define CMD_EEP_WRITE_ACK   (CMD_EEP_WRITE|CMD_ACK_MASK)
00124 #define CMD_EEP_READ_ACK    (CMD_EEP_READ|CMD_ACK_MASK)
00125 #define CMD_RAM_WRITE_ACK   (CMD_RAM_WRITE|CMD_ACK_MASK)
00126 #define CMD_RAM_READ_ACK    (CMD_RAM_READ|CMD_ACK_MASK)
00127 #define CMD_I_JOG_ACK       (CMD_I_JOG|CMD_ACK_MASK)
00128 #define CMD_S_JOG_ACK       (CMD_S_JOG|CMD_ACK_MASK)
00129 #define CMD_STAT_ACK        (CMD_STAT|CMD_ACK_MASK)
00130 #define CMD_ROLLBACK_ACK    (CMD_ROLLBACK|CMD_ACK_MASK)
00131 #define CMD_REBOOT_ACK      (CMD_REBOOT|CMD_ACK_MASK)
00132 
00133 //------------------------------------------------------------------------------
00134 // Status Error
00135 #define STATUS_OK                       = 0x00;
00136 #define ERROR_EXCEED_INPUT_VOLTAGE      = 0x01;
00137 #define ERROR_EXCEED_POT_LIMIT          = 0x02;
00138 #define ERROR_EXCEED_TEMPERATURE_LIMIT  = 0x04;
00139 #define ERROR_INVALID_PACKET            = 0x08;
00140 #define ERROR_OVERLOAD                  = 0x10;
00141 #define ERROR_DRIVER_FAULT              = 0x20;
00142 #define ERROR_EEP_REG_DISTORT           = 0x40;
00143 
00144 //------------------------------------------------------------------------------
00145 // Status Detail
00146 #define MOVING_FLAG                     = 0x01;
00147 #define INPOSITION_FLAG                 = 0x02;
00148 #define CHECKSUM_ERROR                  = 0x04; // Invalid packet`s detailed information
00149 #define UNKNOWN_COMMAND                 = 0x08; // Invalid packet`s detailed information
00150 #define EXCEED_REG_RANGE                = 0x10; // Invalid packet`s detailed information
00151 #define GARBAGE_DETECTED                = 0x20; // Invalid packet`s detailed information
00152 #define MOTOR_ON_FLAG                   = 0x40;
00153 
00154 //------------------------------------------------------------------------------
00155 // Header
00156 #define HEADER                              0xFF
00157 
00158 // Size
00159 #define MIN_PACKET_SIZE                     7
00160 #define MIN_ACK_PACKET_SIZE                 9
00161 #define WRITE_PACKET_SIZE                   13
00162 #define MAX_PACKET_SIZE                     223
00163 #define MAX_DATA_SIZE                       (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
00164 
00165 // ID
00166 #define MAX_PID                             0xFD
00167 #define DEFAULT_ID                          0xFD
00168 #define MAX_ID                              0xFD
00169 #define BROADCAST_ID                        0xFE
00170 
00171 // Checksum
00172 #define CHKSUM_MASK                         0xFE
00173 
00174 // Torque CMD
00175 #define TORQUE_FREE                         0x00
00176 #define BREAK_ON                            0x40
00177 #define TORQUE_ON                           0x60
00178 
00179 // Register Size
00180 #define BYTE1                               1
00181 #define BYTE2                               2
00182 
00183 // Jog Set CMD
00184 #define STOP                                0x01
00185 #define POS_MODE                            0x00
00186 #define TURN_MODE                           0x02
00187 #define GLED_ON                             0x04
00188 #define BLED_ON                             0x08
00189 #define RLED_ON                             0x10
00190 
00191 //------------------------------------------------------------------------------
00192 //------------------------------------------a choisir---------------------------
00193 #define nombre_servomoteur 4                                                   //
00194 
00195 
00196 
00197 //------------------------------------------------------------------------------
00198 
00199 /** Create an Herkulex servo object connected to the serial pins and baudrate
00200  *
00201  * @param tx Transmit pin.
00202  * @param rx Receive pin.
00203  * @param baudRate The serial tx/rx speed.
00204  */
00205 // void Herkulex(PinName tx, PinName rx, uint32_t baudRate);
00206 
00207 /** Destroy an Herkulex servo object
00208  */
00209 //void N_Herkulex();
00210 
00211 /** Transmit packet datas
00212  *
00213  * @param packetSize The packet size.
00214  * @param data The transmit packet data array.
00215  */
00216 void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial);
00217 
00218 /** Receive packet datas
00219  *
00220  * @param packetSize The packet size.
00221  * @param data The receive packet data array.
00222  */
00223 void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial);
00224 
00225 /** Clear error status
00226  *
00227  * @param id The herkulex servo ID.
00228  */
00229 void clear(uint8_t id, uint8_t numero_serial);
00230 
00231 /** Set torque setting
00232  *
00233  * @param id The herkulex servo ID.
00234  * @param cmdTorque The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
00235  */
00236 void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial);
00237 //------------------------------Déplacements-------------------------------------
00238 /** Position Control
00239  *
00240  * @param id The herkulex servo ID.
00241  * @param position The goal position of herkulex servo.
00242  * @param playtime Time to target position.
00243  * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
00244  */
00245 void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial);
00246 
00247 /** Velocity Control
00248  *
00249  * @param id The herkulex servo ID.
00250  * @param speed The goal position of herkulex servo.
00251  * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
00252  */
00253 void velocityControl(uint8_t id, int16_t speed,uint8_t setLED, uint8_t numero_serial);
00254 /**
00255  *
00256  *même paramètres que positionControl();
00257  *permet de donner des instructions à deux servomoteurs simultanéments
00258  */
00259 void positionControl_Mul_ensemble(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t setLED2, uint8_t numero_serial);
00260 /**
00261  *
00262  *même paramètres que positionControl_Mul_ensemble();
00263  *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent
00264  */
00265 void positionControl_Mul_playtime_different(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED,uint8_t id2, uint16_t position2, uint8_t playtime2, uint8_t setLED2, uint8_t numero_serial);
00266 /**
00267  *
00268  *
00269  *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent
00270  */
00271 void positionControl_Mul_ensemble_complex(uint8_t nb_servo, uint8_t playtime, uint8_t* data, uint16_t* pos, uint8_t numero_serial); // uint16_t position, uint8_t setLED, uint8_t id
00272 /**
00273  *
00274  *même paramètres que positionControl_Mul_ensemble();
00275  *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent
00276  */
00277 void positionControl_Mul_ensemble_different_complex(uint8_t nb_servo, uint8_t* data, uint16_t* pos, uint8_t numero_serial); // uint16_t position, uint8_t setLED, uint8_t id, uint8_t playtime
00278 
00279 
00280 void compteTour(int ID,int16_t speed,uint8_t tour, uint16_t position,uint8_t setLED,uint8_t serial);
00281 
00282 //-------------------------------------Demandes d'informations
00283 /** Get Status
00284  *
00285  * @param id The herkulex servo ID.
00286  * @return -1 is getStatus failed. other is servo`s status error value.
00287  */
00288 int8_t getStatus(uint8_t id, uint8_t numero_serial);
00289 
00290 /** Get Position
00291  *
00292  * @param id The herkulex servo ID.
00293  * @return -1 is getPos failed. other is servo's current position.
00294  */
00295 int16_t getPos(uint8_t id, uint8_t numero_serial);
00296 
00297 //void recevoir_irq(void);
00298 
00299 void receive_Sv(void);
00300 bool verification();
00301 void verification_3_bras(uint8_t sens_avant);
00302 
00303 int8_t Get_Torque(int8_t id, uint8_t numero_serial);
00304 
00305 int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial);
00306 
00307 int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial);
00308 
00309 void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial);
00310 
00311 //fonctions capteur de couleurs
00312 
00313 int8_t Get_Led(uint8_t id, uint8_t numero_serie);
00314 
00315 
00316 int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial);
00317 
00318 //--------------------------------------------Fonctions d'interruptions et de vérifications------------------------------------------------------------------
00319 
00320 void servo_interrupt_en(void);
00321 
00322 void Interrupt1_en(void);
00323 
00324 void Interrupt2_en(void);
00325 
00326 void Interrupt3_en(void);
00327 
00328 void Interrupt4_en(void);
00329 
00330 void Interrupt5_en(void);
00331 
00332 void automate_serial1(void);
00333 
00334 void automate_serial2(void);
00335 
00336 void automate_serial3(void);
00337 
00338 void automate_serial4(void);
00339 
00340 void automate_serial5(void);
00341 /*
00342 void Pompe_init(PinName Pompe_pin);
00343 
00344 void Pompe_essai(PinName Pompe_pin);*/
00345 
00346 //----------------Définition des ports série en variables externes---------------------------
00347 /*
00348  * permet d'utiliser les ports série dans le main.ci
00349  */
00350 //extern RawSerial pc;
00351 extern RawSerial serial5;
00352 extern RawSerial serial4;
00353 extern RawSerial serial3;
00354 extern RawSerial serial2;
00355 extern RawSerial serial1;
00356 //extern RawSerial serial1;
00357 //#define Serial1 serial4
00358 
00359 #endif