diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 16c87d0..531def3 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -92,7 +92,7 @@ bool Balises_OK = 0; int tolerance_position =100; -char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position"}; +char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"}; char* statu[] = {"/..","./.","../"}; int index_statu=0; @@ -152,7 +152,7 @@ void loop() { static int64_t time; struct chassis_reception_t chassis_reception; - strategie(); + gestion_match(); affichage_resultats(); delay(10); } @@ -256,7 +256,7 @@ void affiche_msg(char * chaine1, char * chaine2){ M5.Lcd.print(chaine2); } -void strategie(){ +void gestion_match(){ struct chassis_reception_t chassis_reception; struct chassis_emission_t chassis_emission; struct triangulation_reception_t triangulation_reception; @@ -267,6 +267,8 @@ void strategie(){ ATTENTE_ORDRE=0, LECTURE_TRIANGULATION=1, DEPLACEMENT_RELATIF=2, + MATCH_EN_COURS=3, + TEST_DEPLACEMENT_ABSOLU=4, }; switch(index_Maitre){ @@ -292,19 +294,21 @@ void strategie(){ } if(M5.BtnA.read() == 1){ // Déplacement en X - translation_x_mm = 10000; + translation_x_mm = 2000; translation_y_mm = 0; rotation_rad = 0; index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnB.read() == 1){ + Serial.println("BtnB"); // Déplacement en Y + translation_x_mm = 0; translation_y_mm = 2000; rotation_rad = 0; - index_Maitre = DEPLACEMENT_RELATIF; + index_Maitre = TEST_DEPLACEMENT_ABSOLU; } if(M5.BtnC.read() == 1){ @@ -336,49 +340,96 @@ void strategie(){ index_Maitre = ATTENTE_ORDRE; } break; + + case MATCH_EN_COURS: + break; + + case TEST_DEPLACEMENT_ABSOLU: + if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){ + index_Maitre = ATTENTE_ORDRE; + } + break; + } +} +/// @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. +void compar_cinematique(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, + struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){ + + compar_X = consigne_x_mm - triangulation_reception.pos_x_mm; //compar de la position théoriquement atteinte avec la position actuel + compar_Y = consigne_y_mm - triangulation_reception.pos_y_mm; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique) + printf("compar_X:%d\ncompar_y:%d\n", compar_X, compar_Y); + + if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){ + chassis_emission->status = MOUVEMENT_FINI; + }else{ + // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue + + // Distance à parcourir + float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y)); + float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X); + + chassis_emission->translation_x_mm = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; + chassis_emission->translation_y_mm = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; + chassis_emission->rotation_z_rad = 0; + chassis_emission->status = MOUVEMENT_EN_COURS; } } -void compar_cinematique(){ - - // Consigne de position à atteindre en X, Y et angle par rapport à la position actuel - // dans le repère du terrain - //Position à atteindre théorique +enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, int evitement){ + static enum{ + DA_INIT, + DA_COMPARE_POSITIONS, + DA_MVT_EN_COUR, + } etat_deplacement = DA_INIT; + static int mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad; + static struct chassis_emission_t chassis_emission; struct triangulation_reception_t triangulation_reception; + enum etat_action_t etat_deplacement_relatif; - X_futur = MemCmd_X; - Y_futur = MemCmd_Y; + switch(etat_deplacement){ + case DA_INIT: + mem_consigne_x_mm = consigne_x_mm; + mem_consigne_y_mm = consigne_y_mm; + mem_consigne_orientation_rad = consigne_orientation_rad; + etat_deplacement = DA_COMPARE_POSITIONS; + Serial.printf("DA_INIT\n"); + break; - Scan_Triangulation(&triangulation_reception); //Prise de la position actuel - if(Balises_OK == true && error == 0){ //triangulation calcul valide ************** a prendre sur I2c - compar_X = X_futur - Position_actuelle_X; //compar de la position théoriquement atteinte avec la position actuel - compar_Y = Y_futur - Position_actuelle_Y; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique) - - if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){ - Mvt_tolerance_OK = true; - }else{ - // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue - - // Distance à parcourir - float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y)); - float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X); - - float distance_Y_ref_robot = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; - float distance_X_ref_robot = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee; - - if(corrige == false){ - mem_x = distance_X_ref_robot; // faire une memoire et travailler avec - mem_y = distance_Y_ref_robot; - corrige = true; + case DA_COMPARE_POSITIONS: + Serial.printf("Scan_Triangulation\n"); + Scan_Triangulation(&triangulation_reception); //Prise de la position actuel + if(triangulation_reception.validite == true){ + Serial.printf("Compare cinematique\n"); + compar_cinematique(mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad, + triangulation_reception, &chassis_emission); + if(chassis_emission.status == MOUVEMENT_EN_COURS){ + // C'est que la fonction compar_cinematique indique qu'on doit se déplacer + // Les valeurs du déplacement sont renseignées dans "chassis_emission". + Serial.printf("DA_MVT_EN_COUR\n"); + Serial.printf("trans_x:%d\ntrans_y:%d\nrot:%d\n",chassis_emission.translation_x_mm, + chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad); + etat_deplacement = DA_MVT_EN_COUR; + }else{ + // Alors nous sommes arrivés + // On réinitialise la mahcine à état + etat_deplacement = DA_INIT; + return ACTION_TERMINEE; + } } - Mvt_tolerance_OK = false; - } - }else{ - compar_X =0 ; //compar de la position théoriquement atteinte avec la position actuel - compar_Y =0 ; - mem_x =0 ; //compar de la position théoriquement atteinte avec la position actuel - mem_y =0 ; + break; + + case DA_MVT_EN_COUR: + etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm, + -chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement); + if(etat_deplacement_relatif == ACTION_TERMINEE){ + Serial.printf("DA_COMPARE_POSITIONS\n"); + etat_deplacement = DA_COMPARE_POSITIONS; + } + break; } + + return ACTION_EN_COURS; } /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire @@ -437,6 +488,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti 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 if (error !=0){ Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0; @@ -464,8 +516,6 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti } if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){ triangulation_reception->validite = true; - }else{ - triangulation_reception->validite = false; } } diff --git a/Cerveau/Communication_chassis.ino b/Cerveau/Communication_chassis.ino index 1ce5b0e..65461c9 100644 --- a/Cerveau/Communication_chassis.ino +++ b/Cerveau/Communication_chassis.ino @@ -13,7 +13,6 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){ affiche_erreur("Scan_Chassi", "Erreur I2C"); while(1); }else{ - Serial.println("I2C OK"); Err_Chassi_com =0; IndexErr = 0;