Premier "scénario" de démonstration fonctionnel, encapsulation des fonctions
This commit is contained in:
parent
8f5a160a75
commit
9fa5c8300b
@ -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
82
Demonstration.c
Normal 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
1
Demonstration.h
Normal file
@ -0,0 +1 @@
|
||||
int Demonstration_menu(void);
|
130
Holonome2023.c
130
Holonome2023.c
@ -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
38
Holonome2023.h
Normal 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);
|
@ -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();
|
||||
|
@ -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
7
Test.c
@ -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--;
|
||||
|
Loading…
Reference in New Issue
Block a user