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

537 lines
18 KiB
C++
Raw Blame History

#include <M5Core2.h>
#include <Wire.h>
#include <WiFi.h>
#include <WebServer.h>
#include <math.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);
WebServer server(80);
const int16_t I2C_MASTER = 0x42;
const int16_t I2C_SLAVE_chassi = 0x55;
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;
struct chassis_reception_t {
unsigned char status;
};
struct chassis_emission_t {
unsigned char status;
int translation_x, translation_y, rotation_y;
};
void Scan_chassis(struct chassis_reception_t * chassis_reception);
#define FORM \
"<!DOCTYPE html>\n" \
"<html>\n" \
"<head>\n" \
"<meta charset=\"UTF-8\">\n" \
"<title>A simple form</title>\n" \
"</head>\n" \
"<body>\n" \
"<form action=\"/form\" method=\"post\">\n" \
"<br>" \
"<label for=\"X\">X : en mm</label>\n" \
"<br>" \
"<input type=\"text\" id=\"X\" name=\"X\">\n" \
"<br>" \
"<label for=\"Y\">Y : en mm</label>\n" \
"<br>" \
"<input type=\"text\" id=\"Y\" name=\"Y\">\n" \
"<br>" \
"<label for=\"R\">Rotation : en degres</label>\n" \
"<br>" \
"<input type=\"text\" id=\"R\" name=\"R\">\n" \
"<br>" \
"<label for=\"V\">Vitesse : </label>\n" \
"<br>" \
"<input type=\"text\" id=\"V\" name=\"V\" value=\"2000\">\n" \
"<br>" \
"<br>" \
"<label for=\"A\">Accel : </label>\n" \
"<br>" \
"<input type=\"text\" id=\"A\" name=\"A\"value=\"1500\">\n" \
"<br>" \
"<br>" \
"<input type=\"submit\" value=\" Vas-Y \">\n" \
"<br>" \
"<br>" \
"<input type=\"reset\" value=\"Reset\">\n" \
"<br>" \
"<br>" \
"<input type=\"submit\" value=\" STOP \">\n" \
"</form>\n" \
"</body>\n" \
"</html>\n"
void handleForm() {
String message;
message += FORM;
server.send(200, "text/html", message);
String myString0 = server.arg("X"); //positon de cmd en X mm
// x= myString0.toInt() * coef_mvt/10;
xA= myString0.toInt();
String myString1 = server.arg("Y"); //positon de cmd en Y mm
// y= myString1.toInt() * coef_mvt/10;
yA= myString1.toInt();
String myString2 = server.arg("R"); //positon de cmd en Rotation Deg °
rot= myString2.toInt() * 13.88888;
String myString3 = server.arg("V"); // Vitesse de cmd en
vit= myString3.toInt();
String myString4 = server.arg("A"); // Acceleration de cmd
acc= myString4.toInt();
}
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();
server.on("/form", handleForm);
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;
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");
//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(500);
}
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) {
server.begin();
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();
server.begin();
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 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;
switch(index_Maitre){
case 0 : //---------------------------------------------------------------------------------------------------------
server.handleClient(); //************* Lecture des données recu pour coordoné (X,Y,rotation)
if(vit!=0 && !corrige){
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;
}
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 : //---------------------------------------------------------------------------------------------------------
Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis
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(Mvt_finit==0){
cmd_chassi_x = xA;
cmd_chassi_y = yA;
Cmd_Angle = rot;
send_Chassis(); //Envoie des coordonées à atteindre
while(Mvt_finit!=1){
Serial.println("while Mvt_finit");
Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
delay(50);
}
Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********)
}else{
send_Chassis_RAZ(); //Envoie des coordonées à atteindre
}
//M5.Lcd.clear();
Serial.println("case 2 fin1/2");
index_Maitre = 0;
Serial.println("case 2 fin2/2");
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 Lit l'état du chassis en I2C
void Scan_chassis(struct chassis_reception_t * chassis_reception){
unsigned char tampon2[14];
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
Serial.println("avant lire registre");
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
Serial.println("apres lire registre");
if (error !=0){
Serial.println("Erreur I2C");
Err_Chassi_com =1;IndexErr = 2;
affiche_erreur("Scan_Chassi", "Erreur I2C");
while(1);
}else{
Serial.println("I2C OK");
Err_Chassi_com =0;
IndexErr = 0;
int valeur;
Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
char chaine[100];
sprintf(chaine, "tampon2: %x %x %x %x %x %x %x %x %x %x %x %x %x %x\n", tampon2[0], tampon2[1], tampon2[2], tampon2[3], tampon2[4],
tampon2[5], tampon2[6], tampon2[7], tampon2[8], tampon2[9], tampon2[10], tampon2[11], tampon2[12], tampon2[13]);
affiche_msg("Scan_Chassi", chaine);
Serial.println("Affect struct");
chassis_reception->status = tampon2[0];
Serial.println("apres Affect struct");
}
}
/// @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 send_Chassis_RAZ(){
uint8_t RAZ_Message[11]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, RAZ_Message, 11);
}
void send_Chassis(){
//if(nbr_essai<=10){
// Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
Mot[0] = MOUVEMENT_EN_COURS;
//y*=-1;
//y = y*direction;
Mot[1] = cmd_chassi_x >>8; Mot[2] = cmd_chassi_x;
Mot[3] = cmd_chassi_y >>8; Mot[4] = cmd_chassi_y;
//Serial.println(y);
Mot[5] = Cmd_Angle >>8; Mot[6] = Cmd_Angle;
Mot[7] = vit >>8; Mot[8] = vit;
Mot[9] = acc >>8; Mot[10] = acc;
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
if (error !=0){Err_Cha_send =1; IndexErr = 5;}
else{Err_Cha_send =0;IndexErr = 0;}
if(error==0){ //si pas d'erreur de transmission alors RAZ des valeurs
nbr_essai ++;
cmd_chassi_x = 0;
cmd_chassi_y = 0;
Cmd_Angle = 0;
vit = 0;
acc = 0;
}
}
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