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

Reprise de la fonction de calage dans un angle.

Browse Source
This commit is contained in:
Samuel 2023-05-08 17:47:39 +02:00
parent 702e99b788
commit 13d153cbeb
8 changed files with 142 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Geometrie.c
View File

@ -35,6 +35,22 @@ unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_m
}
}
/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée,
/// donne l'angle consigne pour limiter les rotations inutiles.
/// Tous les angles sont en radian
/// @param angle_depart
/// @param angle_souhaite
/// @return angle_optimal en radian
float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){
while((angle_depart - angle_souhaite) > M_PI){
angle_souhaite += 2* M_PI;
}
while((angle_depart - angle_souhaite) < -M_PI){
angle_souhaite -= 2* M_PI;
}
return angle_souhaite;
}
/// @brief Indique si les deux plages d'angle se recoupent
/// @param angle1_min Début de la première plage
/// @param angle1_max Fin de la première plage

1
Geometrie.h
View File

@ -16,5 +16,6 @@ struct position_t{
float Geometrie_get_angle_normalisee(float angle);
unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max);
unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max);
float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite);
#endif

12
Monitoring.c
View File

@ -1,6 +1,7 @@
#include "pico/stdlib.h"
#include <stdio.h>
#include "Monitoring.h"
#include "Localisation.h"
uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
@ -32,7 +33,16 @@ void temps_cycle_reset(){
void Monitoring_display(){
while(1){
uint32_t temps;
temps = time_us_32()/1000;
temps_cycle_display();
printf(">DebugVar:%ld:%d\n", temps, debug_var);
printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
struct position_t position = Localisation_get();
printf(">pos_x:%ld:%f\n", temps, position.x_mm);
printf(">pos_y:%ld:%f\n", temps, position.y_mm);
}
}
@ -41,8 +51,6 @@ void temps_cycle_display(){
temps = time_us_32()/1000;
printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
printf(">DebugVar:%ld:%d\n", temps, debug_var);
printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
temps_cycle_reset();
}

208
Strategie.c
View File

@ -25,14 +25,26 @@ struct objectif_t{
} objectifs[NB_OBJECTIFS];
struct objectif_t * objectif_courant;
uint32_t Score_nb_cerises = 0;
uint32_t Score_cerises_dans_robot=0;
void Score_set_cerise_dans_robot(uint32_t nb_cerises){
Score_cerises_dans_robot = nb_cerises;
}
uint32_t Score_get_cerise_dans_robot(void){
return Score_cerises_dans_robot;
}
// TODO: Peut-être à remetttre en variable locale après
float distance_obstacle;
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);
int Robot_est_dans_quart_haut_gauche();
int Robot_est_dans_quart_haut_droit();
@ -124,6 +136,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
if(etat_action == ACTION_TERMINEE){
Score_set_cerise_dans_robot(6);
etat_strategie = ALLER_PANIER;
}
break;
@ -144,6 +157,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT;
Score_set_cerise_dans_robot(6);
}
break;
@ -170,7 +184,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
break;
case LANCER_PANIER:
if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){
objectif_courant->etat = FAIT;
etat_strategie = DECISION_STRATEGIE;
}
@ -235,7 +249,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
465,2830,
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
}
}else{
}else{ // COULEUR_VERT
// Si le robot est déjà dans le quart haut droit,
// On va en ligne droite
if(Robot_est_dans_quart_haut_droit()){
@ -245,7 +259,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
}else{
// Sinon, on a une courbe de bézier
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
2000-485, Localisation_get().y_mm,
2000-465, 857,
2000-465, 2830,
-150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
@ -279,113 +293,14 @@ int Robot_est_dans_quart_haut_droit(){
return 0;
}
void Homologation(uint32_t step_ms){
enum etat_action_t etat_action;
enum etat_trajet_t etat_trajet;
struct trajectoire_t trajectoire;
switch(etat_homologation){
case STRATEGIE_INIT:
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
etat_homologation = ATTENTE_TIRETTE;
break;
case ATTENTE_TIRETTE:
if(attente_tirette() == 0){
etat_homologation = APPROCHE_CERISE_1_A;
}
break;
case APPROCHE_CERISE_1_A:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_homologation = ATTRAPE_CERISE_1;
}
break;
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT);
if(etat_action == ACTION_TERMINEE){
etat_homologation = APPROCHE_PANIER_1;
}
break;
case APPROCHE_PANIER_1:
Trajet_config(500, 500);
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_homologation = CALAGE_PANIER_1;
}
break;
case CALAGE_PANIER_1:
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_homologation = RECULE_PANIER;
}
break;
case RECULE_PANIER:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_homologation = LANCE_DANS_PANIER;
}
break;
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_homologation = STRATEGIE_FIN;
}
break;
case APPROCHE_CERISE_2:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
830, 3000 - 156,
Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_homologation = STRATEGIE_FIN;
}
break;
case ATTRAPPE_CERISE_2:
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT);
if(etat_action == ACTION_TERMINEE){
etat_homologation = APPROCHE_PANIER_2;
}
break;
case STRATEGIE_FIN:
i2c_annexe_desactive_propulseur();
commande_vitesse_stop();
break;
}
}
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
static enum {
CALAGE_PANIER_1,
RECULE_PANIER,
LANCE_DANS_PANIER,
}etat_lance_balles_dans_panier;
float recal_pos_x, recal_pos_y;
float angle_depart, angle_arrivee;
enum longer_direction_t longer_direction;
float point_x, point_y;
@ -417,9 +332,11 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
point_x = 1735;
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
}
angle_depart = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_RADIAN);
angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
point_x, point_y,
120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
angle_depart, angle_arrivee);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
@ -428,7 +345,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){
etat_action = ACTION_TERMINEE;
etat_lance_balles_dans_panier = CALAGE_PANIER_1;
}
@ -441,7 +358,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
/// @param step_ms : pas de temps.
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t lance_balles(uint32_t step_ms){
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){
enum etat_action_t etat_action = ACTION_EN_COURS;
static uint32_t tempo_ms;
static uint32_t nb_iteration;
@ -473,7 +390,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){
if(temporisation_terminee(&tempo_ms, step_ms)){
i2c_annexe_ouvre_porte();
nb_iteration++;
if(nb_iteration > 10){
if(nb_iteration > nb_cerises){
nb_iteration=0;
etat_action = ACTION_TERMINEE;
etat_lance_balle = LANCE_PROPULSEUR_ON;
@ -498,26 +415,69 @@ enum etat_action_t lance_balles(uint32_t step_ms){
}
/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){
enum etat_action_t etat_action = ACTION_EN_COURS;
struct position_t position;
struct trajectoire_t trajectoire;
static float y_pos_ref;
avance_puis_longe_bordure(longer_direction);
if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) ||
((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){
etat_action = ACTION_TERMINEE;
static enum {
CONTACT_AVANT,
RECULE_BORDURE,
CONTACT_LATERAL,
RECALE_X
}etat_calage_angle;
switch(etat_calage_angle){
case CONTACT_AVANT:
if(cerise_accostage() == ACTION_TERMINEE){
position = Localisation_get();
//if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_y(y_mm);
//}
//if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
Localisation_set_angle(angle_radian);
//}
y_pos_ref = Localisation_get().y_mm;
etat_calage_angle = RECULE_BORDURE;
}
break;
case RECULE_BORDURE:
commande_translation_recule_vers_trompe();
position = Localisation_get();
if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){
etat_calage_angle = CONTACT_LATERAL;
}
break;
case CONTACT_LATERAL:
if(longer_direction == LONGER_VERS_A){
commande_translation_avance_vers_A();
if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){
etat_calage_angle = RECALE_X;
}
}else{
commande_translation_avance_vers_C();
if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){
etat_calage_angle = RECALE_X;
}
}
break;
case RECALE_X:
etat_action = ACTION_TERMINEE;
commande_vitesse_stop();
position = Localisation_get();
//if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_x(x_mm);
//}
etat_calage_angle = CONTACT_AVANT;
break;
position = Localisation_get();
if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_x(x_mm);
}
if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_y(y_mm);
}
if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
Localisation_set_angle(angle_radian);
}
}
return etat_action;
}

