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

Code de location bien avancé, il y a un soucis sur le gain des QEI !

Browse Source
This commit is contained in:
Samuel 2022-12-01 22:47:51 +01:00
parent a170a48e85
commit 90c40c1fdc
5 changed files with 112 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Asser_Moteurs.c
View File

@ -1,5 +1,6 @@
#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
/*** C'est ici que se fait la conversion en mm
* ***/
@ -43,7 +44,7 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
default: break;
}
distance = (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
distance = QEI_get_mm(qei);
temps = step_ms / 1000.0;
return distance / temps;

12
CMakeLists.txt
View File

@ -9,17 +9,17 @@ pico_sdk_init()
add_executable(test
test.c
APDS_9960.c
Asser_Moteurs.c
spi_nb.c
QEI.c
gyro.c
Temps.c
Servomoteur.c
gyro_L3GD20H.c
gyro_ADXRS453.c
QEI.c
APDS_9960.c
Localisation.c
Moteurs.c
)
Temps.c
Servomoteur.c
spi_nb.c)
pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)

36
Localisation.c Normal file
View File

@ -0,0 +1,36 @@
#include "Localisation.h"
#include "QEI.h"
#include "math.h"
#define DISTANCE_ROUES_CENTRE_MM 84.25
#define RACINE_DE_3 1.73205081
struct position_t position;
void Localisation_init(){
position.x_mm = 0;
position.y_mm = 0;
position.angle_radian = 0;
}
void Localisation_gestion(){
// Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html
double distance_roue_a_mm = QEI_get_mm(QEI_A_NAME);
double distance_roue_b_mm = QEI_get_mm(QEI_B_NAME);
double distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
double delta_x_ref_robot, delta_y_ref_robot;
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;
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
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);
}
struct position_t Localisation_get(void){
return position;
}

8
Localisation.h Normal file
View File

@ -0,0 +1,8 @@
struct position_t{
double x_mm, y_mm;
double angle_radian;
};
struct position_t Localisation_get(void);
void Localisation_gestion();
void Localisation_init();

63
test.c
View File

@ -11,6 +11,7 @@
#include "Moteurs.h"
#include "QEI.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
const uint LED_PIN = 25;
const uint LED_PIN_ROUGE = 28;
@ -23,8 +24,10 @@ const uint LED_PIN_NE_PAS_UTILISER = 22;
int mode_test();
int test_moteurs();
int test_QIE();
int test_QIE_mm();
int test_vitesse_moteur(enum t_moteur moteur);
int test_asser_moteur();
int test_asser_moteur(void);
int test_localisation(void);
int test_avance(void);
int main() {
@ -60,6 +63,7 @@ int main() {
Moteur_Init();
QEI_init();
AsserMoteur_Init();
Localisation_init();
while(mode_test());
@ -123,10 +127,12 @@ int main() {
int mode_test(){
static int iteration = 3;
printf("Appuyez sur une touche pour entrer en mode test :\n");
printf("A - pour asser_moteurs\n");
printf("A - pour asser_moteurs (rotation)\n");
printf("B - pour avance (asser_moteur)\n");
printf("C - pour les codeurs\n");
printf("D - pour les codeurs (somme en mm)\n");
printf("M - pour les moteurs\n");
printf("L - pour la localisation\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
@ -145,11 +151,22 @@ int mode_test(){
case 'c':
while(test_QIE());
break;
case 'D':
case 'd':
while(test_QIE_mm());
break;
case 'M':
case 'm':
/* code */
while(test_moteurs());
break;
case 'L':
case 'l':
/* code */
while(test_localisation());
break;
case PICO_ERROR_TIMEOUT:
iteration--;
if(iteration == 0){
@ -224,7 +241,10 @@ int test_QIE(){
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
do{
QEI_update();
printf("Codeur A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n",
QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
sleep_ms(100);
lettre = getchar_timeout_us(0);
@ -233,6 +253,43 @@ int test_QIE(){
}
int test_QIE_mm(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
double a_mm=0, b_mm=0, c_mm=0;
do{
QEI_update();
a_mm += QEI_get_mm(QEI_A_NAME);
b_mm += QEI_get_mm(QEI_B_NAME);
c_mm += QEI_get_mm(QEI_C_NAME);
printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_localisation(){
int lettre;
struct position_t position;
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
do{
QEI_update();
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);
return 0;
}
int test_moteurs(){
int lettre_moteur;