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

Gestion du doigt pour attrapper la plante

Browse Source
This commit is contained in:
Samuel 2024-04-24 22:11:56 +02:00
parent 6acb86c729
commit d539c6846c
6 changed files with 118 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Strategie_2024_plante.c
View File

@ -9,6 +9,29 @@
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
struct position_t liste_zone_plante[]=
{
{.x_mm = 1500, .y_mm = 1500 },
{.x_mm = 1000, .y_mm = 1300 },
{.x_mm = 1000, .y_mm = 700 },
{.x_mm = 1500, .y_mm = 500 },
{.x_mm = 2000, .y_mm = 700 },
{.x_mm = 2000, .y_mm = 1300 }
};
enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){
int zone_plante = 2;
struct position_t position_robot, position_zone_plante;
float angle;
position_zone_plante = liste_zone_plante[zone_plante];
position_robot = Localisation_get();
angle = atan2f(position_zone_plante.x_mm - position_robot.x_mm, position_zone_plante.y_mm - position_robot.y_mm);
return Strategie_tourner_a(angle, step_ms);
}
enum etat_action_t Strat_2024_aller_a_plante(void){
static enum {
SAAP_INIT_DETECTION,
@ -55,6 +78,9 @@ enum etat_action_t Strat_2024_aller_a_plante(void){
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
static enum{
PDP_ALLER_PLANTE,
PDP_PRE_PRISE_PLANTE,
PDP_PRE_PRISE_TEMPO,
PDP_PRE_PRISE_RECULE,
PDP_ATTRAPE_PLANTE,
PDP_RECULE,
PDP_TEMPO,
@ -66,13 +92,38 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
{
case PDP_ALLER_PLANTE:
if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
}
break;
case PDP_PRE_PRISE_PLANTE:
i2c_annexe_ferme_doigt_plante();
tempo_ms = 250;
etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO;
break;
case PDP_PRE_PRISE_TEMPO:
tempo_ms--;
if(tempo_ms <= 0){
etat_plante_dans_pot=PDP_PRE_PRISE_RECULE;
}
break;
case PDP_PRE_PRISE_RECULE:
position_initiale = Localisation_get();
position_initiale.angle_radian += ANGLE_PINCE;
position_recule = Geometrie_deplace(position_initiale, -50);
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();
i2c_annexe_ouvre_doigt_plante();
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){

25
Strategie_2024_pots.c
View File

@ -107,7 +107,8 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
AP_ORIENTE,
AP_APPROCHE_POT,
AP_ATTRAPE_POT,
AP_RETOUR_ET_LEVE_POT
AP_RETOUR_ET_LEVE_POT,
AP_FINALISE
} etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
// Pour chaque pot
@ -170,7 +171,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm);
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
@ -201,16 +201,31 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
etat_attrape_pot = AP_APPROCHE_POT;
pot = get_pot_suivant(pot);
if(pot > 4){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;
tempo_ms=250;
etat_attrape_pot = AP_FINALISE;
break;
}
position_pot = groupe_pot_get_pot(groupe_pot, pot);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm);
}
break;
case AP_FINALISE:
if(tempo_ms >= 0){
tempo_ms -= step_ms;
}else{
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
}
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;
}
break;
default:
break;

25
Test_i2c.c
View File

@ -302,6 +302,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("H - Detection plante\n");
printf("I - Detection stop\n");
printf("J - Detection distance loin\n");
printf("K - Ouvre doigt plante\n");
printf("L - Ferme doigt plante\n");
printf("S - Score + 1\n");
printf("\nQ - Quitter\n");
@ -393,6 +395,28 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("=> Detection distance loin\n");
break;
case 'k':
case 'K':
i2c_annexe_ouvre_doigt_plante();
printf("=> Ouvre doigt plante\n");
break;
case 'l':
case 'L':
i2c_annexe_ferme_doigt_plante();
printf("=> ferme doigt plante\n");
break;
case 'm':
case 'M':
i2c_annexe_attrape_plante(PLANTE_BRAS_1);
i2c_annexe_ouvre_doigt_plante();
printf("=> ouvre doigt plante + attrape plante\n");
break;
case 'q':
case 'Q':
multicore_reset_core1();
@ -408,6 +432,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("lettre non reconnue: %d %c\n", lettre, lettre);
}
}
//sleep_ms(1);
i2c_gestion(i2c0);
i2c_annexe_gestion();

2
Test_strategie_2024.c
View File

@ -221,7 +221,7 @@ int test_attrape_plante(){
}
}
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1);
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6);
}

17
i2c_annexe.c
View File

@ -89,7 +89,7 @@ void i2c_annexe_gestion(){
retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W,
donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION);
if(retour_i2c == I2C_SUCCES){
etat_i2c_annexe = EMISSION_DONNEES_RPi;
etat_i2c_annexe = EMISSION_TEMPO_PIC18F;
temps = time_us_32();
donnees_a_envoyer_pic=0;
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0;
@ -188,7 +188,20 @@ void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doi
/// @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_emission_pic18f4550[ADRESSE_ACTION_PINCE] =
(donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0xF0) | action;
donnees_a_envoyer_pic=1;
}
void i2c_annexe_ouvre_doigt_plante(void){
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] =
(donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x10;
donnees_a_envoyer_pic=1;
}
void i2c_annexe_ferme_doigt_plante(void){
donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] =
(donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x20;
donnees_a_envoyer_pic=1;
}

7
i2c_annexe.h
View File

@ -15,8 +15,8 @@
#define NB_BRAS 6
#define PLANTE_BRAS_1 '1'
#define PLANTE_BRAS_6 '6'
#define PLANTE_BRAS_1 0x01
#define PLANTE_BRAS_6 0x02
enum validite_vl53l8_t{
VL53L8_INVALIDE=0,
@ -45,3 +45,6 @@ void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, floa
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt);
void i2c_annexe_attrape_plante(uint8_t action);
void i2c_annexe_init(void);
void i2c_annexe_ferme_doigt_plante(void);
void i2c_annexe_ouvre_doigt_plante(void);