Diff: odometrie.cpp

Revision:

2:3066e614372f

Parent:

0:38dbe2988e77

Child:

5:3638d7e7c5c1

--- a/odometrie.cpp	Wed Apr 17 15:49:42 2019 +0000
+++ b/odometrie.cpp	Wed May 08 21:19:10 2019 +0000
@@ -4,29 +4,21 @@
 #include "reglages.h"
 #include "math_precalc.h"
 
-long int x_actuel;
-//long int y_actuel;
-//double angle; // angle du robot
+double x_actuel;
+double 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............*/  
+long int nbr_tick_D_prec;
+long int nbr_tick_G_prec;
+    
 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;
+	y_actuel=Y_INIT;
+	angle=THETA_INIT;
+	nbr_tick_D_prec=0;
+	nbr_tick_G_prec=0;
 }
 
 void actualise_position()
@@ -36,32 +28,25 @@
     */
 
 	/*------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();
+	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;
-	//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;
+	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;
-	//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;
+	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---------*/
-	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){
+    if (dep_roue_G != dep_roue_D){
         
     	double R = 0; // rayon du cercle decrit par la trajectoire
 	    double d = 0; // vitesse du robot
@@ -83,28 +68,31 @@
         
     	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);
-    }*/
-      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);
+        x_actuel +=dep_roue_G * cos(angle);
+        y_actuel +=dep_roue_D * sin(angle);
+    }
+        
+    //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);
+    
 } 
 
-long int get_x_actuel()
+
+
+double get_x_actuel()
 {
 	return x_actuel;
 }
 
-/*long int get_y_actuel()
+double get_y_actuel()
 {
 	return y_actuel;
-}*/
+}
 
-/*double get_angle()
+double get_angle()
 {
 	return angle*180/PI;
-}*/
\ No newline at end of file
+}
\ No newline at end of file