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

On attrape une plante et on la dépose dans le pot

Browse Source
This commit is contained in:
Samuel 2024-04-13 22:20:51 +02:00
parent 00f923f7f1
commit 6acb86c729
3 changed files with 132 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
Strategie_2024_plante.c
View File

@ -1,6 +1,7 @@
#include "Commande_vitesse.h"
#include "Strategie_2024_plante.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "i2c_annexe.h"
#include "math.h"
#include <stdio.h>
@ -8,12 +9,10 @@
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
enum etat_action_t Strat_2024_aller_a_plante(){
enum etat_action_t Strat_2024_aller_a_plante(void){
static enum {
SAAP_INIT_DETECTION,
SAAP_ASSERV,
SAAP_ATTRAPE,
SAAP_ATTRAPE_TEMPO,
SAAP_ASSERV
} etat_aller_a_plante = SAAP_INIT_DETECTION;
enum validite_vl53l8_t validite;
float angle, distance, commande_vitesse_plante;
@ -35,8 +34,8 @@ enum etat_action_t Strat_2024_aller_a_plante(){
if(commande_vitesse_plante <= 0){
commande_vitesse_stop();
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
etat_aller_a_plante = SAAP_ATTRAPE;
break;
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_TERMINEE;
}
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
@ -44,23 +43,64 @@ enum etat_action_t Strat_2024_aller_a_plante(){
}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;
}
/// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
/// @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){
static enum{
PDP_ALLER_PLANTE,
PDP_ATTRAPE_PLANTE,
PDP_RECULE,
PDP_TEMPO,
} etat_plante_dans_pot = PDP_ALLER_PLANTE;
static int tempo_ms;
struct position_t position_recule, position_initiale;
switch (etat_plante_dans_pot)
{
case PDP_ALLER_PLANTE:
if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
i2c_annexe_attrape_plante(bras_depose_pot);
tempo_ms = 2000;
etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
}
break;
case PDP_ATTRAPE_PLANTE:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_RECULE;
tempo_ms = 2000;
}
break;
case PDP_RECULE:
tempo_ms--;
position_initiale = Localisation_get();
position_initiale.angle_radian += ANGLE_PINCE;
position_recule = Geometrie_deplace(position_initiale, -100);
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
commande_vitesse_stop();
etat_plante_dans_pot=PDP_TEMPO;
}
break;
case PDP_TEMPO:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_ALLER_PLANTE;
return ACTION_TERMINEE;
}
default:
break;
}
return ACTION_EN_COURS;
}

3
Strategie_2024_plante.h
View File

@ -1,3 +1,4 @@
#include "Strategie.h"
enum etat_action_t Strat_2024_aller_a_plante();
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);

70
Test_strategie_2024.c
View File

@ -23,6 +23,7 @@
int test_calcul_position_pot(void);
int test_calage_debut(void);
int test_attrape_pot(void);
int test_aller_a_plante(void);
int test_attrape_plante(void);
void affichage_test_strategie_2024(void);
@ -30,7 +31,8 @@ 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");
printf("D - Aller a plante.\n");
printf("E - Attrape plante.\n");
int lettre;
do{
@ -54,6 +56,11 @@ int test_strategie_2024(){
case 'd':
case 'D':
while(test_aller_a_plante());
break;
case 'e':
case 'E':
while(test_attrape_plante());
break;
@ -174,7 +181,66 @@ int test_attrape_plante(){
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
printf("test_attrape_plante\n");
printf("test_aller_a_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_plante_dans_pot(_step_ms, PLANTE_BRAS_1);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_aller_a_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_aller_a_plante\n");
i2c_maitre_init();
Trajet_init();