diff --git a/Strategie.h b/Strategie.h index ce302a3..52cead8 100644 --- a/Strategie.h +++ b/Strategie.h @@ -3,4 +3,10 @@ enum etat_action_t{ ACTION_TERMINEE }; -enum etat_action_t cerise_accostage(void); \ No newline at end of file +enum longer_direction_t{ + LONGER_VERS_A, + LONGER_VERS_C +}; + +enum etat_action_t cerise_accostage(void); +enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction); \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 9a316d1..ce754a1 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -1,3 +1,5 @@ +#include "stdio.h" + #include "Strategie_prise_cerises.h" #include "Commande_vitesse.h" #include "Geometrie.h" @@ -8,12 +10,64 @@ // Rotation en rad/s pour accoster les cerises #define ROTATION_CERISE 0.5f +// Translation en mm/s pour aspirer les cerises +#define TRANSLATION_CERISE 15 + void commande_rotation_contacteur_longer_A(); void commande_rotation_contacteur_longer_C(); +void commande_translation_longer_vers_A(); +void commande_translation_longer_vers_C(); double vitesse_accostage_mm_s=100; +enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){ + enum etat_action_t etat_action = ACTION_EN_COURS; + static enum { + LONGE_INIT, + LONGE_VERS_A, + LONGE_VERS_C, + LONGE_COLLE + } etat_longer_bord=LONGE_INIT; + + switch (etat_longer_bord){ + case LONGE_INIT: + if(longer_direction==LONGER_VERS_A){ + etat_longer_bord=LONGE_VERS_A; + }else{ + // longer_direction==LONGER_VERS_C + etat_longer_bord=LONGE_VERS_C; + } + break; + + case LONGE_VERS_A: + commande_translation_longer_vers_A(); + if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || + (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ + etat_longer_bord = LONGE_COLLE; + printf("Longer colle\n"); + } + break; + + case LONGE_VERS_C: + commande_translation_longer_vers_C(); + if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || + (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ + etat_longer_bord = LONGE_COLLE; + printf("Longer colle\n"); + } + break; + + case LONGE_COLLE: + if(cerise_accostage() == ACTION_TERMINEE){ + etat_longer_bord = LONGE_INIT; + printf("Longer INIT\n"); + } + } + return etat_action; + +} + enum etat_action_t cerise_accostage(void){ enum etat_action_t etat_action = ACTION_EN_COURS; double rotation; @@ -31,12 +85,15 @@ enum etat_action_t cerise_accostage(void){ commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0); if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){ etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A; + printf("D ACCOSTE_TOURNE_A\n"); } if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C; + printf("D ACCOSTE_TOURNE_C\n"); } if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ etat_accostage=CERISE_ACCOSTE; + printf("D ACCOSTE\n"); } break; @@ -45,11 +102,14 @@ enum etat_action_t cerise_accostage(void){ if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){ if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){ etat_accostage = CERISE_AVANCE_DROIT; + printf("A ACCOSTE_AVANCE\n"); }else{ etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A; + printf("A ACCOSTE_TOURNE_A\n"); } }else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ etat_accostage = CERISE_ACCOSTE; + printf("A ACCOSTE\n"); } break; @@ -58,18 +118,22 @@ enum etat_action_t cerise_accostage(void){ if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){ if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){ etat_accostage = CERISE_AVANCE_DROIT; + printf("C ACCOSTE_AVANCE\n"); }else{ etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C; + printf("C ACCOSTE_TOURNE_C\n"); } }else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){ etat_accostage = CERISE_ACCOSTE; + printf("C ACCOSTE\n"); } break; case CERISE_ACCOSTE: commande_vitesse_stop(); - //etat_accostage = CERISE_AVANCE_DROIT; + etat_accostage = CERISE_AVANCE_DROIT; etat_action = ACTION_TERMINEE; + break; default: break; @@ -83,5 +147,17 @@ void commande_rotation_contacteur_longer_A(){ } void commande_rotation_contacteur_longer_C(){ - commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2); -} \ No newline at end of file + commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0); +} + +void commande_translation_longer_vers_A(){ + // V_x : V * cos (60°) = V / 2 + // V_y : V * sin (60°) = V * RACINE(3) / 2 + commande_vitesse(TRANSLATION_CERISE/2., TRANSLATION_CERISE / 2. * RACINE_DE_3, 0); +} + +void commande_translation_longer_vers_C(){ + // V_x : -V * cos (60°) = -V / 2 + // V_y : -V * sin (60°) = -V * RACINE(3) / 2 + commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0); +} diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index e883c40..738766b 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1 +1 @@ -#include "Strategie.h" \ No newline at end of file +#include "Strategie.h" diff --git a/Test.c b/Test.c index a3ba22d..27076c0 100644 --- a/Test.c +++ b/Test.c @@ -113,7 +113,7 @@ int mode_test(){ case 'F': case 'f': - while(test_accostage()); + while(test_strategie()); break; case 'H': @@ -690,7 +690,7 @@ int test_aller_retour(){ case 'c': case 'C': Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); - Trajectoire_circulaire(&trajectoire, 0, 700, -90, 90, 350, 0, 0); + Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; diff --git a/Test_strategie.c b/Test_strategie.c index bcf480d..0490589 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -15,6 +15,97 @@ #include "Trajectoire.h" #include "Test.h" +int test_accostage(); +int test_longe(); + +void affichage_test_strategie(){ + uint32_t temps; + temps = time_us_32()/1000; + printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A()); + printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C()); + printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A()); + printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C()); + printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + + sleep_ms(10); +} + + +int test_strategie(){ + printf("L - longer.\n"); + printf("A - Accoster.\n"); + int lettre; + do{ + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); + switch(lettre){ + case 'l': + case 'L': + while(test_longe()); + break; + + case 'a': + case 'A': + while(test_accostage()); + break; + + case 'q': + case 'Q': + return 0; + default: + return 1; + } +} + + +int test_longe(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + + i2c_maitre_init(); + Trajet_init(); + //printf("Init gyroscope\n"); + //Gyro_Init(); + + stdio_flush(); + + //set_position_avec_gyroscope(1); + + temps_ms = Temps_get_temps_ms(); + temps_ms_init = temps_ms; + do{ + i2c_gestion(i2c0); + i2c_annexe_gestion(); + // Routines à 1 ms + if(temps_ms != Temps_get_temps_ms()){ + temps_ms = Temps_get_temps_ms(); + QEI_update(); + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + // Gyro_Read(_step_ms_gyro); + } + + if(temps_ms > temps_ms_init + 200){ + if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){ + printf("Accostage_terminee\n"); + } + } + + } + lettre = getchar_timeout_us(0); + }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + printf("Lettre : %d; %c\n", lettre, lettre); + + if(lettre == 'q' && lettre == 'Q'){ + return 0; + } + return 1; + +} + + int test_accostage(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; @@ -50,15 +141,13 @@ int test_accostage(){ } } - /*printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A()); - printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C()); - printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A()); - printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());*/ - - } lettre = getchar_timeout_us(0); }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); printf("Lettre : %d; %c\n", lettre, lettre); + if(lettre == 'q' && lettre == 'Q'){ + return 0; + } + return 1; } \ No newline at end of file diff --git a/Test_strategie.h b/Test_strategie.h index cf42ebd..bde99ae 100644 --- a/Test_strategie.h +++ b/Test_strategie.h @@ -1 +1 @@ -int test_accostage(void); \ No newline at end of file +int test_strategie(void); \ No newline at end of file