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;