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

Code refactorisé - déplacements relatifs OK

Browse Source
This commit is contained in:
Samuel 2025-02-24 22:45:57 +01:00
parent 9023ae64bc
commit 7f2af55e02
2 changed files with 84 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

249
Cerveau/Cerveau.ino
View File

@ -1,9 +1,11 @@
#include <M5Core2.h> #include <M5Core2.h>
#include <Wire.h> #include <Wire.h>
#include <WiFi.h> #include <WiFi.h>
#include <WebServer.h>
#include <math.h> #include <math.h>
#include "Communication_chassis.h"
#include "ServerWeb.h"
#define WIRE Wire #define WIRE Wire
@ -24,10 +26,10 @@ IPAddress subnet(255, 255, 255, 0);
// IPAddress local_IP(192, 168, 1, 99); // IPAddress local_IP(192, 168, 1, 99);
// IPAddress gateway(192, 168, 1, 1); // IPAddress gateway(192, 168, 1, 1);
// IPAddress subnet(255, 255, 255, 0); // IPAddress subnet(255, 255, 255, 0);
WebServer server(80);
const int16_t I2C_MASTER = 0x42; const int16_t I2C_MASTER = 0x42;
const int16_t I2C_SLAVE_chassi = 0x55;
const int16_t I2C_SLAVE_trian = 0x30; const int16_t I2C_SLAVE_trian = 0x30;
uint8_t test = 32; uint8_t test = 32;
uint32_t i = 0; uint32_t i = 0;
@ -82,78 +84,6 @@ char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd",
char* statu[] = {"/..","./.","../"}; char* statu[] = {"/..","./.","../"};
int index_statu=0; 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() { void setup() {
// put your setup code here, to run once: // put your setup code here, to run once:
@ -162,7 +92,7 @@ void setup() {
M5.Lcd.setTextSize(2); M5.Lcd.setTextSize(2);
M5.Lcd.print("Bonjour\nEcran\n\nRecherche Wifi"); M5.Lcd.print("Bonjour\nEcran\n\nRecherche Wifi");
Initialisation_wifi(); Initialisation_wifi();
server.on("/form", handleForm); Web_init();
WIRE.begin(); WIRE.begin();
M5.Lcd.clear(); M5.Lcd.clear();
@ -177,20 +107,8 @@ void setup() {
//} //}
// MemPosition_X = 800; // MemPosition_X = 800;
// MemPosition_Y = 800; // MemPosition_Y = 800;
M5.Lcd.clear(); affichage_standard_init();
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 :"); //M5.Lcd.setCursor(10, 200);M5.Lcd.print("cmd :");
} }
@ -202,7 +120,7 @@ void loop() {
strategie(); strategie();
affichage_resultats(); affichage_resultats();
delay(500); delay(10);
} }
void affichage_resultats() { void affichage_resultats() {
@ -237,7 +155,7 @@ void Initialisation_wifi(){
if (test_wifi > 10) break; if (test_wifi > 10) break;
} }
if (WiFi.status() == WL_CONNECTED) { if (WiFi.status() == WL_CONNECTED) {
server.begin();
Serial.println("Server started"); Serial.println("Server started");
delay(500); delay(500);
M5.Lcd.clear(); M5.Lcd.clear();
@ -254,7 +172,6 @@ void Initialisation_wifi(){
while(1); while(1);
} }
IPAddress myIP = WiFi.softAPIP(); IPAddress myIP = WiFi.softAPIP();
server.begin();
M5.Lcd.clear(); M5.Lcd.clear();
M5.Lcd.setCursor(10,10); M5.Lcd.setCursor(10,10);
M5.Lcd.print("Creation Hotspot ;-)"); M5.Lcd.print("Creation Hotspot ;-)");
@ -266,6 +183,23 @@ void Initialisation_wifi(){
delay(1500); 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){ void affiche_erreur(char * chaine1, char * chaine2){
M5.Lcd.clear(); M5.Lcd.clear();
M5.Lcd.setCursor(10,10); M5.Lcd.setCursor(10,10);
@ -290,10 +224,19 @@ void affiche_msg(char * chaine1, char * chaine2){
void strategie(){ void strategie(){
struct chassis_reception_t chassis_reception; struct chassis_reception_t chassis_reception;
struct chassis_emission_t chassis_emission;
switch(index_Maitre){ switch(index_Maitre){
case 0 : //--------------------------------------------------------------------------------------------------------- case 0 : //---------------------------------------------------------------------------------------------------------
server.handleClient(); //************* Lecture des données recu pour coordoné (X,Y,rotation) /// Lecture des commandes venant du serveur web et envoi des commandes au chassis
if(vit!=0 && !corrige){ 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); Serial.printf("vit:%d, corrige: %d\n", vit, corrige);
MemCmd_X = xA; //Memorisation de la comande pour comparaison avec arrivé MemCmd_X = xA; //Memorisation de la comande pour comparaison avec arrivé
MemCmd_Y = yA; MemCmd_Y = yA;
@ -301,6 +244,41 @@ void strategie(){
//index_Maitre = 1; // Bloc le serveur WEB si on n'a pas la triangulation //index_Maitre = 1; // Bloc le serveur WEB si on n'a pas la triangulation
index_Maitre = 2; 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; break;
case 1 : //--------------------------------------------------------------------------------------------------------- case 1 : //---------------------------------------------------------------------------------------------------------
@ -315,8 +293,9 @@ void strategie(){
//Si position Pas OK ****************** //Si position Pas OK ******************
break; break;
case 2 : //--------------------------------------------------------------------------------------------------------- case 2 : // Deplacement relatif en cours
Scan_chassis(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis 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){ if(Err_Chassi_com !=0){
for(int j=0; j<10; j++){ for(int j=0; j<10; j++){
affichage_resultats(); affichage_resultats();
@ -325,25 +304,13 @@ void strategie(){
} }
// ********************************* ERREUR DE COM FATAL *********************** // ********************************* ERREUR DE COM FATAL ***********************
} }
if(Mvt_finit==0){ if(chassis_reception.status != MOUVEMENT_FINI){
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{ }else{
send_Chassis_RAZ(); //Envoie des coordonées à atteindre send_Chassis_RAZ(); // Immobilisation du robot
index_Maitre = 0; // On attend l'ordre suivant
} }
//M5.Lcd.clear();
Serial.println("case 2 fin1/2");
index_Maitre = 0;
Serial.println("case 2 fin2/2");
break; break;
} }
} }
@ -390,34 +357,6 @@ void compar_cinematique(){
} }
} }
/// @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 /// @brief Récupère position (X, Y) et l'orientation du robot
void Scan_Triangulation(){ void Scan_Triangulation(){
@ -448,37 +387,7 @@ void Scan_Triangulation(){
} }
} }
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(){ void scan_I2C_bus(){
char error, address; char error, address;

5
Readme.md
View File

@ -4,4 +4,9 @@ Déplacement fonctionnel
Côté cerveau, un serveur web qui permet d'envoyer un ordre de déplacement relatif au châssis. Côté cerveau, un serveur web qui permet d'envoyer un ordre de déplacement relatif au châssis.
Calibration des sens des moteurs pour les translations en X, Y et pour la rotation. Calibration des sens des moteurs pour les translations en X, Y et pour la rotation.
Calibration des translations non effectuées.
Vidéo réalisée le 24 février 2024, paramètres :
- vitesse : 2000
- acceleration : 1500