106 lines
3.4 KiB
C
106 lines
3.4 KiB
C
#include "Commande_vitesse.h"
|
|
#include "Strategie_2024_plante.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "Localisation.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(void){
|
|
static enum {
|
|
SAAP_INIT_DETECTION,
|
|
SAAP_ASSERV
|
|
} 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_INIT_DETECTION;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
|
|
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
|
|
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
|
|
}else{
|
|
}
|
|
break;
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
/// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
|
|
/// @param step_ms
|
|
/// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
|
|
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
|
|
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
|
|
static enum{
|
|
PDP_ALLER_PLANTE,
|
|
PDP_ATTRAPE_PLANTE,
|
|
PDP_RECULE,
|
|
PDP_TEMPO,
|
|
} etat_plante_dans_pot = PDP_ALLER_PLANTE;
|
|
static int tempo_ms;
|
|
struct position_t position_recule, position_initiale;
|
|
|
|
switch (etat_plante_dans_pot)
|
|
{
|
|
case PDP_ALLER_PLANTE:
|
|
if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
|
|
i2c_annexe_attrape_plante(bras_depose_pot);
|
|
tempo_ms = 2000;
|
|
|
|
etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
|
|
}
|
|
break;
|
|
|
|
case PDP_ATTRAPE_PLANTE:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_plante_dans_pot=PDP_RECULE;
|
|
tempo_ms = 2000;
|
|
}
|
|
break;
|
|
|
|
case PDP_RECULE:
|
|
tempo_ms--;
|
|
position_initiale = Localisation_get();
|
|
position_initiale.angle_radian += ANGLE_PINCE;
|
|
position_recule = Geometrie_deplace(position_initiale, -100);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
|
|
if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
|
|
commande_vitesse_stop();
|
|
etat_plante_dans_pot=PDP_TEMPO;
|
|
}
|
|
break;
|
|
|
|
case PDP_TEMPO:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_plante_dans_pot=PDP_ALLER_PLANTE;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
} |