Sébastien Posada Ramirez
Lib herkulex utilisée pour la carte du phare
Diff: fonctions_herkulex.h
- Revision:
- 0:32fd5a08c430
- Child:
- 1:94a29f20ca18
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fonctions_herkulex.h Tue Apr 23 15:41:44 2019 +0000 @@ -0,0 +1,386 @@ +#include "mbed.h" + +//#define ID 0xFD //=253 + +// Herkulex ROM Register +#define ROM_MODEL_NO1 0 +#define ROM_MODEL_NO2 1 +#define ROM_VERSION1 2 +#define ROM_VERSION2 3 +#define ROM_BAUD_RATE 4 +#define ROM_RESERVED5 5 +#define ROM_ID 6 +#define ROM_ACK_POLICY 7 +#define ROM_ALARM_LED_POLICY 8 +#define ROM_TORQUE_POLICY 9 +#define ROM_RESERVED10 10 +#define ROM_MAX_TEMPERATURE 11 +#define ROM_MIN_VOLTAGE 12 +#define ROM_MAX_VOLTAGE 13 +#define ROM_ACCELERATION_RATIO 14 +#define ROM_MAX_ACCELERATION_TIME 15 +#define ROM_DEAD_ZONE 16 +#define ROM_SATURATOR_OFFSET 17 +#define ROM_SATURATOR_SLOPE 18 // 2Byte +#define ROM_PWM_OFFSET 20 +#define ROM_MIN_PWM 21 +#define ROM_MAX_PWM 22 // 2Byte +#define ROM_OVERLOAD_PWM_THRESHOLD 24 // 2Byte +#define ROM_MIN_POSITION 26 // 2Byte +#define ROM_MAX_POSITION 28 // 2Byte +#define ROM_POSITION_KP 30 // 2Byte +#define ROM_POSITION_KD 32 // 2Byte +#define ROM_POSITION_KI 34 // 2Byte +#define ROM_POSITION_FEEDFORWARD_1ST_GAIN 36 // 2Byte +#define ROM_POSITION FEEDFORWARD_2ND_GAIN 38 // 2Byte +#define ROM_RESERVED40 40 // 2Byte +#define ROM_RESERVED42 42 // 2Byte +#define ROM_LED_BLINK_PERIOD 44 +#define ROM_ADC_FAULT_CHECK_PERIOD 45 +#define ROM_PACKET_GARBAGE_CHECK_PERIOD 46 +#define ROM_STOP_DETECTION_PERIOD 47 +#define ROM_OVERLOAD_DETECTION_PERIOD 48 +#define ROM_STOP_THRESHOLD 49 +#define ROM_INPOSITION_MARGIN 50 +#define ROM_RESERVED51 51 +#define ROM_RESERVED52 52 +#define ROM_CALIBRATION_DIFFERENCE 53 + +//------------------------------------------------------------------------------ +// Herkulex RAM Register +#define RAM_ID 0 +#define RAM_ACK_POLICY 1 +#define RAM_ALARM_LED_POLICY 2 +#define RAM_TORQUE_POLICY 3 +#define RAM_RESERVED4 4 +#define RAM_MAX_TEMPERATURE 5 +#define RAM_MIN_VOLTAGE 6 +#define RAM_MAX_VOLTAGE 7 +#define RAM_ACCELERATION_RATIO 8 +#define RAM_MAX_ACCELERATION 9 +#define RAM_DEAD_ZONE 10 +#define RAM_SATURATOR_OFFSET 11 +#define RAM_SATURATOR_SLOPE 12 // 2Byte +#define RAM_PWM_OFFSET 14 +#define RAM_MIN_PWM 15 +#define RAM_MAX_PWM 16 // 2Byte +#define RAM_OVERLOAD_PWM_THRESHOLD 18 // 2Byte +#define RAM_MIN_POSITION 20 // 2Byte +#define RAM_MAX_POSITION 22 // 2Byte +#define RAM_POSITION_KP 24 // 2Byte +#define RAM_POSITION_KD 26 // 2Byte +#define RAM_POSITION_KI 28 // 2Byte +#define RAM_POSITION_FEEDFORWARD_1ST_GAIN 30 // 2Byte +#define RAM_POSITION_FEEDFORWARD 2ND GAIN 32 // 2Byte +#define RAM_RESERVED34 34 // 2Byte +#define RAM_RESERVED36 36 // 2Byte +#define RAM_LED_BLINK_PERIOD 38 +#define RAM_ADC_FAULT_DETECTION_PERIOD 39 +#define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40 +#define RAM_STOP_DETECTION_PERIOD 41 +#define RAM_OVERLOAD_DETECTION_PERIOD 42 +#define RAM_STOP_THRESHOLD 43 +#define RAM_INPOSITION_MARGIN 44 +#define RAM_RESERVED45 45 +#define RAM_RESERVED46 46 +#define RAM_CALIBRATION_DIFFERENCE 47 +#define RAM_STATUS_ERROR 48 +#define RAM_STATUS_DETAIL 49 +#define RAM_RESERVED50 50 +#define RAM_RESERVED51 51 +#define RAM_TORQUE_CONTROL 52 +#define RAM_LED_CONTROL 53 +#define RAM_VOLTAGE 54 +#define RAM_TEMPERATURE 55 +#define RAM_CURRENT_CONTROL_MODE 56 +#define RAM_TICK 57 +#define RAM_CALIBRATED_POSITION 58 // 2Byte +#define RAM_ABSOLUTE_POSITION 60 // 2Byte +#define RAM_DIFFERENTIAL_POSITION 62 // 2Byte +#define RAM_PWM 64 // 2Byte +#define RAM_RESERVED66 66 // 2Byte +#define RAM_ABSOLUTE_GOAL_POSITION 68 // 2Byte +#define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION 70 // 2Byte +#define RAM_DESIRED_VELOCITY 72 // 2Byte + +//------------------------------------------------------------------------------ +// Request Packet [To Servo Module] +#define CMD_ROM_WRITE 0x01 // Write Length number of values to EEP Register Address +#define CMD_ROM_READ 0x02 // Request Length number of values from EEP Register Address +#define CMD_RAM_WRITE 0x03 // Write Length number of values to RAM Register Address +#define CMD_RAM_READ 0x04 // Request Lenght number of values from RAM Register Address +#define CMD_I_JOG 0x05 // Able to send JOG command to maximum 43 servos (operate timing of individual Servo) +#define CMD_S_JOG 0x06 // Able to send JOG command to maximum 53 servos (operate simultaneously at same time) +#define CMD_STAT 0x07 // Status Error, Status Detail request +#define CMD_ROLLBACK 0x08 // Change all EEP Regsters to Factory Default value +#define CMD_REBOOT 0x09 // Request Reboot + +//------------------------------------------------------------------------------ +// ACK Packet [To Controller(ACK)] +#define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40 +#define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK) +#define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK) +#define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK) +#define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK) +#define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK) +#define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK) +#define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK) +#define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK) +#define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK) + +//------------------------------------------------------------------------------ +// Status Error +#define STATUS_OK = 0x00; +#define ERROR_EXCEED_INPUT_VOLTAGE = 0x01; +#define ERROR_EXCEED_POT_LIMIT = 0x02; +#define ERROR_EXCEED_TEMPERATURE_LIMIT = 0x04; +#define ERROR_INVALID_PACKET = 0x08; +#define ERROR_OVERLOAD = 0x10; +#define ERROR_DRIVER_FAULT = 0x20; +#define ERROR_EEP_REG_DISTORT = 0x40; + +//------------------------------------------------------------------------------ +// Status Detail +#define MOVING_FLAG = 0x01; +#define INPOSITION_FLAG = 0x02; +#define CHECKSUM_ERROR = 0x04; // Invalid packet`s detailed information +#define UNKNOWN_COMMAND = 0x08; // Invalid packet`s detailed information +#define EXCEED_REG_RANGE = 0x10; // Invalid packet`s detailed information +#define GARBAGE_DETECTED = 0x20; // Invalid packet`s detailed information +#define MOTOR_ON_FLAG = 0x40; + +//------------------------------------------------------------------------------ +// Header +#define HEADER 0xFF + +// Size +#define MIN_PACKET_SIZE 7 +#define MIN_ACK_PACKET_SIZE 9 +#define WRITE_PACKET_SIZE 13 +#define MAX_PACKET_SIZE 223 +#define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE) + +// ID +#define MAX_PID 0xFD +#define DEFAULT_ID 0xFD +#define MAX_ID 0xFD +#define BROADCAST_ID 0xFE + +// Checksum +#define CHKSUM_MASK 0xFE + +// Torque CMD +#define TORQUE_FREE 0x00 +#define BREAK_ON 0x40 +#define TORQUE_ON 0x60 + +// Register Size +#define BYTE1 1 +#define BYTE2 2 + +// Jog Set CMD +#define STOP 0x01 +#define POS_MODE 0x00 +#define TURN_MODE 0x02 +#define GLED_ON 0x04 +#define BLED_ON 0x08 +#define RLED_ON 0x10 + +//------------------------------------------------------------------------------ +//------------------------------------------a choisir--------------------------- +#define nombre_servomoteur 3 // +//-----------------------servo petit robot-------------------------------------- +#define ID_41 41 //petit robot servo dernier //a reconfigurer +#define ID_42 0x2A //=42 +#define ID_45 45 +#define ID_46 0x2E +#define ID_43 43 +#define ID_44 44 +#define ID_47 47 +#define ID_48 48 +#define ID_40 40 +//-----------------------servo grand robot-------------------------------------- +#define ID_25 25 +#define ID_26 26 + +#define ID_17 42 +#define ID_27 41 + +#define ID_15 15 +#define ID_16 16 + +#define ID_1 0xFD //=253 premiere // bras droite +#define ID_3 0x0D //=13 troixieme // +#define ID_2 0x0C //=12 deuxieme // +#define ID_4 0x09 //Avec engrenage // +#define Pompe_pin1 PA_15 //Pin de Pompe // + +#define ID_2_1 21 //=253 premiere // +#define ID_2_2 22 //=12 deuxieme // +#define ID_2_3 23 //=13 troixieme // bras gauche, a changer +#define ID_2_4 24 //Avec engrenage // +#define Pompe_pin2 PB_2 //Pin de Pompe // a changer +#define ID_01 1 +//------------------------------------------------------------------------------ + +//------------------------------------------------------------------------------ + +/** Create an Herkulex servo object connected to the serial pins and baudrate + * + * @param tx Transmit pin. + * @param rx Receive pin. + * @param baudRate The serial tx/rx speed. + */ +// void Herkulex(PinName tx, PinName rx, uint32_t baudRate); + +/** Destroy an Herkulex servo object + */ +//void N_Herkulex(); + +/** Transmit packet datas + * + * @param packetSize The packet size. + * @param data The transmit packet data array. + */ +void txPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial); + +/** Receive packet datas + * + * @param packetSize The packet size. + * @param data The receive packet data array. + */ +void rxPacket(uint8_t packetSize, uint8_t* data, uint8_t numero_serial); + +/** Clear error status + * + * @param id The herkulex servo ID. + */ +void clear(uint8_t id, uint8_t numero_serial); + +/** Set torque setting + * + * @param id The herkulex servo ID. + * @param cmdTorque The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60) + */ +void setTorque(uint8_t id, uint8_t cmdTorque, uint8_t numero_serial); +//------------------------------Déplacements------------------------------------- +/** Position Control + * + * @param id The herkulex servo ID. + * @param position The goal position of herkulex servo. + * @param playtime Time to target position. + * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) + */ +void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED, uint8_t numero_serial); + +/** Velocity Control + * + * @param id The herkulex servo ID. + * @param speed The goal position of herkulex servo. + * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10) + */ +void velocityControl(uint8_t id, int16_t speed,uint8_t setLED, uint8_t numero_serial); +/** + * + *même paramètres que positionControl(); + *permet de donner des instructions à deux servomoteurs simultanéments + */ +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); +/** + * + *même paramètres que positionControl_Mul_ensemble(); + *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent + */ +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); +/** + * + * + *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent + */ +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 +/** + * + *même paramètres que positionControl_Mul_ensemble(); + *permet de donner des instructions à deux servomoteurs simultanéments avec un playtime différent + */ +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 + + +//-------------------------------------Demandes d'informations +/** Get Status + * + * @param id The herkulex servo ID. + * @return -1 is getStatus failed. other is servo`s status error value. + */ +int8_t getStatus(uint8_t id, uint8_t numero_serial); + +/** Get Position + * + * @param id The herkulex servo ID. + * @return -1 is getPos failed. other is servo's current position. + */ +int16_t getPos(uint8_t id, uint8_t numero_serial); + +//void recevoir_irq(void); + +void receive_Sv(void); + +int8_t Get_Torque(int8_t id, uint8_t numero_serial); + +int8_t Get_Temperature_MAX(int8_t id, uint8_t numero_serial); + +int8_t Get_Tension_MIN(int8_t id, uint8_t numero_serial); + +void Set_Tension_MIN(int8_t id,uint8_t Tension_Min, uint8_t numero_serial); + + +int8_t Get_Tension_actuelle(int8_t id, uint8_t numero_serial); + +//--------------------------------------------Fonctions d'interruptions et de vérifications------------------------------------------------------------------ + +void servo_interrupt_en(void); + +void Interrupt1_en(void); + +void Interrupt2_en(void); + +void Interrupt3_en(void); + +void Interrupt4_en(void); + +void Interrupt5_en(void); + +void automate_serial1(void); + +void automate_serial2(void); + +void automate_serial3(void); + +void automate_serial4(void); + +void automate_serial5(void); + +void Pompe_init(PinName Pompe_pin); + +void Pompe_essai(PinName Pompe_pin); + +//----------------Définition des ports série en variables externes--------------------------- +/* + * permet d'utiliser les ports série dans le main.ci + */ +extern RawSerial pc; +extern RawSerial serial5; +extern RawSerial serial4; +extern RawSerial serial3; +extern RawSerial serial2; +extern RawSerial serial1; +//extern RawSerial serial1; +//#define Serial1 serial4 + +//---------------Fonctions gros robot------------------------------------------- +//-------------------------------------Utiliser la roue de sortie--------------- +/** Get Status + * + * @ tour ==> nombre de fois où la roue descend + */ +//void roueSortie(tour); \ No newline at end of file