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

Tentative de pré-homologation : marquer des points

Browse Source
This commit is contained in:
Samuel 2024-05-01 09:23:28 +02:00
parent 89e057a285
commit 189101b849
6 changed files with 281 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
Strategie_2024_plante.c
View File

@ -3,12 +3,14 @@
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "i2c_annexe.h"
#include "math.h"
#include <math.h>
#include <stdio.h>
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
#define RAYON_ZONE_PLANTE_MM 180
struct position_t liste_zone_plante[]=
{
{.x_mm = 1500, .y_mm = 1500 },
@ -30,26 +32,75 @@ enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante,
angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
}
/// @brief Indique si la plante se trouve dans la zone de récole (en comptant une marge)
/// @param position_plante : position de la plante
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
/// @return 1 si la plante est dans la zone, 0 si le robot ne trouve pas la plante ou qu'elle est hors zone.
bool est_dans_zone(struct position_t position_plante, enum zone_plante_t zone_plante){
struct position_t position_zone_plante;
float distance_plante_zone;
enum etat_action_t Strat_2024_aller_a_plante(void){
position_zone_plante = liste_zone_plante[zone_plante];
distance_plante_zone = sqrtf( powf(position_zone_plante.x_mm - position_zone_plante.x_mm, 2) +
powf(position_zone_plante.y_mm - position_zone_plante.y_mm, 2));
if(distance_plante_zone < RAYON_ZONE_PLANTE_MM){
return 1;
}
return 0;
}
/// @brief Déplace le robot vers une plante, vérifie que la plante est bien dans la zone plante qu'on vise
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
/// @return ACTION_SUCCESS si le robot est prêt à attraper la plante, ACTION_ECHEC si la plante
enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
static enum {
SAAP_INIT_DETECTION,
SAAP_ASSERV
} etat_aller_a_plante = SAAP_INIT_DETECTION;
enum validite_vl53l8_t validite;
float angle, distance, commande_vitesse_plante;
static int tempo;
float angle_rad, distance_mm, commande_vitesse_plante;
static int tempo_ms;
static bool entree_dans_zone;
switch(etat_aller_a_plante){
case SAAP_INIT_DETECTION:
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
tempo_ms = 2000;
entree_dans_zone=false;
etat_aller_a_plante = SAAP_ASSERV;
break;
case SAAP_ASSERV:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
if(validite == VL53L8_PLANTE){
commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
tempo_ms = 2000;
// 1 on s'assure que la plante est dans la zone qu'on recherche !
if(zone_plante != ZONE_PLANTE_AUCUNE){
// Cas où on checher une plante dans une zone
struct position_t position_robot, position_plante;
bool robot_dans_zone;
position_robot = Localisation_get();
position_robot.angle_radian += ANGLE_PINCE + angle_rad;
position_plante = Geometrie_deplace(position_robot, distance_mm);
if( !est_dans_zone(position_plante, zone_plante)){
return ACTION_ECHEC;
}
robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
if(entree_dans_zone == true){
if(robot_dans_zone == false){
// Le robot est sorti de la zone
return ACTION_ECHEC;
}
}
if(robot_dans_zone == true){
entree_dans_zone = true;
}
}
// 2 on s'assure qu'il n'y a pas de robot en face (TODO)
commande_vitesse_plante = (distance_mm - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
if(commande_vitesse_plante > 300){
commande_vitesse_plante = 300;
}
@ -61,8 +112,12 @@ enum etat_action_t Strat_2024_aller_a_plante(void){
}
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle_rad * ASSER_ANGLE_GAIN_PLANTE_P);
}else{
tempo_ms--;
if(tempo_ms <= 0){
return ACTION_ECHEC;
}
}
break;
}
@ -74,7 +129,7 @@ enum etat_action_t Strat_2024_aller_a_plante(void){
/// @param step_ms
/// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){
static enum{
PDP_ALLER_PLANTE,
PDP_PRE_PRISE_PLANTE,
@ -90,7 +145,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
switch (etat_plante_dans_pot)
{
case PDP_ALLER_PLANTE:
if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
if (Strat_2024_aller_a_plante(zone_plante) == ACTION_TERMINEE){
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
}
break;
@ -152,6 +207,12 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_ALLER_PLANTE;
if(bras_depose_pot == PLANTE_BRAS_1){
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
}else{
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
}
return ACTION_TERMINEE;
}
default:

5
Strategie_2024_plante.h
View File

@ -7,8 +7,9 @@ enum zone_plante_t{
ZONE_PLANTE_4=3,
ZONE_PLANTE_5=4,
ZONE_PLANTE_6=5,
ZONE_PLANTE_AUCUNE=99
};
enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms);
enum etat_action_t Strat_2024_aller_a_plante();
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot);
enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante);
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante);

21
Strategie_2024_pots.c
View File

