Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Calage initial : code + test

Browse Source
This commit is contained in:
Samuel 2024-04-06 20:38:46 +02:00
parent 828805ef4c
commit ee589f3f73
5 changed files with 150 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Geometrie_robot.h
View File

@ -2,7 +2,7 @@
// Distance entre le centre du robot et un angle du robot
#define RAYON_ROBOT 160.
#define DISTANCE_CENTRE_CAPTEUR 80.
#define DISTANCE_CENTRE_CAPTEUR 100.
#define ANGLE_PINCE (-150 * DEGRE_EN_RADIAN)
// Distance entre le centre du robot et un bord du robot

30
Strategie.c
View File

@ -279,6 +279,16 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms)
return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE);
}
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
struct trajectoire_t trajectoire;
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
pos_x, pos_y,
Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));
return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
}
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
@ -420,17 +430,19 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
break;
case CD_LECTURE_BORDURE_Y:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite){
if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
Localisation_set_angle((-90 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
etat_calage_debut = CD_ROTATION_VERS_X;
}
break;
case CD_ROTATION_VERS_X:
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
if(couleur == COULEUR_BLEU){
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
(-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
@ -440,21 +452,21 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
}
if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
etat_calage_debut = CD_LECTURE_BORDURE_Y;
etat_calage_debut = CD_LECTURE_BORDURE_X;
}
break;
case CD_LECTURE_BORDURE_X:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite){
if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
if(couleur == COULEUR_BLEU){
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
Localisation_set_angle((-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
}else{
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
Localisation_set_angle((0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
}
etat_calage_debut = CD_ALLER_POSITION_INIT;
@ -462,12 +474,14 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
break;
case CD_ALLER_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(couleur == COULEUR_BLEU){
return Strategie_aller_a(250, 250, step_ms);
return Strategie_tourner_et_aller_a(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE, SANS_EVITEMENT, step_ms);
}else{
return Strategie_aller_a(3000 - 250, 250, step_ms);
return Strategie_tourner_et_aller_a(3000 - 250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE, SANS_EVITEMENT, step_ms);
}
break;
}
return ACTION_EN_COURS;

4
Strategie.h
View File

@ -1,5 +1,9 @@
#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#ifndef STRATEGIE_H
#define STRATEGIE_H

7
Test_i2c.c
View File

@ -300,6 +300,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("F - Attrape plante - bras 6\n");
printf("G - Detection bordure\n");
printf("H - Detection plante\n");
printf("I - Detection stop\n");
printf("S - Score + 1\n");
printf("\nQ - Quitter\n");
@ -380,6 +381,12 @@ int test_i2c_ecriture_pico_annex_nb_2(){
printf("=> Detection plante\n");
break;
case 'I':
case 'i':
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
printf("=> Detection STOP\n");
break;
case 'q':
case 'Q':
multicore_reset_core1();

117
Test_strategie_2024.c
View File

@ -1,10 +1,27 @@
#include <stdio.h>
#include "pico/stdio.h"
#include "pico/error.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Evitement.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "Robot_config.h"
#include "Strategie_2024_pots.h"
#include "Strategie.h"
#include "Trajectoire.h"
#include "QEI.h"
#include "gyro.h"
#include "Moteurs.h"
int test_calcul_position_pot(void);
int test_calage_debut(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
printf("A - Position groupes pot.\n");
@ -19,9 +36,15 @@ int test_strategie_2024(){
while(test_calcul_position_pot());
break;
case 'b':
case 'B':
while(test_calage_debut());
break;
}
}
void print_position(struct position_t position){
printf("x_mm: %.2f, y_mm: %.2f, angle: %.2f\n", position.x_mm, position.y_mm, position.angle_radian/DEGRE_EN_RADIAN);
}
@ -62,4 +85,96 @@ int test_calcul_position_pot(){
return 0;
}
}
int test_calage_debut(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action;
printf("test_calage_debut\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(200,200,0);
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(100, 500);
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_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);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strategie_calage_debut(COULEUR_BLEU, _step_ms);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
void affichage_test_strategie_2024(){
uint32_t temps;
while(true){
temps = time_us_32()/1000;
//temps_cycle_display();
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));
printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm);
printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm);
printf(">pos_angle:%ld:%f\n", temps, (Localisation_get().angle_radian + ANGLE_PINCE)/ DEGRE_EN_RADIAN );
printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
printf(">c_pos_angle:%ld:%f\n", temps, (Trajet_get_consigne().angle_radian+ ANGLE_PINCE) / DEGRE_EN_RADIAN);
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
sleep_ms(100);
}
}