Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Demo_2025_03
Holonome_2024/Strategie_2024.c
Samuel dbe8ea2706 Prepa Match 2 - Coupe IDF
2024-06-22 16:36:31 +02:00

363 lines
12 KiB
C
Raw Permalink Blame History

#include "Strategie_2024.h"
#include "Localisation.h"
#include "Geometrie_robot.h"
#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "i2c_annexe.h"
enum etat_groupe_pot_t{
GROUPE_POT_DISPO,
GROUPE_POT_ECHEC
};
enum etat_groupe_plante_t{
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_ECHEC
};
enum etat_groupe_pot_t etat_groupe_pot[6]={
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO
};
enum etat_groupe_plante_t etat_groupe_plante[6]={
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO
};
struct position_t approche_groupe_plante_bleu[6]={
{.x_mm= 0, .y_mm=0, .angle_radian = 0},
{.x_mm= 580, .y_mm=1530, .angle_radian = 0},
{.x_mm= 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 1220, .y_mm=360, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};
struct position_t approche_groupe_plante_jaune[6]={
{.x_mm= 0, .y_mm=0, .angle_radian = 0},
{.x_mm= 580, .y_mm=1530, .angle_radian = 0},
{.x_mm= 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 1220, .y_mm=360, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};
int ordre_groupe_pot[6];
unsigned int get_groupe_pot(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){
return GROUPE_POT_L1;
}
return GROUPE_POT_R1;
}
enum zone_plante_t get_zone_plante(enum couleur_t couleur){
enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_2, ZONE_PLANTE_3, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_6, ZONE_PLANTE_5, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
enum zone_plante_t *ordre_groupe_plante;
int i;
if(couleur == COULEUR_BLEU){
ordre_groupe_plante = ordre_groupe_plante_bleu;
}else{
ordre_groupe_plante = ordre_groupe_plante_jaune;
}
for(i=0; i<6; i++){
if(etat_groupe_plante[ordre_groupe_plante[i]] == GROUPE_PLANTE_DISPO){
return ordre_groupe_plante[i];
}
}
return ZONE_PLANTE_AUCUNE;
}
struct position_t get_position_approche_zone_plante(enum couleur_t couleur, enum zone_plante_t zone_plante){
if(couleur == COULEUR_BLEU){
return approche_groupe_plante_bleu[zone_plante];
}
return approche_groupe_plante_jaune[zone_plante];
}
void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
float angle_destination, pos_x;
struct trajectoire_t trajectoire;
struct position_t position;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
static int nb_plante_ok = 0;
static int bras_depose = PLANTE_BRAS_1;
static int depose_en_cours = 0;
static int pre_fin_match=0;
static enum {
TAP_CALAGE,
TAP_PASSAGE_JAUNE,
TAP_POT,
TAP_PANNEAUX_SOLAIRES,
TAP_ALLER_PLANTE,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE,
TAP_ECHANGE_POT,
TAP_PLANTE_TERMINEE_ECHEC,
TAP_RENTRE,
TAP_RENTRE_RECALE,
TAP_DEPOSE_0,
TAP_DEPOSE_1,
TAP_PANNEAU_SOLAIRE_1,
TAP_DEPOSE_2,
TAP_DEPOSE_3,
TAP_RECHARGE,
TAP_FINI
} etat_test = TAP_CALAGE;
if(temps_ms > 80000 && depose_en_cours == 0){
etat_test = TAP_RENTRE;
pre_fin_match = 1;
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){
if(couleur == COULEUR_BLEU){
etat_test=TAP_POT;
}else{
etat_test=TAP_PASSAGE_JAUNE;
}
}
break;
case TAP_PASSAGE_JAUNE:
if(Strategie_aller_a(2600, 1600, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_ALLER_PLANTE;
}
break;
case TAP_ALLER_PLANTE:
position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur));
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ORIENTATION;
}
break;
case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE;
}
break;
case TAP_PLANTE_ATTRAPE:
etat_action = Strat_2024_plante_dans_pot(_step_ms, bras_depose, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE;
nb_plante_ok++;
if(bras_depose == PLANTE_BRAS_1){
bras_depose = PLANTE_BRAS_6;
}else{
bras_depose = PLANTE_BRAS_1;
}
if(nb_plante_ok == 2){
etat_test = TAP_ECHANGE_POT;
}
if(nb_plante_ok == 4){
etat_test = TAP_RENTRE;
}
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_PLANTE_TERMINEE_ECHEC;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_ECHANGE_POT:
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE;
}
break;
case TAP_PLANTE_TERMINEE_ECHEC:
// On note que la zone plante actuelle est "occupée/epuisée"
etat_groupe_plante[get_zone_plante(couleur)] = GROUPE_PLANTE_ECHEC;
// On va à la zone suivante ou on rentre
if(get_zone_plante(couleur) == ZONE_PLANTE_AUCUNE){
etat_test=TAP_RENTRE;
}else{
etat_test=TAP_ALLER_PLANTE;
}
break;
case TAP_RENTRE:
depose_en_cours = 1;
if(couleur == COULEUR_BLEU){
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 450;
}else{
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 3000-450;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_RENTRE_RECALE;
}
break;
case TAP_RENTRE_RECALE:
etat_action = Strategie_calage_bas(couleur, step_ms);
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_test=TAP_DEPOSE_0;
}
break;
case TAP_DEPOSE_0:
angle_destination = -30 * DEGRE_EN_RADIAN;
if(couleur == COULEUR_BLEU){
pos_x = 300;
}else{
pos_x = 3000-300;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
i2c_annexe_actionneur_pot(POT_5, BRAS_LEVITE, DOIGT_TIENT);
etat_test=TAP_DEPOSE_1;
}
break;
case TAP_DEPOSE_1:
commande_vitesse_stop();
if(couleur == COULEUR_BLEU){
if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_2 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_PANNEAUX_SOLAIRES;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
}
}else{
if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_2 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_PANNEAUX_SOLAIRES;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
}
}
break;
case TAP_PANNEAUX_SOLAIRES:
if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_2;
}
break;
case TAP_DEPOSE_2:
angle_destination = 150 * DEGRE_EN_RADIAN;
if(couleur == COULEUR_BLEU){
pos_x = 550;
}else{
pos_x = 3000-550;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_3;
}
break;
case TAP_DEPOSE_3:
commande_vitesse_stop();
if(couleur == COULEUR_BLEU){
if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_RECHARGE;
}
}else{
if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_RECHARGE;
}
}
break;
case TAP_RECHARGE:
if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_FINI;
}
break;
case TAP_FINI:
commande_vitesse_stop();
break;
}
}
enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
struct trajectoire_t trajectoire;
enum etat_action_t etat_action;
enum validite_vl53l8_t validite;
float angle, distance, pos_x;
static int tempo_ms;
static enum {
RR_ORIENTE,
RR_RECALE,
RR_DEPLACE,
} etat_rentre_charge = RR_DEPLACE;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
switch (etat_rentre_charge){
case RR_ORIENTE:
if(Strategie_tourner_a(60 * DEGRE_EN_RADIAN, step_ms) == ACTION_TERMINEE){
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
commande_vitesse_stop();
tempo_ms = 2000;
etat_rentre_charge = RR_RECALE;
}
break;
case RR_RECALE:
tempo_ms--;
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
etat_rentre_charge = RR_DEPLACE;
}
if(tempo_ms <= 0){
etat_rentre_charge =RR_DEPLACE;
}
break;
case RR_DEPLACE:
if(couleur == COULEUR_BLEU){
pos_x = Localisation_get().x_mm + 30;
}else{
pos_x = Localisation_get().x_mm - 30;
}
if( Strategie_aller_a(pos_x , 150, EVITEMENT_SANS_EVITEMENT, step_ms) == ACTION_TERMINEE){
etat_rentre_charge =RR_ORIENTE;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
Reference in New Issue View Git Blame Copy Permalink