66 lines
2.0 KiB
C
66 lines
2.0 KiB
C
#include "Commande_vitesse.h"
|
|
#include "Strategie_2024_plante.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "i2c_annexe.h"
|
|
#include "math.h"
|
|
#include <stdio.h>
|
|
|
|
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
|
|
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
|
|
|
|
enum etat_action_t Strat_2024_aller_a_plante(){
|
|
static enum {
|
|
SAAP_INIT_DETECTION,
|
|
SAAP_ASSERV,
|
|
SAAP_ATTRAPE,
|
|
SAAP_ATTRAPE_TEMPO,
|
|
} etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
enum validite_vl53l8_t validite;
|
|
float angle, distance, commande_vitesse_plante;
|
|
static int tempo;
|
|
|
|
switch(etat_aller_a_plante){
|
|
case SAAP_INIT_DETECTION:
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
|
|
etat_aller_a_plante = SAAP_ASSERV;
|
|
break;
|
|
|
|
case SAAP_ASSERV:
|
|
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
|
if(validite == VL53L8_PLANTE){
|
|
commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
|
|
if(commande_vitesse_plante > 300){
|
|
commande_vitesse_plante = 300;
|
|
}
|
|
if(commande_vitesse_plante <= 0){
|
|
commande_vitesse_stop();
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
etat_aller_a_plante = SAAP_ATTRAPE;
|
|
break;
|
|
}
|
|
|
|
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
|
|
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
|
|
}else{
|
|
}
|
|
break;
|
|
|
|
case SAAP_ATTRAPE:
|
|
i2c_annexe_attrape_plante(PLANTE_BRAS_1);
|
|
etat_aller_a_plante = SAAP_ATTRAPE_TEMPO;
|
|
tempo = 2500;
|
|
break;
|
|
|
|
case SAAP_ATTRAPE_TEMPO:
|
|
tempo--;
|
|
if(tempo <= 0){
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
} |