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

Creation de l'action pour récupérer les cerises latérales.

Browse Source
This commit is contained in:
Samuel 2023-05-08 20:29:49 +02:00
parent 13d153cbeb
commit 2537c5d371
6 changed files with 263 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Holonome2023.c
View File

@ -82,7 +82,7 @@ int main() {
AsserMoteur_Init();
Localisation_init();
//while(mode_test());
while(mode_test());
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();

32
Strategie.c
View File

@ -176,6 +176,38 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
etat_strategie = ALLER_PANIER;
}
break;
case ALLER_CERISE_GAUCHE:
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
740, 3000 - 550,
510, 3000 - 1580,
180, 3000 - (1500 - 45),
Localisation_get().angle_radian, angle_fin);
}else{
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
1225, 3000 - 540,
440, 3000 - 775,
225, 3000 - (1500 - 45),
Localisation_get().angle_radian, angle_fin);
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
break;
case ATTRAPER_CERISE_GAUCHE:
// 1 accoster
// 2 Longer en aspirant
// 3 avancer en aspirant
// 4 Revenir au milieu
// 5 accoster
// 6 longer en aspirant
// 7 avancer en aspirant
break;
case ALLER_PANIER:
if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){

134
Strategie_prise_cerises.c
View File

@ -20,12 +20,111 @@
void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction);
enum etat_action_t demarre_turbine(uint32_t step_ms);
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
float vitesse_accostage_mm_s=100;
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
static enum {
ATTRAPE_CERISE_DEMI_BAS,
REVENIR_CENTRE,
ATTRAPE_CERISE_DEMI_HAUT,
}etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
struct trajectoire_t trajectoire;
float angle_fin;
switch (etat_attrappe_cerises_gauche){
case ATTRAPE_CERISE_DEMI_BAS:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = REVENIR_CENTRE;
}
break;
case REVENIR_CENTRE:
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150 * DEGRE_EN_RADIAN);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_HAUT;
}
break;
case ATTRAPE_CERISE_DEMI_HAUT:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){
// 1 accoster
// Demarrer la turbine
// 2 Longer en aspirant
// 3 avancer en aspirant
static enum {
ACCOSTAGE,
DEMARRE_TURBINE,
LONGE,
AVANCE,
}etat_attrappe_demi_cerise=ACCOSTAGE;
switch(etat_attrappe_demi_cerise){
case ACCOSTAGE:
if(cerise_accostage() == ACTION_TERMINEE){
etat_attrappe_demi_cerise = DEMARRE_TURBINE;
}
break;
case DEMARRE_TURBINE:
if(demarre_turbine(step_ms) == ACTION_TERMINEE){
etat_attrappe_demi_cerise = LONGE;
}
break;
case LONGE:
avance_puis_longe_bordure(longer_direction);
// 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
// ou 120 (MAX_LONGE_MM/2) du milieu de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
etat_attrappe_demi_cerise = AVANCE;
}
break;
case AVANCE:
if(longer_direction == LONGER_VERS_A){
commande_translation_longer_vers_A();
}else{
commande_translation_longer_vers_C();
}
if( (Localisation_get().y_mm > 1500 + MAX_ASPIRE_CERISE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_ASPIRE_CERISE_MM/2 )){
etat_attrappe_demi_cerise = ACCOSTAGE;
i2c_annexe_desactive_turbine();
commande_vitesse_stop();
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
/// @param longer_direction : direction dans laquelle se trouve la bordure
@ -84,7 +183,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
avance_puis_longe_bordure(longer_direction_aspire);
// 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
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
etat_attrape = ASPIRE_LIBRE;
@ -114,6 +213,37 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
return etat_action;
}
/// @brief Envoie l'ordre de démarrer la turbine puis attends 1 seconde
/// @param step_ms
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t demarre_turbine(uint32_t step_ms){
static enum {
TURBINE_DEMARRAGE,
TURBINE_DEMARRAGE_TEMPO,
} etat_demarrage_turbine=TURBINE_DEMARRAGE;
static uint32_t tempo_ms;
switch(etat_demarrage_turbine){
case TURBINE_DEMARRAGE:
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
commande_vitesse_stop();
tempo_ms = 1000;
etat_demarrage_turbine = TURBINE_DEMARRAGE_TEMPO;
break;
case TURBINE_DEMARRAGE_TEMPO:
if(temporisation_terminee(&tempo_ms, step_ms)){
etat_demarrage_turbine = TURBINE_DEMARRAGE;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
/// @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
@ -159,7 +289,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
}
/// @brief Viens position le robot contre une bordure ou un support cerise devant lui.
/// @brief Viens positionner le robot contre une bordure ou un support cerise devant lui.
enum etat_action_t cerise_accostage(void){
enum etat_action_t etat_action = ACTION_EN_COURS;
float rotation;

1
Strategie_prise_cerises.h
View File

@ -1,5 +1,6 @@
#include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms);
void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C();
void commande_translation_avance_vers_trompe();

99
Test_strategie.c
View File

@ -13,6 +13,7 @@
#include "QEI.h"
#include "Robot_config.h"
#include "Strategie.h"
#include "Strategie_prise_cerises.h"
#include "Temps.h"
#include "Trajet.h"
#include "Trajectoire.h"
@ -24,6 +25,7 @@ int test_panier(void);
int test_homologation(void);
int test_evitement(void);
int test_tirette_et_couleur();
int test_cerise_laterales(void);
void affichage_test_evitement();
void affichage_test_strategie(){
@ -72,6 +74,7 @@ void affichage_test_strategie(){
int test_strategie(){
printf("A - Accoster.\n");
printf("C - Couleur et tirette.\n");
printf("D - Attraper cerises laterales.\n");
printf("E - Evitement\n");
printf("H - Homologation.\n");
printf("L - Longer.\n");
@ -92,6 +95,11 @@ int test_strategie(){
while(test_tirette_et_couleur());
break;
case 'd':
case 'D':
while(test_cerise_laterales());
break;
case 'e':
case 'E':
while(test_evitement());
@ -99,7 +107,7 @@ int test_strategie(){
case 'h':
case 'H':
while(test_homologation());
//while(test_homologation());
break;
case 'l':
@ -121,6 +129,93 @@ int test_strategie(){
}
int test_cerise_laterales(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle_old;
enum etat_action_t etat_action;
printf("Attaper cerise latérales\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
uint32_t tempo_ms=1000;
Localisation_set(250, 1500, -150 * DEGRE_EN_RADIAN);
do{
etat_action = ACTION_EN_COURS;
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
static enum {
TEMPO_AVANT,
TEST,
TEMPO_APRES
}etat_test;
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_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 TEMPO_AVANT:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEST;
}
break;
case TEST:
if(cerises_attraper_cerises_gauches(_step_ms) == ACTION_TERMINEE){
tempo_ms = 1000;
etat_test = TEMPO_APRES;
}
break;
case TEMPO_APRES:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEMPO_AVANT;
etat_action = ACTION_TERMINEE;
}
break;
}
}
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;
@ -174,7 +269,7 @@ int test_homologation(){
}
}
Homologation(_step_ms);
//Homologation(_step_ms);
}
//lettre = getchar_timeout_us(0);

2
i2c_annexe.c
View File

@ -73,10 +73,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
}
void i2c_annexe_active_turbine(void){
/*
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
donnees_a_envoyer=1;
*/
}
void i2c_annexe_desactive_turbine(void){
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;