TRR 2018 / Mbed 2 deprecated biniou-TRR2019-DLVV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050

odom.cpp@8:f23601373e8b, 2019-10-02 (annotated)

Committer:
GaspardD
Date:
Wed Oct 02 22:25:12 2019 +0000
Revision:
8:f23601373e8b
Parent:
6:ab9f3695633f
Child:
9:1b54bac6d9a7
angle correction working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 4:efa207509f63 1 #include "odom.h"
GaspardD 8:f23601373e8b 2 #include "utils.h"
GaspardD 8:f23601373e8b 3
GaspardD 8:f23601373e8b 4 InterruptIn b_ODOM_odom(PF_8);
GaspardD 4:efa207509f63 5
GaspardD 4:efa207509f63 6 double d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 8:f23601373e8b 7 double d_ODOM_distFromGlobalStart_m = 0.0;
GaspardD 8:f23601373e8b 8
GaspardD 4:efa207509f63 9 double d_ODOM_speed_mps = 0.0;
GaspardD 4:efa207509f63 10 double d_ODOM_accel_mpsps = 0.0;
GaspardD 8:f23601373e8b 11
GaspardD 8:f23601373e8b 12 double d_ODOM_computed_pos_m = 0.0;
GaspardD 4:efa207509f63 13
GaspardD 8:f23601373e8b 14 int i_ODOM_last_time_ms = 0 ;
GaspardD 8:f23601373e8b 15 int i_last_time_ms = 0 ;
GaspardD 8:f23601373e8b 16 int i_ODOM_time_ms = 0;
GaspardD 8:f23601373e8b 17 int i_time_ms;
GaspardD 8:f23601373e8b 18
GaspardD 8:f23601373e8b 19 int now;
GaspardD 8:f23601373e8b 20 double d_distance_since_it_m;
GaspardD 4:efa207509f63 21
GaspardD 4:efa207509f63 22 void it_odom()
GaspardD 4:efa207509f63 23 {
GaspardD 8:f23601373e8b 24 //rs_UTILS_pc.printf("debut it odom\r\n");
GaspardD 8:f23601373e8b 25 if(t_utils_timerSinceStart.read_ms() != i_ODOM_time_ms)
GaspardD 8:f23601373e8b 26 {
GaspardD 8:f23601373e8b 27 i_ODOM_last_time_ms = i_ODOM_time_ms;
GaspardD 8:f23601373e8b 28 i_ODOM_time_ms = t_utils_timerSinceStart.read_ms();
GaspardD 8:f23601373e8b 29 d_ODOM_distFromSectionStart_m += d_CHASSIS_dist_one_step_odom;
GaspardD 8:f23601373e8b 30 d_ODOM_distFromGlobalStart_m += d_CHASSIS_dist_one_step_odom;
GaspardD 8:f23601373e8b 31 }
GaspardD 4:efa207509f63 32 return;
GaspardD 4:efa207509f63 33 }
GaspardD 8:f23601373e8b 34
GaspardD 8:f23601373e8b 35 void update_speed()
GaspardD 8:f23601373e8b 36 {
GaspardD 8:f23601373e8b 37 i_last_time_ms = i_ODOM_last_time_ms;
GaspardD 8:f23601373e8b 38 i_time_ms = i_ODOM_time_ms;
GaspardD 8:f23601373e8b 39
GaspardD 8:f23601373e8b 40 d_ODOM_speed_mps = d_CHASSIS_dist_one_step_odom/((i_time_ms - i_last_time_ms)/1000.0) ;
GaspardD 8:f23601373e8b 41 //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);
GaspardD 8:f23601373e8b 42
GaspardD 8:f23601373e8b 43 now = t_utils_timerSinceStart.read_ms();
GaspardD 8:f23601373e8b 44 d_distance_since_it_m = d_ODOM_speed_mps * ((now - i_time_ms)/1000.0);
GaspardD 8:f23601373e8b 45
GaspardD 8:f23601373e8b 46 //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);
GaspardD 8:f23601373e8b 47
GaspardD 8:f23601373e8b 48 // distance par rapport au debut a l'instant T
GaspardD 8:f23601373e8b 49 d_ODOM_computed_pos_m = d_ODOM_distFromGlobalStart_m + d_distance_since_it_m;
GaspardD 8:f23601373e8b 50
GaspardD 8:f23601373e8b 51
GaspardD 8:f23601373e8b 52 //rs_UTILS_pc.printf("computed dist since start: %f since it: %f\r\n",d_ODOM_computed_pos_m,d_distance_since_it_m);
GaspardD 8:f23601373e8b 53 }