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

Debut du programme principal + detection obtacle pour chercher les plantes

Browse Source
This commit is contained in:
Samuel 2024-05-06 15:03:21 +02:00
parent ba9a1e5411
commit 015d24d0b8
8 changed files with 272 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -34,6 +34,7 @@ Strategie.c
Strategie_deplacement.c
Strategie_2024_plante.c
Strategie_2024_pots.c
Strategie_2024.c
Temps.c
Test.c
Test_gyro.c

6
Holonome2023.c
View File

@ -44,8 +44,8 @@ int main() {
stdio_init_all();
//Demonstration_init();Demonstration_auto();
while(mode_test());
test_pseudo_homologation();
//while(mode_test());
//test_pseudo_homologation();
Holonome2023_init();
multicore_launch_core1(Monitoring_display);
@ -113,7 +113,7 @@ int main() {
printf("MATCH_ARRET_EN_COURS\n");
statu_match = MATCH_ARRET_EN_COURS;
}
Strategie(couleur, _step_ms, timer_match_ms);
Strategie_2024(couleur, _step_ms, timer_match_ms);
break;
case MATCH_ARRET_EN_COURS:

2
Strategie.c
View File

@ -248,7 +248,7 @@ uint attente_tirette(void){
return !gpio_get(TIRETTE);
}
/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
/// @brief Renvoi COULEUR_JAUNE ou COULEUR_BLEU
enum couleur_t lire_couleur(void){
if (gpio_get(COULEUR))
return COULEUR_JAUNE;

174
Strategie_2024.c Normal file
View File

@ -0,0 +1,174 @@
#include "Strategie_2024.h"
#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "i2c_annexe.h"
enum etat_groupe_pot_t{
GROUPE_POT_DISPO,
GROUPE_POT_ECHEC
};
enum etat_groupe_plante_t{
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_ECHEC
};
enum etat_groupe_pot_t etat_groupe_pot[6]={
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO,
GROUPE_POT_DISPO
};
enum etat_groupe_plante_t etat_groupe_plante[6]={
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO,
GROUPE_PLANTE_DISPO
};
int ordre_groupe_pot[6];
unsigned int get_groupe_pot(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){
return GROUPE_POT_B1;
}
return GROUPE_POT_B2;
}
enum zone_plante_t get_zone_plante(enum couleur_t couleur){
enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_4, ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_1};
enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_4, ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_1};
enum zone_plante_t *ordre_groupe_plante;
int i;
if(couleur == COULEUR_BLEU){
ordre_groupe_plante = ordre_groupe_plante_bleu;
}else{
ordre_groupe_plante = ordre_groupe_plante_jaune;
}
for(i=0; i<6; i++){
if(etat_groupe_plante[ordre_groupe_plante[i]] == GROUPE_PLANTE_DISPO){
return ordre_groupe_plante[i];
}
}
return ZONE_PLANTE_AUCUNE;
}
void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
static enum {
TAP_CALAGE,
TAP_POT,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE_1,
TAP_PLANTE_ATTRAPE_2,
TAP_ECHANGE_POT,
TAP_PLANTE_ATTRAPE_3,
TAP_PLANTE_ATTRAPE_4,
TAP_RENTRE,
TAP_DEPOSE
} etat_test = TAP_CALAGE;
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ORIENTATION;
}
break;
case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_1;
}
break;
case TAP_PLANTE_ATTRAPE_1:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_2;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_2:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_ECHANGE_POT;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_ECHANGE_POT:
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_3;
}
break;
case TAP_PLANTE_ATTRAPE_3:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_4;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_4:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_RENTRE:
float angle_destination;
angle_destination = 15 * DEGRE_EN_RADIAN;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms=500;
}
break;
case TAP_DEPOSE:
tempo_ms--;
commande_vitesse_stop();
if(tempo_ms<= 0){
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
}
break;
}
}

17
Strategie_2024.h Normal file
View File

@ -0,0 +1,17 @@
#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Strategie.h"
#include "Strategie_2024_plante.h"
#include "Strategie_2024_pots.h"
#include "Commande_vitesse.h"
#ifndef STRATEGIE_2024_H
#define STRATEGIE_2024_H
void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
// STRATEGIE_H
#endif

12
Strategie_2024_plante.c
View File

