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

Trajectoires avec contrôle de la vitesse

Browse Source
This commit is contained in:
Samuel 2023-02-19 17:56:45 +01:00
parent 3437718548
commit ad9de0d3c0
12 changed files with 267 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build/

5
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,5 @@
{
"files.associations": {
"timer.h": "c"
}
}

4
Localisation.c
View File

@ -21,6 +21,8 @@ void Localisation_gestion(){
double distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
double delta_x_ref_robot, delta_y_ref_robot;
double old_orientation_radian = position.angle_radian;
delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0;
delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0;
@ -31,7 +33,7 @@ void Localisation_gestion(){
position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM);
}
// Projection dans le référentiel du robot
// Projection dans le référentiel de la table
position.x_mm += delta_x_ref_robot * cos(position.angle_radian) - delta_y_ref_robot * sin(position.angle_radian);
position.y_mm += delta_x_ref_robot * sin(position.angle_radian) + delta_y_ref_robot * cos(position.angle_radian);

6
Moteurs.c
View File

@ -100,4 +100,10 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
break;
}
}
void Moteur_Stop(void){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
}

1
Moteurs.h
View File

@ -11,3 +11,4 @@ enum t_moteur {
void Moteur_Init(void);
void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse );
void Moteur_Stop(void);

178
Test.c
View File

