Diff: MiniExplorerCoimbra.hpp

Revision:

14:696187e74411

Parent:

10:d0109d7cbe7c

--- a/MiniExplorerCoimbra.hpp	Wed Jul 12 09:07:31 2017 +0000
+++ b/MiniExplorerCoimbra.hpp	Wed Jul 12 18:08:07 2017 +0000
@@ -51,6 +51,19 @@
 	float repulsionConstantForce;
 	
 	myMatrix covariancePositionEstimationK;
+	float D_cm;
+	float L_cm;
+	
+	float myX_r;
+    float myY_r;
+    float mytheta_r;
+    
+    float myX_r_correct;
+    float myY_r_correct;
+    float mytheta_r_correct;
+    
+    long int ticks2d_Special;
+    long int ticks2e_Special;
 
 	MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap);
 	
@@ -86,18 +99,24 @@
 	
 	void go_to_point_kalman(float targetXWorld, float targetYWorld);
 	
-	void OdometriaKalmanFilter(); 
+	void go_to_point_kalman2(float targetXWorld, float targetYWorld);
 	
 	void test_prediction_sonar();
 	
+	void OdometriaKalmanFilter(); 
+	
 	void printMatrix(myMatrix mat1);
 	
-	private:
+	void myOdometriaBis();
+	
+	void testOdometria(float *D_Special,float *L_Special);
 	
 	void myOdometria();
 	
 	void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
 	
+	private:
+	
 	float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt);
 	
 	void update_sonar_values(float leftMm,float frontMm,float rightMm);