From 0fbf6a4b04e1cd3130dfa3ce90da3d3eab5f57ee Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 22 Mar 2025 17:15:27 +0100 Subject: [PATCH] Interruption du mouvement fonctionnelle --- Cerveau/Cerveau.ino | 213 ++++++++++++++++++++---------- Cerveau/Communication_chassis.ino | 5 +- Chassis/Chassis.ino | 2 +- 3 files changed, 146 insertions(+), 74 deletions(-) diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index b9ac582..16c87d0 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -14,8 +14,19 @@ #define MOUVEMENT_FINI 0x4 #define MOUVEMENT_EN_COURS 0x2 +#define MOUVEMENT_INTERRUPTION 0x1 + +#define VITESSE_STANDARD 1000 +#define ACCELERATION_STANDARD 500 #define gst_server; + +struct triangulation_reception_t { + int pos_x_mm, pos_y_mm; + float angle_rad; + bool validite; +}; + const char* ssid = "riombotique"; const char* password = "password"; // adresse du >Pi qui gere le serveur : 192.168.99.100 @@ -85,6 +96,11 @@ char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", char* statu[] = {"/..","./.","../"}; int index_statu=0; +enum etat_action_t{ + ACTION_EN_COURS, + ACTION_TERMINEE, + ACTION_ECHEC +}; void setup() { // put your setup code here, to run once: @@ -109,15 +125,22 @@ void setup() { // MemPosition_X = 800; // MemPosition_Y = 800; struct detect_adv_reception_t detect_adv_reception; - + struct chassis_reception_t chassis_reception; + int i=0; + /* while(1){ + Detect_adv_lire(&detect_adv_reception); - char chaine[40]; - sprintf(chaine, "Distance: %d cm\n Distance: %d cm\n", detect_adv_reception.distance_cm[0], - detect_adv_reception.distance_cm[11]); - affiche_msg("Scan_Detect_adversaire", chaine); - } - + char chaine[100]; + sprintf(chaine, "Distance: %d cm\n Distance: %d cm\nNb com I2C:%d", detect_adv_reception.distance_cm[0], + detect_adv_reception.distance_cm[11], i++); + if(i%100 == 0){ + affiche_msg("Scan_Detect_adversaire", chaine); + } + + Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis + }*/ + affichage_standard_init(); //M5.Lcd.setCursor(10, 200);M5.Lcd.print("cmd :"); @@ -236,8 +259,18 @@ void affiche_msg(char * chaine1, char * chaine2){ void strategie(){ struct chassis_reception_t chassis_reception; struct chassis_emission_t chassis_emission; + struct triangulation_reception_t triangulation_reception; + static int translation_x_mm, translation_y_mm; + static float rotation_rad; + + enum etat_strategie{ + ATTENTE_ORDRE=0, + LECTURE_TRIANGULATION=1, + DEPLACEMENT_RELATIF=2, + }; + switch(index_Maitre){ - case 0 : //--------------------------------------------------------------------------------------------------------- + case ATTENTE_ORDRE : //--------------------------------------------------------------------------------------------------------- /// Lecture des commandes venant du serveur web et envoi des commandes au chassis Web_gestion(); M5.update(); @@ -246,82 +279,62 @@ void strategie(){ if(Web_nouveau_message()){ if(Web_type_requete() == WEB_DEPLACEMENT_RELATIF){ chassis_emission = Web_get_donnees(); - send_Chassis(&chassis_emission); + translation_x_mm = chassis_emission.translation_x_mm; + translation_y_mm = chassis_emission.translation_y_mm; + rotation_rad = chassis_emission.rotation_z_rad; } Serial.printf("vit:%d, corrige: %d\n", vit, corrige); MemCmd_X = xA; //Memorisation de la comande pour comparaison avec arrivé MemCmd_Y = yA; MemCmd_A = rot; //index_Maitre = 1; // Bloc le serveur WEB si on n'a pas la triangulation - index_Maitre = 2; + index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnA.read() == 1){ // Déplacement en X - chassis_emission.translation_x_mm = 2000; - chassis_emission.translation_y_mm = 0; - chassis_emission.rotation_z_rad = 0; - chassis_emission.vitesse = 2000; - chassis_emission.acceleration = 1500; - - send_Chassis(&chassis_emission); - index_Maitre = 2; + translation_x_mm = 10000; + translation_y_mm = 0; + rotation_rad = 0; + + index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnB.read() == 1){ // Déplacement en Y - chassis_emission.translation_x_mm = 0; - chassis_emission.translation_y_mm = 2000; - chassis_emission.rotation_z_rad = 0; - chassis_emission.vitesse = 2000; - chassis_emission.acceleration = 1500; - - send_Chassis(&chassis_emission); - index_Maitre = 2; + translation_x_mm = 0; + translation_y_mm = 2000; + rotation_rad = 0; + + index_Maitre = DEPLACEMENT_RELATIF; } if(M5.BtnC.read() == 1){ // Rotation - chassis_emission.translation_x_mm = 0; - chassis_emission.translation_y_mm = 0; - chassis_emission.rotation_z_rad = 100; - chassis_emission.vitesse = 2000; - chassis_emission.acceleration = 1500; + translation_x_mm = 0; + translation_y_mm = 0; + rotation_rad = 100; - send_Chassis(&chassis_emission); - index_Maitre = 2; + index_Maitre = DEPLACEMENT_RELATIF; } break; case 1 : //--------------------------------------------------------------------------------------------------------- - Scan_Triangulation(); //Prise de la position actuel - MemPosition_X = Position_actuelle_X; - MemPosition_Y = Position_actuelle_Y; - if(Balises_OK == true && error == 0){ //triangulation calcul valide ************** a prendre sur I2c - index_Maitre = 2; - }else{ - //print probleme de lecture triangulatation - } - //Si position Pas OK ****************** - break; - - case 2 : // Deplacement relatif en cours - Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis - // Pour l'instant, Scan_chassis bloque le programme en cas de problème de communication - if(Err_Chassi_com !=0){ - for(int j=0; j<10; j++){ - affichage_resultats(); - Scan_chassis(&chassis_reception); - //delay(500); - } - // ********************************* ERREUR DE COM FATAL *********************** - } - if(chassis_reception.status != MOUVEMENT_FINI){ - + Scan_Triangulation(&triangulation_reception); //Prise de la position actuel + MemPosition_X = Position_actuelle_X; + MemPosition_Y = Position_actuelle_Y; + if(Balises_OK == true && error == 0){ //triangulation calcul valide ************** a prendre sur I2c + index_Maitre = 2; }else{ - send_Chassis_RAZ(); // Immobilisation du robot - index_Maitre = 0; // On attend l'ordre suivant - } - + //print probleme de lecture triangulatation + } + //Si position Pas OK ****************** + break; + + case DEPLACEMENT_RELATIF : // Deplacement relatif en cours + + if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) == ACTION_TERMINEE){ + index_Maitre = ATTENTE_ORDRE; + } break; } } @@ -331,12 +344,12 @@ 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 - + struct triangulation_reception_t triangulation_reception; X_futur = MemCmd_X; Y_futur = MemCmd_Y; - Scan_Triangulation(); //Prise de la position actuel + 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) @@ -368,18 +381,72 @@ void compar_cinematique(){ } } +/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire +/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire +enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int rotation_rad, int evitement){ + static enum{ + DR_INIT, + DR_MVT_EN_COUR, + } etat_deplacement = DR_INIT; + + struct chassis_emission_t chassis_emission; + struct chassis_reception_t chassis_reception; + struct detect_adv_reception_t detect_adv_reception; + + switch(etat_deplacement){ + case DR_INIT: + chassis_emission.status = MOUVEMENT_EN_COURS; + chassis_emission.translation_x_mm = distance_x_mm; + chassis_emission.translation_y_mm = distance_y_mm; + chassis_emission.rotation_z_rad = rotation_rad; + chassis_emission.vitesse = VITESSE_STANDARD; + chassis_emission.acceleration = ACCELERATION_STANDARD; + + send_Chassis(&chassis_emission); + etat_deplacement = DR_MVT_EN_COUR; + break; + + case DR_MVT_EN_COUR: + if(evitement){ + // On lit les capteurs + Detect_adv_lire(&detect_adv_reception); + // On analyse les valeurs - TODO : créer une fonction plus évoluée + if(detect_adv_reception.distance_cm[0] < 50){ + // Arrêt du mouvement + chassis_emission.status = MOUVEMENT_INTERRUPTION; + send_Chassis(&chassis_emission); + } + } + + Scan_chassis(&chassis_reception); + + if(chassis_reception.status == MOUVEMENT_FINI){ + etat_deplacement = DR_INIT; + return ACTION_TERMINEE; + } + break; + } + + return ACTION_EN_COURS; +} + +/// @brief /// @brief Récupère position (X, Y) et l'orientation du robot -void Scan_Triangulation(){ +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) 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;} + 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){ - Position_actuelle_X = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ; - Position_actuelle_Y = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ; + 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.; @@ -388,13 +455,17 @@ void Scan_Triangulation(){ lec_Balise_3 = (tampon2[0] >>2 )& 0x01; lec_Calcul_ok = (tampon2[0] >>3 )& 0x01; - if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){Position_actuelle_X = 9999;} - if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){Position_actuelle_Y = 9999;} + 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){ - Balises_OK = true; + triangulation_reception->validite = true; }else{ - Balises_OK = false; + triangulation_reception->validite = false; } } diff --git a/Cerveau/Communication_chassis.ino b/Cerveau/Communication_chassis.ino index c525e67..1ce5b0e 100644 --- a/Cerveau/Communication_chassis.ino +++ b/Cerveau/Communication_chassis.ino @@ -25,7 +25,7 @@ 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 - Mot[0] = MOUVEMENT_EN_COURS; + Mot[0] = chassis_emission->status; //y*=-1; //y = y*direction; Mot[1] = chassis_emission->translation_x_mm >>8; @@ -35,7 +35,8 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){ //Serial.println(y); Mot[5] = chassis_emission->rotation_z_rad >>8; Mot[6] = chassis_emission->rotation_z_rad; - Mot[7] = chassis_emission->vitesse >>8; Mot[8] = chassis_emission->vitesse; + Mot[7] = chassis_emission->vitesse >>8; + Mot[8] = chassis_emission->vitesse; Mot[9] = chassis_emission->acceleration >>8; Mot[10] = chassis_emission->acceleration; diff --git a/Chassis/Chassis.ino b/Chassis/Chassis.ino index c1b0d36..171e5e9 100644 --- a/Chassis/Chassis.ino +++ b/Chassis/Chassis.ino @@ -100,7 +100,7 @@ void setup() //Serial.println(Men); - I2C_Slave_init(0x55); + I2C_Slave_init(I2C_DEV_ADDR); I2C_memory = get_i2c_data();