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

Amélioration de la prise des cerises latérales

Browse Source
This commit is contained in:
Samuel 2023-05-13 17:35:27 +02:00
parent 470b5cbc4a
commit b277df988f
4 changed files with 30 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Holonome2023.c
View File

@ -82,7 +82,7 @@ int main() {
AsserMoteur_Init(); AsserMoteur_Init();
Localisation_init(); Localisation_init();
while(mode_test()); //while(mode_test());
i2c_maitre_init(); i2c_maitre_init();
Trajet_init(); Trajet_init();
Balise_VL53L1X_init(); Balise_VL53L1X_init();

8
Monitoring.c
View File

@ -2,6 +2,7 @@
#include <stdio.h> #include <stdio.h>
#include "Monitoring.h" #include "Monitoring.h"
#include "Localisation.h" #include "Localisation.h"
#include "Asser_Moteurs.h"
uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0; uint32_t temps_cycle_max=0;
@ -41,7 +42,12 @@ void Monitoring_display(){
struct position_t position = Localisation_get(); struct position_t position = Localisation_get();
printf(">pos_x:%ld:%f\n", temps, position.x_mm); printf(">pos_x:%ld:%f\n", temps, position.x_mm);
printf(">pos_y:%ld:%f\n", temps, position.y_mm); printf(">pos_y:%ld:%f\n", temps, position.y_mm);
printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1));
printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1));
printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1));
printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A));
printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B));
printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
} }
} }

26
Strategie.c
View File

@ -154,12 +154,11 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
Trajet_config(250, 500); Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 156, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT; etat_strategie = ATTRAPER_CERISE_HAUT;
Strategie_set_cerise_dans_robot(6);
} }
break; break;
@ -175,6 +174,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
if(etat_action == ACTION_TERMINEE){ if(etat_action == ACTION_TERMINEE){
Strategie_set_cerise_dans_robot(6);
etat_strategie = ALLER_PANIER; etat_strategie = ALLER_PANIER;
} }
break; break;
@ -205,6 +205,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
case ATTRAPER_CERISE_GAUCHE: case ATTRAPER_CERISE_GAUCHE:
if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){ if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){
Strategie_set_cerise_dans_robot(10);
etat_strategie = ALLER_PANIER; etat_strategie = ALLER_PANIER;
} }
break; break;
@ -273,12 +274,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
// On va en ligne droite // On va en ligne droite
if(Robot_est_dans_zone_cerise_haut(couleur)){ if(Robot_est_dans_zone_cerise_haut(couleur)){
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
465,2830, 180,2800,
Localisation_get().angle_radian, Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
}else if (Robot_est_dans_zone_cerise_gauche()){ }else if (Robot_est_dans_zone_cerise_gauche()){
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
/// !!! TODO !!! Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
745, 3000 - 475,
180, 3000 - 200,
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
}else{ }else{
// Sinon, on a une courbe de bézier // Sinon, on a une courbe de bézier
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
@ -314,7 +319,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
} }
/// Fonction de localisation aproximatives pour la stratégie. /// Fonction de localisation approximatives pour la stratégie.
/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain /// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
/// @return 1 si oui, 0 si non. /// @return 1 si oui, 0 si non.
@ -343,6 +348,15 @@ int Robot_est_dans_zone_cerise_gauche(){
} }
int Robot_est_dans_zone_cerise_droite(){
if(Localisation_get().x_mm > 2000 - 630 &&
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
return 1;
}
return 0;
}
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){ if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){

4
i2c_annexe.c
View File

@ -73,8 +73,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
} }
void i2c_annexe_active_turbine(void){ void i2c_annexe_active_turbine(void){
//donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
//donnees_a_envoyer=1; donnees_a_envoyer=1;
} }
void i2c_annexe_desactive_turbine(void){ void i2c_annexe_desactive_turbine(void){
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE; donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;