Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Attrape pot + recalage lointain bordure

Browse Source
This commit is contained in:
Samuel 2024-04-12 22:37:19 +02:00
parent c5e76ba440
commit 5318b1eb3f
9 changed files with 290 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Geometrie_robot.h
View File

@ -1,8 +1,8 @@
#define DISTANCE_ROUES_CENTRE_MM 112
#define DISTANCE_ROUES_CENTRE_MM 117.2
// Distance entre le centre du robot et un angle du robot
#define RAYON_ROBOT 160.
#define DISTANCE_CENTRE_CAPTEUR 100.
#define DISTANCE_CENTRE_CAPTEUR 72.5
#define ANGLE_PINCE (-150 * DEGRE_EN_RADIAN)
// Distance entre le centre du robot et un bord du robot

69
Strategie.c
View File

@ -289,6 +289,57 @@ enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float
return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
}
/// @brief Attention - ne marche pas !!!
/// @param pos_x
/// @param pos_y
/// @param angle_radian
/// @param
/// @param step_ms
/// @return
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
struct trajectoire_t trajectoire;
static enum {
AAPT_ALLER,
AAPT_TOURNER,
}etat_aller_a_puis_tourner=AAPT_ALLER;
switch(etat_aller_a_puis_tourner){
case AAPT_ALLER:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){
etat_aller_a_puis_tourner = AAPT_TOURNER;
}
break;
case AAPT_TOURNER:
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){
etat_aller_a_puis_tourner = AAPT_ALLER;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){
struct trajectoire_t trajectoire;
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT);
}
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
@ -417,9 +468,11 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
CD_ROTATION_VERS_X,
CD_LECTURE_BORDURE_X,
CD_ALLER_POSITION_INIT,
CD_ROTATION_POSITION_INIT,
}etat_calage_debut=CD_ENVOI_CDE_BORDURE;
enum validite_vl53l8_t validite;
struct trajectoire_t trajectoire;
enum etat_action_t etat_action;
float angle, distance;
@ -476,12 +529,22 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
case CD_ALLER_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(couleur == COULEUR_BLEU){
return Strategie_tourner_et_aller_a(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE, SANS_EVITEMENT, step_ms);
etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms);
}else{
return Strategie_tourner_et_aller_a(3000 - 250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE, SANS_EVITEMENT, step_ms);
etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms);
}
if(etat_action == ACTION_TERMINEE){
etat_calage_debut = CD_ROTATION_POSITION_INIT;
}
break;
case CD_ROTATION_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
/*Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
(45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
0);
return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT);
//etat_calage_debut = CD_ENVOI_CDE_BORDURE;
}
return ACTION_EN_COURS;

2
Strategie.h
View File

@ -68,6 +68,8 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms)
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms);
extern float distance_obstacle;

114
Strategie_2024_pots.c
View File

@ -1,11 +1,13 @@
#include "math.h"
#include "Strategie.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"
#include "Strategie_2024_pots.h"
#include "i2c_annexe.h"
#include "Localisation.h"
#define DISTANCE_APPROCHE_POT_MM 250.
#define DISTANCE_ATTRAPE_POT_MM 100.
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 180.
float angle_bras[6] =
{
@ -17,6 +19,11 @@ float angle_bras[6] =
-120 * DEGRE_EN_RADIAN
};
enum etat_bras_t{
BRAS_LIBRE,
BRAS_OCCUPE,
} etat_bras[NB_BRAS];
struct position_t position_pots_dans_groupe_pot[5] =
{
{.x_mm = -40, .y_mm = 69.2, .angle_radian = -60 * DEGRE_EN_RADIAN},
@ -30,7 +37,7 @@ struct position_t position_groupe_pot[6] =
{
{.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 1000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN},
{.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN},
{.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN},
{.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN},
{.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN}
@ -60,26 +67,44 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p
return position_pot;
}
/// @brief A Affiner
/// @param
/// @return
int get_bras_libre(void){
return BRAS_1;
for (int i=0; i<NB_BRAS; i++){
if(etat_bras[i] == BRAS_LIBRE){
return i;
}
}
return 9;
}
int get_pot_suivant(int pot){
if(pot < 4){
pot++;
return pot;
}
return POT_INVALIDE;
}
/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms){
// Parcourir la trajectoire pour aller jusqu'au premier pot
struct position_t position_pot, position_approche_pot, position_attrape_pot;
static struct position_t position_pot, position_approche_pot, position_attrape_pot;
enum etat_action_t etat_action;
enum validite_vl53l8_t validite;
float distance, angle;
static int bras, tempo_ms;
// Pour le 1er pot
position_pot = groupe_pot_get_pot(groupe_pot, POT_1);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
static int pot = POT_1;
static enum {
AP_ALLER_VERS_GROUPE_POT,
AP_RECALE,
AP_ORIENTE,
AP_APPROCHE_POT,
AP_ATTRAPE_POT,
AP_RETOUR_ET_LEVE_POT
@ -95,23 +120,63 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
switch (etat_attrape_pot)
{
case AP_ALLER_VERS_GROUPE_POT:
printf("position_pot X:%f Y:%f r:%f\n", position_pot.x_mm, position_pot.y_mm,
position_pot.angle_radian / DEGRE_EN_RADIAN);
printf("position_approche_pot X:%f Y:%f r:%f\n", position_approche_pot.x_mm, position_approche_pot.y_mm,
position_approche_pot.angle_radian / DEGRE_EN_RADIAN);
etat_attrape_pot = AP_APPROCHE_POT;
case AP_APPROCHE_POT:
position_pot = groupe_pot_get_pot(groupe_pot, POT_1);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
etat_action = Strategie_tourner_et_aller_a(
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0],
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ATTRAPE_POT;
etat_attrape_pot = AP_RECALE;
bras = get_bras_libre();
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
}
break;
case AP_RECALE:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_DISTANCE_LOIN){
if(fabs(distance + DISTANCE_CENTRE_CAPTEUR - Localisation_get().x_mm) < 25){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
//if(couleur == COULEUR_BLEU){
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
/*}else{
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
}*/
etat_attrape_pot = AP_ORIENTE;
}else{
printf("Erreur - recalage trop loin\n");
}
}
break;
case AP_ORIENTE:
bras = get_bras_libre();
if(Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras], step_ms) == ACTION_TERMINEE){
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
case AP_APPROCHE_POT:
bras = get_bras_libre();
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm);
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
case AP_ATTRAPE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
etat_action = Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
@ -119,6 +184,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
if (etat_action == ACTION_TERMINEE){
tempo_ms=250;
etat_attrape_pot = AP_RETOUR_ET_LEVE_POT;
etat_bras[bras] = BRAS_OCCUPE;
}
break;
@ -132,9 +198,17 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
// TODO: pot suivant
return ACTION_TERMINEE;
etat_attrape_pot = AP_APPROCHE_POT;
pot = get_pot_suivant(pot);
if(pot > 4){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;
}
position_pot = groupe_pot_get_pot(groupe_pot, pot);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm);
}
break;

