File content as of revision 2:977799d72329:

#include "mbed.h"

#include "tests_moteurs.h"
#include "hardware.h"
#include "reglages.h"
#include "math_precalc.h"

#include "odometrie.h"

void test_ligne_droite(double distance, int vitesse, double angle_vise)
{
    // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
    motors_on();
    double x_ini = get_x_actuel();
    double y_ini = get_y_actuel();
    
    double x_actuel = get_x_actuel();
    double y_actuel = get_y_actuel();
    
    int vitesse_D = vitesse;
    int vitesse_G = vitesse;
    
    double angle = get_angle();
    if(angle_vise == 0.00001) angle_vise = get_angle(); // si on n'a pas passé d'argument pour l'angle, on prend maintient l'angle de départ 
    
    
    while ((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel) < distance*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  : %lf xinit : %lf , distance : %lf\n", x_actuel,x_ini,distance);// 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("y recu  : %lf yinit : %lf , distance : %lf\n\n", y_actuel,y_ini,(x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5);
        //printf("vd : %d , vg : %d\n",vitesse_D,vitesse_G);
        
    }
    
    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);
}