diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 531def3..123af75 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -293,31 +293,31 @@ void gestion_match(){ index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnA.read() == 1){ + Serial.println("BtnB"); // Déplacement en X - translation_x_mm = 2000; + translation_x_mm = 500; translation_y_mm = 0; rotation_rad = 0; - index_Maitre = DEPLACEMENT_RELATIF; + //index_Maitre = DEPLACEMENT_RELATIF; + Scan_Triangulation(&triangulation_reception); } if(M5.BtnB.read() == 1){ Serial.println("BtnB"); - // Déplacement en Y - - translation_x_mm = 0; - translation_y_mm = 2000; - rotation_rad = 0; - + // Déplacement en Y + //Triangulation_send_immobile(0); index_Maitre = TEST_DEPLACEMENT_ABSOLU; } if(M5.BtnC.read() == 1){ // Rotation + Serial.println("BtnC"); translation_x_mm = 0; translation_y_mm = 0; rotation_rad = 100; + Triangulation_send_immobile(1); - index_Maitre = DEPLACEMENT_RELATIF; + //index_Maitre = DEPLACEMENT_RELATIF; } break; @@ -345,7 +345,7 @@ void gestion_match(){ break; case TEST_DEPLACEMENT_ABSOLU: - if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){ + if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){ index_Maitre = ATTENTE_ORDRE; } break; @@ -407,7 +407,9 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int // 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, + Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%d\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm, + triangulation_reception.angle_rad); + Serial.printf("trans_x:%d\ttrans_y:%d\trot:%d\n",chassis_emission.translation_x_mm, chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad); etat_deplacement = DA_MVT_EN_COUR; }else{ @@ -420,6 +422,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int break; case DA_MVT_EN_COUR: + Scan_Triangulation(&triangulation_reception); //Prise de la position actuel 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){ @@ -454,6 +457,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int chassis_emission.acceleration = ACCELERATION_STANDARD; send_Chassis(&chassis_emission); + Triangulation_send_immobile(0); etat_deplacement = DR_MVT_EN_COUR; break; @@ -472,6 +476,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int Scan_chassis(&chassis_reception); if(chassis_reception.status == MOUVEMENT_FINI){ + Triangulation_send_immobile(1); etat_deplacement = DR_INIT; return ACTION_TERMINEE; } @@ -481,45 +486,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int return ACTION_EN_COURS; } -/// @brief - -/// @brief Récupère position (X, Y) et l'orientation du robot -void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){ - 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; - affiche_erreur("Scan_Triangulation", "Erreur I2C"); - while(1); - } - else{Err_Tri_com =0;IndexErr = 0;} - if (error ==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] ; - Angle_Robot_DEG_int = tampon2[9]<< 24 | tampon2[10]<< 16 | tampon2[11]<< 8 | tampon2[12] ; - // Serial.print(tampon2[9], BIN);Serial.print(" ");Serial.print(tampon2[10], BIN);Serial.print(" ");Serial.print(tampon2[11], BIN);Serial.print(" ");Serial.println(tampon2[12], BIN); - Angle_Robot_RAD = Angle_Robot_DEG_int * M_PI / 180.; - lec_Balise_1 = tampon2[0] & 0x01; - lec_Balise_2 = (tampon2[0] >>1) & 0x01; - lec_Balise_3 = (tampon2[0] >>2 )& 0x01; - lec_Calcul_ok = (tampon2[0] >>3 )& 0x01; - - if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){ - triangulation_reception->pos_x_mm = 9999; - } - 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; - } -} - - void scan_I2C_bus(){ char error, address; diff --git a/Cerveau/Com_triangulation.ino b/Cerveau/Com_triangulation.ino new file mode 100644 index 0000000..87e7947 --- /dev/null +++ b/Cerveau/Com_triangulation.ino @@ -0,0 +1,49 @@ +/// @brief Récupère position (X, Y) et l'orientation du robot +void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){ + 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; + affiche_erreur("Scan_Triangulation", "Erreur I2C"); + while(1); + } + else{Err_Tri_com =0;IndexErr = 0;} + if (error ==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] ; + + Angle_Robot_DEG_int = tampon2[9]<< 24 | tampon2[10]<< 16 | tampon2[11]<< 8 | tampon2[12] ; + // Serial.print(tampon2[9], BIN);Serial.print(" ");Serial.print(tampon2[10], BIN);Serial.print(" ");Serial.print(tampon2[11], BIN);Serial.print(" ");Serial.println(tampon2[12], BIN); + Angle_Robot_RAD = Angle_Robot_DEG_int * M_PI / 180.; + Serial.printf("Scan triangulation 0: 0x%X\n", tampon2[0]); + lec_Balise_1 = tampon2[0] & 0x01; + lec_Balise_2 = (tampon2[0] >>1) & 0x01; + lec_Balise_3 = (tampon2[0] >>2 )& 0x01; + lec_Calcul_ok = (tampon2[0] >>3 )& 0x01; + + if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){ + triangulation_reception->pos_x_mm = 9999; + } + 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; + } +} + +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 + if (error !=0){ + affiche_erreur("Send_Triangulation", "Erreur I2C"); + while(1); + } +} \ No newline at end of file diff --git a/Cerveau/Communication_chassis.ino b/Cerveau/Communication_chassis.ino index 65461c9..57d6766 100644 --- a/Cerveau/Communication_chassis.ino +++ b/Cerveau/Communication_chassis.ino @@ -24,16 +24,24 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){ void send_Chassis(struct chassis_emission_t * chassis_emission){ //if(nbr_essai<=10){ // Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet + int nb_pas_x, nb_pas_y, nb_pas_rot; + + // Conversion des mm ou radian en pas + + nb_pas_x = chassis_emission->translation_x_mm * 4.049; + nb_pas_y = chassis_emission->translation_y_mm * 4.953; + nb_pas_rot = chassis_emission->rotation_z_rad * 791.; + Mot[0] = chassis_emission->status; //y*=-1; //y = y*direction; - Mot[1] = chassis_emission->translation_x_mm >>8; - Mot[2] = chassis_emission->translation_x_mm; - Mot[3] = chassis_emission->translation_y_mm >>8; - Mot[4] = chassis_emission->translation_y_mm; + Mot[1] = nb_pas_x >>8; + Mot[2] = nb_pas_x; + Mot[3] = nb_pas_y >>8; + Mot[4] = nb_pas_y; //Serial.println(y); - Mot[5] = chassis_emission->rotation_z_rad >>8; - Mot[6] = chassis_emission->rotation_z_rad; + Mot[5] = nb_pas_rot >>8; + Mot[6] = nb_pas_rot; Mot[7] = chassis_emission->vitesse >>8; Mot[8] = chassis_emission->vitesse; Mot[9] = chassis_emission->acceleration >>8; diff --git a/Cerveau/ServerWeb.ino b/Cerveau/ServerWeb.ino index 31934d6..5bfa271 100644 --- a/Cerveau/ServerWeb.ino +++ b/Cerveau/ServerWeb.ino @@ -91,7 +91,7 @@ void handleForm() { // y= myString1.toInt() * coef_mvt/10; chassis_emission_web.translation_y_mm = myString1.toInt(); String myString2 = server.arg("R"); //positon de cmd en Rotation Deg ° - chassis_emission_web.rotation_z_rad = myString2.toInt() * 13.88888; + chassis_emission_web.rotation_z_rad = myString2.toInt() / 180 * M_PI; String myString3 = server.arg("V"); // Vitesse de cmd en chassis_emission_web.vitesse = myString3.toInt(); String myString4 = server.arg("A"); // Acceleration de cmd diff --git a/Doc/Communication I2C.odt b/Doc/Communication I2C.odt index 97ed388..539e883 100644 Binary files a/Doc/Communication I2C.odt and b/Doc/Communication I2C.odt differ diff --git a/Triangulation/I2C_Slave_lib.ino b/Triangulation/I2C_Slave_lib.ino index 9e1daa5..9953462 100644 --- a/Triangulation/I2C_Slave_lib.ino +++ b/Triangulation/I2C_Slave_lib.ino @@ -5,6 +5,10 @@ byte memoire_I2C_index=0; bool nouveau_message=false; +uint8_t * get_i2c_data(){ + return memoire_I2C; +} + void onRequest(){ uint32_t taille_envoi; taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX); diff --git a/Triangulation/Triangulation.ino b/Triangulation/Triangulation.ino index 328ab0e..bf43e20 100644 --- a/Triangulation/Triangulation.ino +++ b/Triangulation/Triangulation.ino @@ -19,6 +19,7 @@ String Lecture; #define pi 3.141592653589793 #define pi2 6.283185307179586 +uint8_t * data_i2C; int Balise[4][2]; // 4 balises potentielles i de 0 a 3 // Frequence modulation individuelle balises // Balise[i][0] est la frequence de la balise @@ -190,7 +191,13 @@ void setup() { //Initialisation wifi WiFi.config(local_IP, gateway, subnet); WiFi.begin(ssid, password); + + // Pour accéder aux données de l'I2C + data_i2C = get_i2c_data(); + data_i2C[13] = 1; // On dit que le robot est immobile. + int test_wifi = 0; + while (WiFi.status() != WL_CONNECTED){ delay(300); M5.Lcd.print("."); @@ -260,6 +267,8 @@ void setup() { M5.Lcd.print("a RAD = "); M5.Lcd.setCursor(0,100); M5.Lcd.print("angle = "); + M5.Lcd.setCursor(10,120); + M5.Lcd.print("Status : "); M5.Lcd.setCursor(160,180); M5.Lcd.print("X = "); M5.Lcd.setCursor(160,210); @@ -268,6 +277,15 @@ void setup() { void loop() { //Effectue a chaque tour ........................ + if(data_i2C[13] != 1){ + Nb_Balises = 0; + Balise_0 = false; + Balise_1 = false; + Balise_2 = false; + Calcul_Valide = false; + uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3); + I2C_envoi_8bits(etat_balises,0); + } if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){ Old_Nb_tours = Nb_tours; if (!Balise_Valide){ @@ -316,6 +334,10 @@ void loop() { } vitesse_moteur(); } + // Si le robot n'est pas immobile, on invalide les balises. + + + //________________________________________ } void rapport(){ @@ -360,6 +382,13 @@ void affichage_resultats() { M5.Lcd.print(" "); M5.Lcd.setCursor(140,40); M5.Lcd.print(Balise[2][1]); + M5.Lcd.print(" "); + M5.Lcd.setCursor(140,120); + M5.Lcd.print(Balise_0); + M5.Lcd.setCursor(150,120); + M5.Lcd.print(Balise_1); + M5.Lcd.setCursor(160,120); + M5.Lcd.print(Balise_2); // M5.Lcd.print(" / "); // M5.Lcd.print(Angle_B[2][3]); M5.Lcd.print(" "); @@ -447,3 +476,5 @@ void checkForClient(){ } } + +