Petite correction com + start 2nd gradin qui marche pas shunté

This commit is contained in:
Samuel 2025-05-29 22:11:03 +02:00
parent c76c939c78
commit f3dc47a348
5 changed files with 91 additions and 49 deletions

View File

@ -449,6 +449,12 @@ enum etat_action_t Strategie(int couleur){
STRAT_ALLER_GRADINS_1_C, // Déplacement relatif
STRAT_DEPOSE_GRADIN_1A, // Empile gradin
STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
STRAT_ALLER_GRADINS_2_A, // Tourne 80 °
STRAT_ALLER_GRADINS_2_B, // Avance 20 cm
STRAT_ALLER_GRADINS_2_C, // Cherche gradin
STRAT_ALLER_GRADINS_2_D,
//
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
STRAT_ATTENTE_BACKSTAGE, // Attente
STRAT_ALLER_BACKSTAGE, // Déplacement relatif
@ -522,6 +528,44 @@ enum etat_action_t Strategie(int couleur){
etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
}
case STRAT_ALLER_GRADINS_2_A: // Tourne 80 °
translation_x_mm = 0;
if(couleur == COULEUR_JAUNE){
rotation_rad = 80. * M_PI /180.;
}else{
rotation_rad = -80. * M_PI /180.;
}
translation_y_mm = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_2_B;
}
break;
case STRAT_ALLER_GRADINS_2_B: // Avance 20 cm
translation_x_mm = 20;
rotation_rad = 0;
translation_y_mm = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_2_C;
}
break;
case STRAT_ALLER_GRADINS_2_C: // Cherche gradin
etat_action = gradin_approche();
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_strategie = STRAT_ALLER_GRADINS_2_D;
}
break;
case STRAT_ALLER_GRADINS_2_D:
etat_action = actionneur_action(ACTIONNEUR_EMPILE);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_FIN;
}
break;
case STRAT_ALLER_PREPA_BACKSTAGE:
if(couleur == COULEUR_JAUNE){

View File

@ -9,19 +9,20 @@ int Scan_chassis(struct chassis_reception_t * chassis_reception){
/// @brief Lit l'état du chassis en I2C
int Scan_chassis(struct chassis_reception_t * chassis_reception, bool continuous_try){
unsigned char tampon[14];
unsigned char tampon2[14];
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon, 12);
while(error !=0 && continuous_try){
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
while (error !=0 && continuous_try){
Err_Chassi_com =1;IndexErr = 2;
affiche_erreur("Scan_Chassi", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon, 12);
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
}
if(error == 0){
Err_Chassi_com =0;
IndexErr = 0;
Mvt_finit = (MOUVEMENT_FINI == tampon[0]);
chassis_reception->status = tampon[0];
Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
chassis_reception->status = tampon2[0];
}
return error;
@ -53,12 +54,12 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
Mot[9] = chassis_emission->acceleration >>8;
Mot[10] = chassis_emission->acceleration;
do{
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
while (error !=0){
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
}while(!error);
if (error !=0){Err_Cha_send =1; IndexErr = 5;}
else{Err_Cha_send =0;IndexErr = 0;}
Err_Cha_send =1; IndexErr = 5;
}
Err_Cha_send =0;IndexErr = 0;
if(error==0){ //si pas d'erreur de transmission alors RAZ des valeurs
nbr_essai ++;
cmd_chassi_x = 0;

View File

@ -3,16 +3,20 @@
#include <HardwareSerial.h>
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
return Detect_adv_lire(detect_adv_reception, false);
return Detect_adv_lire(detect_adv_reception, true);
}
/// @brief Lit les capteurs VL53L1X
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool continuous_try){
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
unsigned char tampon2[14];
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
while(error !=0 && continuous_try){
if (error !=0){
affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
if(blocking){
while(1);
}
}else{
Serial.println("I2C OK");
}
return error;
}

View File

@ -9,17 +9,19 @@ int Detect_gradin(struct detect_gradin_t * detect_gradin){
}
/// @brief Lit les capteurs VL53L1X
int Detect_gradin(struct detect_gradin_t * detect_gradin, bool continuous_try){
int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
unsigned char tampon[14];
char chaine[200];
int angle_mrad;
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
while (error !=0 && continuous_try){
if (error !=0){
affiche_erreur("Detect_gradin", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
}
if(!error){
if (blocking){
while(1);
}
}else{
detect_gradin->status = tampon[0];
detect_gradin->centre_x_mm = tampon[1] << 24 | tampon[2] << 16 | tampon[3] << 8 | tampon[4];
detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8];

View File

@ -4,19 +4,22 @@ int Scan_Triangulation(struct triangulation_reception_t * triangulation_receptio
/// @brief Récupère position (X, Y) et l'orientation du robot
int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception, bool continuous_try){
int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception, bool blocking){
unsigned char tampon2[14];
lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
triangulation_reception->validite = false;
error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13); // si errror != de 0 alors erreur de communication
while (error !=0 && continuous_try){
if (error !=0){
Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13); // si errror != de 0 alors erreur de communication
affiche_erreur("Scan_Triangulation", "Erreur I2C");
if(blocking){
while(1);
}
return error;
}
else{Err_Tri_com =0;IndexErr = 0;}
if (error ==0){
Err_Tri_com =0;IndexErr = 0;
triangulation_reception->pos_x_mm = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ;
triangulation_reception->pos_y_mm = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ;
@ -36,42 +39,30 @@ int Scan_Triangulation(struct triangulation_reception_t * triangulation_receptio
if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){
triangulation_reception->pos_y_mm = 9999;
}
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
triangulation_reception->validite = true;
}
}
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
triangulation_reception->validite = true;
}
return error;
}
int Triangulation_send_immobile(int immobile){
return Triangulation_send_immobile(immobile, true);
}
int Triangulation_send_immobile(int immobile, bool continuous_try){
void Triangulation_send_immobile(int immobile){
unsigned char donnee=0;
if(immobile){
donnee = 1;
}
error = I2C_ecrire_registre(I2C_SLAVE_trian, 13, &donnee, 1); // si errror != de 0 alors erreur de communication
while (error !=0 && continuous_try){
if (error !=0){
affiche_erreur("Send_Triangulation", "Erreur I2C");
error = I2C_ecrire_registre(I2C_SLAVE_trian, 13, &donnee, 1); // si errror != de 0 alors erreur de communication
while(1);
}
return error;
}
int Triangulation_send_config(unsigned char configuration){
Triangulation_send_config(configuration, true);
}
int Triangulation_send_config(unsigned char configuration, bool continuous_try){
void Triangulation_send_config(unsigned char configuration){
error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1); // si errror != de 0 alors erreur de communication
while (error !=0 || continuous_try){
affiche_erreur("Triangulation_send_config", "Erreur I2C");
error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1); // si errror != de 0 alors erreur de communication
if (error !=0){
affiche_erreur("Send_Triangulation", "Erreur I2C");
while(1);
}
return error;
}
}