Yo Fleyt / ISR_Mini-explorer

ouais ok

Dependents:   roboticLab_withclass_3_July

Fork of ISR_Mini-explorer by ISR UC

Diff: robot.h

Revision:
1:b81277897a5b
Parent:
0:15a30802e719
Child:
2:92ca9a6a1d4e
--- a/robot.h	Thu Feb 02 12:21:11 2017 +0000
+++ b/robot.h	Fri Jul 07 11:49:22 2017 +0000
@@ -20,6 +20,7 @@
 #include "nRF24L01P.h"
 
 void Odometria();
+void OdometriaKalman(float* R_cm, float* L_cm);
 
 // classes adicionais
 nRF24L01P my_nrf24l01p(PTD2, PTD3, PTD1, PTC13, PTC12, PTA13);
@@ -1024,4 +1025,20 @@
     X += CM*cos(theta);
     Y += CM*sin(theta);
 
+}
+
+void OdometriaKalman(float* R_cm, float* L_cm)
+{
+    long int ticks1d=R_encoder();
+    long int ticks1e=L_encoder();
+
+    long int D_ticks=ticks1d - ticks2d;
+    long int E_ticks=ticks1e - ticks2e;
+
+    ticks2d=ticks1d;
+    ticks2e=ticks1e;
+
+    *R_cm= (float)D_ticks*((3.25*3.1415)/4096);
+    *L_cm= (float)E_ticks*((3.25*3.1415)/4096);
+
 }
\ No newline at end of file