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

Ajout de l'erreur critique pour le gyro qui arrete la strategie

Browse Source
This commit is contained in:
Samuel 2023-05-19 01:36:39 +02:00
parent 4b89949265
commit cca330ae89
6 changed files with 32 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Monitoring.c
View File

@ -10,6 +10,7 @@
uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
int erreur_critique = 0;
int lock=0;
uint32_t debug_var=0;
float debug_varf=0;
@ -89,3 +90,12 @@ void Monitoring_Error(char * msg){
gpio_put(LED_PIN_ROUGE, 1);
}
void Monitoring_set_erreur_critique(){
Monitoring_Error("");
erreur_critique=1;
}
int Monitoring_get_erreur_critique(){
return erreur_critique;
}

2
Monitoring.h
View File

@ -9,3 +9,5 @@ void set_debug_var(uint32_t variable);
void set_debug_varf(float variable);
void Monitoring_display();
void Monitoring_Error(char * msg);
void Monitoring_set_erreur_critique();
int Monitoring_get_erreur_critique();

26
Strategie.c
View File

@ -4,6 +4,7 @@
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Moteurs.h"
#include "Score.h"
#include "Strategie_prise_cerises.h"
@ -53,8 +54,6 @@ int Robot_est_dans_zone_cerise_droite();
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
@ -80,8 +79,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
ALLER_PANIER,
LANCER_PANIER,
DECISION_STRATEGIE,
STRATEGIE_FIN
}etat_strategie;
if(Monitoring_get_erreur_critique()){
etat_strategie=STRATEGIE_FIN;
}
switch(etat_strategie){
case STRATEGIE_INIT:
if(couleur == COULEUR_BLEU){
@ -121,7 +125,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 500);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
Localisation_get().angle_radian, angle_fin);
@ -158,7 +162,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 500);
Trajet_config(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
Localisation_get().angle_radian, angle_fin);
@ -185,7 +189,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
break;
case ALLER_CERISE_GAUCHE:
Trajet_config(300, 250);
Trajet_config(250, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
@ -199,7 +203,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
Trajet_config(500, 500);
}
break;
@ -211,7 +214,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
break;
case ALLER_CERISE_DROITE:
Trajet_config(300, 250);
Trajet_config(250, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
@ -225,7 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
Trajet_config(500, 500);
}
break;
@ -298,14 +300,14 @@ enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t coul
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
740, 3000 - 550,
510, 3000 - 1580,
180, 1400,
500, 1400,
Localisation_get().angle_radian, angle_fin);
}else{
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 740, 3000 - 550,
2000 - 510, 3000 - 1580,
2000 - 180, 1400,
2000 - 500, 1400,
Localisation_get().angle_radian, angle_fin);
}
@ -338,7 +340,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire;
Trajet_config(500, 500);
Trajet_config(250, 250);
// Definition des trajectoires
if(couleur == COULEUR_BLEU){
@ -552,7 +554,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
Trajet_config(500, 500);
Trajet_config(250, 250);
}
break;

19
Strategie.h
View File

@ -25,25 +25,6 @@ enum couleur_t{
COULEUR_INCONNUE,
};
extern enum etat_homologation_t{
STRATEGIE_INIT,
ATTENTE_TIRETTE,
APPROCHE_CERISE_1_A,
APPROCHE_CERISE_1_B,
ATTRAPE_CERISE_1,
APPROCHE_PANIER_1,
APPROCHE_PANIER_2,
CALAGE_PANIER_1,
RECULE_PANIER,
TOURNE_PANIER,
LANCE_DANS_PANIER,
APPROCHE_CERISE_2,
ATTRAPPE_CERISE_2,
APPROCHE_PANIER_3,
STRATEGIE_FIN,
}etat_homologation;
enum evitement_t{
SANS_EVITEMENT,
PAUSE_DEVANT_OBSTACLE,

1
Test_strategie.c
View File

@ -51,7 +51,6 @@ void affichage_test_strategie(){
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">etat_strat:%ld:%d\n", temps, etat_homologation);
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());

6
gyro_ADXRS453.c
View File

@ -64,19 +64,21 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
//printf("SPI ERROR\n");
return 1;
}else{
/*while(1){
while(1){
affiche_tampon_32bits(tampon_reception);
printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
}*/
}
//set_position_avec_gyroscope(0);
}
}
if(Gyro_SensorData.ST != 0x1){
Monitoring_Error("Gyro Failed - Status != 0x1\n");
set_position_avec_gyroscope(0);
/*while(1){
printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
affiche_tampon_32bits(tampon_reception);
}*/
Monitoring_set_erreur_critique();
//set_position_avec_gyroscope(0);
return 1;
}