Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Pousse sans servomoteur

Browse Source
This commit is contained in:
Samuel 2023-05-20 02:19:41 +02:00
parent cf1fc7183b
commit 41459008fa
4 changed files with 58 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -31,6 +31,7 @@ Servomoteur.c
Strategie.c
Strategie_deplacement.c
Strategie_prise_cerises.c
Strategie_pousse_gateau.c
Temps.c
Test.c
Test_strategie.c

18
Strategie.c
View File

@ -97,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
if(couleur == COULEUR_BLEU){
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
objectifs[0]= objectif_1;
@ -107,8 +107,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}else{
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE
, .cible = CERISE_DROITE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
objectifs[0]= objectif_1;
objectifs[1]= objectif_2;
@ -206,10 +207,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
break;
case ATTRAPER_CERISE_GAUCHE:
@ -231,10 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie = ATTRAPER_CERISE_DROITE;
}
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
break;
case ATTRAPER_CERISE_DROITE:
@ -315,9 +308,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
struct trajectoire_t trajectoire;
float angle_fin;
return Gateau_pousse_proche(couleur, step_ms);
/*if(couleur == COULEUR_BLEU){

1
Strategie.h
View File

@ -62,6 +62,7 @@ enum etat_action_t Strategie_preparation();
enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
extern float distance_obstacle;

85
Strategie_pousse_gateau.c
View File

@ -1,5 +1,6 @@
#include "i2c_annexe.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Strategie.h"
#include "Trajectoire.h"
#include "Trajet.h"
@ -7,57 +8,75 @@
enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
static enum {
GATEAU_PROCHE_PRE_POUSSE,
GATEAU_PROCHE_POUSSE,
GATEAU_PROCHE_RELACHE,
} etat_pousse_proche;
ALLER_1,
ALLER_2,
ALLER_3
} etat=ALLER_1;
float point_x, point_y;
float angle_fin;
struct trajectoire_t trajectoire;
float angle_fin, point_x, point_y;
switch(etat_pousse_proche){
case GATEAU_PROCHE_PRE_POUSSE:
switch(etat){
case ALLER_1:
if(couleur == COULEUR_BLEU){
angle_fin = 90. * DEGRE_EN_RADIAN;
point_x = 310;
angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 225;
}else{
angle_fin = -135. * DEGRE_EN_RADIAN;
point_x = 1655;
angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 1775;
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 360,
Localisation_get().angle_radian, angle_fin);
Trajet_config(100, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
etat_pousse_proche = GATEAU_PROCHE_POUSSE;
i2c_annexe_deplie_bras();
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat=ALLER_2;
}
break;
case GATEAU_PROCHE_POUSSE:
case ALLER_2:
if(couleur == COULEUR_BLEU){
angle_fin = 90. * DEGRE_EN_RADIAN;
point_x = 390;
angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 225;
}else{
angle_fin = -135. * DEGRE_EN_RADIAN;
point_x = 1585;
angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 1775;
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1665,
Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1650,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
etat_pousse_proche = GATEAU_PROCHE_POUSSE;
i2c_annexe_plie_bras();
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat=ALLER_3;
}
break;
case ALLER_3:
if(couleur == COULEUR_BLEU){
angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 225;
}else{
angle_fin = 30. * DEGRE_EN_RADIAN;
point_x = 1775;
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1500,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat=ALLER_1;
return ACTION_TERMINEE;
}
break;
case GATEAU_PROCHE_RELACHE:
}
return ACTION_EN_COURS;
}