Trajectoire circulaire
This commit is contained in:
parent
e4af4367c1
commit
29d32ff7fa
4
.vscode/tasks.json
vendored
4
.vscode/tasks.json
vendored
@ -2,7 +2,7 @@
|
||||
"tasks": [
|
||||
{
|
||||
"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/",
|
||||
"problemMatcher": [],
|
||||
"group": {
|
||||
@ -12,7 +12,7 @@
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"problemMatcher": [],
|
||||
"group": {
|
||||
|
@ -20,6 +20,7 @@ add_executable(Mon_Projet
|
||||
Localisation.c
|
||||
main.c
|
||||
QEI.c
|
||||
Strategie_PAMI.c
|
||||
Teleplot.c
|
||||
Temps.c
|
||||
Trajectoire_bezier.c
|
||||
|
116
Strategie_PAMI.c
Normal file
116
Strategie_PAMI.c
Normal 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
3
Strategie_PAMI.h
Normal file
@ -0,0 +1,3 @@
|
||||
#include "Trajectoire.h"
|
||||
|
||||
void configure_trajet(int identifiant, int couleur);
|
@ -12,6 +12,7 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "vl53l8cx_api.h"
|
||||
#include "vl53l8cx_buffers.h"
|
||||
|
||||
|
112
main.c
112
main.c
@ -13,6 +13,7 @@
|
||||
#include "i2c_maitre.h"
|
||||
#include "Localisation.h"
|
||||
#include "Moteurs.h"
|
||||
#include "Strategie_PAMI.h"
|
||||
#include "Teleplot.h"
|
||||
#include "Temps.h"
|
||||
#include "Trajectoire.h"
|
||||
@ -27,9 +28,6 @@
|
||||
#define TIRETTE_PIN 6
|
||||
#define COULEUR_PIN 4
|
||||
|
||||
#define COULEUR_BLEU 1
|
||||
#define COULEUR_JAUNE 0
|
||||
|
||||
void affichage(void);
|
||||
void affichage_udp(void);
|
||||
void gestion_affichage(void);
|
||||
@ -119,9 +117,11 @@ void main(void)
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
if(temps_ms % step_ms == 0){
|
||||
// Fonctions cycliques
|
||||
QEI_update();
|
||||
Localisation_gestion();
|
||||
|
||||
// Stratégie
|
||||
if(etat_trajet != TRAJET_TERMINE){
|
||||
etat_trajet = Trajet_avance((float)step_ms/1000.);
|
||||
}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_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("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("m2_c", AsserMoteur_getConsigne_mm_s(MOTEUR_B));
|
||||
|
||||
|
||||
Teleplot_add_variable_int("id", identifiant_lire());
|
||||
|
||||
Teleplot_envoie_tampon();
|
||||
}
|
||||
|
||||
@ -265,104 +266,3 @@ int get_couleur(void){
|
||||
uint identifiant_lire(){
|
||||
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);
|
||||
|
||||
}
|
Loading…
Reference in New Issue
Block a user