2
Strategie.h
View File

@ -52,7 +52,7 @@ enum etat_action_t depose_cerises(uint32_t step_ms);
void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);
enum etat_action_t lance_balles(uint32_t step_ms);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
int test_panier(void);
void Strategie(enum couleur_t couleur, uint32_t step_ms);

26
Strategie_prise_cerises.c
View File

@ -20,8 +20,7 @@
void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C();
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
@ -160,6 +159,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
}
/// @brief Viens position le robot contre une bordure ou un support cerise devant lui.
enum etat_action_t cerise_accostage(void){
enum etat_action_t etat_action = ACTION_EN_COURS;
float rotation;
@ -174,7 +174,7 @@ enum etat_action_t cerise_accostage(void){
switch (etat_accostage)
{
case CERISE_AVANCE_DROIT:
commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
commande_translation_avance_vers_trompe();
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
}
@ -245,6 +245,26 @@ void commande_translation_longer_vers_C(){
commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}
void commande_translation_avance_vers_trompe(){
commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
}
void commande_translation_recule_vers_trompe(){
commande_vitesse(-vitesse_accostage_mm_s * cos(-M_PI/6), -vitesse_accostage_mm_s * sin(-M_PI/6), 0);
}
void commande_translation_avance_vers_A(){
// V_x : V * cos (60°) = V / 2
// V_y : V * sin (60°) = V * RACINE(3) / 2
commande_vitesse(vitesse_accostage_mm_s/2., vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
}
void commande_translation_avance_vers_C(){
// V_x : -V * cos (60°) = -V / 2
// V_y : -V * sin (60°) = -V * RACINE(3) / 2
commande_vitesse(-vitesse_accostage_mm_s/2., -vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
}
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){
if(direction ==LONGER_VERS_A){
return LONGER_VERS_C;

6
Strategie_prise_cerises.h
View File

@ -1,2 +1,8 @@
#include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C();
void commande_translation_avance_vers_trompe();
void commande_translation_recule_vers_trompe();
void commande_translation_avance_vers_A();
void commande_translation_avance_vers_C();

2
Test_strategie.c
View File

@ -331,7 +331,7 @@ int test_panier(){
break;
case TEST_PANIER_LANCE_BALLES:
if(lance_balles(_step_ms) == ACTION_TERMINEE){
if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){
etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
}
break;