On attrappe un gradin en autonome !

This commit is contained in:
Samuel 2025-05-25 22:43:27 +02:00
parent 9941274383
commit 7f435ccf92
5 changed files with 31 additions and 26 deletions

View File

@ -162,10 +162,11 @@ void loop()
static int m_pos=0;
static unsigned long myTime=0;
if(millis() > myTime + 100){
if(millis() > myTime + 30){
myTime = millis();
Serial.print(">millis:");
Serial.println(millis());
Serial.printf(">I2C_c:%d\n>I2C_t:%d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);
}
@ -177,8 +178,6 @@ void loop()
int Move;
int Current;
//Serial.printf("I2C: %d - %d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);
if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){
switch (I2C_memory[I2C_CDE_DEMANDE]){

View File

@ -320,10 +320,9 @@ void gestion_match(){
translation_x_mm = 500;
translation_y_mm = 0;
rotation_rad = 0;
actionneur_action_generique(5);
//index_Maitre = DEPLACEMENT_RELATIF;
//index_Maitre = TEST_APPROCHE_GRADIN;
index_Maitre = TEST_APPROCHE_GRADIN;
}
if(M5.BtnB.read() == 1){
Serial.println("BtnB");
@ -513,7 +512,8 @@ enum etat_action_t gradin_approche(void){
GA_GOTO_EN_FACE,
GA_TOURNE_LARGE,
GA_GOTO_PROCHE,
GA_GOTO_PREND,
GA_GOTO_PREND_1,
GA_GOTO_PREND_2,
GA_RECULE
} statu_approche_gradin = GA_INIT;
static float angle_parcouru, angle_mem;
@ -531,6 +531,7 @@ enum etat_action_t gradin_approche(void){
switch(statu_approche_gradin){
case GA_INIT:
while(actionneur_action(ACTIONNEUR_DEPLACEMENT)!=ACTION_TERMINEE);
angle_parcouru = 0;
statu_approche_gradin = GA_CHERCHE_GAUCHE;
break;
@ -620,7 +621,7 @@ enum etat_action_t gradin_approche(void){
if(fabs(detect_gradin.centre_x_mm) > 10){
statu_approche_gradin = GA_GOTO_LARGE;
}else{
statu_approche_gradin = GA_GOTO_PREND;
statu_approche_gradin = GA_GOTO_PREND_1;
}
}
}
@ -633,14 +634,24 @@ enum etat_action_t gradin_approche(void){
}
break;
case GA_GOTO_PREND:
while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_INIT;
delay(5000);
return ACTION_TERMINEE;
case GA_GOTO_PREND_1:
while(deplacement_relatif((detect_gradin.centre_y_mm - 25 - 110), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_GOTO_PREND_2;
break;
case GA_GOTO_PREND_2:
// On positionne la fourche
while(actionneur_action(ACTIONNEUR_PREPARE_PRISE)!=ACTION_TERMINEE);
// On Avance de 15 cm
while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE);
// On attrape
while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
statu_approche_gradin = GA_CHERCHE_GAUCHE;
return ACTION_TERMINEE;
break;
case GA_RECULE:
while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_CHERCHE_GAUCHE;
@ -857,26 +868,21 @@ enum etat_action_t actionneur_pos_initiale(){
}
enum etat_action_t actionneur_action_generique(int action){
enum etat_action_t actionneur_action(int action){
struct com_actionneur_t com_actionneur_env, com_actionneur_rec;
int action_terminee=0;
com_actionneur_env.demande_action = action;
com_actionneur_rec.action_terminee = 0;
Serial.printf("Demande action %d\n", com_actionneur_env.demande_action);
Actionneur_send(&com_actionneur_env);
Serial.printf("Lecture action...\n");
do{
Actionneur_scan(&com_actionneur_rec);
Serial.printf("Lecture action... %d %d\n", com_actionneur_rec.action_terminee, com_actionneur_env.demande_action);
}while(com_actionneur_env.action_terminee != com_actionneur_env.demande_action);
}while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action);
// On réinitialise la machine à état
com_actionneur_env.demande_action = 0;
Serial.printf("Demande action %d\n", com_actionneur_env.demande_action);
Actionneur_send(&com_actionneur_env);
Serial.printf("Lecture action...\n");
do{
Actionneur_scan(&com_actionneur_rec);

View File

@ -17,9 +17,9 @@ int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_tr
error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
}
Serial.printf(">r0:%d\n>r1:%d\n", tampon[0], tampon[1]);
com_actionneur->action_terminee = tampon[1];
//com_actionneur->demande_action = tampon[0];
Serial.println("I2C OK");
com_actionneur->demande_action = tampon[0];
return error;
}

View File

@ -35,9 +35,9 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
// Conversion des mm ou radian en pas
nb_pas_x = chassis_emission->translation_x_mm * 4.049;
nb_pas_x = chassis_emission->translation_x_mm * -4.049;
nb_pas_y = chassis_emission->translation_y_mm * 4.953;
nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
nb_pas_rot = chassis_emission->rotation_z_rad * -791.;
Mot[0] = chassis_emission->status;
//y*=-1;

View File

@ -26,7 +26,7 @@ uint8_t I2C_lire_registre(int adresse_i2c, uint adresse_registre, unsigned char
WIRE.requestFrom(adresse_i2c, taille_donnees + 1);
// ceci contient l'adresse
if(adresse_i2c == I2C_SLAVE_chassi){
if(adresse_i2c == I2C_SLAVE_chassi || adresse_i2c == I2C_SLAVE_actionneur){
Wire.readBytes(&temp, 1);
}
// Et ceci les données