From 00f923f7f12ca40bca38c95a4b518273493a0199 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 13 Apr 2024 21:50:50 +0200 Subject: [PATCH] On attrape une plante --- CMakeLists.txt | 1 + Geometrie_robot.h | 2 + Strategie_2024_plante.c | 66 +++++++++++++++++++++++++++++++ Strategie_2024_plante.h | 3 ++ Test_strategie_2024.c | 88 ++++++++++++++++++++++++++++++++++++++--- i2c_annexe.c | 15 +++++++ 6 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 Strategie_2024_plante.c create mode 100644 Strategie_2024_plante.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7ea2ec8..a683ba1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ Score.c Servomoteur.c Strategie.c Strategie_deplacement.c +Strategie_2024_plante.c Strategie_2024_pots.c Temps.c Test.c diff --git a/Geometrie_robot.h b/Geometrie_robot.h index 528d81e..c1ec100 100644 --- a/Geometrie_robot.h +++ b/Geometrie_robot.h @@ -1,3 +1,5 @@ +#include "Geometrie.h" + #define DISTANCE_ROUES_CENTRE_MM 117.2 // Distance entre le centre du robot et un angle du robot diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c new file mode 100644 index 0000000..67e8291 --- /dev/null +++ b/Strategie_2024_plante.c @@ -0,0 +1,66 @@ +#include "Commande_vitesse.h" +#include "Strategie_2024_plante.h" +#include "Geometrie_robot.h" +#include "i2c_annexe.h" +#include "math.h" +#include + +#define ASSER_ANGLE_GAIN_PLANTE_P 1.5 +#define ASSER_DISTANCE_GAIN_PLANTE_P 10 + +enum etat_action_t Strat_2024_aller_a_plante(){ + static enum { + SAAP_INIT_DETECTION, + SAAP_ASSERV, + SAAP_ATTRAPE, + SAAP_ATTRAPE_TEMPO, + } etat_aller_a_plante = SAAP_INIT_DETECTION; + enum validite_vl53l8_t validite; + float angle, distance, commande_vitesse_plante; + static int tempo; + + switch(etat_aller_a_plante){ + case SAAP_INIT_DETECTION: + i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE); + etat_aller_a_plante = SAAP_ASSERV; + break; + + case SAAP_ASSERV: + i2c_annexe_get_VL53L8(&validite, &angle, &distance); + if(validite == VL53L8_PLANTE){ + commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P; + if(commande_vitesse_plante > 300){ + commande_vitesse_plante = 300; + } + if(commande_vitesse_plante <= 0){ + commande_vitesse_stop(); + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + etat_aller_a_plante = SAAP_ATTRAPE; + break; + } + + commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante , + sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P); + }else{ + } + break; + + case SAAP_ATTRAPE: + i2c_annexe_attrape_plante(PLANTE_BRAS_1); + etat_aller_a_plante = SAAP_ATTRAPE_TEMPO; + tempo = 2500; + break; + + case SAAP_ATTRAPE_TEMPO: + tempo--; + if(tempo <= 0){ + etat_aller_a_plante = SAAP_INIT_DETECTION; + return ACTION_TERMINEE; + } + break; + + + } + + return ACTION_EN_COURS; +} \ No newline at end of file diff --git a/Strategie_2024_plante.h b/Strategie_2024_plante.h new file mode 100644 index 0000000..115d003 --- /dev/null +++ b/Strategie_2024_plante.h @@ -0,0 +1,3 @@ +#include "Strategie.h" + +enum etat_action_t Strat_2024_aller_a_plante(); \ No newline at end of file diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index e1189df..d235e82 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -11,6 +11,7 @@ #include "Asser_Moteurs.h" #include "Localisation.h" #include "Robot_config.h" +#include "Strategie_2024_plante.h" #include "Strategie_2024_pots.h" #include "Strategie.h" #include "Trajectoire.h" @@ -21,13 +22,15 @@ int test_calcul_position_pot(void); int test_calage_debut(void); -int test_attrape_pot(); +int test_attrape_pot(void); +int test_attrape_plante(void); void affichage_test_strategie_2024(void); int test_strategie_2024(){ printf("A - Position groupes pot.\n"); printf("B - Calage debut.\n"); printf("C - Attrape pot.\n"); + printf("D - Attrape plante.\n"); int lettre; do{ @@ -49,6 +52,11 @@ int test_strategie_2024(){ while(test_attrape_pot()); break; + case 'd': + case 'D': + while(test_attrape_plante()); + break; + case 'q': case 'Q': return 0; @@ -154,13 +162,67 @@ int test_calage_debut(){ 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); Moteur_Stop(); - if(lettre == 'q' && lettre == 'Q'){ - return 0; + return 0; + +} + +int test_attrape_plante(){ + 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; + + printf("test_attrape_plante\n"); + + i2c_maitre_init(); + Trajet_init(); + Balise_VL53L1X_init(); + + + set_position_avec_gyroscope(0); + if(get_position_avec_gyroscope()){ + printf("Init gyroscope\n"); + Gyro_Init(); } + + stdio_flush(); + + multicore_launch_core1(affichage_test_strategie_2024); + + + 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); + Evitement_gestion(_step_ms); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + if(get_position_avec_gyroscope()){ + Gyro_Read(_step_ms_gyro); + } + } + + etat_action = Strat_2024_aller_a_plante(); + + + } + lettre = getchar_timeout_us(0); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(etat_action == ACTION_EN_COURS); + Moteur_Stop(); + return 0; } @@ -248,6 +310,8 @@ int test_attrape_pot(){ void affichage_test_strategie_2024(){ uint32_t temps; + enum validite_vl53l8_t validite_vl53l8; + float angle, distance; while(true){ temps = time_us_32()/1000; @@ -265,6 +329,20 @@ void affichage_test_strategie_2024(){ printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance()); + + i2c_annexe_get_VL53L8(&validite_vl53l8, &angle, &distance); + printf(">v:%d\n", validite_vl53l8); + switch(validite_vl53l8){ + case VL53L8_BORDURE: + printf(">b_angle:%.2f\n>b_distance:%.2f\n", angle, distance); + break; + 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; + } sleep_ms(100); } diff --git a/i2c_annexe.c b/i2c_annexe.c index cb66a18..0e1fc7f 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -44,6 +44,7 @@ void i2c_annexe_gestion(){ EMISSION_TEMPO_RPi, RECEPTION_DONNEES_RPi, EMISSION_PIC18F, + EMISSION_TEMPO_PIC18F, } etat_i2c_annexe=EMISSION_PIC18F; enum i2c_resultat_t retour_i2c; @@ -58,6 +59,8 @@ void i2c_annexe_gestion(){ etat_i2c_annexe = EMISSION_TEMPO_RPi; donnees_a_envoyer=0; temps = time_us_32(); + }else if(retour_i2c == I2C_ECHEC){ + printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE); } }else{ etat_i2c_annexe = EMISSION_TEMPO_RPi; @@ -76,6 +79,8 @@ void i2c_annexe_gestion(){ if(retour_i2c == I2C_SUCCES){ etat_i2c_annexe = EMISSION_PIC18F; temps = time_us_32(); + }else if(retour_i2c == I2C_ECHEC){ + printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE); } break; @@ -88,8 +93,16 @@ void i2c_annexe_gestion(){ temps = time_us_32(); donnees_a_envoyer_pic=0; donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0; + }else if(retour_i2c == I2C_ECHEC){ + printf("ECHEC I2C address: %d\n", ADRESSE_PIC18F4550); } }else{ + etat_i2c_annexe = EMISSION_TEMPO_PIC18F; + } + break; + + case EMISSION_TEMPO_PIC18F: + if(temps + tempo < time_us_32() ){ etat_i2c_annexe = EMISSION_DONNEES_RPi; } break; @@ -172,6 +185,8 @@ void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doi donnees_a_envoyer_pic=1; } +/// @brief Envoi l'ordre d'attraper une plance +/// @param action : PLANTE_BRAS_1 ou PLANTE_BRAS_6 void i2c_annexe_attrape_plante(uint8_t action){ donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = action; donnees_a_envoyer_pic=1;