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

Trajectoire circulaire

Browse Source
This commit is contained in:
Samuel 2024-07-29 19:21:57 +02:00
parent e4af4367c1
commit 29d32ff7fa
6 changed files with 129 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/tasks.json vendored
View File

@ -2,7 +2,7 @@
"tasks": [ "tasks": [
{ {
"type": "shell", "type": "shell",
"command": "cd build; cmake ../ -DPICO_BOARD=pico_w; make", "command": "cd build; cmake ../ -DPICO_BOARD=pico_w -DCMAKE_BUILD_TYPE=Release; make",
"label": "CMake in build/", "label": "CMake in build/",
"problemMatcher": [], "problemMatcher": [],
"group": { "group": {
@ -12,7 +12,7 @@
}, },
{ {
"type": "shell", "type": "shell",
"command": "cd build; cmake ../ -DPICO_BOARD=pico_w; make Flash", "command": "cd build; cmake ../ -DPICO_BOARD=pico_w -DCMAKE_BUILD_TYPE=Release; make Flash",
"label": "CMake & Make & Flash", "label": "CMake & Make & Flash",
"problemMatcher": [], "problemMatcher": [],
"group": { "group": {

1
CMakeLists.txt
View File

@ -20,6 +20,7 @@ add_executable(Mon_Projet
Localisation.c Localisation.c
main.c main.c
QEI.c QEI.c
Strategie_PAMI.c
Teleplot.c Teleplot.c
Temps.c Temps.c
Trajectoire_bezier.c Trajectoire_bezier.c

116
Strategie_PAMI.c Normal file
View File

@ -0,0 +1,116 @@
#include "Localisation.h"
#include "Strategie_PAMI.h"
#include "Geometrie.h"
#include "Trajet.h"
#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0
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);
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);
Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
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;
case 7:
Trajet_config(200, 200);
Localisation_set(100, 0, M_PI/2.);
Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
break;
default:
break;
}
break;
case COULEUR_BLEU:
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);
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);
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;
case 7:
Trajet_config(200, 200);
Localisation_set(100, 0, M_PI/2.);
Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
break;
default:
break;
}
break;
}
Trajet_debut_trajectoire(trajectoire);
}

3
Strategie_PAMI.h Normal file
View File

@ -0,0 +1,3 @@
#include "Trajectoire.h"
void configure_trajet(int identifiant, int couleur);

1
VL53L8CX_ULD_API/src/vl53l8cx_api.c
View File

@ -12,6 +12,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <stdio.h>
#include "vl53l8cx_api.h" #include "vl53l8cx_api.h"
#include "vl53l8cx_buffers.h" #include "vl53l8cx_buffers.h"

112
main.c
View File

@ -13,6 +13,7 @@
#include "i2c_maitre.h" #include "i2c_maitre.h"
#include "Localisation.h" #include "Localisation.h"
#include "Moteurs.h" #include "Moteurs.h"
#include "Strategie_PAMI.h"
#include "Teleplot.h" #include "Teleplot.h"
#include "Temps.h" #include "Temps.h"
#include "Trajectoire.h" #include "Trajectoire.h"
@ -27,9 +28,6 @@
#define TIRETTE_PIN 6 #define TIRETTE_PIN 6
#define COULEUR_PIN 4 #define COULEUR_PIN 4
#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0
void affichage(void); void affichage(void);
void affichage_udp(void); void affichage_udp(void);
void gestion_affichage(void); void gestion_affichage(void);
@ -119,9 +117,11 @@ void main(void)
if(temps_ms != Temps_get_temps_ms()){ if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms(); temps_ms = Temps_get_temps_ms();
if(temps_ms % step_ms == 0){ if(temps_ms % step_ms == 0){
// Fonctions cycliques
QEI_update(); QEI_update();
Localisation_gestion(); Localisation_gestion();
// Stratégie
if(etat_trajet != TRAJET_TERMINE){ if(etat_trajet != TRAJET_TERMINE){
etat_trajet = Trajet_avance((float)step_ms/1000.); etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{ }else{
@ -180,15 +180,16 @@ void affichage_udp(void){
Teleplot_add_variable_float_2decimal("con_x", point.point_xy.x); Teleplot_add_variable_float_2decimal("con_x", point.point_xy.x);
Teleplot_add_variable_float_2decimal("con_y", point.point_xy.y); Teleplot_add_variable_float_2decimal("con_y", point.point_xy.y);
Teleplot_add_variable_float_2decimal("con_angle", point.orientation); Teleplot_add_variable_float_2decimal("con_angle", atan2f(delta_y_mm, delta_x_mm));
Teleplot_add_variable_float_2decimal("m1", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms)); Teleplot_add_variable_float_2decimal("m1", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms));
Teleplot_add_variable_float_2decimal("m2", AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms)); Teleplot_add_variable_float_2decimal("m2", AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms));
Teleplot_add_variable_float_2decimal("m1_c", AsserMoteur_getConsigne_mm_s(MOTEUR_A)); Teleplot_add_variable_float_2decimal("m1_c", AsserMoteur_getConsigne_mm_s(MOTEUR_A));
Teleplot_add_variable_float_2decimal("m2_c", AsserMoteur_getConsigne_mm_s(MOTEUR_B)); Teleplot_add_variable_float_2decimal("m2_c", AsserMoteur_getConsigne_mm_s(MOTEUR_B));
Teleplot_add_variable_int("id", identifiant_lire());
Teleplot_envoie_tampon(); Teleplot_envoie_tampon();
} }
@ -265,104 +266,3 @@ int get_couleur(void){
uint identifiant_lire(){ uint identifiant_lire(){
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
} }
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);
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);
Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
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;
case 7:
break;
default:
break;
}
break;
case COULEUR_BLEU:
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);
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);
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;
case 7:
break;
default:
break;
}
break;
}
Trajet_debut_trajectoire(trajectoire);
}