File content as of revision 0:38dbe2988e77:

#include "mbed.h"
#include "odometrie.h"
#include "hardware.h"
#include "reglages.h"
#include "math_precalc.h"

long int x_actuel;
//long int y_actuel;
//double angle; // angle du robot


//long int nbr_tick_D_prec;
//long int nbr_tick_G_prec;

//long int position_D_prec;
//long int position_G_prec;

long int nbr_tick_prec;
/*.................. LE COMMENTAIRE EST L'ODOMETRIE AVEC CODEUSE............*/  
void init_odometrie()
{
	x_actuel=X_INIT;
	//y_actuel=Y_INIT;
	//angle=THETA_INIT;
	//nbr_tick_D_prec=0;
	//nbr_tick_G_prec=0;
	//position_D_prec=0;
	//position_G_prec=0;
	nbr_tick_prec=0;
}

void actualise_position()
{
    /*
    on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
    */

	/*------recuperation de la rotation des roues---------*/
	//long int nbr_tick_D=get_nbr_tick_D();
	//long int nbr_tick_G=get_nbr_tick_G();
	//long int position_D=-get_position_D();
	//long int position_G=get_position_G();
	long int nbr_tick = get_nbr_tick_G();
	//calcul du nombre de tick
	//long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec;
	//long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec;
	//long int position_D_actuel=position_D-position_D_prec;
	//long int position_G_actuel=position_G-position_G_prec;
	long int nbr_tick_actuel=nbr_tick-nbr_tick_prec;
	//sauvegarde
	//nbr_tick_D_prec=nbr_tick_D;
	//nbr_tick_G_prec=nbr_tick_G;
	//position_D_prec=position_D;
	//position_G_prec=position_G;
	nbr_tick_prec=nbr_tick;
	//double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues
    //double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D;
	//double dep_roue_G = position_G_actuel * DISTANCE_PAR_TICK_G;
	//double dep_roue_D = position_D_actuel * DISTANCE_PAR_TICK_D;
	double dep_roue = nbr_tick_actuel * DISTANCE_PAR_TICK_G;
	/*------calcul de la trajectoire---------*/
	x_actuel += (int) dep_roue+0.5;
    // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
    /*if (dep_roue_G != dep_roue_D){
        
    	double R = 0; // rayon du cercle decrit par la trajectoire
	    double d = 0; // vitesse du robot
        double cx = 0; // position du centre du cercle decrit par la trajectoire
        double cy = 0;
        
        R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle
        cx = x_actuel - R * sin(angle);
        cy = y_actuel + R * cos(angle);
        d = (dep_roue_G + dep_roue_D) / 2;

        // mise à jour des coordonnées du robot
        if (dep_roue_G + dep_roue_D != 0){
            angle += d / R;
        }
        else{
            angle += dep_roue_D * 2 / ECART_ROUE;
        }
        
    	angle = borne_angle_r(angle);

        x_actuel = (int) (cx + R * sin(angle) + 0.5);
        y_actuel = (int) (cy - R * cos(angle) + 0.5);
    }
    else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
        x_actuel += (int) (dep_roue_G * cos(angle) + 0.5);
        y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
    }*/
      printf("tick : %d, x_actuel : %d\n", nbr_tick_actuel,x_actuel);  
    //printf("tick d : %d, tick g : %d, totaux D: %d, toaux G: %d x : %d, y : %d. angle : %lf\n", position_D_actuel,position_G_actuel,position_D,position_G,  x_actuel, y_actuel, angle*180/PI);
} 

long int get_x_actuel()
{
	return x_actuel;
}

/*long int get_y_actuel()
{
	return y_actuel;
}*/

/*double get_angle()
{
	return angle*180/PI;
}*/