assert1
Dependencies: mbed X_NUCLEO_IHM02A1
tests_moteurs.cpp
- Committer:
- Coconara
- Date:
- 2018-12-11
- Revision:
- 0:6ca63d45f0ee
- Child:
- 1:0690cf83f060
File content as of revision 0:6ca63d45f0ee:
#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, int angle_vise) { 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(); while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance)//x_actuel < distance) { //float a = racine((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)); printf(" SQRT : %lf", pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5)); angle = get_angle(); vitesse_G = (int) (vitesse * (1 + 0.05*(angle_vise-angle))); vitesse_D = (int) (vitesse * (1 - 0.05*(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)); } //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(double angle_vise, int vitesse, int nombre) { 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 compteur = 0; int vitesse_D = -vitesse; int vitesse_G = vitesse; double angle = get_angle(); while ((angle < angle_vise - 1) || (angle > angle_vise + 1) || (compteur != nombre) || ((angle < -180 + 1) && (angle_vise == 180))) { set_PWM_moteur_D(vitesse_D); set_PWM_moteur_G(vitesse_G); actualise_position(); if (angle*get_angle() < 0 && angle > 20) { compteur++; } angle = get_angle(); printf("angle recu : %lf", angle);// 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)); } //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(); }