Premier "scénario" de démonstration fonctionnel, encapsulation des fonctions

This commit is contained in:
Samuel 2023-08-23 23:20:14 +02:00
parent 8f5a160a75
commit 9fa5c8300b
8 changed files with 199 additions and 64 deletions

View File

@ -14,6 +14,7 @@ Asser_Moteurs.c
Asser_Position.c
Balise_VL53L1X.c
Commande_vitesse.c
Demonstration.c
Evitement.c
QEI.c
Geometrie.c

82
Demonstration.c Normal file
View File

@ -0,0 +1,82 @@
#include "Holonome2023.h"
#include "Demonstration.h"
#define TEST_TIMEOUT_US 10000000
int Demonstration_calage();
uint32_t temps_ms_demo = 0, temps_ms_old;
int Demonstration_menu(void){
static int iteration = 2;
printf("Mode demo\n");
printf("A - Calage dans l'angle\n");
printf("B - Trajets unitaires\n");
printf("C - Trajets enchaines - manuels\n");
printf("D - Trajets enchaines - auto\n");
printf("E - Asservissement angulaire\n");
printf("Q - Quitter\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
switch (rep)
{
case 'a':
case 'A':
while(Demonstration_calage());
break;
case 'b':
case 'B':
break;
}
return 1;
}
int Demonstration_init(){
Holonome2023_init();
temps_ms_demo = Temps_get_temps_ms();
temps_ms_old = temps_ms_demo;
}
int Demonstration_calage(){
Demonstration_init();
enum {
CALAGE,
CALAGE_TERMINE
} etat_calage = CALAGE;
while(true){
Holonome_cyclique();
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo != Temps_get_temps_ms();
switch (etat_calage)
{
case CALAGE:
if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_calage = CALAGE_TERMINE;
}
break;
case CALAGE_TERMINE:
etat_calage = CALAGE;
Moteur_Stop();
return 0;
default:
break;
}
}
}
return 0;
}

1
Demonstration.h Normal file
View File

@ -0,0 +1 @@
int Demonstration_menu(void);

View File

@ -1,33 +1,4 @@
#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "hardware/timer.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Evitement.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Score.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Strategie.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Holonome2023.h"
const uint LED_PIN = 25;
#define LED_PIN_ROUGE 28
@ -39,6 +10,7 @@ uint temps_cycle;
#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000
int mode_test();
void init_led(uint Numero_de_la_led, uint etat){
@ -58,50 +30,20 @@ int main() {
uint32_t score=0;
uint32_t match_en_cours=1;
stdio_init_all();
init_led(LED_PIN, 1);
init_led(LED_PIN_ROUGE, 0);
gpio_init(COULEUR);
gpio_init(TIRETTE);
gpio_set_dir(COULEUR, GPIO_IN);
gpio_set_dir(TIRETTE, GPIO_IN);
set_position_avec_gyroscope(1);
// 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);
gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);
sleep_ms(3000);
Servomoteur_Init();
//puts("Debut");
//spi_test();
//while(1);
Temps_init();
Moteur_Init();
QEI_init();
AsserMoteur_Init();
Localisation_init();
stdio_init_all();
while(mode_test());
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Holonome2023_init();
multicore_launch_core1(Monitoring_display);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}else{
sleep_ms(5000);
}
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
while (1) {
@ -191,3 +133,65 @@ int main() {
}
}
}
void Holonome2023_init(){
init_led(LED_PIN, 1);
init_led(LED_PIN_ROUGE, 0);
gpio_init(COULEUR);
gpio_init(TIRETTE);
gpio_set_dir(COULEUR, GPIO_IN);
gpio_set_dir(TIRETTE, GPIO_IN);
Log_init();
sleep_ms(3000);
Servomoteur_Init();
Temps_init();
Moteur_Init();
QEI_init();
AsserMoteur_Init();
Localisation_init();
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
if(get_position_avec_gyroscope()){
Gyro_Init();
}else{
sleep_ms(5000);
}
for(int i=0; i<10; i++){
i2c_gestion(i2c0);
i2c_annexe_gestion();
sleep_ms(1);
}
}
/// @brief Fonction à appeler le plus souvent possible. Lit les codeurs, le gyroscope, et asservit les moteurs
void Holonome_cyclique(void){
static uint32_t temps_ms = 0;
// Surveillance du temps d'execution
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
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);
}
}
}
}

38
Holonome2023.h Normal file
View File

@ -0,0 +1,38 @@
#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "hardware/timer.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Evitement.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Log.h"
#include "Monitoring.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Score.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Strategie.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#define STEP_MS_GYRO 2 /*ms*/
#define STEP_MS 1 /*ms*/
void Holonome2023_init(void);
void Holonome_cyclique(void);

View File

@ -40,7 +40,7 @@ float distance_obstacle;
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
int Robot_est_dans_quart_haut_gauche();

View File

@ -46,6 +46,8 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t depose_cerises(uint32_t step_ms);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);

7
Test.c
View File

@ -14,6 +14,7 @@
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Demonstration.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
@ -93,6 +94,7 @@ int mode_test(){
printf("U - Tests i2c...\n");
printf("V - APDS_9960\n");
printf("W - Endurance aller retour\n");
printf("Z - Codes de démonstration\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
@ -211,6 +213,11 @@ int mode_test(){
case 'w':
while(test_endurance_aller_retour());
break;
case 'Z':
case 'z':
while(Demonstration_menu());
break;
case PICO_ERROR_TIMEOUT:
iteration--;