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

457 lines
15 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 gst_server;
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;
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;
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);
}
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;
switch(index_Maitre){
case 0 : //---------------------------------------------------------------------------------------------------------
/// 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();
send_Chassis(&chassis_emission);
}
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;
}
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;
}
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;
}
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;
send_Chassis(&chassis_emission);
index_Maitre = 2;
}
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){
}else{
send_Chassis_RAZ(); // Immobilisation du robot
index_Maitre = 0; // On attend l'ordre suivant
}
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
X_futur = MemCmd_X;
Y_futur = MemCmd_Y;
Scan_Triangulation(); //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 Récupère position (X, Y) et l'orientation du robot
void Scan_Triangulation(){
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;}
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] ;
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){Position_actuelle_X = 9999;}
if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){Position_actuelle_Y = 9999;}
}
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
Balises_OK = true;
}else{
Balises_OK = 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