Petite correction com + start 2nd gradin qui marche pas shunté
This commit is contained in:
parent
c76c939c78
commit
f3dc47a348
@ -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){
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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];
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
Loading…
Reference in New Issue
Block a user