Ajout des test avec perturbations crées par les moteurs des propulseurs
This commit is contained in:
parent
f5711b7a03
commit
498c4d8e3c
147
Test.c
147
Test.c
@ -48,7 +48,8 @@ int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
|
||||
int test_cde_vitesse_rectangle(void);
|
||||
int test_cde_vitesse_cercle(void);
|
||||
int test_asser_position_avance(void);
|
||||
int test_asser_position_avance_et_tourne(int);
|
||||
int test_asser_position_avance_et_tourne(int, int);
|
||||
int test_transition_gyro_pas_gyro(void);
|
||||
int test_trajectoire(void);
|
||||
int test_i2c_bus(void);
|
||||
void affiche_localisation(void);
|
||||
@ -83,6 +84,8 @@ int mode_test(){
|
||||
printf("M - pour les moteurs\n");
|
||||
printf("N - Fonctions geometrique\n");
|
||||
printf("O - Analyse obstacle\n");
|
||||
printf("P - Asser Position - perturbation\n");
|
||||
printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
|
||||
printf("T - Trajectoire\n");
|
||||
printf("U - Scan du bus i2c\n");
|
||||
printf("V - APDS_9960\n");
|
||||
@ -136,12 +139,12 @@ int mode_test(){
|
||||
|
||||
case 'I':
|
||||
case 'i':
|
||||
while(test_asser_position_avance_et_tourne(1));
|
||||
while(test_asser_position_avance_et_tourne(1, 0));
|
||||
break;
|
||||
|
||||
case 'J':
|
||||
case 'j':
|
||||
while(test_asser_position_avance_et_tourne(0));
|
||||
while(test_asser_position_avance_et_tourne(0, 0));
|
||||
break;
|
||||
|
||||
case 'K':
|
||||
@ -169,6 +172,16 @@ int mode_test(){
|
||||
while(test_angle_balise());
|
||||
break;
|
||||
|
||||
case 'P':
|
||||
case 'p':
|
||||
while(test_asser_position_avance_et_tourne(1, 1));
|
||||
break;
|
||||
|
||||
case 'Q':
|
||||
case 'q':
|
||||
while(test_transition_gyro_pas_gyro());
|
||||
break;
|
||||
|
||||
case 'T':
|
||||
case 't':
|
||||
while(test_trajectoire());
|
||||
@ -915,12 +928,73 @@ int test_trajectoire(){
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
|
||||
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
|
||||
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
|
||||
/// @return
|
||||
int test_asser_position_avance_et_tourne(int m_gyro){
|
||||
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2;
|
||||
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old;
|
||||
int test_transition_gyro_pas_gyro(void){
|
||||
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
|
||||
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
|
||||
struct position_t position_consigne;
|
||||
|
||||
position_consigne.angle_radian = 0;
|
||||
position_consigne.x_mm = 0;
|
||||
position_consigne.y_mm = 0;
|
||||
|
||||
printf("Le robot tourne sur lui-même, transition sans gyro @ t=5s\n");
|
||||
|
||||
printf("Init gyroscope\n");
|
||||
Gyro_Init();
|
||||
printf("C'est parti !\n");
|
||||
stdio_flush();
|
||||
|
||||
set_position_avec_gyroscope(1);
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_old = temps_ms;
|
||||
temps_ms_init = temps_ms;
|
||||
|
||||
multicore_launch_core1(affiche_localisation);
|
||||
do{
|
||||
|
||||
while(temps_ms == Temps_get_temps_ms());
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_old = temps_ms;
|
||||
|
||||
QEI_update();
|
||||
if(get_position_avec_gyroscope()){
|
||||
if(temps_ms % _step_ms_gyro == 0){
|
||||
Gyro_Read(_step_ms_gyro);
|
||||
}
|
||||
}
|
||||
|
||||
if(temps_ms - temps_ms_init > 5000){
|
||||
set_position_avec_gyroscope(0);
|
||||
}
|
||||
Localisation_gestion();
|
||||
AsserMoteur_Gestion(_step_ms);
|
||||
|
||||
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
|
||||
|
||||
Asser_Position(position_consigne);
|
||||
|
||||
lettre = getchar_timeout_us(0);
|
||||
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
|
||||
|
||||
printf("lettre : %c, %d\n", lettre, lettre);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
|
||||
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
|
||||
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
|
||||
/// @return
|
||||
int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
|
||||
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
|
||||
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
|
||||
struct position_t position_consigne;
|
||||
|
||||
position_consigne.angle_radian = 0;
|
||||
@ -933,6 +1007,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){
|
||||
Gyro_Init();
|
||||
printf("C'est parti !\n");
|
||||
}
|
||||
if(propulseur){
|
||||
i2c_maitre_init();
|
||||
}
|
||||
stdio_flush();
|
||||
|
||||
set_position_avec_gyroscope(m_gyro);
|
||||
@ -942,7 +1019,26 @@ int test_asser_position_avance_et_tourne(int m_gyro){
|
||||
|
||||
multicore_launch_core1(affiche_localisation);
|
||||
do{
|
||||
while(temps_ms == Temps_get_temps_ms());
|
||||
|
||||
while(temps_ms == Temps_get_temps_ms()){
|
||||
if(propulseur){
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
}
|
||||
}
|
||||
if(propulseur){
|
||||
tempo_prop++;
|
||||
if(tempo_prop>1000){
|
||||
tempo_prop=0;
|
||||
if(propulseur_on){
|
||||
i2c_annexe_active_propulseur();
|
||||
}else{
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}
|
||||
propulseur_on = !propulseur_on;
|
||||
}
|
||||
}
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_old = temps_ms;
|
||||
|
||||
@ -954,7 +1050,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){
|
||||
AsserMoteur_Gestion(_step_ms);
|
||||
|
||||
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
|
||||
if(!propulseur){
|
||||
position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.;
|
||||
}
|
||||
|
||||
Asser_Position(position_consigne);
|
||||
|
||||
@ -1147,8 +1245,8 @@ void affiche_localisation(){
|
||||
struct position_t position;
|
||||
while(1){
|
||||
position = Localisation_get();
|
||||
printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
@ -1222,13 +1320,17 @@ int test_QIE_mm(){
|
||||
|
||||
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;
|
||||
uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
|
||||
uint32_t m_gyro = 0;
|
||||
int16_t vitesse;
|
||||
int propulseur=0;
|
||||
|
||||
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();
|
||||
@ -1240,6 +1342,10 @@ int test_localisation(){
|
||||
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);
|
||||
@ -1255,10 +1361,12 @@ int test_localisation(){
|
||||
|
||||
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){
|
||||
@ -1268,6 +1376,23 @@ int test_localisation(){
|
||||
}
|
||||
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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user