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

Fonction echange pot avant<->arriere ok + amélioration de la prise des plantes

Browse Source
This commit is contained in:
Samuel 2024-05-05 22:43:19 +02:00
parent e79b7614f5
commit ba9a1e5411
7 changed files with 338 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Holonome2023.c
View File

@ -44,7 +44,7 @@ int main() {
stdio_init_all();
//Demonstration_init();Demonstration_auto();
//while(mode_test());
while(mode_test());
test_pseudo_homologation();
Holonome2023_init();

54
Strategie_2024_plante.c
View File

@ -61,13 +61,19 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
} etat_aller_a_plante = SAAP_INIT_DETECTION;
enum validite_vl53l8_t validite;
float angle_rad, distance_mm, commande_vitesse_plante;
static int tempo_ms;
static float distance_min_mm;
static int tempo_ms, tempo_asserv;
static bool entree_dans_zone;
switch(etat_aller_a_plante){
case SAAP_INIT_DETECTION:
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_mi_ferme_doigt_plante();
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
tempo_ms = 2000;
tempo_ms = 2000;
distance_min_mm = 2000;
tempo_asserv = 500;
entree_dans_zone=false;
etat_aller_a_plante = SAAP_ASSERV;
break;
@ -100,7 +106,21 @@ 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)
commande_vitesse_plante = (distance_mm - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
// 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){
distance_min_mm = distance_mm;
tempo_asserv = 500;
}else{
tempo_asserv--;
if(tempo_asserv <= 0){
commande_vitesse(0, 0, 0);
return ACTION_ECHEC;
}
}
// 3 on asservi
commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
if(commande_vitesse_plante > 300){
commande_vitesse_plante = 300;
}
@ -111,8 +131,8 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
return ACTION_TERMINEE;
}
commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
sinf(ANGLE_PINCE) * commande_vitesse_plante , angle_rad * ASSER_ANGLE_GAIN_PLANTE_P);
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);
}else{
tempo_ms--;
if(tempo_ms <= 0){
@ -128,7 +148,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
/// @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
/// @return ACTION_EN_COURS ou ACTION_TERMINEE ou ACTION_ECHEC (plante tombée devant le robot)
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,
@ -141,18 +161,23 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
} etat_plante_dans_pot = PDP_ALLER_PLANTE;
static int tempo_ms;
struct position_t position_recule, position_initiale;
enum validite_vl53l8_t validite;
enum etat_action_t etat_action;
float angle_rad, distance_mm;
switch (etat_plante_dans_pot)
{
case PDP_ALLER_PLANTE:
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
if (Strat_2024_aller_a_plante(zone_plante) == ACTION_TERMINEE){
etat_action = Strat_2024_aller_a_plante(zone_plante);
if (etat_action == ACTION_TERMINEE){
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
}else if (etat_action == ACTION_ECHEC){
return ACTION_ECHEC;
}
break;
case PDP_PRE_PRISE_PLANTE:
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
i2c_annexe_ferme_doigt_plante();
tempo_ms = 250;
etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO;
@ -167,6 +192,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
break;
case PDP_PRE_PRISE_RECULE:
i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
tempo_ms--;
if(tempo_ms <= 0){
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
@ -177,10 +203,20 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
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){
if(validite == VL53L8_PLANTE){
if(distance_mm > 90){
// Nous n'avons pas attrapé la plante avec le doigt, on recommence:
etat_plante_dans_pot = PDP_ALLER_PLANTE;
i2c_annexe_ouvre_doigt_plante();
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
break;
}
}
commande_vitesse_stop();
i2c_annexe_ouvre_doigt_plante();
i2c_annexe_attrape_plante(bras_depose_pot);
tempo_ms = 5000;
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
}
break;

182
Strategie_2024_pots.c
View File

@ -63,6 +63,8 @@ struct position_t position_groupe_pot[6] =
};
enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms);
/// @brief renvoie la position du centre du pot ainsi que l'ange par lequel l'attraper
/// @param groupe_pot Position du groupe de pot
/// @param num_pot Pot à prendre, entre 0 et 4 (ou utiliser les macros POT_x)
@ -258,3 +260,183 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
return ACTION_EN_COURS;
}
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;
static int tempo_ms;
static enum {
EPAA_DEPOSE_POT,
EPAA_TOURNE,
EPAA_ATTRAPE_POT,
EPAA_AVANCE_1,
EPAA_RECULE_1,
EPAA_RECENTRE,
EPAA_AVANCE_2,
EPAA_RECULE_2,
EPAA_LEVE,
EPAA_RETOURNE,
} etat_echange_pot = EPAA_DEPOSE_POT;
switch (etat_echange_pot)
{
case EPAA_DEPOSE_POT:
position_initiale = Localisation_get();
// On dépose les pots 1, 3, 4 et 6 : 0b00101101 : 0x2D
if(Strat_2024_depose_pot(0x2D, step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_TOURNE;
}
break;
case EPAA_TOURNE:
if(Strategie_tourner_a(position_initiale.angle_radian + M_PI, step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_ATTRAPE_POT;
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);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms = 500;
}
break;
case EPAA_ATTRAPE_POT:
tempo_ms--;
if(tempo_ms <= 0){
etat_echange_pot = EPAA_AVANCE_1;
}
break;
case EPAA_AVANCE_1:
position_but = Geometrie_deplace(position_initiale, DISTANCE_ECHANGE_POT);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_RECULE_1;
}
break;
case EPAA_RECULE_1:
position_but = Geometrie_deplace(position_initiale, -DISTANCE_ECHANGE_POT);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_RECENTRE;
}
break;
case EPAA_RECENTRE:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(position_initiale.x_mm, position_initiale.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_AVANCE_2;
}
break;
case EPAA_AVANCE_2:
position_tmp = position_initiale;
position_tmp.angle_radian += 40. * DEGRE_EN_RADIAN ;
position_but = Geometrie_deplace(position_tmp, DISTANCE_ECHANGE_POT);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_RECULE_2;
}
break;
case EPAA_RECULE_2:
position_tmp = position_initiale;
position_tmp.angle_radian += 60. * DEGRE_EN_RADIAN ;
position_but = Geometrie_deplace(position_tmp, -DISTANCE_ECHANGE_POT);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_LEVE;
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(2, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(3, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
tempo_ms = 500;
}
break;
case EPAA_LEVE:
tempo_ms--;
if(tempo_ms <= 0){
etat_echange_pot = EPAA_LEVE;
}
break;
case EPAA_RETOURNE:
if(Strategie_tourner_a(position_initiale.angle_radian - M_PI, step_ms) == ACTION_TERMINEE){
etat_echange_pot = EPAA_DEPOSE_POT;
return ACTION_TERMINEE;
}
break;
default:
break;
}
return ACTION_EN_COURS;
}
/// @brief dépose 1 ou plusieurs pot au pied du robot.
/// @param masque_pot: choisi les pots à déposer, 0x01 pour le bras 1, 0x02 pour le bras 2, 0x03 pour les bras 1 & 2
enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
static int tempo_ms;
uint8_t masque=1;
static enum {
DP_BAISSE_BRAS,
DP_BAISSE_BRAS_TEMPO,
DP_RANGE_DOIGT,
} etat_depose_pot = DP_BAISSE_BRAS;
switch (etat_depose_pot)
{
case 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_POT_SOL, DOIGT_TIENT);
}
}
tempo_ms=350;
etat_depose_pot = DP_BAISSE_BRAS_TEMPO;
break;
case DP_BAISSE_BRAS_TEMPO:
tempo_ms--;
if(tempo_ms < 0){
for (int i=0; i< NB_BRAS; i++){
masque =1;
masque = masque << i;
if(masque_pot & masque){
i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_LACHE);
}
}
tempo_ms=250;
etat_depose_pot = DP_RANGE_DOIGT;
}
break;
case DP_RANGE_DOIGT:
tempo_ms--;
if(tempo_ms < 0){
etat_depose_pot = DP_BAISSE_BRAS;
return ACTION_TERMINEE;
}
break;
default:
break;
}
return ACTION_EN_COURS;
}

