Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
9892e5d923
Holonome_2024/Strategie_2024.c
Samuel 9892e5d923 Match 1
2024-05-09 12:34:16 +02:00

297 lines
9.5 KiB
C
Raw Blame History

#include "Strategie_2024.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
};
int ordre_groupe_pot[6];
unsigned int get_groupe_pot(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){
return GROUPE_POT_B1;
}
return GROUPE_POT_B2;
}
enum zone_plante_t get_zone_plante(enum couleur_t couleur){
enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_4, ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_1};
enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_4, ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_1};
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;
}
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;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
static enum {
TAP_CALAGE,
TAP_POT,
TAP_PANNEAUX_SOLAIRES,
TAP_ALLER_PLANTE,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE_1,
TAP_PLANTE_ATTRAPE_2,
TAP_ECHANGE_POT,
TAP_PLANTE_ATTRAPE_3,
TAP_PLANTE_ATTRAPE_4,
TAP_RENTRE,
TAP_RENTRE_RECALE,
TAP_DEPOSE_0,
TAP_DEPOSE_1,
TAP_DEPOSE_2,
TAP_DEPOSE_3,
TAP_RECHARGE,
TAP_FINI
} etat_test = TAP_CALAGE, next_etat_test;
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut_manuel(couleur, _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_PANNEAUX_SOLAIRES;
}
break;
case TAP_PANNEAUX_SOLAIRES:
if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_ALLER_PLANTE;
}
break;
case TAP_ALLER_PLANTE:
if(couleur == COULEUR_BLEU){
pos_x = 940;
}else{
pos_x = 3000 - 940;
}
if(Strategie_aller_a(pos_x, 300, 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_1;
}
break;
case TAP_PLANTE_ATTRAPE_1:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_2;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_2:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_ECHANGE_POT;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
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_3;
}
break;
case TAP_PLANTE_ATTRAPE_3:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_4;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_4:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_RENTRE:
if(couleur == COULEUR_BLEU){
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 350;
}else{
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 3000-350;
}
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_debut(couleur, step_ms);
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_test=TAP_DEPOSE_0;
}
break;
case TAP_DEPOSE_0:
if(couleur == COULEUR_BLEU){
pos_x = 300;
angle_destination = 15 * DEGRE_EN_RADIAN;
}else{
pos_x = 3000-300;
angle_destination = (90-15) * DEGRE_EN_RADIAN;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a_puis_tourner(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_1;
}
break;
case TAP_DEPOSE_1:
commande_vitesse_stop();
if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_DEPOSE_2;
}
break;
case TAP_DEPOSE_2:
if(couleur == COULEUR_BLEU){
angle_destination = (180+15) * DEGRE_EN_RADIAN;
pos_x = 450;
}else{
angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN;
pos_x = 3000-450;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a_puis_tourner(pos_x, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_3;
}
break;
case TAP_DEPOSE_3:
if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _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;
static enum {
ZONE_HAUTE,
ZONE_OPPOSEE,
} etat_rentre_charge = ZONE_OPPOSEE;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
switch (etat_rentre_charge){
case ZONE_OPPOSEE:
if(couleur == COULEUR_BLEU){
etat_action = Strategie_aller_a(2700, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{
etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}
if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_HAUTE;
}else if(etat_action == ACTION_TERMINEE){
return ACTION_TERMINEE;
}
break;
case ZONE_HAUTE:
if(couleur == COULEUR_BLEU){
etat_action = Strategie_aller_a(300, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{
etat_action = Strategie_aller_a(2700, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}
if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_OPPOSEE;
}else if(etat_action == ACTION_TERMINEE){
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
Reference in New Issue View Git Blame Copy Permalink