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

Re-modification de Trajet_terminee, recalage X lorsqu'on attrappe les cerise, Gestion de l'évitement : fonction geométriques + logique à base de cônes de détection

Browse Source
This commit is contained in:
Samuel 2023-04-13 19:43:35 +02:00
parent f5994a7f52
commit 4e898151f5
13 changed files with 252 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@ -6,6 +6,8 @@
"strategie.h": "c",
"trajectoire.h": "c",
"trajet.h": "c",
"asser_position.h": "c"
"asser_position.h": "c",
"strategie_prise_cerises.h": "c",
"geometrie.h": "c"
}
}

87
Balise_VL53L1X.c
View File

@ -1,18 +1,24 @@
#include <stdio.h>
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "Localisation.h"
#include "math.h"
#define NB_CAPTEURS 12
#define DISTANCE_OBSTACLE_IGNORE 200
#define DISTANCE_OBSTACLE_IGNORE_MM 2000
#define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4
#define DISTANCE_TROP_PRES_CM 3
#define DEMI_CONE_CAPTEUR_RADIAN 0.2225
uint8_t distance_capteur_cm[NB_CAPTEURS];
struct capteur_VL53L1X_t{
uint8_t distance_cm;
double angle_ref_robot;
double angle_ref_terrain;
uint8_t distance_lue_cm; // Distance entre le capteur et l'obstacle.
uint8_t distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
double angle_ref_robot; // Orientation du capteur dans le référentiel du robot
double angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
double angle_ref_terrain_min; // Cone de détection du capteur (min)
double angle_ref_terrain_max; // Cone de détection du capteur (max)
uint donnee_valide;
}capteurs_VL53L1X[NB_CAPTEURS];
@ -32,13 +38,16 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
// Actualisation de l'angle du capteur
capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian;
// Maintien de l'angle entre -PI et PI
while(capteur_VL53L1X->angle_ref_terrain > 2* M_PI){
while(capteur_VL53L1X->angle_ref_terrain > M_PI){
capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
}
while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
}
capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM;
capteur_VL53L1X->distance_obstacle_robot_mm = 10 * (distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM);
capteur_VL53L1X->angle_ref_terrain_min = capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN;
capteur_VL53L1X->angle_ref_terrain_max = capteur_VL53L1X->angle_ref_terrain + DEMI_CONE_CAPTEUR_RADIAN;
invalide_obstacle(capteur_VL53L1X, position_robot);
@ -50,12 +59,12 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio
// Positionne l'obstacle sur le terrain
struct position_t position_obstacle;
//printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm);
position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm * 10);
capteur_VL53L1X->donnee_valide=1;
// Si la distance vaut 0, à invalider
if(capteur_VL53L1X->distance_cm <= DISTANCE_CAPTEUR_CENTRE_ROBOT_CM){
if(capteur_VL53L1X->distance_lue_cm <= DISTANCE_TROP_PRES_CM){
capteur_VL53L1X->donnee_valide=0;
}
// Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm)
@ -83,17 +92,67 @@ void Balise_VL53L1X_init(){
}
uint8_t Balise_VL53L1X_get_min_distance(void){
uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
uint8_t min_distance_cm = DISTANCE_OBSTACLE_IGNORE_MM / 10;
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
if(capteurs_VL53L1X[capteur].donnee_valide){
if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
min_distance = capteurs_VL53L1X[capteur].distance_cm;
if(min_distance_cm> capteurs_VL53L1X[capteur].distance_lue_cm){
min_distance_cm = capteurs_VL53L1X[capteur].distance_lue_cm;
}
}
}
return min_distance;
return min_distance_cm;
}
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
return capteurs_VL53L1X[capteur].distance_cm;
return capteurs_VL53L1X[capteur].distance_lue_cm;
}
/// @brief Renvoi l'obstacle le plus proche en tenant compte de la direction d'avancement du robot dans le référentiel du terrain.
/// La fonction utilise 3 cônes de détection :
/// * +/- 22°, à 800 mm
/// * +/- 50°, à 580 mm
/// * +/- 90°, à 350 mm
/// @param angle_avancement_radiant : angle d'avancement du robot entre -PI et PI
/// @return
double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
const uint8_t NB_CONE=3;
struct cone_t{
double distance_mm;
double angle;
} cone[NB_CONE];
cone[0].angle = 22 * DEGRE_EN_RADIAN;
cone[0].distance_mm = 800;
cone[1].angle = 50 * DEGRE_EN_RADIAN;
cone[1].distance_mm = 580;
cone[2].angle = 90 * DEGRE_EN_RADIAN;
cone[2].distance_mm = 350;
double angle_min, angle_max;
double distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
if(Geometrie_intersecte_plage_angle(
angle_avancement_radiant - cone[cone_index].angle, angle_avancement_radiant + cone[cone_index].angle,
capteurs_VL53L1X[capteur].angle_ref_terrain_min, capteurs_VL53L1X[capteur].angle_ref_terrain_max)){
if(capteurs_VL53L1X[capteur].donnee_valide){
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
}
}
}
}
}
// On enlève le rayon du robot et la taille du robot adverse
if(distance_minimale < DISTANCE_OBSTACLE_IGNORE_MM){
distance_minimale -= (RAYON_ROBOT + RAYON_ROBOT_ADVERSE_MM);
if(distance_minimale < 0){
distance_minimale = 0;
}
}
return distance_minimale;
}