1
Strategie_2024_pots.h
View File

@ -5,6 +5,7 @@
#define POT_3 2
#define POT_4 3
#define POT_5 4
#define POT_INVALIDE 255
#define GROUPE_POT_L1 0
#define GROUPE_POT_L2 1

9
Test_i2c.c
View File

@ -301,6 +301,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("G - Detection bordure\n");
printf("H - Detection plante\n");
printf("I - Detection stop\n");
printf("J - Detection distance loin\n");
printf("S - Score + 1\n");
printf("\nQ - Quitter\n");
@ -386,6 +387,11 @@ int test_i2c_ecriture_pico_annex_nb_2(){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
printf("=> Detection STOP\n");
break;
case 'J':
case 'j':
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
printf("=> Detection distance loin\n");
break;
case 'q':
case 'Q':
@ -415,6 +421,9 @@ int test_i2c_ecriture_pico_annex_nb_2(){
case VL53L8_PLANTE:
printf(">p_angle:%.2f\n>p_distance:%.2f\n", angle, distance);
break;
case VL53L8_DISTANCE_LOIN:
printf("\n>v_distance:%.2f\n", distance);
break;
}
}

23
Test_strategie_2024.c
View File

@ -156,8 +156,7 @@ int test_calage_debut(){
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
Moteur_Stop();
if(lettre == 'q' && lettre == 'Q'){
return 0;
@ -170,8 +169,12 @@ int test_attrape_pot(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action;
enum etat_action_t etat_action=ACTION_EN_COURS;
enum {
TAP_CALAGE,
TAP_POT
} etat_test = TAP_CALAGE;
printf("test_attrape_pot\n");
@ -182,7 +185,7 @@ int test_attrape_pot(){
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(0);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
@ -215,8 +218,16 @@ int test_attrape_pot(){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);
break;
}
}
lettre = getchar_timeout_us(0);

99
Tests_deplacement.c
View File

@ -25,6 +25,7 @@ int test_cde_vitesse_cercle(void);
int test_trajectoire(void);
int test_aller_retour(void);
int test_endurance_aller_retour(void);
int test_trajectoire_calibration(void);
bool continuous_printf=true;
@ -38,7 +39,8 @@ int mode_test_deplacement(){
printf("B - AsserMoteur - Commande en vitesse...\n");
printf("C - Trajectoires - sans gyroscope...\n");
printf("D - Aller-retour - avec gyroscope...\n");
printf("D - Aller-retour - Endurance - avec gyroscope...\n");
printf("E - Aller-retour - Endurance - avec gyroscope...\n");
printf("F - Calibration distance\n");
stdio_flush();
@ -70,6 +72,11 @@ int mode_test_deplacement(){
while(test_endurance_aller_retour());
break;
case 'f':
case 'F':
while(test_trajectoire_calibration());
break;
case PICO_ERROR_TIMEOUT:
iteration--;
if(iteration == 0){
@ -287,7 +294,7 @@ int test_trajectoire(){
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 700, 0, 0, 0);
Trajectoire_droite(&trajectoire, 0, 0, 1000, 0, 0, 0);
printf("Trajectoire droite\n");
break;
@ -585,4 +592,92 @@ int test_endurance_aller_retour(){
return 0;
}
/// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite)
/// Localisation avec Gyroscope
/// @return
int test_trajectoire_calibration(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("A - Rotation sans gyro\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'a':
case 'A':
Localisation_set(0, 0, 0);
set_position_avec_gyroscope(0);
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
Trajectoire_rotation(&trajectoire, 0, 0, 0, 4 * M_PI);
printf("Rotation sans gyro\n");
break;
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
printf("Trajectoire Bezier\n");
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Localisation_set(250, 250, 180. * DEGRE_EN_RADIAN );
set_position_avec_gyroscope(1);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm + 1000,
Localisation_get().angle_radian, Localisation_get().angle_radian - (M_PI / 2.));
printf("Trajectoire droite\n");
break;
default: return 0;
}
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
do{
// Routines à 1 ms
QEI_update();
Localisation_gestion();
// Routine à 2 ms
if(get_position_avec_gyroscope()){
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
}
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
}else{
AsserMoteur_Gestion(_step_ms);
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
lettre = PICO_ERROR_TIMEOUT;
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}

3
i2c_annexe.h
View File

@ -21,7 +21,8 @@
enum validite_vl53l8_t{
VL53L8_INVALIDE=0,
VL53L8_BORDURE=1,
VL53L8_PLANTE=2
VL53L8_PLANTE=2,
VL53L8_DISTANCE_LOIN=3,
};
void i2c_annexe_gestion(void);