Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Demo_2025_03
Holonome_2024/Demonstration.c
Samuel 5d03b5d4e9 Demo 15 mars 2025
2025-03-15 15:24:02 +01:00

903 lines
30 KiB
C
Raw Permalink Blame History

#include "Holonome2023.h"
#include "Strategie_2024.h"
#include "Demonstration.h"
#define TEST_TIMEOUT_US 10000000
#define CAPTEUR_POUR_ATTENTE 6
#define CAPTEUR_POUR_ATTENTE_DEVANT 6
#define CAPTEUR_POUR_ATTENTE_GAUCHE 9
#define CAPTEUR_POUR_ATTENTE_DROIT 3
#define CAPTEUR_POUR_ATTENTE_ARRIERE 0
int Demonstration_init(void);
enum etat_action_t Demonstration_calage();
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
enum etat_action_t Demonstration_tourne(float angle_degrees);
enum etat_action_t Demonstration_bezier();
enum etat_action_t Demonstration_attente();
enum etat_action_t Demonstration_leve_bras(uint32_t bras);
enum etat_action_t Demonstration_baisse_bras(void);
enum etat_action_t Demonstration_attrape_plante();
enum etat_action_t Demonstration_attente_capteur(int capteur);
void Demonstration_menu_balise(void);
void Demonstration_prise_plante(void);
void Demonstration_actionneurs(void);
uint32_t temps_ms_demo = 0, temps_ms_old;
void demo_affiche_localisation(){
struct position_t position;
while(1){
position = Localisation_get();
printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
sleep_ms(50);
}
}
int Demonstration_menu(void){
static int iteration = 2;
int rep;
printf("Mode demo - init\n");
set_position_avec_gyroscope(false);
Demonstration_init();
while(1){
do{
printf("A - Calage dans l'angle\n");
printf("B - Trajets unitaires - Rectangle\n");
printf("C - Trajets unitaires - Droit puis tourne\n");
printf("D - Trajets unitaires - Bezier\n");
printf("E - Test attente\n");
/*printf("C - Trajets enchaines - manuels\n");
printf("D - Trajets enchaines - auto\n");
printf("E - Asservissement angulaire\n");*/
printf("F - Rotation et actionneurs\n");
printf("G - Prise plante\n");
printf("M - Menu Balise\n");
printf("Z - Sem-automatique\n");
printf("Q - Quitter\n");
rep = getchar_timeout_us(TEST_TIMEOUT_US);
}while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
switch (rep)
{
case 'a':
case 'A':
Demonstration_calage();
break;
case 'b':
case 'B':
printf("Demonstration rectangle\n");
Demonstration_rectangle(550, 1000);
printf("Recalage\n");
Demonstration_calage();
break;
case 'c':
case 'C':
Demonstration_avance_puis_tourne(300, 1000, 720.);
break;
case 'd':
case 'D':
Demonstration_bezier();
break;
case 'e':
case 'E':
printf("Début attente\n");
Demonstration_attente();
printf("Fin attente\n");
break;
case 'f':
case 'F':
while(1){
printf("Demo actionneur\n");
Demonstration_actionneurs();
}
break;
case 'g':
case 'G':
printf("Début prise plante\n");
Demonstration_prise_plante();
printf("Fin prise plante\n");
break;
case 'm':
case 'M':
printf("Menu balise\n");
Demonstration_menu_balise();
printf("Fin menu balise\n");
break;
case 'q':
case 'Q':
return 0;
break;
case 'z':
case 'Z':
printf("Demo semi-auto\n");
Demonstration_semiauto();
break;
}
}
return 1;
}
int Demonstration_init(){
Holonome2023_init();
temps_ms_demo = Temps_get_temps_ms();
temps_ms_old = temps_ms_demo;
multicore_launch_core1(demo_affiche_localisation);
}
void Demonstration_semiauto(){
Demonstration_calage();
Demonstration_attente();
while(true){
Demonstration_rectangle(550, 1000);
Demonstration_calage();
Demonstration_attente();
Demonstration_avance_puis_tourne(300, 1000, 720.);
Demonstration_calage();
Demonstration_attente();
Demonstration_bezier();
Demonstration_calage();
Demonstration_attente();
}
}
void Demonstration_auto(){
Demonstration_calage();
Demonstration_attente();
while(true){
Demonstration_rectangle(550, 1000);
Demonstration_calage();
Demonstration_avance_puis_tourne(300, 1000, 720.);
Demonstration_calage();
Demonstration_bezier();
Demonstration_calage();
Demonstration_attente();
}
}
void Demonstration_actionneurs(){
Demonstration_attente();
Demonstration_attrape_plante();
Demonstration_tourne(30);
Demonstration_leve_bras(0);
Demonstration_tourne(-60);
Demonstration_leve_bras(5);
Demonstration_tourne(120);
Demonstration_leve_bras(1);
Demonstration_tourne(-180);
Demonstration_leve_bras(4);
Demonstration_tourne(240);
Demonstration_leve_bras(2);
Demonstration_tourne(-300);
Demonstration_leve_bras(3);
Demonstration_tourne(150);
Demonstration_baisse_bras();
}
void Demonstration_prise_plante(){
static int32_t m_temps_ms, m_timer;
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_action_t etat_action_step;
static enum {
INIT,
TIENT_POT,
LEVE_POT,
ATTRAPE_PLANTE_1,
ATTRAPE_PLANTE_2,
RETOUR_MAISON
} etat_prise_plante = INIT;
while(etat_action == ACTION_EN_COURS){
Holonome_cyclique(PARAM_DEFAULT);
if(m_temps_ms != Temps_get_temps_ms()){
m_temps_ms = Temps_get_temps_ms();
switch (etat_prise_plante){
case INIT:
m_timer = 3000;
etat_prise_plante = TIENT_POT;
etat_action = ACTION_EN_COURS;
case TIENT_POT:
m_timer--;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
if(m_timer <= 0){
etat_prise_plante = LEVE_POT;
m_timer = 1000;
}
break;
case LEVE_POT:
m_timer--;
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
if(m_timer <= 0){
etat_prise_plante = ATTRAPE_PLANTE_1;
}
break;
case ATTRAPE_PLANTE_1:
etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_1, ZONE_PLANTE_1);
if(etat_action_step == ACTION_TERMINEE){
etat_prise_plante = ATTRAPE_PLANTE_2;
}
break;
case ATTRAPE_PLANTE_2:
etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_6, ZONE_PLANTE_1);
if(etat_action_step == ACTION_TERMINEE){
etat_prise_plante = RETOUR_MAISON;
}
break;
case RETOUR_MAISON:{
etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
if(etat_action == ACTION_TERMINEE){
etat_prise_plante = INIT;
}
}
}
}
}
Moteur_Stop();
}
void Balise_cli_orange_maintenance(int nb_cli){
int32_t temps_court_ms = 150, temps_long_ms = 700;
static int32_t temps_ms_led_cli, timer_led_ms = 0;
static int32_t m_nb_cli;
static enum etat_led_cli_orange_maintenance_t{
CLI_ON_COURT,
CLI_OFF_COURT,
CLI_OFF_LONG,
}etat_led_cli_orange_maintenance=CLI_OFF_LONG;
// Toutes les 1 ms.
if(temps_ms_led_cli != Temps_get_temps_ms()){
temps_ms_led_cli = Temps_get_temps_ms();
timer_led_ms--;
switch(etat_led_cli_orange_maintenance){
case CLI_ON_COURT:
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0, 0xFFFF);
timer_led_ms = temps_court_ms;
m_nb_cli--;
if(m_nb_cli){
etat_led_cli_orange_maintenance=CLI_OFF_COURT;
timer_led_ms = temps_court_ms;
}else{
etat_led_cli_orange_maintenance=CLI_OFF_LONG;
timer_led_ms = temps_long_ms;
}
}
break;
case CLI_OFF_COURT:
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
timer_led_ms = temps_court_ms;
etat_led_cli_orange_maintenance=CLI_ON_COURT;
}
break;
case CLI_OFF_LONG:
m_nb_cli = nb_cli;
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
timer_led_ms = temps_court_ms;
etat_led_cli_orange_maintenance=CLI_ON_COURT;
}
break;
}
}
}
void Balise_pulse_vert(){
static int32_t temps_ms_led_cli, timer_led_ms = 800;
int32_t timer_led_max_ms =800;
int32_t vert_led, vert_led_max = 0b111;
static enum etat_led_pulse_vert_t{
PULSE_RISE,
PULSE_FALL,
}etat_led_pulse_vert=PULSE_FALL;
// Toutes les 1 ms.
if(temps_ms_led_cli != Temps_get_temps_ms()){
temps_ms_led_cli = Temps_get_temps_ms();
timer_led_ms--;
switch(etat_led_pulse_vert){
case PULSE_RISE:
vert_led = (vert_led_max-1) - (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
vert_led = vert_led & 0b111;
i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0, 0xFFFF);
timer_led_ms = timer_led_max_ms;
etat_led_pulse_vert=PULSE_FALL;
}
break;
case PULSE_FALL:
vert_led = (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
vert_led = vert_led & 0b111;
i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
if(timer_led_ms<= 0){
timer_led_ms = timer_led_max_ms;
etat_led_pulse_vert=PULSE_RISE;
}
break;
}
}
}
void Demonstration_menu_balise(void){
int nb_cli=2;
while(1){
Holonome_cyclique(PARAM_NO_MOTORS);
Balise_cli_orange_maintenance(nb_cli);
if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DROIT) == ACTION_TERMINEE){
nb_cli++;
printf(">nb_cli%d\n",nb_cli);
}
if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_GAUCHE) == ACTION_TERMINEE){
nb_cli--;
printf(">nb_cli%d\n",nb_cli);
}
if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DEVANT) == ACTION_TERMINEE){
return;
}
if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_ARRIERE) == ACTION_TERMINEE){
switch(nb_cli){
case 2:
Demonstration_actionneurs();
break;
case 3:
Demonstration_prise_plante();
break;
}
}
}
}
/// @brief Comme la fonction demonstration attente, mais en choisissant le capteur. Fonction non-bloquante
/// @param capteur: numéro du capteur, 0 à l'arrière, 3 à droite, 6 devant, 9, à gauche
/// @return ACTION_TERMINEE si nous avons un signal pour ce capteur
enum etat_action_t Demonstration_attente_capteur(int capteur){
static enum {
ATTENTE_DETECTION,
DETECTION_PROCHE,
FIN_ATTENTE
} etat_attente[12] = {ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,
ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION};
static uint32_t temps_debut_tempo[12];
uint32_t duree_tempo_ms = 50;
switch(etat_attente[capteur]){
case ATTENTE_DETECTION:
if(Balise_VL53L1X_get_capteur_cm(capteur) < 15 && Balise_VL53L1X_get_capteur_cm(capteur) > 1){
/// Sans obstacle, le capteur peut renvoyer 0;
etat_attente[capteur]=DETECTION_PROCHE;
temps_debut_tempo[capteur] = time_us_32();
}
break;
case DETECTION_PROCHE:
if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
// On a perdu la detection avant le temps écoulé
etat_attente[capteur]=ATTENTE_DETECTION;
}
if((temps_debut_tempo[capteur] + (duree_tempo_ms * 1000)) < time_us_32()){
// temps écoulé
etat_attente[capteur]=FIN_ATTENTE;
}
break;
case FIN_ATTENTE:
if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
// On a perdu la detection après le temps écoulé
etat_attente[capteur]=ATTENTE_DETECTION;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
//sleep_ms(20);
}
enum etat_action_t Demonstration_attente(){
enum {
ATTENTE_DETECTION,
DETECTION_PROCHE,
FIN_ATTENTE
} etat_attente = ATTENTE_DETECTION;
uint32_t temps_debut_tempo, duree_tempo_ms = 50;
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
Balise_cli_orange_maintenance(2);
switch(etat_attente){
case ATTENTE_DETECTION:
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 15 && Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 1){
/// Sans obstacle, le capteur peut renvoyer 0;
etat_attente=DETECTION_PROCHE;
temps_debut_tempo = time_us_32();
}
break;
case DETECTION_PROCHE:
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
// On a perdu la detection avant le temps écoulé
etat_attente=ATTENTE_DETECTION;
}
if((temps_debut_tempo + (duree_tempo_ms * 1000)) < time_us_32()){
// temps écoulé
etat_attente=FIN_ATTENTE;
}
break;
case FIN_ATTENTE:
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
// On a perdu la detection après le temps écoulé
etat_attente=ATTENTE_DETECTION;
return ACTION_TERMINEE;
}
break;
}
//sleep_ms(20);
}
}
enum etat_action_t Demonstration_calage(){
enum {
CALAGE,
DECALAGE,
CALAGE_TERMINE
} etat_calage = CALAGE;
while(true){
Holonome_cyclique(PARAM_DEFAULT);
struct trajectoire_t trajectoire;
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
switch (etat_calage)
{
case CALAGE:
// TODO: Appeler la nouvelle fonction de prise de référentiel
/*if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_calage = DECALAGE;
}*/
break;
case DECALAGE:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_calage = CALAGE_TERMINE;
}
break;
case CALAGE_TERMINE:
etat_calage = CALAGE;
Moteur_Stop();
return ACTION_TERMINEE;
default:
break;
}
}
}
return ACTION_ECHEC;
}
/// @brief Deplacement suivant un rectangle.
/// @param avance_x_mm : Distance en X (conseillé 620)
/// @param avance_y_mm : Distance en Y (conseillé 1000)
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
enum {
AVANCE_Y,
AVANCE_X,
RECULE_Y,
RECULE_X,
RECTANGLE_TERMINE
} etat_rectangle = AVANCE_Y;
while(true){
struct trajectoire_t trajectoire;
Holonome_cyclique(PARAM_DEFAULT);
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
switch (etat_rectangle)
{
case AVANCE_Y:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = AVANCE_X;
}
break;
case AVANCE_X:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECULE_Y;
}
break;
case RECULE_Y:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECULE_X;
}
break;
case RECULE_X:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_rectangle = RECTANGLE_TERMINE;
}
break;
case RECTANGLE_TERMINE:
etat_rectangle = AVANCE_Y;
Moteur_Stop();
return ACTION_TERMINEE;
default:
break;
}
}
}
return 0;
}
/// @brief Deplacement suivant une droite, rotation sur lui-même du robot une fois arrivée à destination,
/// puis retour en ligne droite
/// @param avance_x_mm : Distance en X (conseillé 300)
/// @param avance_y_mm : Distance en Y (conseillé 1000)
/// @param angle_degrees : Rotation du robot sur lui-même
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees){
enum {
APT_AVANCE,
APT_TOURNE,
APT_RECULE,
APT_TERMINE
} etat_avance_puis_tourne = APT_AVANCE;
int pos_x_init_mm, pos_y_init_mm;
while(true){
struct trajectoire_t trajectoire;
Holonome_cyclique(PARAM_DEFAULT);
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
switch (etat_avance_puis_tourne)
{
case APT_AVANCE:
pos_x_init_mm = Localisation_get().x_mm;
pos_y_init_mm = Localisation_get().y_mm;
Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm,
pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_TOURNE;
}
break;
case APT_TOURNE:
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_RECULE;
Trajet_config(TRAJECT_CONFIG_STD);
}
break;
case APT_RECULE:
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = APT_TERMINE;
}
break;
case APT_TERMINE:
etat_avance_puis_tourne = APT_AVANCE;
Moteur_Stop();
return ACTION_TERMINEE;
default:
break;
}
}
}
return ACTION_ECHEC;
}
/// @brief Rotation du robot sur lui-même du robot
/// @param angle_degrees : Rotation du robot sur lui-même
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_tourne(float angle_degrees){
enum {
DEMO_TOURNE,
DEMO_TOURNE_TERMINE
} etat_avance_puis_tourne = DEMO_TOURNE;
int pos_x_init_mm, pos_y_init_mm;
while(true){
struct trajectoire_t trajectoire;
Holonome_cyclique(PARAM_DEFAULT);
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
switch (etat_avance_puis_tourne)
{
case DEMO_TOURNE:
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_avance_puis_tourne = DEMO_TOURNE_TERMINE;
Trajet_config(TRAJECT_CONFIG_STD);
}
break;
case DEMO_TOURNE_TERMINE:
etat_avance_puis_tourne = DEMO_TOURNE;
Moteur_Stop();
return ACTION_TERMINEE;
default:
break;
}
}
}
return ACTION_ECHEC;
}
/// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_bezier(){
enum {
BEZIER_ALLER,
BEZIER_RETOUR,
BEZIER_TERMINE
} etat_bezier = BEZIER_ALLER;
while(true){
struct trajectoire_t trajectoire;
static int pos_x_init_mm, pos_y_init_mm;
Holonome_cyclique(PARAM_DEFAULT);
Trajet_config(200, 200);
// Toutes les 1 ms.
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
switch (etat_bezier)
{
case BEZIER_ALLER:
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
1386, Localisation_get().y_mm,
-576, 1500 - 276,
545, 1500 - 276,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_bezier = BEZIER_RETOUR;
}
break;
case BEZIER_RETOUR:
Trajectoire_bezier(&trajectoire,
Localisation_get().x_mm, Localisation_get().y_mm,
1391, Localisation_get().y_mm,
-76, 1500 - 788,
242, 1500 - 1289,
Localisation_get().angle_radian, Localisation_get().angle_radian);
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_bezier = BEZIER_TERMINE;
}
break;
case BEZIER_TERMINE:
etat_bezier = BEZIER_ALLER;
Trajet_config(TRAJECT_CONFIG_STD);
return ACTION_TERMINEE;
default:
break;
}
}
}
return ACTION_ECHEC;
}
/// @brief Leve le bras, et agite le doigt
/// @param bras
enum etat_action_t Demonstration_leve_bras(uint32_t bras){
static enum{
LB_LEVE_BRAS,
LB_TEMPO_BRAS,
LB_DOIGT_ATTRAPE_1,
LB_DOIGT_LACHE,
LB_DOIGT_ATTRAPE_2,
} DLB_status;
int timer_ms;
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
timer_ms--;
switch(DLB_status){
case LB_LEVE_BRAS:
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
DLB_status = LB_TEMPO_BRAS;
timer_ms = 250;
break;
case LB_TEMPO_BRAS:
if(timer_ms <= 0){
DLB_status = LB_DOIGT_ATTRAPE_1;
timer_ms=150;
}
break;
case LB_DOIGT_ATTRAPE_1:
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
if(timer_ms <= 0){
DLB_status = LB_DOIGT_LACHE;
timer_ms=200;
}
break;
case LB_DOIGT_LACHE:
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
if(timer_ms <= 0){
DLB_status = LB_DOIGT_ATTRAPE_2;
timer_ms=200;
}
break;
case LB_DOIGT_ATTRAPE_2:
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
if(timer_ms <= 0){
DLB_status = LB_LEVE_BRAS;
return ACTION_TERMINEE;
}
break;
}
}
}
}
enum etat_action_t Demonstration_attrape_plante(void){
int32_t timer_ms;
enum {
DAP_ENVOI,
DAP_ATTENTE
} dap_status=DAP_ENVOI;
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
timer_ms--;
switch(dap_status){
case DAP_ENVOI:
i2c_annexe_attrape_plante(PLANTE_BRAS_1);
dap_status = DAP_ATTENTE;
timer_ms=5000;
break;
case DAP_ATTENTE:
if (timer_ms <= 0){
dap_status=DAP_ENVOI;
return ACTION_TERMINEE;
}
break;
}
}
}
}
enum etat_action_t Demonstration_baisse_bras(void){
static int32_t timer_ms;
enum {
DBB_ENVOI,
DBB_ATTENTE
} dbb_status=DBB_ENVOI;
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
if(temps_ms_demo != Temps_get_temps_ms()){
temps_ms_demo = Temps_get_temps_ms();
timer_ms--;
switch(dbb_status){
case DBB_ENVOI:
for(int bras=0; bras < 6; bras++){
i2c_annexe_actionneur_pot(bras, BRAS_PLIE, DOIGT_LACHE);
}
dbb_status = DBB_ATTENTE;
timer_ms=500;
break;
case DBB_ATTENTE:
if (timer_ms <= 0){
dbb_status = DBB_ENVOI;
return ACTION_TERMINEE;
}
break;
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink