A simple form

commit 42c1f8dcf084da66a3d1a1c3b3153e4b41299f8c Author: Samuel Date: Fri Feb 21 22:38:35 2025 +0100 Code pour valider le fonctionnement de la communication I2C diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino new file mode 100644 index 0000000..f2f4d77 --- /dev/null +++ b/Cerveau/Cerveau.ino @@ -0,0 +1,552 @@ +#include +#include +#include +#include +#include + + +#define WIRE Wire + +#define Rad_Deg 57.2957795130823 + +#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 { + char status; +}; + +struct chassis_emission_t { + char status; + int translation_x, translation_y, rotation_y; +}; + + + +#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; + + Serial.println("Scan_chassis"); + + erreur = Scan_Chassi(&chassis_reception); + Serial.println("fin Scan_chassis"); + + // Si le chassis est disponible + // Attente 1 seconde avant d'envoyer un messa + + Serial.println("if 0x02"); + if(chassis_reception.status == 0x02 && wait == 0){ + time = esp_timer_get_time(); + wait = 1; + } + + Serial.println("wait 1s"); + if(wait == 1 && esp_timer_get_time() > time + 1000000){ + Serial.println("send_Chassis"); + send_Chassis(); + wait = 0; + } + + + //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_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis + if(Err_Chassi_com !=0){ + affiche_erreur("case 2", "Err_Chassi_com !=0"); + for(int j=0; j<10; j++){ + affichage_resultats(); + Scan_Chassi(&chassis_reception); + //delay(500); + } + // ********************************* ERREUR DE COM FATAL *********************** + } + if(Mvt_finit==0){ + send_Chassis(); //Envoie des coordonées à atteindre + while(Mvt_finit!=1){ + Scan_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********) + delay(50); + } + Scan_Chassi(&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(); + + index_Maitre = 0; + 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 +/// @return renvoi 0 si tout s'est bien passé, un code d'erreur sinon +int Scan_Chassi(struct chassis_reception_t * chassis_reception){ + unsigned char tampon2[10]; + //(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, 8); + Serial.println("apres lire registre"); + if (error !=0){ + Serial.println("scan chassis erreur"); + Err_Chassi_com =1;IndexErr = 2; + affiche_erreur("Scan_Chassi", "Erreur I2C"); + while(1); + }else{ + Serial.println("scan chassis ok"); + Err_Chassi_com =0; + IndexErr = 0; + int valeur; + + Mvt_finit = (0x04 == tampon2[0] & 0x04); + char chaine[32]; + sprintf(chaine, "tampon2: %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]); + affiche_msg("Scan_Chassi", chaine); + chassis_reception->status = tampon2[0]; + } + return 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 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){ + Cmd_Angle = MemCmd_A; + // Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet + Mot[0] = 2; + //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); + +} \ No newline at end of file diff --git a/Cerveau/I2C_Master_lib.ino b/Cerveau/I2C_Master_lib.ino new file mode 100644 index 0000000..44654a2 --- /dev/null +++ b/Cerveau/I2C_Master_lib.ino @@ -0,0 +1,33 @@ +size_t _I2C_write(uint8_t *data, size_t quantity) +{ + for(size_t i = 0; i < quantity; ++i) { + if(!WIRE.write(data[i])) { + return i; + } + } + return quantity; + +} + +uint8_t I2C_ecrire_registre(int adresse_i2c, uint adresse_registre, unsigned char * donnees, uint taille_donnees){ + // Ecriture + WIRE.beginTransmission(adresse_i2c); + WIRE.write(adresse_registre); + _I2C_write(donnees, taille_donnees); + return WIRE.endTransmission(true); +} + +uint8_t I2C_lire_registre(int adresse_i2c, uint adresse_registre, unsigned char * donnees_recues, uint taille_donnees){ + int status; + byte temp; + WIRE.beginTransmission(adresse_i2c); + WIRE.write(adresse_registre); + status = WIRE.endTransmission(true); + + WIRE.requestFrom(adresse_i2c, taille_donnees + 1); + // ceci contient l'adresse + Wire.readBytes(&temp, 1); + // Et ceci les données + Wire.readBytes(donnees_recues, taille_donnees); + return status; +} \ No newline at end of file diff --git a/Chassis/Chassis.ino b/Chassis/Chassis.ino new file mode 100644 index 0000000..4a09ee4 --- /dev/null +++ b/Chassis/Chassis.ino @@ -0,0 +1,288 @@ +// Programme Chassi Robot Riom Robotique +// Version : 5=6 +// Etat : (En cours de dev / à tester / Fonctionnel) +// +// à tester +// en cours de test i2c, 02/03/2024 je recoit bien la trame i2c avec les donées dans le bon sens +// +//********************************************************************* +//******************** Integration et config WIFI ********************* +//********************************************************************* +// serveur - depuis machine reseau http://192.XXX.XXX.XX?variable extrait la variable +//#include +#include "Wire.h" +#define I2C_DEV_ADDR 0x55 + +//Definition des Variantes : +#define serialP; +#define gst_I2C; + +byte * I2C_memory; + +//********************************************************************* +//******************** Integration et config Moteur ******************* +//********************************************************************* +#include +#define pul1 D10 +#define dir1 D9 +#define pul2 D2 +#define dir2 D1 +#define pul3 D8 +#define dir3 D7 +#define pul4 D6 +#define dir4 D3 + +// AccelStepper stepperX(1.8, dir1, pul1); // X +// AccelStepper stepperY(1.8, dir2, pul2); // Y +// AccelStepper stepperZ(1.8, dir3, pul3); // Z +// AccelStepper stepperA(1.8, dir4, pul4); // A +AccelStepper stepperX(1.8, pul1, dir1); // X +AccelStepper stepperY(1.8, pul2, dir2); // Y +AccelStepper stepperZ(1.8, pul3, dir3); // Z +AccelStepper stepperA(1.8, pul4, dir4); // A + +int Men= D0; //Motor All enable pin + +// Data pour la lecture de donnée +String nom = "Arduino"; +String msg; +int inChar; +int data[] = {0, 0}; +int com[] = {0, 0, 0}; + +// -------------- Direction des moteurs ( pour un sens de marche commun ) +int dirX= +1; +int dirY= -1; +int dirZ= +1; +int dirA= -1; + +int cli_led=0; // initial value of input +int Move = 0; +String rc; + +int16_t Target[] = {0,0,0,0,0}; //Tableau de la commande de position (X,Y,Rotation,speed,accel) +int16_t AxeX[] = {0,0,0,0}; // distance en X pour chaque moteur +int16_t AxeY[] = {0,0,0,0}; // distance en Y pour chaque moteur +int16_t AxeRot[] = {0,0,0,0}; // distance en R pour chaque moteur +int16_t Vecteur[] = {0,0,0,0};// distance en X pour chaque moteur + +int PosX = 0; //Position X du robot +int PosY = 0; //Position Y du robot +int Rot = 0; //Rotation du robot +int vitesse = 2000; +int accel = 2000; + + +uint8_t Mot[] = {0,0,0,0,0,0,0,0,0,0,0,0} ; //11*8bits Mot de commande I2C du Chassi ****Cmd, X, Y, Rot, Vit, Acc**** +uint8_t c[] = {0,0,0,0,0,0,0,0,0,0,0} ; +bool Interrupt = 0; // commande d'intéruption de mouvement (Arret d'Urgence) premier bit du mot de Cmd +bool Modif_Mvt = 0; // commande de modification de mouvement deuxieme bit du mot de Cmd +bool fin_mvt = 0; +int h=0; + + +void setup() +{ + pinMode(Men,OUTPUT); + digitalWrite(Men, LOW);// Low Level Enable + #ifdef serialP + Serial.begin(115200); + #endif + pinMode(LED_BUILTIN, OUTPUT); + for(int i=0; i<6;i++){ + digitalWrite(LED_BUILTIN, LOW); + delay(150); + digitalWrite(LED_BUILTIN, HIGH); + delay(150); + } + + // X + stepperX.setMaxSpeed(vitesse); stepperX.setAcceleration(accel); + // Y + stepperY.setMaxSpeed(vitesse); stepperY.setAcceleration(accel); + // Z + stepperZ.setMaxSpeed(vitesse); stepperZ.setAcceleration(accel); + // A + stepperA.setMaxSpeed(vitesse); stepperA.setAcceleration(accel); + + + //Serial.println(Men); + I2C_Slave_init(0x55); + I2C_memory = get_i2c_data(); + I2C_memory[0] = 0x1; + I2C_memory[1] = 0x2; + I2C_memory[2] = 0x3; + I2C_memory[3] = 0x4; + I2C_memory[4] = 0x5; + I2C_memory[5] = 0x6; + I2C_memory[6] = 0x7; + I2C_memory[7] = 0x8; + + + //digitalWrite(Men, LOW);// Low Level Enable + pinMode(pul4, OUTPUT); + +} + + +void loop() +{ + static char wait = 0; + static int64_t time; + + for(int i=0; i<8; i++){ + Serial.printf("0x%x ", I2C_memory[i]); + } + Serial.printf("\n"); + + // Reset octet 0 à 0 au bout d'une seconede + if(I2C_memory[0] !=0 && wait == 0){ + time = esp_timer_get_time(); + wait = 1; + } + if(esp_timer_get_time() > time + 1000000 && wait == 1){ + I2C_memory[0] = 0x4; + wait = 0; + } + + /* + + if(((I2C_memory[0] & 0x02)==2) && Modif_Mvt==0){ + Modif_Mvt = 1; + } + if(Modif_Mvt == 1){ + //Mot = I2C_memory; + Target[0]= (I2C_memory[1] << 8) | I2C_memory[2]; + Target[1]= (I2C_memory[3] << 8) | I2C_memory[4]; + Target[2]= (I2C_memory[5] << 8) | I2C_memory[6]; + Target[3]= (I2C_memory[7] << 8) | I2C_memory[8]; + Target[4]= (I2C_memory[9] << 8) | I2C_memory[10]; + for(int i=1; i<9; i++){ + I2C_envoi_8bits(0,i); + } + Position_Calculation(); //************* Calcul du Vecteur XY (incrémental) + Mvmt(); //************* Envoie de la position à ateindre pour chaque moteur + Execution du mouvement + + I2C_memory[0] = 0x04; + I2C_envoi_8bits(I2C_memory[0] ,0); + } + + + // else{ + // I2C_memory = get_i2c_data(); + // I2C_envoi_8bits(I2C_memory[0] | 1<<2,0); + // I2C_envoi_8bits(I2C_memory[0] | 1<<1,0); + // for(int i=1; i<9; i++){ + // I2C_envoi_8bits(0,i); + + if((I2C_memory[0] & 0x02)==0 && Modif_Mvt==1){ + Modif_Mvt = 0; + } + + //Dé-asservissement des moteurs + stop(); // Servo Off + */ + +} + +void Position_Calculation(){ + PosX = Target[0]; + AxeX[0] = {PosX}; //x + AxeX[1] = {-PosX}; //y + AxeX[2] = {-PosX}; //z + AxeX[3] = {PosX}; //a + PosY = Target[1]; + AxeY[0] = {PosY}; //x + AxeY[1] = {PosY}; //y + AxeY[2] = {PosY}; //z + AxeY[3] = {PosY}; //a + Rot = Target[2]; + AxeRot[0] = {-Rot}; //x + AxeRot[1] = {Rot}; //y + AxeRot[2] = {-Rot}; //z + AxeRot[3] = {Rot}; //a + + vitesse = Target[3]; + accel = Target[4]; + + for (int i=0; i<4; i++){ + Vecteur[i] = (AxeX[i] + AxeY[i] + AxeRot[i])/2; + Serial.printf("Vecteur[%d]: %d\n", i, Vecteur[i]); + } + + stepperX.moveTo(stepperX.currentPosition() + Vecteur[0]*dirX); + stepperY.moveTo(stepperY.currentPosition() + Vecteur[1]*dirY); + stepperZ.moveTo(stepperZ.currentPosition() + Vecteur[2]*dirZ); + stepperA.moveTo(stepperA.currentPosition() + Vecteur[3]*dirA); + + stepperX.setMaxSpeed(vitesse); stepperX.setAcceleration(accel); // X + stepperY.setMaxSpeed(vitesse); stepperY.setAcceleration(accel); // Y + stepperZ.setMaxSpeed(vitesse); stepperZ.setAcceleration(accel); // Z + stepperA.setMaxSpeed(vitesse); stepperA.setAcceleration(accel); // A + + + #ifdef serialP + // Serial.print("Target 0 ");Serial.print("\tTarget 1");;Serial.print("\tTarget 2");;Serial.print("\tTarget 3");Serial.println("\tTarget 4"); + // Serial.print(Target[0]);Serial.print("\t\t");Serial.print(Target[1]);Serial.print("\t\t");Serial.print(Target[2]);Serial.print("\t\t");Serial.print(Target[3]);Serial.print("\t\t");Serial.println(Target[4]); + // Serial.println(); + // Serial.print("X ");Serial.print("\t\tY");;Serial.print("\t\tZ");;Serial.print("\t\tA");Serial.println("\t\tRot"); + // Serial.print(Vecteur[0]);Serial.print("\t\t");Serial.print(Vecteur[1]);Serial.print("\t\t");Serial.print(Vecteur[2]);Serial.print("\t\t");Serial.print(Vecteur[3]);Serial.print("\t\t");Serial.println(Vecteur[4]); + // delay(100); + #endif + for (int i=0; i<4; i++){ + Target[i] = 0; + } + // RAZ du flag de mouvement modifié + Modif_Mvt = 0; + + for (int i=0; i < sizeof(Mot); i++){ + Mot[i] = 0; + } +} +void Mvmt(){ + int64_t time = esp_timer_get_time(); + + // Mise en Servo ON des moteurs + digitalWrite(Men,LOW);// Low Level Enable + if(stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0){ + //Execution du mouvement global (resort de la boucle une fois arrivé) + #ifdef serialP + // Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif"); + // Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt); + #endif + while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) /*&& (Interrupt != 1 || Modif_Mvt != 1)*/){ + stepperX.run(); // X + stepperY.run(); // Y + stepperZ.run(); // Z + stepperA.run(); // A + //------------------- + #ifdef serialP + // Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif"); + // Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt); + // delay(100); + #endif + if(esp_timer_get_time() > time + 100000){ + time = esp_timer_get_time(); + digitalWrite(LED_BUILTIN, cli_led); + cli_led = !cli_led; + } + } + } + digitalWrite(LED_BUILTIN, HIGH); + // if(Modif_Mvt == 1){ + // Position_Calculation(); + // Serial.println("\t\tRe Calculation !!!"); + // } +} +void stop(){ + if(Interrupt== 1){ + //delay(1000); + Interrupt = 0; + } + #ifdef serialP + // h++; + // Serial.print("**********STOP****** ");Serial.println(h); + #endif + digitalWrite(Men,HIGH);// Low Level Enable + delay(500); + } diff --git a/Chassis/I2C_Slave_lib.ino b/Chassis/I2C_Slave_lib.ino new file mode 100644 index 0000000..9b41738 --- /dev/null +++ b/Chassis/I2C_Slave_lib.ino @@ -0,0 +1,62 @@ +#define TAILLE_MEMOIRE_I2C 256 +#define TAILLE_MESSAGE_ENVOI_MAX 32 +byte memoire_I2C[TAILLE_MEMOIRE_I2C]; +byte memoire_I2C_index=0; + +bool nouveau_message=false; + +uint8_t * get_i2c_data(){ + return memoire_I2C; +} +void onRequest(){ + uint32_t taille_envoi; + taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX); + + Wire.write(&memoire_I2C[memoire_I2C_index], taille_envoi); + memoire_I2C_index += taille_envoi; + if(memoire_I2C_index>=TAILLE_MEMOIRE_I2C){ + Serial.printf("memoire_I2C_index>=TAILLE_MEMOIRE_I2C\n"); + } +} + +void onReceive(int len){ + digitalWrite(LED_BUILTIN, LOW); + memoire_I2C_index = Wire.read(); + while(Wire.available()){ + nouveau_message=true; + memoire_I2C[memoire_I2C_index] = Wire.read(); + memoire_I2C_index++; + } + //Serial.printf("I2C reg: %d, val %d\n", memoire_I2C_index, memoire_I2C[memoire_I2C_index]); +} + +void I2C_Slave_init(int addr){ + Wire.onReceive(onReceive); + Wire.onRequest(onRequest); + Wire.begin(addr); +} + +bool I2C_Slave_nouveau_message(){ + if(nouveau_message){ + nouveau_message=false; + return true; + } + return false; +} + +void I2C_envoi_8bits(byte value, char adresse){ + //printf("I2C_envoi_8bits a:%d v:%d %x %b\n",adresse, value, value, value); + memoire_I2C[adresse] = value; +} + +void I2C_envoi_16bits(int16_t value, char adresse){ + memoire_I2C[adresse] = (value >> 8) & 0xFF; + memoire_I2C[adresse+1] = value & 0xFF; +} + +void I2C_envoi_32bits(int32_t value, char adresse){ + memoire_I2C[adresse] = value >> 24; + memoire_I2C[adresse+1] = (value >> 16) & 0xFF; + memoire_I2C[adresse+2] = (value >> 8) & 0xFF; + memoire_I2C[adresse+3] = value & 0xFF; +} diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..7dd1087 --- /dev/null +++ b/Readme.md @@ -0,0 +1,10 @@ +Test de la liaison I2C entre le châssis et le cerveau +===================================================== + +Ce code permet de tester la communication I2C entre le châssis et le cerveau. Le cerveau demande 8 octet au châssis. +Si l'octet 0 vaut 4, au bout d'une seconde, le cerveau envoi une trame pour réinitialise l'octet à 2. + +Le châssis scrute l'octet 0 et si celui-ci vaut 2, il attend une seconde puis le passe à 1. + +Le châssis envoie l'état de sa mémoire par la liaison USB. +Le cerveau affiche l'état de sa mémoire sur l'écran.