Diff: odom.cpp

Revision:

8:f23601373e8b

Parent:

6:ab9f3695633f

Child:

9:1b54bac6d9a7

--- a/odom.cpp	Sun Sep 29 20:22:43 2019 +0000
+++ b/odom.cpp	Wed Oct 02 22:25:12 2019 +0000
@@ -1,13 +1,53 @@
 #include "odom.h"
+#include "utils.h"
+
+InterruptIn b_ODOM_odom(PF_8);
 
 double d_ODOM_distFromSectionStart_m = 0.0;
+double d_ODOM_distFromGlobalStart_m = 0.0;
+
 double d_ODOM_speed_mps = 0.0;
 double d_ODOM_accel_mpsps = 0.0;
-InterruptIn odom(PF_8);
+
+double d_ODOM_computed_pos_m = 0.0;
 
+int i_ODOM_last_time_ms = 0 ;
+int i_last_time_ms = 0 ;
+int i_ODOM_time_ms = 0;
+int i_time_ms;
+
+int now;
+double d_distance_since_it_m;
 
 void it_odom()
 {
-    d_ODOM_distFromSectionStart_m += d_dist_one_step_odom;
+    //rs_UTILS_pc.printf("debut it odom\r\n");
+    if(t_utils_timerSinceStart.read_ms() != i_ODOM_time_ms)
+    {
+        i_ODOM_last_time_ms = i_ODOM_time_ms;
+        i_ODOM_time_ms = t_utils_timerSinceStart.read_ms();
+        d_ODOM_distFromSectionStart_m += d_CHASSIS_dist_one_step_odom;
+        d_ODOM_distFromGlobalStart_m += d_CHASSIS_dist_one_step_odom;
+    }
     return;
 }
+
+void update_speed()
+{
+    i_last_time_ms = i_ODOM_last_time_ms;
+    i_time_ms = i_ODOM_time_ms;
+
+    d_ODOM_speed_mps = d_CHASSIS_dist_one_step_odom/((i_time_ms - i_last_time_ms)/1000.0) ;
+    //rs_UTILS_pc.printf("vitesse : %f mps; i_time_ms: %d; i_last_time_ms: %d \r\n",d_ODOM_speed_mps,i_time_ms, i_last_time_ms);
+    
+    now = t_utils_timerSinceStart.read_ms();
+    d_distance_since_it_m = d_ODOM_speed_mps * ((now - i_time_ms)/1000.0);
+
+    //rs_UTILS_pc.printf("d_distance_since_it_m = %f =  %f * ((%ld - %ld)/1000)\r\n",d_distance_since_it_m,d_ODOM_speed_mps,now,i_last_time_ms);
+
+    // distance par rapport au debut a l'instant T
+    d_ODOM_computed_pos_m = d_ODOM_distFromGlobalStart_m + d_distance_since_it_m;
+    
+    
+    //rs_UTILS_pc.printf("computed dist since start: %f  since it: %f\r\n",d_ODOM_computed_pos_m,d_distance_since_it_m);
+}
\ No newline at end of file