1
CMakeLists.txt
View File

@ -15,6 +15,7 @@ Asser_Position.c
Balise_VL53L1X.c
Commande_vitesse.c
QEI.c
Geometrie.c
gyro.c
gyro_L3GD20H.c
gyro_ADXRS453.c

19
Geometrie.c
View File

@ -38,3 +38,22 @@ unsigned int Geometrie_compare_angle(double angle, double angle_min, double angl
return 0;
}
}
/// @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
/// @param angle2_min Début de la seconde plage
/// @param angle2_max Fin de la seconde plage
/// @return 1 si les deux plages s'intersectent, 0 sinon
unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max){
// Pour que les plages s'intersectent, soit :
// * angle1_min est compris entre angle2_min et angle2_max
// * angle1_max est compris entre angle2_min et angle2_max
// * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant)
if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) ||
Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) ||
Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){
return 1;
}
return 0;
}

1
Geometrie.h
View File

@ -14,5 +14,6 @@ struct position_t{
double Geometrie_get_angle_normalisee(double angle);
unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max);
unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max);
#endif

3
Geometrie_robot.h
View File

@ -6,3 +6,6 @@
// Distance entre le centre du robot et un bord du robot
#define PETIT_RAYON_ROBOT_MM 108.2
#define RACINE_DE_3 1.73205081
// Géométrie liée à l'adversaire
#define RAYON_ROBOT_ADVERSE_MM 240

5
Holonome2023.c
View File

@ -75,8 +75,9 @@ int main() {
AsserMoteur_Init();
Localisation_init();
//while(mode_test());
test_homologation();
while(mode_test());
//test_panier();
//test_homologation();
Gyro_Init();

35
Strategie.c
View File

@ -12,7 +12,7 @@
#include "math.h"
#define SEUIL_RECAL_DIST_MM 75
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
#define DISTANCE_OBSTACLE_CM 50
#define DISTANCE_PAS_OBSTACLE_MM 2000
@ -31,7 +31,7 @@ void Homologation(uint32_t step_ms){
switch(etat_strategie){
case STRATEGIE_INIT:
Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
etat_strategie = ATTENTE_TIRETTE;
break;
@ -43,14 +43,14 @@ void Homologation(uint32_t step_ms){
case APPROCHE_CERISE_1_A:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, 30. * DEGREE_EN_RADIAN);
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_strategie = ATTRAPE_CERISE_1;
}
break;
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_1;
}
@ -62,7 +62,7 @@ void Homologation(uint32_t step_ms){
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
@ -72,7 +72,7 @@ void Homologation(uint32_t step_ms){
case APPROCHE_PANIER_2:
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
265,2830,
+120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
@ -81,7 +81,7 @@ void Homologation(uint32_t step_ms){
break;
case CALAGE_PANIER_1:
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_strategie = RECULE_PANIER;
}
break;
@ -90,7 +90,7 @@ void Homologation(uint32_t step_ms){
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. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCE_DANS_PANIER;
@ -100,7 +100,7 @@ void Homologation(uint32_t step_ms){
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_strategie = APPROCHE_CERISE_2;
etat_strategie = STRATEGIE_FIN;
}
break;
@ -108,7 +108,7 @@ void Homologation(uint32_t step_ms){
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
830, 3000 - 156,
Localisation_get().angle_radian, 30. * DEGREE_EN_RADIAN);
Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
@ -116,7 +116,7 @@ void Homologation(uint32_t step_ms){
break;
case ATTRAPPE_CERISE_2:
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms);
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_2;
}
@ -266,3 +266,16 @@ enum couleur_t lire_couleur(void){
return COULEUR_BLEU;
}
/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
/// @param tempo_ms
/// @param step_ms
/// @return 1 si la temporisation est écoulée, 0 sinon.
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
if(*tempo_ms < step_ms){
return 1;
}else{
*tempo_ms -= step_ms;
return 0;
}
}

6
Strategie.h
View File

@ -6,7 +6,7 @@
#define COULEUR 15
#define TIRETTE 14
#define DEGREE_EN_RADIAN (M_PI / 180.)
#define CORR_ANGLE_DEPART_DEGREE (-1.145)
enum etat_action_t{
ACTION_EN_COURS,
@ -50,6 +50,10 @@ 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);
int test_panier(void);
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);

3
Strategie_prise_cerises.c
View File

@ -31,7 +31,7 @@ double vitesse_accostage_mm_s=100;
/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
/// @param longer_direction : direction dans laquelle se trouve la bordure
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms){
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double pos_recalage_x_mm){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum longer_direction_t longer_direction_aspire;
static uint32_t tempo_ms = 0;
@ -54,6 +54,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
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) ){
Localisation_set_x(pos_recalage_x_mm);
etat_attrape = TURBINE_DEMARRAGE;
}
break;

