Sébastien Posada Ramirez
Lib herkulex utilisée pour la carte du phare
fonctions_herkulex.h
- Committer:
- AdrienSalle
- Date:
- 2019-04-23
- Revision:
- 0:32fd5a08c430
- Child:
- 1:94a29f20ca18
File content as of revision 0:32fd5a08c430:
#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);