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

528 lines
17 KiB
C++
Raw 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 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;
char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position"};
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;
strategie();
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 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 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){
// Déplacement en X
translation_x_mm = 10000;
translation_y_mm = 0;
rotation_rad = 0;
index_Maitre = DEPLACEMENT_RELATIF;
}
if(M5.BtnB.read() == 1){
// Déplacement en Y
translation_x_mm = 0;
translation_y_mm = 2000;
rotation_rad = 0;
index_Maitre = DEPLACEMENT_RELATIF;
}
if(M5.BtnC.read() == 1){
// Rotation
translation_x_mm = 0;
translation_y_mm = 0;
rotation_rad = 100;
index_Maitre = DEPLACEMENT_RELATIF;
}
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;
}
}
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(&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;
}
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 ;
}
}
/// @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(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;
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;
}else{
triangulation_reception->validite = false;
}
}
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