@ -46,6 +46,8 @@ void affiche_localisation(void);
int test_i2c_lecture_pico_annex();
int test_i2c_lecture_pico_annex_nb();
int test_i2c_lecture_pico_annex_nb2();
int test_aller_retour();
void test_trajectoire_teleplot();
// Mode test : renvoie 0 pour quitter le mode test
@ -62,8 +64,9 @@ int mode_test(){
printf("H - Asser Position - avance\n");
printf("I - Asser Position - avance et tourne (gyro)\n");
printf("J - Asser Position - avance et tourne (sans gyro)\n");
printf("M - pour les moteurs\n");
printf("K - Trajets aller retour avec Gyro\n");
printf("L - pour la localisation\n");
printf("M - pour les moteurs\n");
printf("T - Trajectoire\n");
printf("U - Scan du bus i2c\n");
printf("V - APDS_9960\n");
@ -123,15 +126,21 @@ int mode_test(){
while(test_asser_position_avance_et_tourne(0));
break;
case 'M':
case 'm':
while(test_moteurs());
case 'K':
case 'k':
while(test_aller_retour());
break;
case 'L':
case 'l':
while(test_localisation());
break;
case 'M':
case 'm':
while(test_moteurs());
break;
case 'T':
case 't':
while(test_trajectoire());
@ -434,6 +443,104 @@ void test_trajectoire_printf(){
}
void test_trajectoire_teleplot(){
struct position_t _position, _consigne;
_consigne = Trajet_get_consigne();
while(1){
_consigne = Trajet_get_consigne();
_position = Localisation_get();
uint32_t temps;
temps = time_us_32()/1000;
printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180);
printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180);
printf(">Position:%f:%f|xy\n>Consigne_Position:%f:%f|xy\n", _position.x_mm, _position.y_mm, _consigne.x_mm, _consigne.y_mm);
}
}
int test_aller_retour(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
printf("E - Avance et tourne\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0);
printf("Trajectoire de Bézier\n");
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 350, -90, 90, 350);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700);
printf("Trajectoire droite\n");
break;
case 'e':
case 'E':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700);
trajectoire.orientation_debut_rad = 0;
trajectoire.orientation_fin_rad = -M_PI;
printf("Trajectoire droite avec rotation\n");
break;
default: return 0;
}
printf("Init gyroscope\n");
Gyro_Init();
//printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(1);
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
do{
// Routines à 1 ms
QEI_update();
Localisation_gestion();
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Trajectoire_inverse(&trajectoire);
Trajet_debut_trajectoire(trajectoire);
}else{
AsserMoteur_Gestion(_step_ms);
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
//lettre = PICO_ERROR_TIMEOUT;
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
printf("Lettre : %d; %c\n", lettre, lettre);
Moteur_Stop();
return 0;
}
int test_trajectoire(){
int lettre, _step_ms = 1, temps_ms=0;
Trajet_init();
@ -450,23 +557,28 @@ int test_trajectoire(){
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0);
printf("Trajectoire Bezier\n");
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700);
printf("Trajectoire droite\n");
break;
default: return 0;
}
sleep_ms(3000);
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_printf);
multicore_launch_core1(test_trajectoire_teleplot);
do{
// Routines à 1 ms
QEI_update();
@ -482,6 +594,7 @@ int test_trajectoire(){
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
lettre = PICO_ERROR_TIMEOUT;
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
@ -501,9 +614,11 @@ int test_asser_position_avance_et_tourne(int m_gyro){
position_consigne.y_mm = 0;
printf("Le robot avance à 100 mm/s\n");
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
if(m_gyro){
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
}
stdio_flush();
set_position_avec_gyroscope(m_gyro);
@ -731,17 +846,58 @@ int test_QIE_mm(){
int test_localisation(){
int lettre;
struct position_t position;
uint32_t temps_ms;
uint32_t _step_ms_gyro = 2, _step_ms=1;
uint32_t m_gyro = 0;
printf("A - Sans gyroscope\n");
printf("B - Avec Gyroscope\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'A':
case 'a':
set_position_avec_gyroscope(0);
printf("Sans gyroscope\n");
break;
case 'B':
case 'b':
set_position_avec_gyroscope(1);
printf("Avec gyroscope, initialisation...\n");
m_gyro=1;
Gyro_Init();
break;
default:
return 0;
}
temps_ms = Temps_get_temps_ms();
multicore_launch_core1(affiche_localisation);
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
do{
while(temps_ms == Temps_get_temps_ms());
QEI_update();
if(m_gyro){
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
}
Localisation_gestion();
position = Localisation_get();
printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
temps_ms += _step_ms;
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
multicore_reset_core1();
return 0;

56
Trajectoire.c
View File

@ -17,6 +17,8 @@ void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x,
trajectoire->angle_fin_degre = angle_fin_degre;
trajectoire->rayon = rayon;
trajectoire->longueur = -1;
trajectoire->orientation_debut_rad = 0;
trajectoire->orientation_fin_rad = 0;
}
void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y){
@ -26,6 +28,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double
trajectoire->p2.x = p2_x;
trajectoire->p2.y = p2_y;
trajectoire->longueur = -1;
trajectoire->orientation_debut_rad = 0;
trajectoire->orientation_fin_rad = 0;
}
void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y){
@ -39,6 +43,36 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double
trajectoire->p4.x = p4_x;
trajectoire->p4.y = p4_y;
trajectoire->longueur = -1;
trajectoire->orientation_debut_rad = 0;
trajectoire->orientation_fin_rad = 0;
}
void Trajectoire_inverse(struct trajectoire_t * trajectoire){
struct trajectoire_t old_trajectoire;
old_trajectoire = *trajectoire;
trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad;
trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
return;
}
if(trajectoire->type == TRAJECTOIRE_DROITE){
trajectoire->p1 = old_trajectoire.p2;
trajectoire->p2 = old_trajectoire.p1;
return;
}
if(trajectoire->type == TRAJECTOIRE_BEZIER){
trajectoire->p1 = old_trajectoire.p4;
trajectoire->p2 = old_trajectoire.p3;
trajectoire->p3 = old_trajectoire.p2;
trajectoire->p4 = old_trajectoire.p1;
}
}
/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
@ -68,21 +102,29 @@ double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
/// @brief Renvoie le point d'une trajectoire à partir de son abscisse
/// @param abscisse : abscisse sur la trajectoire
/// @return point en coordonnées X/Y
struct point_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xyo_t point_xyo;
switch(trajectoire->type){
case TRAJECTOIRE_DROITE:
return Trajectoire_droite_get_point(trajectoire, abscisse);
point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse);
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
break;
case TRAJECTOIRE_CIRCULAIRE:
return Trajectoire_circulaire_get_point(trajectoire, abscisse);
point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse);
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
break;
case TRAJECTOIRE_BEZIER:
return Trajectoire_bezier_get_point(trajectoire, abscisse);
point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
break;
}
return point_xyo;
}
double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse){
return (double) trajectoire->orientation_debut_rad * (1-abscisse) + (double) trajectoire->orientation_fin_rad * abscisse;
}
/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée
@ -97,14 +139,14 @@ double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, d
}
// Ceci permet d'avoir une abscisse exact sur les trajectoires droites et les trajectoires circulaires
delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire);
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) );
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
// Sur les trajectoires de bézier, il peut être nécessaire d'affiner
// Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire.
erreur_relative = 1 - delta_mm / distance_mm;
while(fabs(erreur_relative) > PRECISION_ABSCISSE){
delta_abscisse = delta_abscisse * distance_mm / delta_mm;
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) );
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
erreur_relative = 1 - delta_mm / distance_mm;
}

11
Trajectoire.h
View File

@ -11,20 +11,27 @@ struct point_xy_t{
double x, y;
};
struct point_xyo_t{
struct point_xy_t point_xy;
double orientation;
};
struct trajectoire_t {
enum trajectoire_type_t type;
struct point_xy_t p1, p2, p3, p4;
double orientation_debut, orientation_fin;
double orientation_debut_rad, orientation_fin_rad;
double rayon, angle_debut_degre, angle_fin_degre;
double longueur;
};
double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
struct point_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse);
double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
double distance_points(struct point_xy_t point, struct point_xy_t point_old);
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon);
void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y);
void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y);
void Trajectoire_inverse(struct trajectoire_t * trajectoire);
#endif

