Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Travail sur l'homologation - fonctions d'émission i2c non fonctionnelles

Browse Source
This commit is contained in:
Samuel 2023-03-28 23:03:35 +02:00
parent d1a249e0d5
commit 7c9dd0bab3
5 changed files with 230 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Holonome2023.c
View File

@ -16,6 +16,7 @@
#include "QEI.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Strategie.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
@ -51,6 +52,12 @@ int main() {
gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
gpio_put(LED_PIN_ROUGE, 0);
gpio_init(COULEUR);
gpio_init(TIRETTE);
gpio_set_dir(COULEUR, GPIO_IN);
gpio_set_dir(TIRETTE, GPIO_IN);
// Il faut neutraliser cette broche qui pourrait interférer avec
// la lecture des codeurs. (problème sur la carte électrique)...
gpio_init(LED_PIN_NE_PAS_UTILISER);

167
Strategie.c
View File

@ -1,23 +1,25 @@
#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
#include "math.h"
#define DEGREE_EN_RADIAN (M_PI / 180.)
#define SEUIL_RECAL_DIST_MM 75
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);
enum etat_strategie_t etat_strategie=STRATEGIE_INIT;
void Homologation(uint32_t step_ms){
static enum etat_strategie_t{
STRATEGIE_INIT,
APPROCHE_CERISE_1_A,
APPROCHE_CERISE_1_B,
ATTRAPE_CERISE_1,
APPROCHE_PANIER_1_A,
APPROCHE_PANIER_1_B,
CALAGE_PANIER_1,
STRATEGIE_FIN
}etat_strategie=STRATEGIE_INIT;
enum etat_action_t etat_action;
enum etat_trajet_t etat_trajet;
@ -47,30 +49,157 @@ void Homologation(uint32_t step_ms){
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_1_A;
etat_strategie = APPROCHE_PANIER_1;
}
break;
case APPROCHE_PANIER_1_A:
case APPROCHE_PANIER_1:
Trajet_config(500, 500);
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
Trajet_debut_trajectoire(trajectoire);
etat_strategie = APPROCHE_PANIER_1_B;
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
}
break;
case APPROCHE_PANIER_1_B:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_strategie = ATTRAPE_CERISE_1;
case CALAGE_PANIER_1:
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
etat_strategie = RECULE_PANIER;
}
break;
case RECULE_PANIER:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
225, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 120,
120. * DEGREE_EN_RADIAN, +240. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCE_DANS_PANIER;
}
break;
case LANCE_DANS_PANIER:
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
}
break;
case STRATEGIE_FIN:
Moteur_Stop();
break;
}
}
/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
/// @param step_ms : pas de temps.
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t lance_balles(uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
uint32_t tempo_ms;
static enum{
LANCE_PROPULSEUR_ON,
LANCE_TEMPO_PROP_ON,
LANCE_PORTE_OUVERTE,
} etat_lance_balle = LANCE_PROPULSEUR_ON;
switch(etat_lance_balle){
case LANCE_PROPULSEUR_ON:
i2c_annexe_active_propulseur();
tempo_ms = 2000;
etat_lance_balle = LANCE_TEMPO_PROP_ON;
break;
case LANCE_TEMPO_PROP_ON:
if (tempo_ms < step_ms){
etat_lance_balle = LANCE_PORTE_OUVERTE;
i2c_annexe_ouvre_porte();
tempo_ms = 6000;
}else{
tempo_ms -= step_ms;
}
break;
case LANCE_PORTE_OUVERTE:
if (tempo_ms < step_ms){
etat_lance_balle = LANCE_PROPULSEUR_ON;
i2c_annexe_desactive_propulseur();
etat_action = ACTION_TERMINEE;
}
break;
}
return etat_action;
}
/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian){
enum etat_action_t etat_action = ACTION_EN_COURS;
struct position_t position;
avance_puis_longe_bordure(longer_direction);
if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) ||
((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){
etat_action = ACTION_TERMINEE;
position = Localisation_get();
if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_x(x_mm);
}
if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
Localisation_set_y(y_mm);
}
if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
Localisation_set_angle(angle_radian);
}
}
return etat_action;
}
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
static enum {
PARCOURS_INIT,
PARCOURS_AVANCE,
} etat_parcourt=PARCOURS_INIT;
switch (etat_parcourt){
case PARCOURS_INIT:
Trajet_debut_trajectoire(trajectoire);
etat_parcourt = PARCOURS_AVANCE;
break;
case PARCOURS_AVANCE:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
break;
}
return etat_action;
}
/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
uint attente_tirette(void){
return gpio_get(TIRETTE);
}
/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
enum couleur_t lire_couleur(void){
if (gpio_get(COULEUR))
return COULEUR_VERT;
return COULEUR_BLEU;
}