@ -60,7 +60,9 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
SAAP_ASSERV
} etat_aller_a_plante = SAAP_INIT_DETECTION;
enum validite_vl53l8_t validite;
float angle_rad, distance_mm, commande_vitesse_plante;
float angle_rad, distance_mm, distance_obstacle, commande_vitesse_plante;
float distance_contrainte_obstacle, vitesse_max_contrainte_obstacle;
const float acceleration_mm_ss_obstacle=500;
static float distance_min_mm;
static int tempo_ms, tempo_asserv;
static bool entree_dans_zone;
@ -106,6 +108,10 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
}
// 2 on s'assure qu'il n'y a pas de robot en face (TODO)
distance_contrainte_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(ANGLE_PINCE);
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
}
// 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête
if(distance_mm < distance_min_mm){
@ -130,6 +136,10 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_TERMINEE;
}
// On limite la vitesse avec l'obstacle
if (commande_vitesse_plante > vitesse_max_contrainte_obstacle){
commande_vitesse_plante = vitesse_max_contrainte_obstacle;
}
commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante ,
sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P);

73
Strategie_2024_pots.c
View File

@ -18,19 +18,10 @@ float angle_bras[6] =
-120 * DEGRE_EN_RADIAN
};
float angle_bras_correction[6] =
{
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0,
0 * DEGRE_EN_RADIAN,
7 * DEGRE_EN_RADIAN
};
float distance_bras_correction_mm[6] =
{
0,
-5,
0,
-10,
-15,
@ -125,7 +116,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
static enum {
AP_ALLER_VERS_GROUPE_POT,
AP_RECALE,
AP_ORIENTE,
AP_APPROCHE_POT,
AP_ATTRAPE_POT,
@ -153,31 +143,13 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
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),
EVITEMENT_SANS_EVITEMENT, step_ms);
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_RECALE;
etat_attrape_pot = AP_ORIENTE;
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){
@ -193,7 +165,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_aller_a_puis_tourner(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
if (etat_action == ACTION_TERMINEE){
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
@ -222,7 +194,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_APPROCHE_POT;
pot = get_pot_suivant(pot);
@ -245,7 +217,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;
@ -261,12 +233,15 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}
/// @brief Echange les pots avant avec les pots arrière
/// @param step_ms Le pas de temps
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
static struct position_t position_initiale;
struct position_t position_but, position_tmp;
enum etat_action_t etat_action;
struct trajectoire_t trajectoire;
static int tempo_ms;
static enum {
@ -294,9 +269,13 @@ enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
break;
case EPAA_TOURNE:
if(Strategie_tourner_a(position_initiale.angle_radian + M_PI, step_ms) == ACTION_TERMINEE){
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, position_initiale.angle_radian - M_PI);
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_echange_pot = EPAA_ATTRAPE_POT;
position_initiale.angle_radian += M_PI;
position_initiale.angle_radian -= M_PI;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
@ -397,6 +376,7 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
DP_BAISSE_BRAS,
DP_BAISSE_BRAS_TEMPO,
DP_RANGE_DOIGT,
DP_RANGE_BRAS_TEMPO,
} etat_depose_pot = DP_BAISSE_BRAS;
switch (etat_depose_pot)
{
@ -430,7 +410,22 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
case DP_RANGE_DOIGT:
tempo_ms--;
if(tempo_ms < 0){
etat_depose_pot = DP_BAISSE_BRAS;
for (int i=0; i< NB_BRAS; i++){
masque =1;
masque = masque << i;
if(masque_pot & masque){
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
tempo_ms=250;
}
}
etat_depose_pot = DP_RANGE_BRAS_TEMPO;
}
break;
case DP_RANGE_BRAS_TEMPO:
tempo_ms--;
if(tempo_ms < 0){
return ACTION_TERMINEE;
}
break;

31
Test_strategie_2024.c
View File

@ -480,6 +480,9 @@ int test_pseudo_homologation(){
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE_1,
TAP_PLANTE_ATTRAPE_2,
TAP_ECHANGE_POT,
TAP_PLANTE_ATTRAPE_3,
TAP_PLANTE_ATTRAPE_4,
TAP_RENTRE,
TAP_DEPOSE
} etat_test = TAP_CALAGE;
@ -557,6 +560,34 @@ int test_pseudo_homologation(){
case TAP_PLANTE_ATTRAPE_2:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_ECHANGE_POT;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_ECHANGE_POT:
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_3;
}
break;
case TAP_PLANTE_ATTRAPE_3:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_4;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_4:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;