Diff: odometrie.cpp

Revision:

0:6ca63d45f0ee

Child:

1:0690cf83f060

diff -r 000000000000 -r 6ca63d45f0ee odometrie.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometrie.cpp	Tue Dec 11 19:12:55 2018 +0000
@@ -0,0 +1,106 @@
+#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;
+    
+void init_odometrie()
+{
+	x_actuel=X_INIT;
+	y_actuel=Y_INIT;
+	angle=0;
+	nbr_tick_D_prec=0;
+	nbr_tick_G_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();
+	
+	//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(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 : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/3.14);
+} 
+
+double borne_angle(double angle)
+{
+	if (angle > 3.14) {
+		angle -= 2*3.14;
+	}
+	else if (angle <= -3.14) {
+		angle += 2*3.14;
+	}
+	return angle;
+}
+
+long int get_x_actuel()
+{
+	return x_actuel;
+}
+
+long int get_y_actuel()
+{
+	return y_actuel;
+}
+
+double get_angle()
+{
+	return angle*180/3.14;
+}
\ No newline at end of file