Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
master
Robot2025/Cerveau/Cerveau.ino
Samuel bce282cc61 Premisse de la stratégie
2025-04-01 17:54:52 +02:00

611 lines
20 KiB
C++
Raw Permalink Blame History

#include <M5Core2.h>
#include <Wire.h>
#include <WiFi.h>
#include <math.h>
#include "Communication_chassis.h"
#include "Communication_detection_adversaire.h"
#include "ServerWeb.h"
#define WIRE Wire
#define Rad_Deg 57.2957795130823
#define MOUVEMEMENT_ROTATION 0x10
#define MOUVEMEMENT_TRANSLATION 0x8
#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
IPAddress local_IP(192, 168, 99, 101);
IPAddress gateway(192, 168, 99, 1);
IPAddress subnet(255, 255, 255, 0);
// const char* ssid = "metrofocus_etage";
// const char* password = "19282915001";
// IPAddress local_IP(192, 168, 1, 99);
// IPAddress gateway(192, 168, 1, 1);
// IPAddress subnet(255, 255, 255, 0);
const int16_t I2C_MASTER = 0x42;
const int16_t I2C_SLAVE_trian = 0x30;
uint8_t test = 32;
uint32_t i = 0;
int16_t cmd_chassi_x = 0;
int16_t xA =0;
int16_t cmd_chassi_y = 0;
int16_t yA =0;
int16_t rot = 0;
int16_t MemCmd_A = 0;
int16_t Cmd_Angle = 0;
int16_t vit = 0;
int16_t acc = 0;
int16_t MemCmd_X = 800;
int16_t MemCmd_Y = 800;
int16_t mem_x = 0; // faire une memoire et travailler avec
int16_t mem_y = 0;
bool corrige = false;
int coef_mvt = 39; // coef mise en mm de la commande de mouvement
uint8_t Mot[] = {0,0,0,0,0,0,0,0,0,0,0,0}; /*{0,0,0,0,205,206,207,208,209,210,211};*/
bool Mvt_finit = 0;
int toto = 0;
int direction = 1;
int nbr_essai = 0;
int Position_actuelle_X, Position_actuelle_Y, MemPosition_X, MemPosition_Y, X_futur, Y_futur, compar_X, compar_Y, Angle_Robot_DEG_int; // Coordonée X Y de la triangulation
float Angle_Robot_RAD =0;
int PosLimNeg = -100000;
int PosLimPos = 100000;
bool lec_Balise_1 =0;
bool lec_Balise_2 =0;
bool lec_Balise_3 =0;
bool lec_Calcul_ok =0;
uint8_t error =0; // erreur de lecture I2C
bool Err_Tri_com =0;
bool Err_Chassi_com=0;
bool Err_Cha_send=0;
const char* ErreurListe[] = {"......", "Triang", "Chassi", "ChaRaz", "TriRaz", "ChSend"};
int IndexErr = 0;
int index_Maitre = 0;
//enum Step {Initial, MemCmdMvt, MemActPos, Verif_Chassis_Rdy, Send_Chassis, Verif_Chassis_CmdOk, Verif_Fin_Mvt, ComparPos};
//Step stp;
bool Mvt_tolerance_OK =false;
bool Balises_OK = 0;
int tolerance_position =100;
float tolerance_orientation =0.05; // 3°
char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
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:
Serial.begin(115200);
M5.begin(); //Initialize M5Core2
M5.Lcd.setTextSize(2);
M5.Lcd.print("Bonjour\nEcran\n\nRecherche Wifi");
Initialisation_wifi();
Web_init();
WIRE.begin();
M5.Lcd.clear();
M5.Lcd.setCursor(0, 10);
M5.Lcd.print("Scan Triangulation en cours");
//scan_I2C_bus();
//int compteur = 0;
//while(compteur < 3){ //triangulation calcul valide ************** a prendre sur I2c)
// Scan_Triangulation(); //Prise de la position actuel
// delay(50);
// if (Balises_OK && error == 0 && Xr > 0 && Yr > 0) compteur ++;
//}
// 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[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 :");
}
void loop() {
static char wait = 0;
char erreur;
static int64_t time;
struct chassis_reception_t chassis_reception;
gestion_match();
affichage_resultats();
delay(10);
}
void affichage_resultats() {
index_statu ++;
if(index_statu >=3){index_statu =0;}
// affichage des resultats .........................
M5.Lcd.setCursor(75, 0);M5.Lcd.print(MemPosition_X);M5.Lcd.print(" "); M5.Lcd.setCursor(200, 0);M5.Lcd.print(X_futur);M5.Lcd.print(" ");
M5.Lcd.setCursor(75, 20);M5.Lcd.print(MemPosition_Y);M5.Lcd.print(" "); M5.Lcd.setCursor(200, 20);M5.Lcd.print(Y_futur);M5.Lcd.print(" ");
M5.Lcd.setCursor(75, 40);M5.Lcd.print(Position_actuelle_X);M5.Lcd.print(" ");
M5.Lcd.setCursor(75, 60);M5.Lcd.print(Position_actuelle_Y);M5.Lcd.print(" ");
M5.Lcd.setCursor(220, 80);M5.Lcd.print(ErreurListe[IndexErr]);M5.Lcd.print(" ");
M5.Lcd.setCursor(220, 100);M5.Lcd.print(lec_Balise_1);M5.Lcd.print(lec_Balise_2);M5.Lcd.print(lec_Balise_3);M5.Lcd.print(lec_Calcul_ok);M5.Lcd.print(" ");
M5.Lcd.setCursor(150, 120);M5.Lcd.print(Angle_Robot_RAD);M5.Lcd.print(" ");
M5.Lcd.setCursor(90, 140);M5.Lcd.print(compar_X);M5.Lcd.print(" ");
M5.Lcd.setCursor(90, 160);M5.Lcd.print(compar_Y);M5.Lcd.print(" ");
M5.Lcd.setCursor(70, 180);M5.Lcd.print(index_Maitre);M5.Lcd.print(", ");M5.Lcd.print(tableau[index_Maitre]);M5.Lcd.print(" ");
//M5.Lcd.setCursor(90, 200);M5.Lcd.print(x);M5.Lcd.print(" ");M5.Lcd.print(y);M5.Lcd.print(" ");M5.Lcd.print(rot);M5.Lcd.print(" ");
M5.Lcd.setCursor(50, 220);M5.Lcd.print(statu[index_statu]);M5.Lcd.print(" ");M5.Lcd.print(index_statu);
}
void Initialisation_wifi(){
//Initialisation wifi
//------------WIFI------------
//Initialisation wifi
WiFi.config(local_IP, gateway, subnet);
WiFi.begin(ssid, password);
int test_wifi = 0;
while (WiFi.status() != WL_CONNECTED){
delay(300);
M5.Lcd.print(".");
test_wifi ++;
if (test_wifi > 10) break;
}
if (WiFi.status() == WL_CONNECTED) {
Serial.println("Server started");
delay(500);
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.print("Connecte au reseau ;-)");
M5.Lcd.setCursor(10,30);
M5.Lcd.print(ssid);
}
else {
// le routeur riombotique n'a pas été trouvé - création d'un point d'accès
WiFi.mode(WIFI_OFF);
if (!WiFi.softAP("cerveau", "ilestsecret")) {
log_e("Soft AP creation failed.");
while(1);
}
IPAddress myIP = WiFi.softAPIP();
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.print("Creation Hotspot ;-)");
M5.Lcd.setCursor(10,30);
M5.Lcd.print("cerveau 192.168.4.1");
M5.Lcd.setCursor(10,50);
M5.Lcd.print("mdp : ilestsecret");
}
delay(1500);
}
void affichage_standard_init(){
M5.Lcd.clear();
M5.Lcd.setCursor(0, 0);M5.Lcd.print("Pos");M5.Lcd.setCursor(50, 0);M5.Lcd.print("X :");M5.Lcd.setCursor(150, 0);M5.Lcd.print("|Xp:");
M5.Lcd.setCursor(0, 20);M5.Lcd.print("n-1");
M5.Lcd.setCursor(50, 20);M5.Lcd.print("Y :");M5.Lcd.setCursor(150, 20);M5.Lcd.print("|Yp:");
M5.Lcd.setCursor(0, 40);M5.Lcd.print("Pos");
M5.Lcd.setCursor(50, 40);M5.Lcd.print("X :");
M5.Lcd.setCursor(0, 60);M5.Lcd.print("Act");
M5.Lcd.setCursor(50, 60);M5.Lcd.print("Y :");
M5.Lcd.setCursor(10, 80);M5.Lcd.print("Trans I2C");
M5.Lcd.setCursor(10, 100);M5.Lcd.print("Triangulation ");
M5.Lcd.setCursor(10, 120);M5.Lcd.print("AngleRad");
M5.Lcd.setCursor(10, 140);M5.Lcd.print("Tol x:");
M5.Lcd.setCursor(10, 160);M5.Lcd.print("Tol y:");
M5.Lcd.setCursor(10, 180);M5.Lcd.print("Case");
}
void affiche_erreur(char * chaine1, char * chaine2){
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.setTextColor(RED);
M5.Lcd.print("Erreur fatale :-(");
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(10,30);
M5.Lcd.print(chaine1);
M5.Lcd.setCursor(10,50);
M5.Lcd.print(chaine2);
}
void affiche_msg(char * chaine1, char * chaine2){
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.print("Message :");
M5.Lcd.setCursor(10,30);
M5.Lcd.print(chaine1);
M5.Lcd.setCursor(10,50);
M5.Lcd.print(chaine2);
}
void gestion_match(){
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,
MATCH_EN_COURS=3,
TEST_DEPLACEMENT_ABSOLU=4,
};
switch(index_Maitre){
case ATTENTE_ORDRE : //---------------------------------------------------------------------------------------------------------
/// Lecture des commandes venant du serveur web et envoi des commandes au chassis
Web_gestion();
M5.update();
/// Si on a une réponse du client
/// On lit la réponse
if(Web_nouveau_message()){
if(Web_type_requete() == WEB_DEPLACEMENT_RELATIF){
chassis_emission = Web_get_donnees();
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 = DEPLACEMENT_RELATIF;
}
if(M5.BtnA.read() == 1){
Serial.println("BtnA");
// Déplacement en X
translation_x_mm = 500;
translation_y_mm = 0;
rotation_rad = 0;
//index_Maitre = DEPLACEMENT_RELATIF;
Scan_Triangulation(&triangulation_reception);
}
if(M5.BtnB.read() == 1){
Serial.println("BtnB");
// 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 = MATCH_EN_COURS;
}
break;
case 1 : //---------------------------------------------------------------------------------------------------------
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{
//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;
case MATCH_EN_COURS:
if(Strategie() == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
case TEST_DEPLACEMENT_ABSOLU:
if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
}
}
enum etat_action_t Strategie(void){
static enum {
STRAT_RECULE_BANDEROLE, // Deplacement relatif
STRAT_ALLER_GRADINS_1, // Déplacement absolu
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
STRAT_ALLER_BACKSTAGE // Déplacement relatif
}etat_strategie = STRAT_RECULE_BANDEROLE;
enum etat_action_t etat_action;
int translation_x_mm, translation_y_mm;
float rotation_rad;
switch(etat_strategie){
case STRAT_RECULE_BANDEROLE:
// Déplacement en X
translation_x_mm = -450;
translation_y_mm = 0;
rotation_rad = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_1;
}
break;
case STRAT_ALLER_GRADINS_1:
etat_action = deplacement_absolu(730, 550, M_PI/2., 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_RECULE_BANDEROLE;
return ACTION_TERMINEE;
}
}
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.
void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad,
struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){
float compar_rotation;
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)
compar_rotation = consigne_orientation_rad - triangulation_reception.angle_rad;
while(compar_rotation < -M_PI){
compar_rotation += 2* M_PI;
}
while(compar_rotation > M_PI){
compar_rotation -= 2* M_PI;
}
printf("compar_X:%d\tcompar_y:%d\tcompar_rot:%f\n", compar_X, compar_Y, compar_rotation);
if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
if(abs(compar_rotation) > tolerance_orientation) {
chassis_emission->translation_x_mm = 0;
chassis_emission->translation_y_mm = 0;
chassis_emission->rotation_z_rad = compar_rotation;
chassis_emission->status = MOUVEMENT_EN_COURS;
}else{
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;
}
}
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_COURS,
DA_ATTENTE,
} 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;
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;
case DA_COMPARE_POSITIONS:
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("pos_x:%d\tpos_y:%d\tOrientation:%f\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm,
triangulation_reception.angle_rad);
Serial.printf("trans_x:%d\ttrans_y:%d\trot:%f\n",chassis_emission.translation_x_mm,
chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
etat_deplacement = DA_MVT_EN_COURS;
}else{
// Alors nous sommes arrivés
// On réinitialise la mahcine à état
etat_deplacement = DA_INIT;
return ACTION_TERMINEE;
}
}
break;
case DA_MVT_EN_COURS:
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){
Serial.printf("DA_COMPARE_POSITIONS\n");
etat_deplacement = DA_ATTENTE;
}
break;
case DA_ATTENTE:
delay(3000);
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
/// 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, float 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);
Triangulation_send_immobile(0);
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){
Triangulation_send_immobile(1);
etat_deplacement = DR_INIT;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
void scan_I2C_bus(){
char error, address;
char tampon[10];
int nDevices=0;
M5.Lcd.print(" ");
for(address = 0; address < 16; address++ ){
sprintf(tampon, "%x", address);
M5.Lcd.print(tampon);
}
M5.Lcd.print("\n");
for(address = 0; address < 128; address++ )
{
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
if(address !=0){
WIRE.beginTransmission(address);
error = WIRE.endTransmission(true);
}else{
error = -1;
}
if((nDevices % 16) == 0){
M5.Lcd.print("\n");
sprintf(tampon, "%x ", address/16);
M5.Lcd.print(tampon);
}
if (error == 0)
{
M5.Lcd.setTextColor(GREEN);
M5.Lcd.print("X");
Serial.printf("SUCCESS - endTransmission: %u\n", error);
M5.Lcd.setTextColor(WHITE);
WIRE.beginTransmission(address);
WIRE.write("Hello !\n");
WIRE.endTransmission(true);
}
else
{
Serial.printf("endTransmission: %u\n", error);
M5.Lcd.print(".");
}
nDevices++;
}
delay(10000);
}
Reference in New Issue View Git Blame Copy Permalink