22
Trajet.c
View File

@ -25,14 +25,15 @@ void Trajet_init(){
void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
abscisse = 0;
vitesse_mm_s = 0;
acceleration_mm_ss = 1500;
acceleration_mm_ss = 500;
position_mm = 0;
trajet_trajectoire = trajectoire;
}
int Trajet_avance(double pas_de_temps_s){
double distance_mm, orientation_radian;
struct point_xy_t point;
enum trajet_etat_t Trajet_avance(double pas_de_temps_s){
double distance_mm;
enum trajet_etat_t trajet_etat = TRAJET_EN_COURS;
struct point_xyo_t point;
struct position_t position;
// Calcul de la vitesse
@ -48,20 +49,17 @@ int Trajet_avance(double pas_de_temps_s){
// Obtention du point consigne
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
// Obtention de l'orientation consigne
orientation_radian = 0; // TODO
position.x_mm = point.x;
position.y_mm = point.y;
position.angle_radian = orientation_radian;
position.x_mm = point.point_xy.x;
position.y_mm = point.point_xy.y;
position.angle_radian = point.orientation;
position_consigne=position;
Asser_Position(position);
if(abscisse >= 1 ){
return TRAJET_TERMINE;
trajet_etat = TRAJET_TERMINE;
}
return TRAJET_EN_COURS;
return trajet_etat;
}

10
Trajet.h
View File

@ -1,8 +1,12 @@
#define TRAJET_EN_COURS 0
#define TRAJET_TERMINE 1
#include "Trajectoire.h"
enum trajet_etat_t{
TRAJET_EN_COURS,
TRAJET_TERMINE
};
void Trajet_init();
void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
int Trajet_avance(double temps_s);
enum trajet_etat_t Trajet_avance(double temps_s);
struct position_t Trajet_get_consigne(void);

6
gyro.c
View File

@ -60,7 +60,7 @@ void Gyro_Init(void){
//uint speed = spi_init(spi0, 10 * 1000); // SPI init @ 10 kHz
uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
printf("vitesse SPI : %d\n", speed);
//printf("vitesse SPI : %d\n", speed);
spi_set_format(spi0, 8, SPI_CPHA_0, SPI_CPOL_0, SPI_MSB_FIRST);
@ -70,9 +70,9 @@ void Gyro_Init(void){
puts("Gyroscope non trouve");
while(1); // On s'arrête là !
}else{
puts("Gyroscope trouve");
//puts("Gyroscope trouve");
if(!gyro_config()){
puts("gyro_config ok !");
//puts("gyro_config ok !");
}else{
puts("gyro_config FAILED !");

12
gyro_ADXRS453.c
View File

@ -76,7 +76,7 @@ int gyro_init_check(){
// On suit les instructions de la page 20 de la fiche technique
sleep_ms(100); // init du gyro - On ignore la réponse
printf("T=100ms\n");
//printf("T=100ms\n");
tampon_envoi[0] = 0x30;
tampon_envoi[1] = 0x00;
tampon_envoi[2] = 0x00;
@ -88,7 +88,7 @@ int gyro_init_check(){
sleep_ms(50); // t=150ms - On ignore, les données ne sont pas actualisées
printf("T=150ms\n");
//printf("T=150ms\n");
tampon_envoi[0] = 0x30;
tampon_envoi[1] = 0x00;
tampon_envoi[2] = 0x00;
@ -103,7 +103,7 @@ int gyro_init_check(){
affiche_tampon_32bits(tampon_reception);
sleep_ms(50); // t=200ms - En cours d'autotest
printf("T=200ms\n");
//printf("T=200ms\n");
tampon_envoi[0] = 0x30;
tampon_envoi[1] = 0x00;
tampon_envoi[2] = 0x00;
@ -118,7 +118,7 @@ int gyro_init_check(){
affiche_tampon_32bits(tampon_reception);
sleep_us(1); // t=200ms + TD - résultats de 200ms + TD, en cours d'autotest.
printf("T=200ms+TD\n");
//printf("T=200ms+TD\n");
tampon_envoi[0] = 0x30;
tampon_envoi[1] = 0x00;
tampon_envoi[2] = 0x00;
@ -130,10 +130,10 @@ int gyro_init_check(){
printf("Gyro_Init - SQ bits (%#01x)!= 0x4", Gyro_SensorData.SQ);
return 1;
}
affiche_tampon_32bits(tampon_reception);
//affiche_tampon_32bits(tampon_reception);
sleep_us(1); // t=200ms + 2TD - doit être nominal
printf("T=200ms+2TD\n");
//printf("T=200ms+2TD\n");
tampon_envoi[0] = 0x00;
tampon_envoi[1] = 0x00;
tampon_envoi[2] = 0x00;