diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 9caa08f..3714860 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -3,8 +3,9 @@ #include #include -#include "Communication_chassis.h" -#include "Communication_detection_adversaire.h" +#include "Com_chassis.h" +#include "Com_detection_adversaire.h" +#include "Com_gradins.h" #include "ServerWeb.h" @@ -99,7 +100,7 @@ int tolerance_position =100; float tolerance_orientation =0.03; // 2° -char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"}; +char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu", "Approche gradin"}; char* statu[] = {"/..","./.","../"}; int index_statu=0; @@ -267,6 +268,8 @@ void gestion_match(){ struct chassis_reception_t chassis_reception; struct chassis_emission_t chassis_emission; struct triangulation_reception_t triangulation_reception; + struct detect_gradin_t detect_gradin; + enum etat_action_t etat_action; static int translation_x_mm, translation_y_mm; static float rotation_rad; static int couleur; @@ -277,6 +280,7 @@ void gestion_match(){ DEPLACEMENT_RELATIF=2, MATCH_EN_COURS=3, TEST_DEPLACEMENT_ABSOLU=4, + TEST_APPROCHE_GRADIN=5 }; switch(index_Maitre){ @@ -307,8 +311,7 @@ void gestion_match(){ translation_y_mm = 0; rotation_rad = 0; - index_Maitre = DEPLACEMENT_RELATIF; - Scan_Triangulation(&triangulation_reception); + index_Maitre = TEST_APPROCHE_GRADIN; } if(M5.BtnB.read() == 1){ Serial.println("BtnB"); @@ -365,6 +368,28 @@ void gestion_match(){ index_Maitre = ATTENTE_ORDRE; } break; + + case TEST_APPROCHE_GRADIN: + if(gradin_approche() != ACTION_EN_COURS){ + index_Maitre = ATTENTE_ORDRE; + affichage_standard_init(); + } + + /* + do{ + char chaine[200]; + Detect_gradin(&detect_gradin); + sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, + detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180); + affiche_msg("Detect gradin", chaine); + // if(detect_gradin.status == 2){ + // while(deplacement_relatif(0, 0, - detect_gradin.angle_rad, 0) != ACTION_TERMINEE); + // } + }while(fabs(detect_gradin.angle_rad / M_PI * 180) > 0.5); + //index_Maitre = ATTENTE_ORDRE; + //affichage_standard_init(); + break; + */ } } @@ -466,6 +491,83 @@ enum etat_action_t Strategie(int couleur){ } +enum etat_action_t gradin_approche(void){ + static enum{ + GA_INIT, + GA_CHERCHE_GAUCHE, + GA_CHERCHE_DROIT, + GA_GOTO_LARGE, + GA_GOTO_PROCHE, + GA_GOTO_PREND + } statu_approche_gradin = GA_INIT; + static float angle_parcouru; + static int nb_erreur; + int translation_x, translation_y; + struct detect_gradin_t detect_gradin; + + Detect_gradin(&detect_gradin); + char chaine[200]; + sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, + detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180); + affiche_msg("Detect gradin", chaine); + + switch(statu_approche_gradin){ + case GA_INIT: + angle_parcouru = 0; + statu_approche_gradin = GA_CHERCHE_GAUCHE; + break; + + case GA_CHERCHE_GAUCHE: + if(detect_gradin.status == 2){ + // On a trouvé ! + statu_approche_gradin = GA_GOTO_LARGE; + nb_erreur = 0; + }else if(detect_gradin.status == 0){ + // On a perdu la détection + statu_approche_gradin = GA_CHERCHE_DROIT; + }else{ + // On tourne à gauche de quelques degrés + while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS); + } + break; + + case GA_CHERCHE_DROIT: + if(detect_gradin.status == 2){ + // On a trouvé ! + statu_approche_gradin = GA_GOTO_LARGE; + nb_erreur = 0; + }else if(detect_gradin.status == 0){ + // On a perdu la détection + statu_approche_gradin = GA_INIT; + return ACTION_ECHEC; + }else{ + // On tourne à gauche de quelques degrés + while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS); + } + break; + + case GA_GOTO_LARGE: + Detect_gradin(&detect_gradin); + if(detect_gradin.status != 2){ + nb_erreur++; + if(nb_erreur > 100){ + affiche_erreur("Gradin Approche", "GA_GOTO_LARGE\n Status != 2"); + while(1); + } + } + translation_x = detect_gradin.centre_y_mm - 400 * cos(detect_gradin.angle_rad); + translation_y = -400 * sin(detect_gradin.angle_rad); + if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){ + statu_approche_gradin = GA_INIT; + return ACTION_TERMINEE; + } + break; + + } + return ACTION_EN_COURS; + +} + /// @brief : compare la position actuelle et la position lue par la balise /// Note : Pour l'instant, on ne déclenche un mouvment qu'en cas d'ecart sur la distance, pas sur l'orientation. @@ -578,6 +680,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, floa /// @param angle_deplacement direction dans laquelle avance le robot, dans le référentiel du robot int detection_adversaire(float angle_deplacement){ int capteur_central, capteur_precedant, capteur_suivant; + struct detect_adv_reception_t detect_adv_reception; // On ramène l'angle entre 0 et 2 PI. while(angle_deplacement < 0){ angle_deplacement += 2 * M_PI; @@ -595,6 +698,8 @@ int detection_adversaire(float angle_deplacement){ if(capteur_suivant > 11){ capteur_suivant = 0; } + Detect_adv_lire(&detect_adv_reception); + if(detect_adv_reception.distance_cm[capteur_central] < 50 || detect_adv_reception.distance_cm[capteur_precedant] < 50 || detect_adv_reception.distance_cm[capteur_suivant] < 50 ){ diff --git a/Cerveau/Communication_chassis.h b/Cerveau/Com_chassis.h similarity index 100% rename from Cerveau/Communication_chassis.h rename to Cerveau/Com_chassis.h diff --git a/Cerveau/Communication_chassis.ino b/Cerveau/Com_chassis.ino similarity index 100% rename from Cerveau/Communication_chassis.ino rename to Cerveau/Com_chassis.ino diff --git a/Cerveau/Communication_detection_adversaire.h b/Cerveau/Com_detection_adversaire.h similarity index 100% rename from Cerveau/Communication_detection_adversaire.h rename to Cerveau/Com_detection_adversaire.h diff --git a/Cerveau/Communication_detection_adversaire.ino b/Cerveau/Com_detection_adversaire.ino similarity index 100% rename from Cerveau/Communication_detection_adversaire.ino rename to Cerveau/Com_detection_adversaire.ino diff --git a/Cerveau/Com_gradins.h b/Cerveau/Com_gradins.h new file mode 100644 index 0000000..8325890 --- /dev/null +++ b/Cerveau/Com_gradins.h @@ -0,0 +1,12 @@ +#ifndef COM_GRADINS_H +#define COM_GRADINS_H + +#define I2C_SLAVE_detect_gradin 0x19 + +struct detect_gradin_t{ + char status; + int centre_x_mm, centre_y_mm; + float angle_rad; +}; + +#endif \ No newline at end of file diff --git a/Cerveau/Com_gradins.ino b/Cerveau/Com_gradins.ino new file mode 100644 index 0000000..fc2dbe4 --- /dev/null +++ b/Cerveau/Com_gradins.ino @@ -0,0 +1,28 @@ +//#include "Chassis.h" +#include +#include +#include "Com_gradins.h" + + +/// @brief Lit les capteurs VL53L1X +void Detect_gradin(struct detect_gradin_t * detect_gradin){ + 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); + if (error !=0){ + affiche_erreur("Detect_gradin", "Erreur I2C"); + 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]; + angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12]; + detect_gradin->angle_rad = angle_mrad / 1000.; + + + + } +} diff --git a/Doc/Communication I2C.odt b/Doc/Communication I2C.odt index 539e883..b78cc56 100644 Binary files a/Doc/Communication I2C.odt and b/Doc/Communication I2C.odt differ