Test unitaires : ajout du gyroscope et de la localisation
This commit is contained in:
parent
97a87dccf3
commit
79d51fbc56
@ -8,6 +8,8 @@
|
||||
struct position_t position;
|
||||
|
||||
void Localisation_init(){
|
||||
Temps_init();
|
||||
QEI_init();
|
||||
position.x_mm = 0;
|
||||
position.y_mm = 0;
|
||||
position.angle_radian = 0;
|
||||
|
8
Temps.c
8
Temps.c
@ -3,6 +3,7 @@
|
||||
#include "Temps.h"
|
||||
|
||||
uint32_t temps_ms=0;
|
||||
bool temps_est_init=false;
|
||||
struct repeating_timer timer;
|
||||
|
||||
bool Temps_increment(struct repeating_timer *t){
|
||||
@ -11,8 +12,11 @@ bool Temps_increment(struct repeating_timer *t){
|
||||
}
|
||||
|
||||
void Temps_init(void){
|
||||
temps_ms=0;
|
||||
add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
|
||||
if(!temps_est_init){
|
||||
temps_ms=0;
|
||||
add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
|
||||
temps_est_init = true;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t Temps_get_temps_ms(void){
|
||||
|
115
Test.c
115
Test.c
@ -76,7 +76,6 @@ int mode_test(){
|
||||
printf("P - Asser Position - perturbation\n");
|
||||
printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
|
||||
printf("R - Test des logs...\n");
|
||||
printf("S - Test du gyroscope...\n");
|
||||
printf("T - Trajectoire\n");
|
||||
printf("U - Tests i2c...\n");
|
||||
printf("V - APDS_9960\n");
|
||||
@ -156,11 +155,6 @@ int mode_test(){
|
||||
case 'r':
|
||||
while(test_log());
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
case 's':
|
||||
while(test_gyro());
|
||||
break;
|
||||
|
||||
case 'T':
|
||||
case 't':
|
||||
@ -261,16 +255,6 @@ int test_APDS9960(){
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
void test_trajectoire_printf(){
|
||||
struct position_t _position;
|
||||
while(1){
|
||||
_position = Trajet_get_consigne();
|
||||
printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void test_trajectoire_teleplot(){
|
||||
struct position_t _position, _consigne;
|
||||
_consigne = Trajet_get_consigne();
|
||||
@ -791,107 +775,8 @@ int test_asser_position_avance(){
|
||||
}
|
||||
|
||||
|
||||
void affiche_localisation(){
|
||||
struct position_t position;
|
||||
while(1){
|
||||
position = Localisation_get();
|
||||
printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
|
||||
printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int test_localisation(){
|
||||
int lettre;
|
||||
int moteurs = 0;
|
||||
struct position_t position;
|
||||
uint32_t temps_ms;
|
||||
uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
|
||||
uint32_t m_gyro = 0;
|
||||
int16_t vitesse;
|
||||
int propulseur=0;
|
||||
|
||||
Temps_init();
|
||||
QEI_init();
|
||||
Localisation_init();
|
||||
|
||||
printf("A - Sans gyroscope\n");
|
||||
printf("B - Avec Gyroscope\n");
|
||||
printf("C - Avec Gyroscope + moteurs\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 'c':
|
||||
case 'C':
|
||||
moteurs = 1;
|
||||
i2c_maitre_init();
|
||||
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);
|
||||
|
||||
vitesse = (int16_t) 32766.0;
|
||||
|
||||
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
|
||||
do{
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
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();
|
||||
if(moteurs){
|
||||
tempo_moteur++;
|
||||
if(tempo_moteur > 1000){
|
||||
tempo_moteur = 0;
|
||||
vitesse = -vitesse;
|
||||
Moteur_SetVitesse(MOTEUR_A ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_B ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_C ,vitesse);
|
||||
if(propulseur){
|
||||
i2c_annexe_active_propulseur();
|
||||
}else{
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}
|
||||
propulseur = !propulseur;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
lettre = getchar_timeout_us(0);
|
||||
|
||||
temps_ms += _step_ms;
|
||||
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
|
||||
|
||||
multicore_reset_core1();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int test_geometrie(){
|
||||
float angle = 270, angle_min, angle_max;
|
||||
printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
|
||||
|
@ -50,6 +50,7 @@ int test_gyro_vitesse_brute(void){
|
||||
printf("Lecture vitesse brute\n");
|
||||
uint16_t tampon_envoi[4];
|
||||
uint8_t tampon_reception[4];
|
||||
char lettre;
|
||||
char message[LOG_MAX_MESSAGE_SIZE];
|
||||
Log_init();
|
||||
Gyro_init_spi();
|
||||
@ -62,7 +63,12 @@ int test_gyro_vitesse_brute(void){
|
||||
gyro_get_vitesse_brute(&angle_gyro, NULL);
|
||||
if(time_us_32() % 100000){
|
||||
sprintf(message,">angle:%d\n", angle_gyro.rot_z);
|
||||
Log_message(message, TELEPLOT);
|
||||
//Log_message(message, TELEPLOT);
|
||||
printf("%s", message);
|
||||
lettre = getchar_timeout_us(0);
|
||||
if(lettre != PICO_ERROR_TIMEOUT){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -4,6 +4,15 @@
|
||||
#include "QEI.h"
|
||||
#include "Moteurs.h"
|
||||
#include "Asser_Moteurs.h"
|
||||
#include "Localisation.h"
|
||||
#include "i2c_maitre.h"
|
||||
#include "i2c_annexe.h"
|
||||
#include "Robot_config.h"
|
||||
#include "gyro.h"
|
||||
#include "Temps.h"
|
||||
|
||||
#include "Test_gyro.h"
|
||||
|
||||
|
||||
#define TEST_TIMEOUT_US 10000000
|
||||
|
||||
@ -12,6 +21,8 @@ int test_QIE_mm(void);
|
||||
int test_asser_moteur(void);
|
||||
int test_vitesse_moteur(enum t_moteur moteur);
|
||||
int test_moteurs(void);
|
||||
void affiche_localisation();
|
||||
int test_localisation();
|
||||
|
||||
|
||||
// Mode test : renvoie 0 pour quitter le mode test
|
||||
@ -22,6 +33,8 @@ int mode_test_unitaire(){
|
||||
printf("B - pour les codeurs (somme en mm)\n");
|
||||
printf("C - pour les moteurs\n");
|
||||
printf("D - pour asser_moteurs (rotation)\n");
|
||||
printf("E - pour le gyroscope\n");
|
||||
printf("F - pour la localisation\n");
|
||||
|
||||
|
||||
stdio_flush();
|
||||
@ -48,6 +61,17 @@ int mode_test_unitaire(){
|
||||
while(test_asser_moteur());
|
||||
break;
|
||||
|
||||
case 'E':
|
||||
case 'e':
|
||||
while(test_gyro());
|
||||
break;
|
||||
|
||||
|
||||
case 'F':
|
||||
case 'f':
|
||||
while(test_localisation());
|
||||
break;
|
||||
|
||||
case PICO_ERROR_TIMEOUT:
|
||||
iteration--;
|
||||
if(iteration == 0){
|
||||
@ -226,3 +250,102 @@ int test_asser_moteur(){
|
||||
return 0;
|
||||
}
|
||||
|
||||
void affiche_localisation(){
|
||||
struct position_t position;
|
||||
while(1){
|
||||
position = Localisation_get();
|
||||
printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
|
||||
printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int test_localisation(){
|
||||
int lettre;
|
||||
int moteurs = 0;
|
||||
struct position_t position;
|
||||
uint32_t temps_ms;
|
||||
uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
|
||||
uint32_t m_gyro = 0;
|
||||
int16_t vitesse;
|
||||
int propulseur=0;
|
||||
|
||||
|
||||
Localisation_init();
|
||||
|
||||
printf("A - Sans gyroscope\n");
|
||||
printf("B - Avec Gyroscope\n");
|
||||
printf("C - Avec Gyroscope + moteurs\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 'c':
|
||||
case 'C':
|
||||
moteurs = 1;
|
||||
i2c_maitre_init();
|
||||
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);
|
||||
|
||||
vitesse = (int16_t) 32766.0;
|
||||
|
||||
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
|
||||
do{
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
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();
|
||||
if(moteurs){
|
||||
tempo_moteur++;
|
||||
if(tempo_moteur > 1000){
|
||||
tempo_moteur = 0;
|
||||
vitesse = -vitesse;
|
||||
Moteur_SetVitesse(MOTEUR_A ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_B ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_C ,vitesse);
|
||||
if(propulseur){
|
||||
i2c_annexe_active_propulseur();
|
||||
}else{
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}
|
||||
propulseur = !propulseur;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
lettre = getchar_timeout_us(0);
|
||||
|
||||
temps_ms += _step_ms;
|
||||
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
|
||||
|
||||
multicore_reset_core1();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
Loading…
Reference in New Issue
Block a user