3
Strategie_2024_pots.h
View File

@ -24,5 +24,8 @@
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 200.
#define DISTANCE_ECHANGE_POT 50.
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);
enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms);

104
Test_strategie_2024.c
View File

@ -29,6 +29,7 @@ int test_attrape_plante(void);
int test_aller_zone_plante();
int test_pseudo_homologation(void);
int test_attrape_1_pot(void);
int test_echange_pot(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
@ -39,7 +40,8 @@ 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");
printf("H - reglage pots.\n");
printf("I - echange pots.\n");
int lettre;
do{
@ -86,6 +88,12 @@ int test_strategie_2024(){
while(test_attrape_1_pot());
break;
case 'i':
case 'I':
while(test_echange_pot());
break;
case 'q':
case 'Q':
return 0;
@ -476,7 +484,7 @@ int test_pseudo_homologation(){
TAP_DEPOSE
} etat_test = TAP_CALAGE;
printf("test_attrape_pot\n");
printf("test_pseudo_homologation\n");
i2c_maitre_init();
@ -597,7 +605,7 @@ int test_pseudo_homologation(){
}
extern float abscisse;
void affichage_test_strategie_2024(){
uint32_t temps;
enum validite_vl53l8_t validite_vl53l8;
@ -615,6 +623,8 @@ void affichage_test_strategie_2024(){
printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
printf(">c_pos_angle:%ld:%f\n", temps, (Trajet_get_consigne().angle_radian+ ANGLE_PINCE) / DEGRE_EN_RADIAN);
printf(">abscisse:%ld:%f\n", temps, abscisse);
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
@ -634,7 +644,7 @@ void affichage_test_strategie_2024(){
break;
}
sleep_ms(100);
sleep_ms(10);
}
}
@ -779,3 +789,89 @@ int test_attrape_1_pot(void){
return 0;
}
int test_echange_pot(){
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;
static int tempo_ms;
static enum {
TEP_INIT,
TEP_ACTION,
} etat_echange_pot = TEP_INIT;
printf("test_echange_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(1);
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);
}
}
switch(etat_echange_pot){
case TEP_INIT:
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(1, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(4, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms=3000;
etat_echange_pot = TEP_ACTION;
break;
case TEP_ACTION:
tempo_ms--;
if (tempo_ms <= 0)
{
etat_action = Strat_2024_echange_pot_avant_arriere(_step_ms);
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}

6
i2c_annexe.c
View File

@ -205,6 +205,12 @@ void i2c_annexe_ferme_doigt_plante(void){
donnees_a_envoyer_pic=1;
}
void i2c_annexe_mi_ferme_doigt_plante(void){
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] =
(donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x30;
donnees_a_envoyer_pic=1;
}
void i2c_annexe_init(void){
for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){
i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE);

1
i2c_annexe.h
View File

@ -49,3 +49,4 @@ void i2c_annexe_init(void);
void i2c_annexe_ferme_doigt_plante(void);
void i2c_annexe_ouvre_doigt_plante(void);
void i2c_annexe_mi_ferme_doigt_plante(void);