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

Ajout des corrections en distance par pot + renommage des variables d'évitement

Browse Source
This commit is contained in:
Samuel 2024-05-04 11:22:39 +02:00
parent b834b0d677
commit e79b7614f5
12 changed files with 97 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Demonstration.c
View File

@ -210,7 +210,7 @@ enum etat_action_t Demonstration_calage(){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_calage = CALAGE_TERMINE;
}
break;
@ -260,7 +260,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = AVANCE_X;
}
break;
@ -269,7 +269,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECULE_Y;
}
break;
@ -278,7 +278,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECULE_X;
}
break;
@ -287,7 +287,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECTANGLE_TERMINE;
}
break;
@ -338,7 +338,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm,
pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_TOURNE;
}
break;
@ -347,7 +347,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_RECULE;
Trajet_config(TRAJECT_CONFIG_STD);
}
@ -357,7 +357,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_TERMINE;
}
break;
@ -404,7 +404,7 @@ enum etat_action_t Demonstration_bezier(){
-576, 1500 - 276,
545, 1500 - 276,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_bezier = BEZIER_RETOUR;
}
break;
@ -416,7 +416,7 @@ enum etat_action_t Demonstration_bezier(){
-76, 1500 - 788,
242, 1500 - 1289,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_bezier = BEZIER_TERMINE;
}
break;

6
Holonome2023.c
View File

@ -11,6 +11,9 @@ uint temps_cycle;
#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000
// A supprimer
int test_pseudo_homologation(void);
int mode_test();
@ -41,7 +44,8 @@ int main() {
stdio_init_all();
//Demonstration_init();Demonstration_auto();
while(mode_test());
//while(mode_test());
test_pseudo_homologation();
Holonome2023_init();
multicore_launch_core1(Monitoring_display);

2
Monitoring.c
View File

@ -1,4 +1,5 @@
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include <stdio.h>
#include "Monitoring.h"
#include "Localisation.h"
@ -92,6 +93,7 @@ void set_debug_varf(float variable){
void Monitoring_Error(char * msg){
gpio_put(LED_PIN_ROUGE, 1);
Log_message(msg, ERROR);
multicore_reset_core1();
while(1){
sleep_ms(500);
printf(msg);

14
Strategie.c
View File

@ -276,7 +276,7 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms)
pos_x, pos_y,
Localisation_get().angle_radian, Localisation_get().angle_radian);
return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE);
return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
}
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
@ -318,7 +318,7 @@ enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, floa
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_aller_a_puis_tourner = AAPT_ALLER;
return ACTION_TERMINEE;
}
@ -337,7 +337,7 @@ enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){
Localisation_get().angle_radian,
Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT);
return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
}
@ -503,7 +503,7 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
(0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
}
if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
etat_calage_debut = CD_LECTURE_BORDURE_X;
}
@ -529,9 +529,9 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
case CD_ALLER_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(couleur == COULEUR_BLEU){
etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms);
etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
}else{
etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms);
etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
}
if(etat_action == ACTION_TERMINEE){
etat_calage_debut = CD_ROTATION_POSITION_INIT;
@ -543,7 +543,7 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
(45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
0);
return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT);
return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
//etat_calage_debut = CD_ENVOI_CDE_BORDURE;
}

10
Strategie.h
View File

@ -31,11 +31,11 @@ enum couleur_t{
};
enum evitement_t{
SANS_EVITEMENT,
PAUSE_DEVANT_OBSTACLE,
ARRET_DEVANT_OBSTACLE,
RETOUR_SI_OBSTABLE,
CONTOURNEMENT
EVITEMENT_SANS_EVITEMENT,
EVITEMENT_PAUSE_DEVANT_OBSTACLE,
EVITEMENT_ARRET_DEVANT_OBSTACLE,
EVITEMENT_RETOUR_SI_OBSTABLE,
EVITEMENT_CONTOURNEMENT
};
struct objectif_t{

15
Strategie_2024_plante.c
View File

@ -145,6 +145,8 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
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_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
}
@ -159,21 +161,22 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
case PDP_PRE_PRISE_TEMPO:
tempo_ms--;
if(tempo_ms <= 0){
tempo_ms=500;
etat_plante_dans_pot=PDP_PRE_PRISE_RECULE;
}
break;
case PDP_PRE_PRISE_RECULE:
tempo_ms--;
if(tempo_ms <= 0){
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
}
position_initiale = Localisation_get();
position_initiale.angle_radian += ANGLE_PINCE;
position_recule = Geometrie_deplace(position_initiale, -50);
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(bras_depose_pot == PLANTE_BRAS_1){
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
}else{
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
}
commande_vitesse_stop();
i2c_annexe_ouvre_doigt_plante();
i2c_annexe_attrape_plante(bras_depose_pot);

27
Strategie_2024_pots.c
View File

@ -28,6 +28,16 @@ float angle_bras_correction[6] =
7 * DEGRE_EN_RADIAN
};
float distance_bras_correction_mm[6] =
{
0,
0,
-10,
-15,
0,
0
};
enum etat_bras_t{
BRAS_LIBRE,
BRAS_OCCUPE,
@ -131,19 +141,19 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
switch (etat_attrape_pot)
{
case AP_ALLER_VERS_GROUPE_POT:
bras = get_bras_libre();
position_pot = groupe_pot_get_pot(groupe_pot, POT_1);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0],
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
SANS_EVITEMENT, step_ms);
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_RECALE;
bras = get_bras_libre();
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
}
break;
@ -161,7 +171,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}*/
etat_attrape_pot = AP_ORIENTE;
}else{
printf("Erreur - recalage trop loin\n");
//printf("Erreur - recalage trop loin\n");
}
}
break;
@ -181,9 +191,10 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_aller_a_puis_tourner(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
@ -193,7 +204,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
etat_action = Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
tempo_ms=250;
etat_attrape_pot = AP_RETOUR_ET_LEVE_POT;
@ -209,7 +220,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}
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);
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_APPROCHE_POT;
pot = get_pot_suivant(pot);
@ -232,7 +243,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
}
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);
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;

14
Strategie_deplacement.c
View File

@ -22,7 +22,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
break;
case PARCOURS_AVANCE:
if(evitement != SANS_EVITEMENT){
if(evitement != EVITEMENT_SANS_EVITEMENT){
angle_avancement = Trajet_get_orientation_avance();
distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
Trajet_set_obstacle_mm(distance_obstacle);
@ -30,23 +30,23 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
switch(evitement){
case SANS_EVITEMENT:
//printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n");
case EVITEMENT_SANS_EVITEMENT:
//printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
break;
case PAUSE_DEVANT_OBSTACLE:
case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
// Rien à faire ici
break;
case ARRET_DEVANT_OBSTACLE:
case EVITEMENT_ARRET_DEVANT_OBSTACLE:
etat_parcourt = PARCOURS_INIT;
return ACTION_ECHEC;
case RETOUR_SI_OBSTABLE:
case EVITEMENT_RETOUR_SI_OBSTABLE:
trajet_inverse = !trajet_inverse;
Trajet_inverse();
break;
case CONTOURNEMENT: // TODO
case EVITEMENT_CONTOURNEMENT: // TODO
break;
}

10
Test_i2c.c
View File

@ -304,6 +304,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("J - Detection distance loin\n");
printf("K - Ouvre doigt plante\n");
printf("L - Ferme doigt plante\n");
printf("M - ouvre doigt + attrape plante\n");
printf("L - Pots avant levite\n");
printf("S - Score + 1\n");
printf("\nQ - Quitter\n");
@ -415,6 +417,14 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("=> ouvre doigt plante + attrape plante\n");
break;
case 'n':
case 'N':
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
printf("=> Levite pots avant\n");
break;
case 'q':

10
Test_strategie.c
View File

@ -126,15 +126,15 @@ int test_evitement(){
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'a':
case 'A':evitement=SANS_EVITEMENT;break;
case 'A':evitement=EVITEMENT_SANS_EVITEMENT;break;
case 'b':
case 'B':evitement=PAUSE_DEVANT_OBSTACLE;break;
case 'B':evitement=EVITEMENT_PAUSE_DEVANT_OBSTACLE;break;
case 'c':
case 'C':evitement=ARRET_DEVANT_OBSTACLE;break;
case 'C':evitement=EVITEMENT_ARRET_DEVANT_OBSTACLE;break;
case 'd':
case 'D':evitement=RETOUR_SI_OBSTABLE;break;
case 'D':evitement=EVITEMENT_RETOUR_SI_OBSTABLE;break;
case 'e':
case 'E':evitement=CONTOURNEMENT;break;
case 'E':evitement=EVITEMENT_CONTOURNEMENT;break;
}
i2c_maitre_init();

22
Test_strategie_2024.c
View File

@ -3,6 +3,7 @@
#include "pico/error.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "Commande_vitesse.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Evitement.h"
@ -463,6 +464,7 @@ int test_pseudo_homologation(){
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
enum {
TAP_CALAGE,
@ -518,7 +520,7 @@ int test_pseudo_homologation(){
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
@ -557,14 +559,24 @@ int test_pseudo_homologation(){
break;
case TAP_RENTRE:
if(Strategie_aller_a(250, 250, _step_ms) == ACTION_TERMINEE){
float angle_destination;
angle_destination = 15 * DEGRE_EN_RADIAN;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms=500;
}
break;
case TAP_DEPOSE:
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
tempo_ms--;
commande_vitesse_stop();
if(tempo_ms<= 0){
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
}
break;
}
@ -737,7 +749,7 @@ int test_attrape_1_pot(void){
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
if( Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras],
SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
EVITEMENT_SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
tempo_ms = 2000;
etat_test = TAP_ATTRAPE_TEMPO;

1
i2c_annexe.h
View File

@ -12,6 +12,7 @@
#define BRAS_ECARTE 3
#define BRAS_DEPOSE_JARDINIERE 4
#define BRAS_HAUT 5
#define BRAS_LEVITE 6
#define NB_BRAS 6