On attrappe un gradin en autonome !
This commit is contained in:
parent
9941274383
commit
7f435ccf92
@ -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]){
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user