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

Compare commits

base: Keuronde:b0e750106d7c4084b9bce7ac1499fcf81d405222
Branches Tags
Keuronde:master
..
compare: Keuronde:f08edb590e60b3efaadf347ba1a616cccc663230
Branches Tags
Keuronde:master

No commits in common. "b0e750106d7c4084b9bce7ac1499fcf81d405222" and "f08edb590e60b3efaadf347ba1a616cccc663230" have entirely different histories.
b0e750106d ... f08edb590e

15 changed files with 54 additions and 122 deletions
Show Stats Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -8,7 +8,6 @@
"vl53l8_2024.h": "c",
"trajet.h": "c",
"trajectoire.h": "c",
"compare": "c",
"asser_position.h": "c"
"compare": "c"
}
}

4
Asser_Moteurs.c
View File

@ -8,8 +8,8 @@
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
float commande_I[3]; // Terme integral
void AsserMoteur_Init(int id){
QEI_init(id);
void AsserMoteur_Init(){
QEI_init();
Moteur_Init();
for(unsigned int i =0; i< 2; i ++){
commande_I[i]=0;

2
Asser_Moteurs.h
View File

@ -5,4 +5,4 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
void AsserMoteur_Gestion(int step_ms);
void AsserMoteur_Init(int);
void AsserMoteur_Init();

23
Asser_Position.c
View File

@ -2,11 +2,9 @@
#include "Commande_vitesse.h"
#include "math.h"
#define GAIN_P_POSITION 15
#define GAIN_P_POSITION 1
#define GAIN_P_ORIENTATION 10
#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
struct position_t position_maintien;
/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
@ -32,14 +30,14 @@ void Asser_Position(struct position_t position_consigne){
delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
// On asservi sur +PI/2 / -PI/2
if(delta_orientation_radian > (M_PI/2)){
/*if(delta_orientation_radian > (M_PI/2)){
delta_orientation_radian -= M_PI;
delta_avance_mm = -delta_avance_mm;
}
if(delta_orientation_radian < -(M_PI/2)){
delta_orientation_radian += M_PI;
delta_avance_mm = -delta_avance_mm;
}
}*/
@ -62,17 +60,4 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
void Asser_Position_maintien(){
Asser_Position(position_maintien);
}
float Asser_Position_get_erreur_angle(){
return delta_orientation_radian;
}
/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
/// @return 1 si panic, 0 si nominal
int Asser_Position_panic_angle(){
if(delta_orientation_radian > MAX_ERREUR_ANGLE){
return 1;
}
return 0;
}
}

2
Asser_Position.h
View File

@ -2,5 +2,3 @@
void Asser_Position(struct position_t position_consigne);
void Asser_Position_set_Pos_Maintien(struct position_t position);
void Asser_Position_maintien();
int Asser_Position_panic_angle();

4
Localisation.c
View File

@ -6,9 +6,9 @@
struct position_t position;
void Localisation_init(int id){
void Localisation_init(){
Temps_init();
QEI_init(id);
QEI_init();
position.x_mm = 0;
position.y_mm = 0;
position.angle_radian = 0;

2
Localisation.h
View File

@ -2,7 +2,7 @@
struct position_t Localisation_get(void);
void Localisation_gestion();
void Localisation_init(int);
void Localisation_init();
void Localisation_set(float x_mm, float y_mm, float angle_radian);
void Localisation_set_x(float x_mm);

12
QEI.c
View File

@ -17,10 +17,8 @@
// Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44
#define IMPULSION_PAR_MM_50_1 (12.45f)
#define IMPULSION_PAR_MM_30_1 (7.47f)
#define IMPULSION_PAR_MM (12.45f)
float impulsion_par_mm;
struct QEI_t QEI_A, QEI_B;
@ -30,7 +28,7 @@ bool QEI_est_init = false;
PIO pio_QEI = pio0;
void QEI_init(int identifiant){
void QEI_init(){
// Initialisation des 3 modules QEI
// Chaque module QEI sera dans une machine à état du PIO 0
if(!QEI_est_init){
@ -50,10 +48,6 @@ void QEI_init(int identifiant){
QEI_B.value=0;
QEI_est_init=true;
}
impulsion_par_mm = IMPULSION_PAR_MM_50_1;
if(identifiant == 0 || identifiant >= 4){
impulsion_par_mm = IMPULSION_PAR_MM_30_1;
}
}
@ -97,5 +91,5 @@ int QEI_get(enum QEI_name_t qei){
/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
float QEI_get_mm(enum QEI_name_t qei){
return ((float) QEI_get(qei)) / impulsion_par_mm;
return ((float) QEI_get(qei)) / (float)IMPULSION_PAR_MM;
}

2
QEI.h
View File

@ -11,6 +11,6 @@ enum QEI_name_t{
extern struct QEI_t QEI_A, QEI_B, QEI_C;
void QEI_update(void);
void QEI_init(int);
void QEI_init(void);
int QEI_get(enum QEI_name_t qei);
float QEI_get_mm(enum QEI_name_t qei);

12
Trajet.c
View File

@ -25,9 +25,9 @@ const float distance_pas_obstacle = 2000;
float vitesse_max_contrainte_obstacle;
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
void Trajet_init(int id){
void Trajet_init(){
Temps_init();
AsserMoteur_Init(id);
AsserMoteur_Init();
abscisse = 0;
vitesse_mm_s = 0;
position_mm = 0;
@ -141,16 +141,16 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
vitesse_max_contrainte = 0;
}
distance_contrainte_obstacle = Trajet_get_obstacle_mm();
/*distance_contrainte_obstacle = Trajet_get_obstacle_mm();
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
}
}/*
if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){
vitesse = 0;
}*/
if(Trajet_get_obstacle_mm() < 50){
vitesse = 0;
}
// Selection de la vitesse la plus faible

3
Trajet.h
View File

@ -11,7 +11,6 @@ enum etat_trajet_t{
// Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
#define TRAJECT_CONFIG_RAPIDE 300, 1200
#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200
// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
// Vitesse et acceleration - standard (en mm et mm/s²)
@ -21,7 +20,7 @@ enum etat_trajet_t{
extern const float distance_pas_obstacle;
void Trajet_init(int);
void Trajet_init();
void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
enum etat_trajet_t Trajet_avance(float temps_s);

9
VL53L8_2024.c
View File

@ -14,8 +14,6 @@ int masque[64]={
195,200,202,206,213,200,200,202
};
float old_min_distance;
void VL53L8_init(VL53L8CX_Configuration * Dev){
uint8_t status, isAlive, isReady, i;
@ -154,7 +152,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
int min_distance = 2000;
int col, row;
for(col=0; col<8; col++){
for(row=0; row<=3; row++){
for(row=4; row<=6; row++){
if(min_distance > Results.distance_mm[col + 8*row]){
min_distance = Results.distance_mm[col + 8*row];
}
@ -164,12 +162,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
if(min_distance-50 > 0){
*distance = min_distance-50;
}
old_min_distance = *distance;
}
float VL53L8_get_old_min_distance(){
return old_min_distance;
}

1
VL53L8_2024.h
View File

@ -4,4 +4,3 @@ void VL53L8_init(VL53L8CX_Configuration * Dev);
void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
float VL53L8_get_old_min_distance(void);

0
build/rien.ici Normal file
View File

97
main.c
View File

@ -37,13 +37,13 @@ uint16_t tension_batterie_lire(void);
void identifiant_init(void);
uint identifiant_lire(void);
int get_tirette(int);
int get_tirette(void);
int get_couleur(void);
void configure_trajet(int identifiant, int couleur);
void gestion_VL53L8CX(void);
const uint32_t step_ms=1;
uint32_t step_ms=1;
float distance1_mm=0, distance2_mm=0;
// DEBUG
@ -56,7 +56,6 @@ void main(void)
{
int ledpower = 500;
VL53L8CX_ResultsData Results;
bool fin_match = false;
@ -64,8 +63,8 @@ void main(void)
Temps_init();
//tension_batterie_init();
identifiant_init();
Localisation_init(identifiant_lire());
Trajet_init(identifiant_lire());
Localisation_init();
Trajet_init();
i2c_maitre_init();
@ -83,8 +82,7 @@ void main(void)
//multicore_launch_core1(gestion_affichage);
multicore_launch_core1(gestion_VL53L8CX);
sleep_ms(5000);
printf("Demarrage...\n");
sleep_ms(4000);
@ -96,20 +94,16 @@ void main(void)
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
while(get_tirette(identifiant_lire()));
gpio_put(LED1PIN, 0);
while(get_tirette());
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
if(identifiant_lire() == 3){
sleep_ms(90000);
}
// sleep_ms(90000);
temps_depart_ms = Temps_get_temps_ms();
while(1){
// Fin du match
if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
Moteur_Stop();
while(1);
}
@ -118,16 +112,16 @@ void main(void)
if(temps_ms % step_ms == 0){
QEI_update();
Localisation_gestion();
if(etat_trajet != TRAJET_TERMINE){
etat_trajet = Trajet_avance((float)step_ms/1000.);
if(Trajet_get_obstacle_mm() < 50){
Moteur_Stop();
}else{
Asser_Position_maintien();
if(Asser_Position_panic_angle()){
fin_match=1;
if(etat_trajet != TRAJET_TERMINE){
etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{
Asser_Position_maintien();
}
AsserMoteur_Gestion(step_ms);
}
AsserMoteur_Gestion(step_ms);
}
}
@ -179,7 +173,7 @@ void affichage(void){
printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire());
}
void tension_batterie_init(void){
@ -219,12 +213,8 @@ void identifiant_init(){
gpio_set_dir(COULEUR_PIN, GPIO_IN);
}
int get_tirette(int id){
if(id == 3){
return !gpio_get(TIRETTE_PIN);
}
return (VL53L8_get_old_min_distance() <50);
int get_tirette(void){
return !gpio_get(TIRETTE_PIN);
}
int get_couleur(void){
@ -240,43 +230,28 @@ uint identifiant_lire(){
void configure_trajet(int identifiant, int couleur){
struct trajectoire_t trajectoire;
Trajet_config(TRAJECT_CONFIG_RAPIDE);
switch(couleur){
case COULEUR_JAUNE:
switch (identifiant)
{
case 0:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
break;
case 1:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
Localisation_set(1465, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 3000-1465, 2000-63, 3000-1260, 2000-63,
3000-600, 1400, 3000-0, 2000, M_PI, M_PI);
break;
case 2:
Localisation_set(3000-1245, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63,
3000-540, 1400, 3100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(3000-1130, 2000-63, 0);
Localisation_set(3000 - 1130, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
3000-606, 2000-590, 3000-225, 2000-225, 0, 0);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1364, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63,
2700-900, 600, 2700-0, 0, 0, 0);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1450, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63,
3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
@ -292,36 +267,22 @@ void configure_trajet(int identifiant, int couleur){
switch (identifiant)
{
case 0:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
break;
case 1:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
Localisation_set(1465, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63,
600, 1400, 0, 2000, M_PI, M_PI);
break;
case 2:
Localisation_set(1245, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63,
540, 1400, -100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(1121, 2000-63, M_PI);
Localisation_set(1130, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1364, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63,
900, 600, 0, 0, -M_PI / 2., M_PI);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1450, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63,
930, 970, 3000, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
@ -334,6 +295,10 @@ void configure_trajet(int identifiant, int couleur){
break;
}
Trajet_config(TRAJECT_CONFIG_RAPIDE);
Trajet_debut_trajectoire(trajectoire);
}