A simple form

#include #include #include #include #include #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 \ "\n" \ "\n" \ "\n" \ "\n" \ "A simple form\n" \ "\n" \ "\n" \ "\n" \ "\n" \ "\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); }