596 lines
20 KiB
C
596 lines
20 KiB
C
#include "hardware/gpio.h"
|
|
#include "i2c_annexe.h"
|
|
#include "Asser_Position.h"
|
|
#include "Commande_vitesse.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "Localisation.h"
|
|
#include "Monitoring.h"
|
|
#include "Moteurs.h"
|
|
#include "Score.h"
|
|
#include "Strategie.h"
|
|
#include "Trajet.h"
|
|
#include "math.h"
|
|
|
|
#define SEUIL_RECAL_DIST_MM 75
|
|
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
|
|
#define DISTANCE_OBSTACLE_CM 50
|
|
#define DISTANCE_PAS_OBSTACLE_MM 2000
|
|
|
|
#define NB_OBJECTIFS 4
|
|
|
|
struct objectif_t objectifs[NB_OBJECTIFS];
|
|
struct objectif_t * objectif_courant;
|
|
|
|
uint32_t Score_cerises_dans_robot=0;
|
|
|
|
void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
|
|
Score_cerises_dans_robot = nb_cerises;
|
|
}
|
|
|
|
uint32_t Strategie_get_cerise_dans_robot(void){
|
|
return Score_cerises_dans_robot;
|
|
}
|
|
|
|
|
|
|
|
// TODO: Peut-être à remetttre en variable locale après
|
|
float distance_obstacle;
|
|
|
|
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
|
|
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
|
|
|
|
|
|
|
|
int Robot_est_dans_quart_haut_gauche();
|
|
int Robot_est_dans_quart_haut_droit();
|
|
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
|
|
int Robot_est_dans_zone_cerise_gauche();
|
|
int Robot_est_dans_zone_cerise_droite();
|
|
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
|
|
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
|
|
|
|
enum etat_action_t Strategie_calage(enum couleur_t couleur);
|
|
|
|
|
|
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
|
|
const uint32_t temps_pre_fin_match = (97000 - 10000);
|
|
static bool pre_fin_match_active=false;
|
|
float angle_fin;
|
|
float recal_pos_x, recal_pos_y;
|
|
float point_x, point_y;
|
|
enum longer_direction_t longer_direction;
|
|
enum etat_action_t etat_action;
|
|
enum etat_trajet_t etat_trajet;
|
|
|
|
struct trajectoire_t trajectoire;
|
|
|
|
static enum etat_strategie_t {
|
|
STRATEGIE_INIT,
|
|
ALLER_CERISE_BAS,
|
|
ATTRAPER_CERISE_BAS,
|
|
ALLER_CERISE_HAUT,
|
|
ATTRAPER_CERISE_HAUT,
|
|
ALLER_CERISE_GAUCHE,
|
|
ATTRAPER_CERISE_GAUCHE,
|
|
ALLER_CERISE_DROITE,
|
|
ATTRAPER_CERISE_DROITE,
|
|
ALLER_PANIER,
|
|
LANCER_PANIER,
|
|
RETOUR_MAISON,
|
|
DECISION_STRATEGIE,
|
|
STRATEGIE_FIN
|
|
}etat_strategie;
|
|
|
|
if(Monitoring_get_erreur_critique()){
|
|
etat_strategie=STRATEGIE_FIN;
|
|
}
|
|
|
|
if(temps_ms > temps_pre_fin_match && pre_fin_match_active == false){
|
|
if(etat_strategie != LANCER_PANIER){
|
|
etat_strategie = RETOUR_MAISON;
|
|
}
|
|
pre_fin_match_active=true;
|
|
}
|
|
|
|
switch(etat_strategie){
|
|
case STRATEGIE_INIT:
|
|
if(Strategie_calage_bas(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
|
|
etat_strategie = STRATEGIE_FIN;
|
|
}
|
|
break;
|
|
|
|
case RETOUR_MAISON:
|
|
if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){
|
|
etat_strategie=STRATEGIE_FIN;
|
|
}
|
|
break;
|
|
|
|
|
|
case STRATEGIE_FIN:
|
|
pre_fin_match_active=true;
|
|
commande_vitesse_stop();
|
|
i2c_annexe_couleur_balise(0xE0, 0x0FFF);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/// Fonction de localisation approximatives pour la stratégie.
|
|
|
|
/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_quart_haut_gauche(){
|
|
if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_quart_haut_droit(){
|
|
if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_gauche(){
|
|
if(Localisation_get().x_mm < 630 &&
|
|
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_droite(){
|
|
if(Localisation_get().x_mm > 2000 - 630 &&
|
|
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
|
|
if(couleur == COULEUR_BLEU){
|
|
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){
|
|
return 1;
|
|
}
|
|
}else{
|
|
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm > 1000){
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
int Robot_est_dans_zone_depose_panier(enum couleur_t couleur){
|
|
float x_mm, y_mm;
|
|
x_mm = Localisation_get().x_mm;
|
|
y_mm = Localisation_get().y_mm;
|
|
if(couleur == COULEUR_BLEU){
|
|
// Zone 1
|
|
if(x_mm < 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
}else{
|
|
// Zone 1
|
|
if(x_mm > 2000 - 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Renvoi 1 si le robot intersect une zone de dépose
|
|
/// @param couleur du robot
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_zone_depose(enum couleur_t couleur){
|
|
float x_mm, y_mm;
|
|
x_mm = Localisation_get().x_mm;
|
|
y_mm = Localisation_get().y_mm;
|
|
if(couleur == COULEUR_BLEU){
|
|
// Zone 1
|
|
if(x_mm < 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
// Zone 2
|
|
if(x_mm < 550 && y_mm > 800 && y_mm < 1450){
|
|
return 1;
|
|
}
|
|
// Zone 3
|
|
if(x_mm > 400 && x_mm < 1050 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 4
|
|
if(x_mm > 1450 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 5
|
|
if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}else{
|
|
// VERT
|
|
// Zone 1
|
|
if(x_mm > (2000 - 550) && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
// Zone 2
|
|
if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){
|
|
return 1;
|
|
}
|
|
// Zone 3
|
|
if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 4
|
|
if(x_mm < (2000 - 1450) && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 5
|
|
if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
|
|
uint attente_tirette(void){
|
|
return !gpio_get(TIRETTE);
|
|
}
|
|
|
|
/// @brief Renvoi COULEUR_JAUNE ou COULEUR_BLEU
|
|
enum couleur_t lire_couleur(void){
|
|
if (gpio_get(COULEUR))
|
|
return COULEUR_JAUNE;
|
|
return COULEUR_BLEU;
|
|
|
|
}
|
|
|
|
/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
|
|
/// @param tempo_ms
|
|
/// @param step_ms
|
|
/// @return 1 si la temporisation est écoulée, 0 sinon.
|
|
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
|
|
if(*tempo_ms < step_ms){
|
|
return 1;
|
|
}else{
|
|
*tempo_ms -= step_ms;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
pos_x, pos_y,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
|
|
return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
|
|
}
|
|
|
|
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
pos_x, pos_y,
|
|
Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
|
|
|
|
return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
|
|
}
|
|
|
|
/// @brief Avance puis tourne
|
|
/// @param pos_x
|
|
/// @param pos_y
|
|
/// @param angle_radian
|
|
/// @param
|
|
/// @param step_ms
|
|
/// @return
|
|
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
static enum {
|
|
AAPT_ALLER,
|
|
AAPT_TOURNER,
|
|
}etat_aller_a_puis_tourner=AAPT_ALLER;
|
|
|
|
switch(etat_aller_a_puis_tourner){
|
|
case AAPT_ALLER:
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){
|
|
etat_aller_a_puis_tourner = AAPT_TOURNER;
|
|
}
|
|
break;
|
|
case AAPT_TOURNER:
|
|
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
|
|
|
|
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_aller_a_puis_tourner = AAPT_ALLER;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
|
|
|
|
return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
|
|
}
|
|
|
|
|
|
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
|
|
static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
|
|
enum etat_action_t etat_action;
|
|
//Trajet_config(500,500); //750, 500 => Ne marche pas
|
|
|
|
if(objectif_plat_courant==NULL){
|
|
objectif_plat_courant = &objectifs_plats[0];
|
|
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1};
|
|
struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2};
|
|
struct objectif_t objectif_3 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_3};
|
|
struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4};
|
|
struct objectif_t objectif_5 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_5};
|
|
objectifs_plats[0] = objectif_1;
|
|
objectifs_plats[1] = objectif_2;
|
|
objectifs_plats[2] = objectif_3;
|
|
objectifs_plats[3] = objectif_4;
|
|
objectifs_plats[4] = objectif_5;
|
|
}
|
|
|
|
etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms);
|
|
switch(etat_action){
|
|
case ACTION_TERMINEE:
|
|
return ACTION_TERMINEE;
|
|
case ACTION_EN_COURS:
|
|
return ACTION_EN_COURS;
|
|
case ACTION_ECHEC:
|
|
// 1. Marquer comme bloqué l'objectif en cours
|
|
objectif_plat_courant->etat = BLOQUE;
|
|
// 2. Si tous les objectifs sont bloqués, les marquer tous comme A_FAIRE
|
|
uint8_t non_bloque = 0;
|
|
for(uint i=0 ; i<5 ; i++){
|
|
if(objectifs_plats[i].etat == A_FAIRE){
|
|
non_bloque=1;
|
|
}
|
|
}
|
|
if(!non_bloque){
|
|
for(uint i=0 ; i<5 ; i++){
|
|
if (objectifs_plats[i].etat == BLOQUE){
|
|
objectifs_plats[i].etat = A_FAIRE;
|
|
}
|
|
}
|
|
}
|
|
// 3. Trouver le prochain objectif (priorité la plus basse + etat A_FAIRE)
|
|
// Si notre objectif est FAIT, on prend le premier objectif "A_FAIRE"
|
|
// Si notre objectif est A_FAIRE, on prend le nouvel objectif que si sa priorité est plus basse.
|
|
for(uint i=0; i < 5; i++){
|
|
if(objectif_plat_courant->etat == BLOQUE && objectifs_plats[i].etat == A_FAIRE){
|
|
objectif_plat_courant = &(objectifs_plats[i]);
|
|
}else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){
|
|
if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){
|
|
objectif_plat_courant = &(objectifs_plats[i]);
|
|
}
|
|
|
|
}
|
|
}
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms){
|
|
float pos_x;
|
|
float pos_y;
|
|
|
|
switch (objectif_plat_courant->cible){
|
|
case ZONE_1:
|
|
pos_y = 2775;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 250;
|
|
}else{
|
|
pos_x = 2000 - 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
|
|
case ZONE_2:
|
|
pos_y = 1125;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 250;
|
|
}else{
|
|
pos_x = 2000 - 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
|
|
case ZONE_3:
|
|
pos_y = 250;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 725;
|
|
}else{
|
|
pos_x = 2000 - 725;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
|
|
case ZONE_4:
|
|
pos_y = 250;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 2000 - 250;
|
|
}else{
|
|
pos_x = 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
|
|
case ZONE_5:
|
|
pos_y = 1850;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 2000 - 250;
|
|
}else{
|
|
pos_x = 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
}
|
|
}
|
|
|
|
enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms){
|
|
// 1 Envoyer la commande pour détecter la bordure
|
|
// 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
|
|
// Recaler la distance Y
|
|
// 3 Se tourner du bon angle (en fonction de la couleur et de l'angle lu)
|
|
// 4 Si la valeur de la bordure est valide, lire l'angle et la distance.
|
|
// 5 Se positionner à (250, 250), PINCE orientée à 45°.
|
|
static enum{
|
|
CD_ENVOI_CDE_BORDURE,
|
|
CD_LECTURE_BORDURE_Y,
|
|
CD_ROTATION_VERS_X,
|
|
CD_LECTURE_BORDURE_X,
|
|
CD_ALLER_POSITION_INIT,
|
|
CD_ROTATION_POSITION_INIT,
|
|
}etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
|
enum validite_vl53l8_t validite;
|
|
struct trajectoire_t trajectoire;
|
|
enum etat_action_t etat_action;
|
|
static int tempo_ms;
|
|
|
|
float angle, distance;
|
|
|
|
switch(etat_calage_debut){
|
|
case CD_ENVOI_CDE_BORDURE:
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
|
|
tempo_ms = 2000;
|
|
etat_calage_debut = CD_LECTURE_BORDURE_Y;
|
|
break;
|
|
|
|
case CD_LECTURE_BORDURE_Y:
|
|
tempo_ms--;
|
|
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
|
if(validite == VL53L8_DISTANCE_LOIN){
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
commande_vitesse_stop();
|
|
Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
|
|
Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
|
|
etat_calage_debut = CD_ROTATION_VERS_X;
|
|
}
|
|
if(tempo_ms <= 0){
|
|
etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
|
return ACTION_ECHEC;
|
|
}
|
|
break;
|
|
|
|
case CD_ROTATION_VERS_X:
|
|
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
|
if(couleur == COULEUR_BLEU){
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
|
|
(-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
|
}else{
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
|
|
(0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
|
}
|
|
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
|
|
etat_calage_debut = CD_LECTURE_BORDURE_X;
|
|
tempo_ms = 2000;
|
|
}
|
|
break;
|
|
|
|
case CD_LECTURE_BORDURE_X:
|
|
tempo_ms--;
|
|
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
|
if(validite == VL53L8_DISTANCE_LOIN){
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
commande_vitesse_stop();
|
|
if(couleur == COULEUR_BLEU){
|
|
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
|
|
Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
|
|
}else{
|
|
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
|
|
Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
|
|
}
|
|
etat_calage_debut = CD_ROTATION_POSITION_INIT;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
if(tempo_ms <= 0){
|
|
etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
|
return ACTION_ECHEC;
|
|
}
|
|
break;
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
|
|
enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms){
|
|
// Le robot est positionné avec une cale à 70 du bord
|
|
// Si l'angle avec la bordure est négatif : bleu, sinon jaune
|
|
// On en déduit X
|
|
static enum{
|
|
CD_ENVOI_CDE_BORDURE,
|
|
CD_LECTURE_BORDURE_Y,
|
|
CD_ROTATION_VERS_X,
|
|
CD_LECTURE_BORDURE_X,
|
|
CD_ALLER_POSITION_INIT,
|
|
CD_ROTATION_POSITION_INIT,
|
|
}etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
|
enum validite_vl53l8_t validite;
|
|
struct trajectoire_t trajectoire;
|
|
enum etat_action_t etat_action;
|
|
|
|
float angle, distance;
|
|
|
|
switch(etat_calage_debut){
|
|
case CD_ENVOI_CDE_BORDURE:
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
|
|
etat_calage_debut = CD_LECTURE_BORDURE_Y;
|
|
break;
|
|
|
|
case CD_LECTURE_BORDURE_Y:
|
|
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
|
if(validite == VL53L8_BORDURE){
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
commande_vitesse_stop();
|
|
|
|
if(couleur == COULEUR_BLEU){
|
|
Localisation_set_x(215.);
|
|
Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR));
|
|
//Localisation_set_angle((90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
|
|
Localisation_set_angle((210. * DEGRE_EN_RADIAN));
|
|
}else{
|
|
Localisation_set_x(3000 - 215.);
|
|
Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR));
|
|
Localisation_set_angle((270. * DEGRE_EN_RADIAN));
|
|
}
|
|
|
|
etat_calage_debut = CD_ALLER_POSITION_INIT;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|