@ -6,8 +6,7 @@
#include "i2c_annexe.h"
#include "Localisation.h"
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 185.
float angle_bras[6] =
{
@ -19,6 +18,16 @@ 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
};
enum etat_bras_t{
BRAS_LIBRE,
BRAS_OCCUPE,
@ -68,12 +77,12 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p
}
/// @brief A Affiner
/// @param
/// @return
/// @return numéro du bras libre (entre 0 et 5)
int ordre_bras[NB_BRAS]={5, 0, 1, 2, 3, 4};
int get_bras_libre(void){
for (int i=0; i<NB_BRAS; i++){
if(etat_bras[i] == BRAS_LIBRE){
return i;
if(etat_bras[ordre_bras[i]] == BRAS_LIBRE){
return ordre_bras[i];
}
}
return 9;

3
Strategie_2024_pots.h
View File

@ -21,5 +21,8 @@
#define BRAS_5 4
#define BRAS_6 5
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 185.
struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms);

6
Test_i2c.c
View File

@ -292,9 +292,9 @@ int test_i2c_ecriture_pico_annex_nb_2(){
enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
printf("A - Aimant tient pot\n");
printf("A - Aimant lache pot\n");
printf("B - Bras attrape pot\n");
printf("C - Aimant lache pot\n");
printf("C - Aimant tient pot\n");
printf("D - Bras lève pot\n");
printf("E - Attrape plante - bras 1\n");
printf("F - Attrape plante - bras 6\n");
@ -432,7 +432,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("lettre non reconnue: %d %c\n", lettre, lettre);
}
}
//sleep_ms(1);
sleep_ms(1);
i2c_gestion(i2c0);
i2c_annexe_gestion();

194
Test_strategie_2024.c
View File

@ -27,6 +27,7 @@ int test_aller_a_plante(void);
int test_attrape_plante(void);
int test_aller_zone_plante();
int test_pseudo_homologation(void);
int test_attrape_1_pot(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
@ -37,6 +38,7 @@ int test_strategie_2024(){
printf("E - Attrape plante.\n");
printf("F - Aller zone plante.\n");
printf("G - Pseudo homologation.\n");
printf("H - réglage pots.\n");
int lettre;
do{
@ -78,6 +80,11 @@ int test_strategie_2024(){
while(test_pseudo_homologation());
break;
case 'h':
case 'H':
while(test_attrape_1_pot());
break;
case 'q':
case 'Q':
return 0;
@ -235,7 +242,7 @@ int test_attrape_plante(){
}
}
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6);
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
}
@ -294,7 +301,7 @@ int test_aller_a_plante(){
}
}
etat_action = Strat_2024_aller_a_plante();
etat_action = Strat_2024_aller_a_plante(ZONE_PLANTE_AUCUNE);
}
@ -461,7 +468,10 @@ int test_pseudo_homologation(){
TAP_CALAGE,
TAP_POT,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE
TAP_PLANTE_ATTRAPE_1,
TAP_PLANTE_ATTRAPE_2,
TAP_RENTRE,
TAP_DEPOSE
} etat_test = TAP_CALAGE;
printf("test_attrape_pot\n");
@ -520,15 +530,43 @@ int test_pseudo_homologation(){
case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE;
etat_test=TAP_PLANTE_ATTRAPE_1;
}
break;
case TAP_PLANTE_ATTRAPE:
if(Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1) == ACTION_TERMINEE){
etat_action= ACTION_TERMINEE;
case TAP_PLANTE_ATTRAPE_1:
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_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, ZONE_PLANTE_3);
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:
if(Strategie_aller_a(250, 250, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE;
}
break;
case TAP_DEPOSE:
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
break;
}
}
@ -587,3 +625,145 @@ void affichage_test_strategie_2024(){
sleep_ms(100);
}
}
/// @brief Fonction abandonnée - pour le réglage des bras un par un
/// @param
/// @return
int test_attrape_1_pot(void){
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=ACTION_EN_COURS;
int32_t tempo_ms;
enum {
TAP_CALAGE,
TAP_APPROCHE_POT,
TAP_TOURNE_POT,
TAP_ATTRAPE_POT,
TAP_ATTRAPE_TEMPO
} etat_test = TAP_CALAGE;
printf("test_attrape_1_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(TRAJECT_CONFIG_STD);
multicore_launch_core1(affichage_test_strategie_2024);
float angle_bras[6] =
{
180 * DEGRE_EN_RADIAN,
120 * DEGRE_EN_RADIAN,
60 * DEGRE_EN_RADIAN,
0,
-60 * DEGRE_EN_RADIAN,
-120 * DEGRE_EN_RADIAN
};
float angle_bras_correction[6] =
{
11 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0,
0 * DEGRE_EN_RADIAN,
13 * DEGRE_EN_RADIAN
};
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
static struct position_t position_pot, position_approche_pot, position_attrape_pot;
position_pot = groupe_pot_get_pot(GROUPE_POT_B1, POT_5);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
int bras = BRAS_1;
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);
Evitement_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){
case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_APPROCHE_POT;
}
break;
case TAP_APPROCHE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a(position_approche_pot.x_mm, position_approche_pot.y_mm, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_TOURNE_POT;
}
break;
case TAP_TOURNE_POT:
if( Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras], _step_ms) == ACTION_TERMINEE){
etat_test=TAP_ATTRAPE_POT;
}
break;
case TAP_ATTRAPE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
if( Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras],
SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
tempo_ms = 2000;
etat_test = TAP_ATTRAPE_TEMPO;
}
break;
case TAP_ATTRAPE_TEMPO:
tempo_ms--;
if(tempo_ms < 0){
etat_action = ACTION_TERMINEE;
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}