Gestion du doigt pour attrapper la plante
This commit is contained in:
parent
6acb86c729
commit
d539c6846c
@ -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){
|
||||
|
@ -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
25
Test_i2c.c
@ -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();
|
||||
|
@ -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
17
i2c_annexe.c
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
Loading…
Reference in New Issue
Block a user