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

Corection nommage contacteur + début homologation

Browse Source
This commit is contained in:
Samuel 2023-03-26 21:28:13 +02:00
parent 0e66ae9e4b
commit 82e0f68d78
7 changed files with 182 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -23,6 +23,7 @@ Localisation.c
Moteurs.c
Robot_config.c
Servomoteur.c
Strategie.c
Strategie_prise_cerises.c
Temps.c
Test.c

50
Strategie.c
View File

@ -1,12 +1,56 @@
#include "Localisation.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
#include "math.h"
void Homologation(void){
#define DEGREE_EN_RADIAN (M_PI / 180.)
void Homologation(uint32_t step_ms){
static enum etat_strategie_t{
STRATEGIE_INIT
}etat_strategie;
STRATEGIE_INIT,
APPROCHE_CERISE_1_A,
APPROCHE_CERISE_1_B,
ATTRAPE_CERISE_1,
STRATEGIE_FIN
}etat_strategie=STRATEGIE_INIT;
enum etat_action_t etat_action;
enum trajet_etat_t etat_trajet;
struct trajectoire_t trajectoire;
switch(etat_strategie){
case STRATEGIE_INIT:
Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
etat_strategie = APPROCHE_CERISE_1_A;
break;
case APPROCHE_CERISE_1_A:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
Trajet_debut_trajectoire(trajectoire);
etat_strategie = APPROCHE_CERISE_1_B;
break;
case APPROCHE_CERISE_1_B:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_strategie = ATTRAPE_CERISE_1;
}
break;
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
}
break;
case STRATEGIE_FIN:
break;
}
}

10
Strategie.h
View File

@ -1,3 +1,8 @@
#include "pico/stdlib.h"
#ifndef STRATEGIE_H
#define STRATEGIE_H
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE
@ -10,4 +15,7 @@ enum longer_direction_t{
enum etat_action_t cerise_accostage(void);
enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
void Homologation(void);
void Homologation(uint32_t step_ms);
// STRATEGIE_H
#endif

96
Strategie_prise_cerises.c
View File

@ -4,25 +4,104 @@
#include "Commande_vitesse.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "math.h"
#include "i2c_annexe.h"
// Rotation en rad/s pour accoster les cerises
#define ROTATION_CERISE 0.5f
// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure.
#define MAX_LONGE_MM 240
// Translation en mm/s pour aspirer les cerises
#define TRANSLATION_CERISE 15
#define TRANSLATION_CERISE 45
void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C();
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
double vitesse_accostage_mm_s=100;
enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
/// @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
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum longer_direction_t longer_direction_aspire;
static uint32_t tempo_ms = 0;
static enum {
ATTRAPE_INIT,
ATTRAPE_VERS_BORDURE,
TURBINE_DEMARRAGE,
TURBINE_DEMARRAGE_TEMPO,
ASPIRE_LONGE,
ASPIRE_LIBRE
} etat_attrape=ATTRAPE_INIT;
switch(etat_attrape){
case ATTRAPE_INIT:
etat_attrape=ATTRAPE_VERS_BORDURE;
printf("ATTRAPE_VERS_BORDURE\n");
break;
case ATTRAPE_VERS_BORDURE:
cerise_longer_bord(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;
printf("TURBINE_DEMARRAGE\n");
}
break;
case TURBINE_DEMARRAGE:
i2c_annexe_ferme_porte();
//i2c_annexe_active_turbine();
commande_vitesse_stop();
tempo_ms = 2000;
etat_attrape = TURBINE_DEMARRAGE_TEMPO;
printf("TURBINE_DEMARRAGE_TEMPO\n");
break;
case TURBINE_DEMARRAGE_TEMPO:
if(tempo_ms < step_ms){
etat_attrape = ASPIRE_LONGE;
printf("ASPIRE_LONGE\n");
}
tempo_ms -= step_ms;
break;
case ASPIRE_LONGE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
cerise_longer_bord(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
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;
printf("ASPIRE_LIBRE\n");
}
break;
case ASPIRE_LIBRE:
commande_vitesse_stop();
etat_action = ACTION_TERMINEE;
break;
}
return etat_action;
}
/// @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){
static enum {
LONGE_INIT,
LONGE_VERS_A,
@ -64,7 +143,7 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
printf("Longer INIT\n");
}
}
return etat_action;
return ACTION_EN_COURS;
}
@ -143,11 +222,11 @@ enum etat_action_t cerise_accostage(void){
}
void commande_rotation_contacteur_longer_A(){
commande_rotation(-ROTATION_CERISE, RAYON_ROBOT, 0);
commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0);
}
void commande_rotation_contacteur_longer_C(){
commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
}
void commande_translation_longer_vers_A(){
@ -161,3 +240,10 @@ void commande_translation_longer_vers_C(){
// V_y : -V * sin (60°) = -V * RACINE(3) / 2
commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){
if(direction ==LONGER_VERS_A){
return LONGER_VERS_C;
}
return LONGER_VERS_A;
}

1
Strategie_prise_cerises.h
View File

@ -1 +1,2 @@
#include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms);

2
Test.c
View File

@ -68,7 +68,7 @@ int mode_test(){
printf("C - pour les codeurs\n");
printf("D - pour les codeurs (somme en mm)\n");
printf("E - Commande en vitesse...\n");
printf("F - Accostage\n");
printf("F - Strategie...\n");
printf("H - Asser Position - avance\n");
printf("I - Asser Position - avance et tourne (gyro)\n");
printf("J - Asser Position - avance et tourne (sans gyro)\n");

48
Test_strategie.c
View File

@ -1,6 +1,7 @@
#include "pico/multicore.h"
#include "stdio.h"
#include "hardware/i2c.h"
#include "math.h"
#include "Asser_Moteurs.h"
#include "i2c_annexe.h"
@ -21,14 +22,24 @@ int test_homologation(void);
void affichage_test_strategie(){
uint32_t temps;
temps = time_us_32()/1000;
printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
sleep_ms(10);
while(true){
temps = time_us_32()/1000;
printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm);
printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm);
printf(">pos_angle:%ld:%f\n", temps, Localisation_get().angle_radian/M_PI*180.);
printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
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);
sleep_ms(100);
}
}
@ -68,15 +79,18 @@ int test_strategie(){
int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
printf("Homologation\n");
i2c_maitre_init();
Trajet_init();
//printf("Init gyroscope\n");
Gyro_Init();
//Gyro_Init();
stdio_flush();
set_position_avec_gyroscope(1);
//set_position_avec_gyroscope(1);
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
@ -92,24 +106,24 @@ int test_homologation(){
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
// Gyro_Read(_step_ms_gyro);
}
if(temps_ms > temps_ms_init + 200){
if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
printf("Accostage_terminee\n");
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
Homologation(_step_ms);
}
lettre = getchar_timeout_us(0);
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 1;
return 0;
}