Diff: odometrie.cpp

Revision:

4:deef042e9c02

Parent:

3:1dba6eca01ad

--- a/odometrie.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/odometrie.cpp	Wed May 08 20:46:46 2019 +0000
@@ -4,8 +4,8 @@
 #include "reglages.h"
 #include "math_precalc.h"
 
-long int x_actuel;
-long int y_actuel;
+double x_actuel;
+double y_actuel;
 double angle; // angle du robot
 
 
@@ -68,83 +68,26 @@
         
     	angle = borne_angle_r(angle);
 
-        x_actuel = (int) (cx + R * sin(angle) + 0.5);
-        y_actuel = (int) (cy - R * cos(angle) + 0.5);
+        x_actuel = cx + R * sin(angle);
+        y_actuel = cy - R * cos(angle);
     }
     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);
+        x_actuel +=dep_roue_G * cos(angle);
+        y_actuel +=dep_roue_D * sin(angle);
     }
         
-    printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
+    //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);
+    
 } 
 
-void actualise_position_test()
-{
-    
-    //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();
-	
-	//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;
-	
-	//sauvegarde
-	nbr_tick_D_prec=nbr_tick_D;
-	nbr_tick_G_prec=nbr_tick_G;
-	
-	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;
-	
-	
-	//------calcul de la trajectoire---------
-	
-    // 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);
-        printf("cx : %lf cy : %lf ",cx + R * sin(angle) + 0.5,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 : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
-}
 
 
-long int get_x_actuel()
+double get_x_actuel()
 {
 	return x_actuel;
 }
 
-long int get_y_actuel()
+double get_y_actuel()
 {
 	return y_actuel;
 }