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();
Localisation_init();
while(mode_test());
//while(mode_test());
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();

8
Monitoring.c
View File

@ -2,6 +2,7 @@
#include <stdio.h>
#include "Monitoring.h"
#include "Localisation.h"
#include "Asser_Moteurs.h"
uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
@ -41,7 +42,12 @@ void Monitoring_display(){
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);
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);
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);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_HAUT;
Strategie_set_cerise_dans_robot(6);
}
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);
if(etat_action == ACTION_TERMINEE){
Strategie_set_cerise_dans_robot(6);
etat_strategie = ALLER_PANIER;
}
break;
@ -205,6 +205,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
case ATTRAPER_CERISE_GAUCHE:
if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){
Strategie_set_cerise_dans_robot(10);
etat_strategie = ALLER_PANIER;
}
break;
@ -273,12 +274,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
// On va en ligne droite
if(Robot_est_dans_zone_cerise_haut(couleur)){
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
465,2830,
180,2800,
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
}else if (Robot_est_dans_zone_cerise_gauche()){
/// !!! TODO !!!
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
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{
// Sinon, on a une courbe de bézier
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
/// @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){
if(couleur == COULEUR_BLEU){
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){
//donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
//donnees_a_envoyer=1;
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
donnees_a_envoyer=1;
}
void i2c_annexe_desactive_turbine(void){
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;