2
Strategie_prise_cerises.h
View File

@ -1,2 +1,2 @@
#include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms);
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double pos_x_mm);

115
Test_strategie.c
View File

@ -19,6 +19,7 @@
int test_accostage(void);
int test_longe(void);
int test_panier(void);
int test_homologation(void);
int test_evitement(void);
int test_tirette_et_couleur();
@ -64,10 +65,12 @@ void affichage_test_strategie(){
int test_strategie(){
printf("L - longer.\n");
printf("A - Accoster.\n");
printf("H - Homologation.\n");
printf("C - Couleur et tirette.\n");
printf("H - Homologation.\n");
printf("L - Longer.\n");
printf("P - Panier.\n");
int lettre;
do{
lettre = getchar_timeout_us(0);
@ -98,6 +101,11 @@ int test_strategie(){
while(test_longe());
break;
case 'p':
case 'P':
while(test_panier());
break;
case 'q':
case 'Q':
return 0;
@ -121,8 +129,6 @@ int test_homologation(){
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
@ -234,6 +240,107 @@ int test_evitement(){
}
int test_panier(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
printf("Test panier\n");
static enum{
TEST_PANIER_INIT,
TEST_PANIER_ASPIRE,
TEST_PANIER_TEMPO,
TEST_PANIER_LANCE_BALLES,
TEST_PANIER_PORTE_OUVERTE,
} etat_test_panier=TEST_PANIER_INIT;
static uint32_t tempo_ms;
i2c_maitre_init();
Trajet_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test_panier){
case TEST_PANIER_INIT:
if(lire_couleur()== COULEUR_VERT){
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
etat_test_panier=TEST_PANIER_ASPIRE;
}
break;
case TEST_PANIER_ASPIRE:
if(lire_couleur()== COULEUR_BLEU){
i2c_annexe_desactive_turbine();
tempo_ms = 3000;
etat_test_panier=TEST_PANIER_TEMPO;
}
break;
case TEST_PANIER_TEMPO:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test_panier=TEST_PANIER_LANCE_BALLES;
}
break;
case TEST_PANIER_LANCE_BALLES:
if(lance_balles(_step_ms) == ACTION_TERMINEE){
etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
}
break;
case TEST_PANIER_PORTE_OUVERTE:
if(temporisation_terminee(&tempo_ms, _step_ms)){
i2c_annexe_ouvre_porte();
}
if(lire_couleur()== COULEUR_BLEU){
etat_test_panier=TEST_PANIER_INIT;
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
int test_longe(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;

8
Trajet.c
View File

@ -86,10 +86,10 @@ void Trajet_stop(double pas_de_temps_s){
/// @param abscisse : abscisse sur la trajectoire
/// @return 1 si le trajet est terminé, 0 sinon
int Trajet_terminee(double abscisse){
if(abscisse >= 0.99 ){
/*if(abscisse >= 0.99 ){
return 1;
}
/*
}*/
if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
if(abscisse >= 1 ){
return 1;
@ -98,7 +98,7 @@ int Trajet_terminee(double abscisse){
if(abscisse >= 0.99 ){
return 1;
}
}*/
}
return 0;
}