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

Test évitemment ok sur fonction de test, soucis lors de l'Homologation

Browse Source
This commit is contained in:
Samuel 2023-04-01 10:40:30 +02:00
parent ac3f5d2bae
commit e9c15d7e8f
6 changed files with 133 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -1,5 +1,8 @@
{
"files.associations": {
"timer.h": "c"
"timer.h": "c",
"localisation.h": "c",
"math.h": "c",
"strategie.h": "c"
}
}

43
Balise_VL53L1X.c
View File

@ -1,10 +1,11 @@
#include <stdio.h>
#include "i2c_annexe.h"
#include "Localisation.h"
#include "math.h"
#define NB_CAPTEURS 12
#define DISTANCE_OBSTACLE_IGNORE 200
#define DISTANCE_CAPTEUR_CENTRE_ROBOT 40
#define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4
uint8_t distance_capteur_cm[NB_CAPTEURS];
@ -12,9 +13,11 @@ struct capteur_VL53L1X_t{
uint8_t distance_cm;
double angle_ref_robot;
double angle_ref_terrain;
uint donnee_valide;
}capteurs_VL53L1X[NB_CAPTEURS];
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot);
void Balise_VL53L1X_gestion(){
i2c_annexe_get_distances(distance_capteur_cm);
@ -24,8 +27,10 @@ void Balise_VL53L1X_gestion(){
}
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
struct position_t position_robot;
position_robot = Localisation_get();
// Actualisation de l'angle du capteur
capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + Localisation_get().angle_radian;
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){
capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
@ -33,7 +38,33 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
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;
capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM;
invalide_obstacle(capteur_VL53L1X, position_robot);
}
/// @brief Definit si l'obstable doit être pris en comptre
/// @param
void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot){
// 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);
capteur_VL53L1X->donnee_valide=1;
// Si la distance vaut 0, à invalider
if(capteur_VL53L1X->distance_cm <= DISTANCE_CAPTEUR_CENTRE_ROBOT_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)
/*printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm);
if((position_obstacle.x_mm < 50) || (position_obstacle.y_mm < 50) || (position_obstacle.x_mm > 1950) || (position_obstacle.y_mm > 2950))
{
capteur_VL53L1X->donnee_valide=0;
}*/
}
void Balise_VL53L1X_init(){
@ -54,8 +85,10 @@ void Balise_VL53L1X_init(){
uint8_t Balise_VL53L1X_get_min_distance(void){
uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
min_distance = capteurs_VL53L1X[capteur].distance_cm;
if(capteurs_VL53L1X[capteur].donnee_valide){
if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
min_distance = capteurs_VL53L1X[capteur].distance_cm;
}
}
}
return min_distance;

8
Strategie.c
View File

@ -1,6 +1,7 @@
#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
@ -9,11 +10,10 @@
#include "Trajet.h"
#include "math.h"
#define DEGREE_EN_RADIAN (M_PI / 180.)
#define SEUIL_RECAL_DIST_MM 75
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
#define DISTANCE_OBSTACLE_CM 35
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);
@ -195,6 +195,10 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
break;
case PARCOURS_AVANCE:
if(Balise_VL53L1X_get_min_distance() <DISTANCE_OBSTACLE_CM){
Trajet_avance(0.);
break;
}
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_action = ACTION_TERMINEE;

3
Strategie.h
View File

@ -1,10 +1,12 @@
#include "pico/stdlib.h"
#include "Trajectoire.h"
#ifndef STRATEGIE_H
#define STRATEGIE_H
#define COULEUR 15
#define TIRETTE 14
#define DEGREE_EN_RADIAN (M_PI / 180.)
enum etat_action_t{
ACTION_EN_COURS,
@ -36,6 +38,7 @@ extern enum etat_strategie_t{
enum etat_action_t cerise_accostage(void);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);

3
Test.c
View File

@ -223,6 +223,8 @@ int test_continue_test(){
int test_capteurs_balise(void){
printf("Test de la balise\n");
i2c_maitre_init();
Localisation_set(0,0,0);
Balise_VL53L1X_init();
while(true){
uint8_t min_distance;
@ -235,6 +237,7 @@ int test_capteurs_balise(void){
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
sleep_ms(20);
}
}

79
Test_strategie.c
View File

@ -4,6 +4,7 @@
#include "math.h"
#include "Asser_Moteurs.h"
#include "Balise_VL53L1X.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "gyro.h"
@ -19,6 +20,7 @@
int test_accostage(void);
int test_longe(void);
int test_homologation(void);
int test_evitement(void);
int test_tirette_et_couleur();
void affichage_test_strategie(){
@ -78,6 +80,11 @@ int test_strategie(){
while(test_tirette_et_couleur());
break;
case 'e':
case 'E':
while(test_evitement());
break;
case 'h':
case 'H':
while(test_homologation());
@ -149,6 +156,78 @@ int test_homologation(){
}
void affichage_test_evitement(){
while(1){
printf(">min_dist:%d\n",Balise_VL53L1X_get_min_distance());
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
}
}
int test_evitement(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
printf("Evitement\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(200,200,0);
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
Trajet_config(100, 500);
multicore_launch_core1(affichage_test_evitement);
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);
}
}
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
1000,0,
0, 0); // Angles
if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
}
}
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;