Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Test accostage + longer bordure

Browse Source
This commit is contained in:
Samuel 2023-03-26 16:34:06 +02:00
parent 0237ccb8fe
commit 2710dadaa2
6 changed files with 185 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Strategie.h
View File

@ -3,4 +3,10 @@ enum etat_action_t{
ACTION_TERMINEE
};
enum etat_action_t cerise_accostage(void);
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);

82
Strategie_prise_cerises.c
View File

@ -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);
}
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);
}

2
Strategie_prise_cerises.h
View File

@ -1 +1 @@
#include "Strategie.h"
#include "Strategie.h"

4
Test.c
View File

@ -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;

101
Test_strategie.c
View File

@ -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;
}

2
Test_strategie.h
View File

@ -1 +1 @@
int test_accostage(void);
int test_strategie(void);