26
Strategie.h
View File

@ -3,6 +3,9 @@
#ifndef STRATEGIE_H
#define STRATEGIE_H
#define COULEUR 15
#define TIRETTE 14
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE
@ -13,9 +16,30 @@ enum longer_direction_t{
LONGER_VERS_C
};
enum couleur_t{
COULEUR_BLEU=0,
COULEUR_VERT
};
extern enum etat_strategie_t{
STRATEGIE_INIT=0,
APPROCHE_CERISE_1_A=1,
APPROCHE_CERISE_1_B=2,
ATTRAPE_CERISE_1=3,
APPROCHE_PANIER_1=4,
CALAGE_PANIER_1=5,
RECULE_PANIER=6,
LANCE_DANS_PANIER=7,
STRATEGIE_FIN=254,
}etat_strategie;
enum etat_action_t cerise_accostage(void);
enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);
// STRATEGIE_H
#endif

7
Strategie_prise_cerises.c
View File

@ -49,7 +49,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
break;
case ATTRAPE_VERS_BORDURE:
cerise_longer_bord(longer_direction);
avance_puis_longe_bordure(longer_direction);
if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
(longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
etat_attrape = TURBINE_DEMARRAGE;
@ -59,6 +59,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
case TURBINE_DEMARRAGE:
i2c_annexe_ferme_porte();
//i2c_annexe_active_turbine();
i2c_annexe_active_propulseur();
commande_vitesse_stop();
tempo_ms = 2000;
etat_attrape = TURBINE_DEMARRAGE_TEMPO;
@ -76,7 +77,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
case ASPIRE_LONGE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
cerise_longer_bord(LONGER_VERS_A);
avance_puis_longe_bordure(LONGER_VERS_A);
// La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
// Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition
@ -99,7 +100,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
/// @brief Fonction pour accoster et longer une bordure
/// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure
/// @return ACTION_EN_COURS
enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction){
static enum {
LONGE_INIT,

48
Test_strategie.c
View File

@ -19,6 +19,7 @@
int test_accostage(void);
int test_longe(void);
int test_homologation(void);
int test_tirette_et_couleur();
void affichage_test_strategie(){
uint32_t temps;
@ -38,6 +39,20 @@ void affichage_test_strategie(){
printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
printf(">c_pos_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian);
printf(">etat_strat:%d\n",etat_strategie);
/*switch(etat_strategie){
case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
case APPROCHE_CERISE_1_A: printf(">etat_strat:APPROCHE_CERISE_1_A|t\n");break;
case APPROCHE_CERISE_1_B: printf(">etat_strat:APPROCHE_CERISE_1_B|t\n");break;
case ATTRAPE_CERISE_1: printf(">etat_strat:ATTRAPE_CERISE_1|t\n");break;
case APPROCHE_PANIER_1: printf(">etat_strat:APPROCHE_PANIER_1|t\n");break;
case CALAGE_PANIER_1: printf(">etat_strat:CALAGE_PANIER_1|t\n");break;
case RECULE_PANIER: printf(">etat_strat:RECULE_PANIER|t\n");break;
case LANCE_DANS_PANIER: printf(">etat_strat:LANCE_DANS_PANIER|t\n");break;
case STRATEGIE_FIN: printf(">etat_strat:STRATEGIE_FIN|t\n");break;
}*/
sleep_ms(100);
}
}
@ -47,6 +62,7 @@ int test_strategie(){
printf("L - longer.\n");
printf("A - Accoster.\n");
printf("H - Homologation.\n");
printf("C - Couleur et tirette.\n");
int lettre;
do{
lettre = getchar_timeout_us(0);
@ -57,6 +73,11 @@ int test_strategie(){
while(test_accostage());
break;
case 'c':
case 'C':
while(test_tirette_et_couleur());
break;
case 'h':
case 'H':
while(test_homologation());
@ -67,8 +88,6 @@ int test_strategie(){
while(test_longe());
break;
case 'q':
case 'Q':
return 0;
@ -158,7 +177,7 @@ int test_longe(){
}
if(temps_ms > temps_ms_init + 200){
if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
if(avance_puis_longe_bordure(LONGER_VERS_A) == ACTION_TERMINEE){
printf("Accostage_terminee\n");
}
}
@ -221,3 +240,26 @@ int test_accostage(){
}
return 1;
}
int test_tirette_et_couleur(){
int lettre;
uint couleur, tirette;
printf("Tirette et couleur\n");
stdio_flush();
do{
printf(">tirette:%d\n", attente_tirette());
if(lire_couleur() == COULEUR_VERT){
printf(">couleur:Vert|t\n");
}else{
printf(">couleur:Bleu|t\n");
}
sleep_ms(10);
lettre = getchar_timeout_us(0);
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}