Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Attrappe mieux les pots, on évite les déplacements rotations + translation. On se cale en manuel

Browse Source
This commit is contained in:
Samuel 2024-05-01 12:03:41 +02:00
parent 189101b849
commit b834b0d677
6 changed files with 81 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Strategie.c
View File

@ -289,7 +289,7 @@ enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float
return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
}
/// @brief Attention - ne marche pas !!!
/// @brief Avance puis tourne
/// @param pos_x
/// @param pos_y
/// @param angle_radian
@ -318,7 +318,7 @@ enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, floa
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){
etat_aller_a_puis_tourner = AAPT_ALLER;
return ACTION_TERMINEE;
}
@ -547,5 +547,54 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
//etat_calage_debut = CD_ENVOI_CDE_BORDURE;
}
return ACTION_EN_COURS;
}
enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms){
// Le robot est positionné avec une cale à 70 du bord
// Si l'angle avec la bordure est négatif : bleu, sinon jaune
// On en déduit X
static enum{
CD_ENVOI_CDE_BORDURE,
CD_LECTURE_BORDURE_Y,
CD_ROTATION_VERS_X,
CD_LECTURE_BORDURE_X,
CD_ALLER_POSITION_INIT,
CD_ROTATION_POSITION_INIT,
}etat_calage_debut=CD_ENVOI_CDE_BORDURE;
enum validite_vl53l8_t validite;
struct trajectoire_t trajectoire;
enum etat_action_t etat_action;
float angle, distance;
switch(etat_calage_debut){
case CD_ENVOI_CDE_BORDURE:
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
etat_calage_debut = CD_LECTURE_BORDURE_X;
break;
case CD_LECTURE_BORDURE_X:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
Localisation_set_y(215.);
if(angle < 0){
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
Localisation_set_angle((-60. * DEGRE_EN_RADIAN));
}else{
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
}
etat_calage_debut = CD_ALLER_POSITION_INIT;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}

1
Strategie.h
View File

@ -67,6 +67,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms);

33
Strategie_2024_plante.c
View File

@ -177,17 +177,29 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
commande_vitesse_stop();
i2c_annexe_ouvre_doigt_plante();
i2c_annexe_attrape_plante(bras_depose_pot);
tempo_ms = 2000;
tempo_ms = 5000;
etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
}
break;
case PDP_ATTRAPE_PLANTE:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_TEMPO;
if(bras_depose_pot == PLANTE_BRAS_1){
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
}else{
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
}
tempo_ms = 250;
}
break;
case PDP_TEMPO:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_RECULE;
tempo_ms = 2000;
}
break;
@ -199,22 +211,11 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
commande_vitesse_stop();
etat_plante_dans_pot=PDP_TEMPO;
etat_plante_dans_pot=PDP_ALLER_PLANTE;
return ACTION_TERMINEE;
}
break;
case PDP_TEMPO:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_ALLER_PLANTE;
if(bras_depose_pot == PLANTE_BRAS_1){
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
}else{
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
}
return ACTION_TERMINEE;
}
default:
break;
}

16
Strategie_2024_pots.c
View File

@ -87,11 +87,12 @@ int get_bras_libre(void){
}
return 9;
}
int ordre_pot[5]={POT_1, POT_2, POT_4, POT_5, POT_3};
int get_pot_suivant(int pot){
if(pot < 4){
pot++;
return pot;
for(int i=0; i<4; i++){
if(ordre_pot[i] == pot){
return ordre_pot[++i];
}
}
return POT_INVALIDE;
}
@ -154,7 +155,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
//if(couleur == COULEUR_BLEU){
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
// Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
/*}else{
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
}*/
@ -175,7 +176,10 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
case AP_APPROCHE_POT:
bras = get_bras_libre();
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
etat_action = Strategie_tourner_et_aller_a(
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_aller_a_puis_tourner(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);

2
Strategie_2024_pots.h
View File

@ -22,7 +22,7 @@
#define BRAS_6 5
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 185.
#define DISTANCE_ATTRAPE_POT_MM 200.
struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms);

2
Test_strategie_2024.c
View File

@ -433,7 +433,7 @@ int test_attrape_pot(){
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;