On attrape une plante et on la dépose dans le pot
This commit is contained in:
parent
00f923f7f1
commit
6acb86c729
@ -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;
|
||||
}
|
@ -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);
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user