Corentin Courtot
asser
Dependencies: mbed X_NUCLEO_IHM02A1
Revision 2:5764f89a27f6, committed 2019-04-17
- Comitter:
- GuillaumeCH
- Date:
- Wed Apr 17 18:55:22 2019 +0000
- Parent:
- 1:0690cf83f060
- Child:
- 3:1dba6eca01ad
- Commit message:
- p
Changed in this revision
--- a/X_NUCLEO_IHM02A1.lib Wed Dec 12 20:03:07 2018 +0000 +++ b/X_NUCLEO_IHM02A1.lib Wed Apr 17 18:55:22 2019 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM02A1/#ff67801d7cd7 +https://os.mbed.com/teams/AREM-CDFR-2019/code/asser1/#612e6272cf09
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/commande_moteurs.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -0,0 +1,202 @@
+
+#include "mbed.h"
+
+#include "commande_moteurs.h"
+#include "hardware.h"
+#include "reglages.h"
+#include "math_precalc.h"
+
+#include "odometrie.h"
+
+
+void commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
+
+ int sens_G=signe(vitesse_G);
+ int sens_D=signe(vitesse_D);
+ double vitesse_local_G=abs(vitesse_G);
+ double vitesse_local_D=abs(vitesse_D);
+
+ if(abs(vitesse_G) > 900){
+ vitesse_local_G=900;
+ }
+ if(abs(vitesse_G)<10){
+ vitesse_local_G=10;
+ }
+ if(abs(vitesse_D) > 900){
+ vitesse_local_D=900;
+ }
+ if(abs(vitesse_D)< 10){
+ vitesse_local_D=10;
+
+ }
+ ;
+ int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+ int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+ float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+ float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+ float centieme_D = (VD_f-VD_int)*1000;
+ float centieme_G = (VG_f-VG_int)*1000;
+ if ((rand()%1000) < centieme_G){
+ VG_int+=1;
+ }
+ if ((rand()%1000) < centieme_D){
+ VD_int+=1;
+ }
+ printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
+ set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
+ set_PWM_moteur_D(VG_int);//
+}
+void vitesse_nulle_G(int zero){
+ if(zero == 0){
+ set_PWM_moteur_G(0);
+ }
+}
+void vitesse_nulle_D(int zero){
+ if(zero == 0){
+ set_PWM_moteur_D(0);
+ }
+}
+void reculer_un_peu(int distance){
+ motors_on();
+ long int x_ini = get_x_actuel();
+ long int y_ini = get_y_actuel();
+ double angle_vise_deg = get_angle();
+ double angle_vise=angle_vise_deg*3.142/180;
+ double angle = get_angle();
+
+ long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
+ long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+
+ long int x_actuel = get_x_actuel();
+ long int y_actuel = get_y_actuel();
+ long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+
+ float vitesse_G;
+ float vitesse_D;
+
+ angle = get_angle();
+
+ printf("YOOOO\n\n ");
+ while (distance+x_local>0){
+
+ vitesse_G = (distance+x_local)/70;
+ vitesse_D = vitesse_G;
+ if(vitesse_G >150){
+ vitesse_G=150;
+ vitesse_D=150;
+ }
+ if (vitesse_G<-150){
+ vitesse_G=-150;
+ vitesse_D=-150;
+ }
+
+ angle = get_angle();
+ vitesse_G = vitesse_G - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local; // petit asser en angle
+ vitesse_D = vitesse_D + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local;
+
+ commande_vitesse(-vitesse_G,-vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_y_actuel();
+ x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+ printf(" VG : %f VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+
+ }
+ test_rotation_abs(angle_vise_deg);
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ wait(0.3);
+ motors_stop();
+}
+
+void ligne_droite(long int distance)
+{
+ // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
+ motors_on();
+ long int x_ini = get_x_actuel();
+ long int y_ini = get_y_actuel();
+ double angle_vise_deg = get_angle();
+ double angle_vise=angle_vise_deg*3.142/180;
+ double angle = get_angle();
+
+ long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
+ long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+
+ long int x_actuel = get_x_actuel();
+ long int y_actuel = get_y_actuel();
+ long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+
+ float vitesse_G;
+ float vitesse_D;
+
+ angle = get_angle();
+
+ printf("YOOOO\n\n ");
+ while (distance-x_local>0){
+
+ vitesse_G = (distance-x_local)/70;
+ vitesse_D = vitesse_G;
+ if(vitesse_G >400){
+ vitesse_G=400;
+ vitesse_D=400;
+ }
+ if (vitesse_G<-400){
+ vitesse_G=-400;
+ vitesse_D=-400;
+ }
+
+ angle = get_angle();
+ vitesse_G = vitesse_G + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local; // petit asser en angle
+ vitesse_D = vitesse_D - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local;
+
+ commande_vitesse(vitesse_G,vitesse_D);
+ actualise_position();
+ x_actuel = get_x_actuel();
+ y_actuel = get_y_actuel();
+ x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+ y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+ printf(" VG : %f VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+
+ }
+ test_rotation_abs(angle_vise_deg);
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ wait(0.3);
+ motors_stop();
+}
+
+void test_rotation_rel(double angle_vise)
+{
+ // rotation de angle_vise
+ motors_on();
+ float vitesse;
+ double angle = get_angle();
+ angle_vise+=angle;
+ borne_angle_d(angle_vise);
+
+ while (abs(diff_angle(angle,angle_vise))>0.08)
+ {
+ vitesse=1.3*diff_angle(angle,angle_vise);
+ commande_vitesse(-vitesse,vitesse);
+ actualise_position();
+ angle = get_angle();
+ printf("vitesse : %f", vitesse);
+ }
+
+ //printf(" x et y recu : %lf, %ld. distance parcourue : %ld ", sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+
+ vitesse_nulle_G(0);
+ vitesse_nulle_D(0);
+ wait(0.2);
+ motors_stop();
+}
+
+
+void test_rotation_abs(double angle_vise)
+{
+ double angle_rel = borne_angle_d(angle_vise-get_angle());
+ test_rotation_rel(angle_rel);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/commande_moteurs.h Wed Apr 17 18:55:22 2019 +0000 @@ -0,0 +1,16 @@ +#ifndef TESTS_MOTEURS_H +#define TESTS_MOTEURS_H + +//fonctions de débug et de calibrage du robot + + +//void test_ligne_droite(long int distance, int vitesse); + +void test_rotation_rel(double angle_vise); +void test_rotation_abs(double angle_vise); +void ligne_droite(long int distance); +void commande_vitesse(float vitesse_G, float vitesse_D); +void vitesse_nulle_D(int zero); +void vitesse_nulle_G(int zero); +void reculer_un_peu(int distance); +#endif \ No newline at end of file
--- a/hardware.cpp Wed Dec 12 20:03:07 2018 +0000
+++ b/hardware.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -13,27 +13,27 @@
L6470_init_t init[L6470DAISYCHAINSIZE] = {
/* First Motor. */
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the motor. */
- 1.7, /* Max motor phase voltage in A. */
- 3.06, /* Max motor phase voltage in V. */
- 300.0, /* Motor initial speed [step/s]. */
+ 10, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 4, /* Max motor phase voltage in A. */
+ 7.06, /* Max motor phase voltage in V. */
+ 300, /* Motor initial speed [step/s]. */
500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
1500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
- 3.06, /* Holding kval [V]. */
- 3.06, /* Constant speed kval [V]. */
- 3.06, /* Acceleration starting kval [V]. */
- 3.06, /* Deceleration starting kval [V]. */
+ 4.5, /* Holding kval [V]. */
+ 4.5, /* Constant speed kval [V]. */
+ 4.5, /* Acceleration starting kval [V]. */
+ 4.5, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
@@ -41,27 +41,27 @@
/* Second Motor. */
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the motor. */
- 1.7, /* Max motor phase voltage in A. */
- 3.06, /* Max motor phase voltage in V. */
- 300.0, /* Motor initial speed [step/s]. */
+ 10, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 4, /* Max motor phase voltage in A. */
+ 7.06, /* Max motor phase voltage in V. */
+ 300, /* Motor initial speed [step/s]. */
500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
- 1500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 1500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
- 3.06, /* Holding kval [V]. */
- 3.06, /* Constant speed kval [V]. */
- 3.06, /* Acceleration starting kval [V]. */
- 3.06, /* Deceleration starting kval [V]. */
+ 4.5, /* Holding kval [V]. */
+ 4.5, /* Constant speed kval [V]. */
+ 4.5, /* Acceleration starting kval [V]. */
+ 4.5, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
@@ -76,10 +76,16 @@
//Connections codeuses
-InterruptIn ENCAL(D6);
-InterruptIn ENCAJ(D5);
-InterruptIn ENCBL(D8);
-InterruptIn ENCBJ(D7);
+//Nucleo 401re
+InterruptIn ENCAL(D9);
+InterruptIn ENCAJ(D8);
+InterruptIn ENCBL(D6);
+InterruptIn ENCBJ(D5);
+/*//Nucelo 746zg
+InterruptIn ENCAL(D8);
+InterruptIn ENCAJ(D7);
+InterruptIn ENCBL(D4);
+InterruptIn ENCBJ(D0);*/
volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
@@ -150,6 +156,47 @@
x_nucleo_ihm02a1->perform_prepared_actions();
}
+void set_step_moteur_D(int steps)
+{
+ if (!moteurs_arret) {
+ if (1) {
+ motors[0]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive
+ /*} else if (PWM <-PWM_MAX) {
+ motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX);
+ } else if (PWM > 0) {
+ motors[0]->prepare_run(StepperMotor::BWD, PWM);
+ } else if (PWM < 0) {
+ motors[0]->prepare_run(StepperMotor::FWD, -PWM);
+ } else if (PWM == 0) {
+ motors[0]->prepare_run(StepperMotor::BWD, 0);
+ */}
+ /*} else {
+ motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/
+ }
+ x_nucleo_ihm02a1->perform_prepared_actions();
+}
+/*
+void set_step_moteur_G(int PWM)
+{
+
+ if (!moteurs_arret) {
+ if (PWM > PWM_MAX) {
+ motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX);
+ } else if (PWM <-PWM_MAX) {
+ motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX);
+ } else if (PWM > 0) {
+ motors[1]->prepare_run(StepperMotor::FWD, PWM);
+ } else if (PWM < 0) {
+ motors[1]->prepare_run(StepperMotor::BWD, -PWM);
+ } else if (PWM == 0) {
+ motors[1]->prepare_run(StepperMotor::BWD, 0);
+ }
+ } else {
+ motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main
+ }
+ x_nucleo_ihm02a1->perform_prepared_actions();
+}*/
+
long int get_nbr_tick_D()
{
return encoderValueA;
--- a/hardware.h Wed Dec 12 20:03:07 2018 +0000 +++ b/hardware.h Wed Apr 17 18:55:22 2019 +0000 @@ -5,6 +5,10 @@ void init_hardware(void); void set_PWM_moteur_D(int PWM); void set_PWM_moteur_G(int PWM); + +void set_step_moteur_D(int steps); + + long int get_nbr_tick_D(void); long int get_nbr_tick_G(void); void attente_synchro(void);
--- a/main.cpp Wed Dec 12 20:03:07 2018 +0000
+++ b/main.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -3,39 +3,29 @@
#include "hardware.h"
#include "odometrie.h"
#include "reglages.h"
-#include "tests_moteurs.h"
+#include "commande_moteurs.h"
int main()
{
//init
init_odometrie();
init_hardware();
-
-
+ srand(time(NULL));
+ motors_on();
+ //Pause pour sauver le robot et l'ordi
wait(3);
-
- test_ligne_droite(162000, 600);
- test_rotation_rel(90,100);
- test_ligne_droite(74000,300);
-
- /*while(1)
- {
- for (int i =0; i<4; i++)
- {
- test_ligne_droite(30000,500);
-
- test_rotation_rel(90,100);
+ //ligne_droite(150000);
+ //commande_vitesse(500,500);
+ /*for(int j = 0; j<5;j++){
+ for (int i =0; i< 4;i++){
+ ligne_droite(50000);
+ test_rotation_rel(90);
}
- test_rotation_abs(0,100);
-
- for (int i =0; i<4; i++)
- {
- test_ligne_droite(150000, 600);
- test_rotation_rel(180,100);
- }
- test_rotation_abs(0,100);
+ wait(1);
}*/
-
-
+ reculer_un_peu(50000);
+ /*for (int i = 0; i< 10; i++){
+ test_rotation_rel(180);
+ }*/
return 0;
}
--- a/math_precalc.cpp Wed Dec 12 20:03:07 2018 +0000
+++ b/math_precalc.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -77,3 +77,20 @@
return angle;
}
+int signe(float entier){
+ if(entier >=0){
+ return 1;
+ }
+ if(entier <0){
+ return -1;
+ }
+ return 0;
+}
+float min(float f1, float f2){
+ if (f1-f2>0){
+ return f2;
+ }
+ else{
+ return f1;
+ }
+}
\ No newline at end of file
--- a/math_precalc.h Wed Dec 12 20:03:07 2018 +0000 +++ b/math_precalc.h Wed Apr 17 18:55:22 2019 +0000 @@ -16,6 +16,7 @@ double borne_angle_d(double angle); double borne_angle_r(double angle); - +int signe(float entier); +float min(float f1, float f2); #endif \ No newline at end of file
--- a/mbed.bld Wed Dec 12 20:03:07 2018 +0000 +++ b/mbed.bld Wed Apr 17 18:55:22 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- a/odometrie.cpp Wed Dec 12 20:03:07 2018 +0000
+++ b/odometrie.cpp Wed Apr 17 18:55:22 2019 +0000
@@ -16,7 +16,7 @@
{
x_actuel=X_INIT;
y_actuel=Y_INIT;
- angle=0;
+ angle=THETA_INIT;
nbr_tick_D_prec=0;
nbr_tick_G_prec=0;
}
--- a/reglages.h Wed Dec 12 20:03:07 2018 +0000 +++ b/reglages.h Wed Apr 17 18:55:22 2019 +0000 @@ -4,20 +4,16 @@ #define THETA_INIT 0 //propre a chaque robot -#define ECART_ROUE 31800 // a augmenter si l'angle reel est plus grand que l'angle vise -#define DIAMETRE_ROUE 8410 -#define DISTANCE_PAR_TICK_D 100000/11600 -#define DISTANCE_PAR_TICK_G 100000/11600 +#define ECART_ROUE 32600 // a augmenter si l'angle reel est plus grand que l'angle vise +#define DISTANCE_PAR_TICK_D 100000/11865 +#define DISTANCE_PAR_TICK_G 100000/11865 -//calibrage -#define TICK_PAR_MM 11 // nombre de ticks par mm (tick/mm) -#define TICK_PAR_TOUR 8109 //nombre de ticks par tour complet du robot sur lui-même (tick/tour) + + //correction mécanique -#define COEFF_CODEUR_D 1 -#define COEFF_CODEUR_G 1.00 //1.07 -#define COEFF_MOTEUR_D 1.20 //1.085 -#define COEFF_MOTEUR_G 1 //1.10 +#define COEFF_MOTEUR_D 1.00 //1.085 +#define COEFF_MOTEUR_G 1.00 //1.10 //contraintes mecaniques #define PWM_MAX 900 //PWM maximal, à cette valeur le robot est à sa vitesse maximale admissible
--- a/tests_moteurs.cpp Wed Dec 12 20:03:07 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,96 +0,0 @@
-
-#include "mbed.h"
-
-#include "tests_moteurs.h"
-#include "hardware.h"
-#include "reglages.h"
-#include "math_precalc.h"
-
-#include "odometrie.h"
-
-void test_ligne_droite(long int distance, int vitesse)
-{
- // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
- motors_on();
- long int x_ini = get_x_actuel();
- long int y_ini = get_y_actuel();
-
- long int x_actuel = get_x_actuel();
- long int y_actuel = get_y_actuel();
-
- int vitesse_D = vitesse;
- int vitesse_G = vitesse;
-
- double angle = get_angle();
- double angle_vise = get_angle();
-
-
- while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance) // a revoir : utilisation de sqrt
- {
- angle = get_angle();
- vitesse_G = (int) (vitesse * (1 - 0.05*diff_angle(angle_vise,angle))); // petit asser en angle
- vitesse_D = (int) (vitesse * (1 + 0.05*diff_angle(angle_vise,angle)));
-
- set_PWM_moteur_D(vitesse_D);
- set_PWM_moteur_G(vitesse_G);
- actualise_position();
- x_actuel = get_x_actuel();
- y_actuel = get_y_actuel();
- printf("x recu : %ld", x_actuel);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
-
- }
-
- set_PWM_moteur_D(0);
- set_PWM_moteur_G(0);
- wait(0.5);
- motors_stop();
-}
-
-
-void test_rotation_rel(double angle_vise, int vitesse, int nombreTours)
-{
- // rotation de angle_vise
- motors_on();
-
- int compteur = 0;
-
- int sensRotation = 1;
- if(angle_vise>0)
- {
- sensRotation = -1;
- }
-
- int vitesse_D = vitesse*sensRotation;
- int vitesse_G = -vitesse*sensRotation;
-
- double angle = get_angle();
- angle_vise+=angle;
- borne_angle_d(angle_vise);
-
- while (abs(diff_angle(angle,angle_vise))>1 || (compteur != nombreTours))
- {
- set_PWM_moteur_D(vitesse_D);
- set_PWM_moteur_G(vitesse_G);
- actualise_position();
- if (angle*get_angle() < 0 && angle > 100 && compteur<nombreTours) // angle>100 pour que compteur ne s'incremente qu'au passage de 180 a -180, pas de -1 a 1
- {
- compteur++;
- }
- angle = get_angle();
- //printf("angle recu : %lf", angle);
- }
-
- //printf(" x et y recu : %lf, %ld. distance parcourue : %ld ", sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
-
- set_PWM_moteur_D(0);
- set_PWM_moteur_G(0);
- wait(0.5);
- motors_stop();
-}
-
-
-void test_rotation_abs(double angle_vise, int vitesse, int nombre)
-{
- double angle_rel = borne_angle_d(angle_vise-get_angle());
- test_rotation_rel(angle_rel, vitesse, nombre);
-}
\ No newline at end of file
--- a/tests_moteurs.h Wed Dec 12 20:03:07 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,12 +0,0 @@ -#ifndef TESTS_MOTEURS_H -#define TESTS_MOTEURS_H - -//fonctions de débug et de calibrage du robot - - -void test_ligne_droite(long int distance, int vitesse); - -void test_rotation_rel(double angle_vise, int vitesse, int nombre = 0); -void test_rotation_abs(double angle_vise, int vitesse, int nombre = 0); - -#endif \ No newline at end of file