odometrie.cpp

00001 #include "mbed.h"
00002 #include "odometrie.h"
00003 #include "hardware.h"
00004 #include "reglages.h"
00005 #include "math_precalc.h"
00006 
00007 double x_actuel;
00008 double y_actuel;
00009 double angle; // angle du robot
00010 
00011 
00012 long int nbr_tick_D_prec;
00013 long int nbr_tick_G_prec;
00014     
00015 void init_odometrie()
00016 {
00017     x_actuel=X_INIT;
00018     y_actuel=Y_INIT;
00019     angle=THETA_INIT;
00020     nbr_tick_D_prec=0;
00021     nbr_tick_G_prec=0;
00022 }
00023 
00024 void actualise_position()
00025 {
00026     /*
00027     on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
00028     */
00029 
00030     /*------recuperation de la rotation des roues---------*/
00031     long int nbr_tick_D=get_nbr_tick_D();
00032     long int nbr_tick_G=get_nbr_tick_G();
00033     
00034     //calcul du nombre de tick
00035     long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec;
00036     long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec;
00037     
00038     //sauvegarde
00039     nbr_tick_D_prec=nbr_tick_D;
00040     nbr_tick_G_prec=nbr_tick_G;
00041     
00042     double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues
00043     double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D;
00044     
00045     
00046     /*------calcul de la trajectoire---------*/
00047     
00048     // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
00049     if (dep_roue_G != dep_roue_D){
00050         
00051         double R = 0; // rayon du cercle decrit par la trajectoire
00052         double d = 0; // vitesse du robot
00053         double cx = 0; // position du centre du cercle decrit par la trajectoire
00054         double cy = 0;
00055         
00056         R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle
00057         cx = x_actuel - R * sin(angle);
00058         cy = y_actuel + R * cos(angle);
00059         d = (dep_roue_G + dep_roue_D) / 2;
00060 
00061         // mise à jour des coordonnées du robot
00062         if (dep_roue_G + dep_roue_D != 0){
00063             angle += d / R;
00064         }
00065         else{
00066             angle += dep_roue_D * 2 / ECART_ROUE;
00067         }
00068         
00069         angle = borne_angle_r(angle);
00070 
00071         x_actuel = cx + R * sin(angle);
00072         y_actuel = cy - R * cos(angle);
00073     }
00074     else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
00075         x_actuel +=dep_roue_G * cos(angle);
00076         y_actuel +=dep_roue_D * sin(angle);
00077     }
00078         
00079     //printf("tick d : %d, tick g : %d, x : %lf, y : %lf. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
00080     
00081 } 
00082 
00083 
00084 
00085 double get_x_actuel()
00086 {
00087     return x_actuel;
00088 }
00089 
00090 double get_y_actuel()
00091 {
00092     return y_actuel;
00093 }
00094 
00095 double get_angle()
00096 {
00097     return angle*180/PI;
00098 }