diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 6390d26..68c6dc3 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -9,6 +9,9 @@ #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"; @@ -80,15 +83,15 @@ char* statu[] = {"/..","./.","../"}; int index_statu=0; struct chassis_reception_t { - char status; + unsigned char status; }; struct chassis_emission_t { - char status; + unsigned char status; int translation_x, translation_y, rotation_y; }; - +void Scan_chassis(struct chassis_reception_t * chassis_reception); #define FORM \ "\n" \ @@ -197,31 +200,9 @@ void loop() { 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 0x04"); - if(chassis_reception.status == 0x04 && 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); + strategie(); + affichage_resultats(); + delay(500); } void affichage_resultats() { @@ -335,29 +316,34 @@ void strategie(){ break; case 2 : //--------------------------------------------------------------------------------------------------------- - Scan_Chassi(&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 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); + 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){ - Scan_Chassi(&chassis_reception); //Verif de la fin de mouvement remonté par le chassis (time out ?**********) + Serial.println("while Mvt_finit"); + Scan_chassis(&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 ?**********) + 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(); + //M5.Lcd.clear(); + Serial.println("case 2 fin1/2"); index_Maitre = 0; + Serial.println("case 2 fin2/2"); break; } } @@ -405,29 +391,32 @@ void compar_cinematique(){ } /// @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]; +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, 8); + 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 = (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]); + 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"); } - return 0; } /// @brief Récupère position (X, Y) et l'orientation du robot @@ -467,9 +456,8 @@ void send_Chassis_RAZ(){ 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; + Mot[0] = MOUVEMENT_EN_COURS; //y*=-1; //y = y*direction; Mot[1] = cmd_chassi_x >>8; Mot[2] = cmd_chassi_x; diff --git a/Chassis/Chassis.ino b/Chassis/Chassis.ino index 4a09ee4..c1b0d36 100644 --- a/Chassis/Chassis.ino +++ b/Chassis/Chassis.ino @@ -17,7 +17,7 @@ #define serialP; #define gst_I2C; -byte * I2C_memory; +volatile byte * I2C_memory; //********************************************************************* //******************** Integration et config Moteur ******************* @@ -52,9 +52,9 @@ int com[] = {0, 0, 0}; // -------------- Direction des moteurs ( pour un sens de marche commun ) int dirX= +1; -int dirY= -1; +int dirY= +1; int dirZ= +1; -int dirA= -1; +int dirA= +1; int cli_led=0; // initial value of input int Move = 0; @@ -84,17 +84,10 @@ int h=0; void setup() { pinMode(Men,OUTPUT); - digitalWrite(Men, LOW);// Low Level Enable + stop(); #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); @@ -109,18 +102,17 @@ void setup() //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(pul1, OUTPUT); + pinMode(pul2, OUTPUT); + pinMode(pul3, OUTPUT); pinMode(pul4, OUTPUT); + pinMode(dir1, OUTPUT); + pinMode(dir2, OUTPUT); + pinMode(dir3, OUTPUT); + pinMode(dir4, OUTPUT); } @@ -135,17 +127,6 @@ void loop() } 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; @@ -162,6 +143,7 @@ void loop() } Position_Calculation(); //************* Calcul du Vecteur XY (incrémental) Mvmt(); //************* Envoie de la position à ateindre pour chaque moteur + Execution du mouvement + stop(); // Servo Off I2C_memory[0] = 0x04; I2C_envoi_8bits(I2C_memory[0] ,0); @@ -180,17 +162,17 @@ void loop() } //Dé-asservissement des moteurs - stop(); // Servo Off - */ + + } void Position_Calculation(){ PosX = Target[0]; AxeX[0] = {PosX}; //x - AxeX[1] = {-PosX}; //y + AxeX[1] = {PosX}; //y AxeX[2] = {-PosX}; //z - AxeX[3] = {PosX}; //a + AxeX[3] = {-PosX}; //a PosY = Target[1]; AxeY[0] = {PosY}; //x AxeY[1] = {PosY}; //y @@ -199,8 +181,8 @@ void Position_Calculation(){ Rot = Target[2]; AxeRot[0] = {-Rot}; //x AxeRot[1] = {Rot}; //y - AxeRot[2] = {-Rot}; //z - AxeRot[3] = {Rot}; //a + AxeRot[2] = {Rot}; //z + AxeRot[3] = {-Rot}; //a vitesse = Target[3]; accel = Target[4]; @@ -261,14 +243,8 @@ void Mvmt(){ // 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 !!!"); diff --git a/Chassis/I2C_Slave_lib.ino b/Chassis/I2C_Slave_lib.ino index 9b41738..1cf38b4 100644 --- a/Chassis/I2C_Slave_lib.ino +++ b/Chassis/I2C_Slave_lib.ino @@ -20,7 +20,6 @@ void onRequest(){ } void onReceive(int len){ - digitalWrite(LED_BUILTIN, LOW); memoire_I2C_index = Wire.read(); while(Wire.available()){ nouveau_message=true; diff --git a/Readme.md b/Readme.md index 7dd1087..f99cf83 100644 --- a/Readme.md +++ b/Readme.md @@ -1,10 +1,7 @@ -Test de la liaison I2C entre le châssis et le cerveau -===================================================== +Déplacement fonctionnel +======================= -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. +Côté cerveau, un serveur web qui permet d'envoyer un ordre de déplacement relatif au châssis. -Le châssis scrute l'octet 0 et si celui-ci vaut 2, il attend une seconde puis le passe à 1. +Calibration des sens des moteurs pour les translations en X, Y et pour la rotation. -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.