From 7e24a79115bb7e69f52d79390e636af06336b4b7 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sun, 9 Mar 2025 21:33:00 +0100
Subject: [PATCH] =?UTF-8?q?Mode=20colonne=20lumineuse=20+=20nouveau=20code?=
=?UTF-8?q?=20de=20d=C3=A9monstration?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Demonstration.c | 298 +++++++++++++++++++++++++++++++++++++++++++-
Holonome2023.c | 2 +-
Tests_deplacement.c | 4 +-
3 files changed, 299 insertions(+), 5 deletions(-)
diff --git a/Demonstration.c b/Demonstration.c
index a2e390b..1596daa 100644
--- a/Demonstration.c
+++ b/Demonstration.c
@@ -2,16 +2,20 @@
#include "Demonstration.h"
#define TEST_TIMEOUT_US 10000000
-#define CAPTEUR_POUR_ATTENTE 11
+#define CAPTEUR_POUR_ATTENTE 6
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();
+void Demonstration_actionneurs(void);
uint32_t temps_ms_demo = 0, temps_ms_old;
@@ -22,6 +26,7 @@ void demo_affiche_localisation(){
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);
}
}
@@ -29,6 +34,7 @@ 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{
@@ -40,6 +46,8 @@ int Demonstration_menu(void){
/*printf("C - Trajets enchaines - manuels\n");
printf("D - Trajets enchaines - auto\n");
printf("E - Asservissement angulaire\n");*/
+ printf("F - Rotation et actionneurs\n");
+ printf("Z - Sem-automatique\n");
printf("Q - Quitter\n");
rep = getchar_timeout_us(TEST_TIMEOUT_US);
}while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
@@ -75,6 +83,13 @@ int Demonstration_menu(void){
printf("Fin attente\n");
break;
+ case 'F':
+ while(1){
+ printf("Demo actionneur\n");
+ Demonstration_actionneurs();
+ }
+ break;
+
case 'q':
case 'Q':
return 0;
@@ -82,6 +97,7 @@ int Demonstration_menu(void){
case 'z':
case 'Z':
+ printf("Demo semi-auto\n");
Demonstration_semiauto();
break;
@@ -135,6 +151,112 @@ void Demonstration_auto(){
}
}
+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 Balise_cli_orange_maintenance(){
+ static int32_t temps_ms_led_cli, timer_led_ms = 0;
+ static enum etat_led_cli_orange_maintenance_t{
+ CLI_ON_1,
+ CLI_OFF_1,
+ CLI_ON_2,
+ CLI_OFF_2,
+ }etat_led_cli_orange_maintenance=CLI_ON_1;
+ // 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_1:
+ if(timer_led_ms<= 0){
+ i2c_annexe_couleur_balise(0, 0xFFFF);
+ timer_led_ms = 100;
+ etat_led_cli_orange_maintenance=CLI_OFF_1;
+ }
+ break;
+ case CLI_OFF_1:
+ if(timer_led_ms<= 0){
+ i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
+ timer_led_ms = 100;
+ etat_led_cli_orange_maintenance=CLI_ON_2;
+ }
+ break;
+ case CLI_ON_2:
+ if(timer_led_ms<= 0){
+ i2c_annexe_couleur_balise(0, 0xFFFF);
+ timer_led_ms = 800;
+ etat_led_cli_orange_maintenance=CLI_OFF_2;
+ }
+ break;
+ case CLI_OFF_2:
+ if(timer_led_ms<= 0){
+ i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
+ timer_led_ms = 100;
+ etat_led_cli_orange_maintenance=CLI_ON_1;
+ }
+ 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;
+ }
+
+ }
+}
+
enum etat_action_t Demonstration_attente(){
enum {
ATTENTE_DETECTION,
@@ -145,6 +267,7 @@ enum etat_action_t Demonstration_attente(){
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
+ Balise_cli_orange_maintenance();
switch(etat_attente){
case ATTENTE_DETECTION:
@@ -376,6 +499,52 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
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(){
@@ -434,3 +603,128 @@ enum etat_action_t Demonstration_bezier(){
}
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;
+ }
+
+ }
+ }
+}
\ No newline at end of file
diff --git a/Holonome2023.c b/Holonome2023.c
index 57f059e..dd9a598 100644
--- a/Holonome2023.c
+++ b/Holonome2023.c
@@ -45,7 +45,7 @@ int main() {
stdio_init_all();
//Demonstration_init();Demonstration_auto();
- //while(mode_test());
+ while(mode_test());
//test_pseudo_homologation();
Holonome2023_init();
diff --git a/Tests_deplacement.c b/Tests_deplacement.c
index 17f5e52..49cf6b4 100644
--- a/Tests_deplacement.c
+++ b/Tests_deplacement.c
@@ -345,8 +345,8 @@ int test_aller_retour(){
switch(lettre){
case 'b':
case 'B':
- Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
- Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
+ Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+ Trajectoire_bezier(&trajectoire, 0, 0, 2500, 0, 250, 1300, 0, 0, 0, 0);
printf("Trajectoire de Bézier\n");
break;