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

Le robot part de la zone de départ, prend les pots, attrape une plante et la met dans un pot

Browse Source
This commit is contained in:
Samuel 2024-04-25 20:10:03 +02:00
parent d539c6846c
commit 89e057a285
5 changed files with 195 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Strategie_2024_plante.c
View File

@ -21,15 +21,14 @@ struct position_t liste_zone_plante[]=
enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){
int zone_plante = 2;
enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms){
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);
angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
}
enum etat_action_t Strat_2024_aller_a_plante(void){
@ -115,6 +114,11 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
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){
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);

12
Strategie_2024_plante.h
View File

@ -1,4 +1,14 @@
#include "Strategie.h"
enum zone_plante_t{
ZONE_PLANTE_1=0,
ZONE_PLANTE_2=1,
ZONE_PLANTE_3=2,
ZONE_PLANTE_4=3,
ZONE_PLANTE_5=4,
ZONE_PLANTE_6=5,
};
enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms);
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);
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot);

2
Strategie_2024_pots.c
View File

@ -7,7 +7,7 @@
#include "Localisation.h"
#define DISTANCE_APPROCHE_POT_MM 300.
#define DISTANCE_ATTRAPE_POT_MM 180.
#define DISTANCE_ATTRAPE_POT_MM 185.
float angle_bras[6] =
{

174
Test_strategie_2024.c
View File

@ -25,6 +25,8 @@ int test_calage_debut(void);
int test_attrape_pot(void);
int test_aller_a_plante(void);
int test_attrape_plante(void);
int test_aller_zone_plante();
int test_pseudo_homologation(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
@ -33,6 +35,8 @@ int test_strategie_2024(){
printf("C - Attrape pot.\n");
printf("D - Aller a plante.\n");
printf("E - Attrape plante.\n");
printf("F - Aller zone plante.\n");
printf("G - Pseudo homologation.\n");
int lettre;
do{
@ -64,6 +68,16 @@ int test_strategie_2024(){
while(test_attrape_plante());
break;
case 'f':
case 'F':
while(test_aller_zone_plante());
break;
case 'g':
case 'G':
while(test_pseudo_homologation());
break;
case 'q':
case 'Q':
return 0;
@ -293,6 +307,70 @@ int test_aller_a_plante(){
}
int test_aller_zone_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();
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);
Localisation_set(800, 200, 0);
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_aller_zone_plante(ZONE_PLANTE_3, _step_ms);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_attrape_pot(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
@ -373,6 +451,102 @@ int test_attrape_pot(){
}
int test_pseudo_homologation(){
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;
enum {
TAP_CALAGE,
TAP_POT,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE
} etat_test = TAP_CALAGE;
printf("test_attrape_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(TRAJECT_CONFIG_STD);
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_test){
case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
if(Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ORIENTATION;
}
break;
case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE;
}
break;
case TAP_PLANTE_ATTRAPE:
if(Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1) == ACTION_TERMINEE){
etat_action= ACTION_TERMINEE;
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
void affichage_test_strategie_2024(){
uint32_t temps;

2
i2c_annexe.c
View File

@ -173,7 +173,7 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
}
/// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs
/// @param actionneur de 1 à 6, pour le "bras" correspondant".
/// @param actionneur de 0 à 5, pour le "bras" correspondant".
/// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h
/// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){