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

Premier pot attrapé

Browse Source
This commit is contained in:
Samuel 2024-04-07 22:26:36 +02:00
parent ee589f3f73
commit c5e76ba440
8 changed files with 174 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Geometrie.c
View File

@ -75,8 +75,8 @@ unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max
struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){
struct position_t position_arrivée;
position_arrivée.angle_radian = position_depart.angle_radian;
position_arrivée.x_mm = cosf(position_depart.angle_radian) * distance_mm;
position_arrivée.y_mm = sinf(position_depart.angle_radian) * distance_mm;
position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm;
position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm;
return position_arrivée;
}

1
Geometrie.h
View File

@ -17,5 +17,6 @@ float Geometrie_get_angle_normalisee(float angle);
unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max);
unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max);
float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite);
struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm);
#endif

1
Strategie.h
View File

@ -67,6 +67,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms);
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);
extern float distance_obstacle;

85
Strategie_2024_pots.c
View File

@ -2,8 +2,10 @@
#include "Strategie.h"
#include "Geometrie_robot.h"
#include "Strategie_2024_pots.h"
#include "i2c_annexe.h"
#define DISTANCE_APPROCHE_POT
#define DISTANCE_APPROCHE_POT_MM 250.
#define DISTANCE_ATTRAPE_POT_MM 100.
float angle_bras[6] =
{
@ -17,8 +19,8 @@ float angle_bras[6] =
struct position_t position_pots_dans_groupe_pot[5] =
{
{.x_mm = -40, .y_mm = 69.2, .angle_radian = 120 * DEGRE_EN_RADIAN},
{.x_mm = 40, .y_mm = 69.2, .angle_radian = 60 * DEGRE_EN_RADIAN},
{.x_mm = -40, .y_mm = 69.2, .angle_radian = -60 * DEGRE_EN_RADIAN},
{.x_mm = 40, .y_mm = 69.2, .angle_radian = -120 * DEGRE_EN_RADIAN},
{.x_mm = -80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 0, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN}
@ -53,14 +55,35 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p
sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm +
cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm;
position_pot.angle_radian = my_position_groupe_pot.angle_radian + angle_groupe_pot;
position_pot.angle_radian = position_pots_dans_groupe_pot[num_pot].angle_radian + angle_groupe_pot;
return position_pot;
}
int get_bras_libre(void){
return BRAS_1;
}
/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms){
// Parcourir la trajectoire pour aller jusqu'au premier pot
struct position_t position_pot, position_approche_pot, position_attrape_pot;
enum etat_action_t etat_action;
static int bras, tempo_ms;
// Pour le 1er pot
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);
static enum {
AP_ALLER_VERS_GROUPE_POT,
AP_APPROCHE_POT,
AP_ATTRAPE_POT,
AP_RETOUR_ET_LEVE_POT
} etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
// Pour chaque pot
// Baisser le bras correspondant
@ -69,4 +92,56 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){
// Reculer dans l'axe de prise et rejoindre le point de prise suivant
// Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras
switch (etat_attrape_pot)
{
case AP_ALLER_VERS_GROUPE_POT:
printf("position_pot X:%f Y:%f r:%f\n", position_pot.x_mm, position_pot.y_mm,
position_pot.angle_radian / DEGRE_EN_RADIAN);
printf("position_approche_pot X:%f Y:%f r:%f\n", position_approche_pot.x_mm, position_approche_pot.y_mm,
position_approche_pot.angle_radian / DEGRE_EN_RADIAN);
etat_attrape_pot = AP_APPROCHE_POT;
case AP_APPROCHE_POT:
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);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ATTRAPE_POT;
bras = get_bras_libre();
}
break;
case AP_ATTRAPE_POT:
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);
if (etat_action == ACTION_TERMINEE){
tempo_ms=250;
etat_attrape_pot = AP_RETOUR_ET_LEVE_POT;
}
break;
case AP_RETOUR_ET_LEVE_POT:
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;
// TODO: pot suivant
return ACTION_TERMINEE;
}
break;
default:
break;
}
return ACTION_EN_COURS;
}

1
Strategie_2024_pots.h
View File

@ -21,3 +21,4 @@
#define BRAS_6 5
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);

10
Test_i2c.c
View File

@ -327,7 +327,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
switch(lettre){
case 'a':
case 'A':
for(int i=1; i<7; i++){
for(int i=0; i<6; i++){
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
}
printf("=> Lache pot\n");
@ -335,7 +335,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
case 'b':
case 'B':
for(int i=1; i<7; i++){
for(int i=0; i<6; i++){
i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT);
}
printf("=> Attrape pot\n");
@ -343,7 +343,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
case 'c':
case 'C':
for(int i=1; i<7; i++){
for(int i=0; i<6; i++){
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT);
}
printf("=> Tient pot\n");
@ -351,10 +351,10 @@ int test_i2c_ecriture_pico_annex_nb_2(){
case 'D':
case 'd':
for(int i=1; i<7; i++){
for(int i=0; i<6; i++){
i2c_annexe_actionneur_pot(i, BRAS_HAUT, DOIGT_TIENT);
}
printf("=> Attrape pot\n");
printf("=>Pot haut\n");
break;
case 'E':

82
Test_strategie_2024.c
View File

@ -21,10 +21,13 @@
int test_calcul_position_pot(void);
int test_calage_debut(void);
int test_attrape_pot();
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
printf("A - Position groupes pot.\n");
printf("B - Calage debut.\n");
printf("C - Attrape pot.\n");
int lettre;
do{
@ -41,6 +44,15 @@ int test_strategie_2024(){
while(test_calage_debut());
break;
case 'c':
case 'C':
while(test_attrape_pot());
break;
case 'q':
case 'Q':
return 0;
}
}
@ -99,7 +111,7 @@ int test_calage_debut(){
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(200,200,0);
Localisation_set(250,250,0);
set_position_avec_gyroscope(0);
@ -154,6 +166,74 @@ int test_calage_debut(){
}
int test_attrape_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;
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(0);
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);
}
}
etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);
}
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;

6
i2c_annexe.c
View File

@ -164,11 +164,11 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
/// @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){
if(actionneur < 1 || actionneur > 6){
if(actionneur < 0 || actionneur > 5){
printf("Erreur: i2c_annexe_actionneur_pot\n" );
return;
}
donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt;
donnees_emission_pic18f4550[actionneur] = (pos_bras << 2) | pos_doigt;
donnees_a_envoyer_pic=1;
}
@ -178,7 +178,7 @@ void i2c_annexe_attrape_plante(uint8_t action){
}
void i2c_annexe_init(void){
for(unsigned int actionneur=1; actionneur<= NB_BRAS; actionneur++){
for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){
i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE);
}
}