// 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; volatile 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); stop(); #ifdef serialP Serial.begin(115200); #endif // 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(I2C_DEV_ADDR); I2C_memory = get_i2c_data(); //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); } 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"); 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 stop(); // Servo Off 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 } 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(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); }