Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Interruption du mouvement fonctionnelle

Browse Source
This commit is contained in:
Samuel 2025-03-22 17:15:27 +01:00
parent 37b1b75d5f
commit 0fbf6a4b04
3 changed files with 146 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
Cerveau/Cerveau.ino
View File

@ -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;
}
}

5
Cerveau/Communication_chassis.ino
View File

@ -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;

2
Chassis/Chassis.ino
View File

@ -100,7 +100,7 @@ void setup()
//Serial.println(Men);
I2C_Slave_init(0x55);
I2C_Slave_init(I2C_DEV_ADDR);
I2C_memory = get_i2c_data();