Revision 9:1b54bac6d9a7, committed 2019-10-03

Comitter:

GaspardD

Date:

Thu Oct 03 23:28:56 2019 +0000

Parent:

8:f23601373e8b

Child:

10:e63fe4080760

Commit message:

a tester ;

Changed in this revision

--- a/chassis_mode.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/chassis_mode.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -3,15 +3,18 @@
 DigitalIn b_CHASSIS_Mode(PE_11);
 bool b_CHASSIS_with_pushbutton;
 double d_CHASSIS_dist_one_step_odom;
+double d_CHASSIS_inversion = 1.0;
 
 void init_chassis_mode()
 {
     b_CHASSIS_Mode.mode(PullUp);
     if(b_CHASSIS_Mode.read()) {
         b_CHASSIS_with_pushbutton = false;
-        d_CHASSIS_dist_one_step_odom = 0.08;
+        d_CHASSIS_inversion = -1.0;
+        d_CHASSIS_dist_one_step_odom = 0.066;
     } else {
         b_CHASSIS_with_pushbutton = true;
+        d_CHASSIS_inversion = 1.0;
         d_CHASSIS_dist_one_step_odom = 0.066;
     }
     return;

--- a/chassis_mode.h	Wed Oct 02 22:25:12 2019 +0000
+++ b/chassis_mode.h	Thu Oct 03 23:28:56 2019 +0000
@@ -8,5 +8,6 @@
 extern DigitalIn b_CHASSIS_Mode;
 extern bool b_CHASSIS_with_pushbutton;
 extern double d_CHASSIS_dist_one_step_odom;
+extern double d_CHASSIS_inversion;
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/log.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -0,0 +1,41 @@
+#include "log.h"
+
+Timer t_loggingTimer;
+int i_logging_duration;
+bool b_LOG_READY = false;
+Serial rs_LOG_pc(USBTX, USBRX);
+
+
+s_Sample s_LOG_history[LOG_SIZE];
+int i_LOG_index_data = 0;
+bool b_LOG_flag = false;
+
+void log_start(int duration_ms)
+{
+    b_LOG_flag = true;
+    b_LOG_READY = false;
+    t_loggingTimer.start();
+    i_logging_duration = duration_ms;
+    t_loggingTimer.reset();
+}
+
+void log_check()
+{
+    if(t_loggingTimer.read_ms() > i_logging_duration) {
+        b_LOG_READY = true;
+        b_LOG_flag = false;
+        t_loggingTimer.stop();
+        t_loggingTimer.reset();
+    }
+    if(b_LOG_flag && i_LOG_index_data < LOG_SIZE) {
+        s_LOG_history[i_LOG_index_data].timestamp = t_loggingTimer.read_ms();
+        i_LOG_index_data + 1;
+    }
+}
+void log_pc()
+{
+    rs_LOG_pc.printf("timestamp;left_90;right_90;left_45;right_45;pwm_dir;odom;speed;\r\n");
+    for(int i = 0; i<LOG_SIZE; i++) {
+        rs_LOG_pc.printf("%d;%f;%f;%f;%f;%d;%f;%f;\r\n",s_LOG_history[i_LOG_index_data].timestamp,s_LOG_history[i_LOG_index_data].left_90,s_LOG_history[i_LOG_index_data].right_90,s_LOG_history[i_LOG_index_data].left_45,s_LOG_history[i_LOG_index_data].right_45,s_LOG_history[i_LOG_index_data].pwm_dir,s_LOG_history[i_LOG_index_data].odom,s_LOG_history[i_LOG_index_data].speed);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/log.h	Thu Oct 03 23:28:56 2019 +0000
@@ -0,0 +1,28 @@
+#ifndef H_LOG
+#define H_LOG
+
+
+#define LOG_SIZE 2000
+#include "mbed.h"
+
+typedef struct sample {
+   int timestamp;
+   double left_90;
+   double right_90;
+   double left_45;
+   double right_45;
+   double pwm_dir;
+   double odom;
+   double speed;
+}s_Sample;
+
+
+void log_start(int duration_ms);
+void log_check();
+extern Serial rs_LOG_pc;
+extern bool b_LOG_READY;
+extern s_Sample s_LOG_history[LOG_SIZE];
+extern int i_LOG_index_data;
+
+
+#endif
\ No newline at end of file

--- a/main.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/main.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -9,7 +9,7 @@
 {
     //on met le jumper sur le chassis le plus récent. Avec le jumper, b_CHASSIS_Mode = 0.
     init_chassis_mode();
-   
+
     //init timers
     t_utils_timerSinceStart.start();
 
@@ -19,7 +19,7 @@
     b_UTILS_emergency_signal.rise(&it_emergency_signal_recieved);
 
     //init state machines
-    rs_UTILS_pc.printf("init state machines\r\n");
+    rs_LOG_pc.printf("init state machines\r\n");
 
     init_sm_mpu();
     init_sm_sections();
@@ -27,7 +27,8 @@
     init_sm_servo();
 
     while(1) {
-        
+        rs_LOG_pc.printf("\r\n\r\nTIME: %d\r\n",t_utils_timerSinceStart.read_ms());
+
         update_sm_mpu();
         update_sm_sections();
         update_sm_esc();
@@ -38,6 +39,5 @@
         output_sm_esc();
         output_sm_servo();
         
-        rs_UTILS_pc.printf("\r\n");
     }
 }

--- a/odom.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/odom.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -21,7 +21,7 @@
 
 void it_odom()
 {
-    //rs_UTILS_pc.printf("debut it odom\r\n");
+    //rs_LOG_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;
@@ -38,16 +38,16 @@
     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);
+    //rs_LOG_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);
+    //rs_LOG_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);
+    //rs_LOG_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

--- a/sm_esc.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_esc.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -4,7 +4,6 @@
 PwmOut pwm_ESC(PD_12);
 DigitalOut relay_ESC(PG_0);
 E_STATE_ESC e_ESC_state;
-Timer t_emergency_brake;
 
 void init_sm_esc()
 {
@@ -43,7 +42,7 @@
     switch(e_ESC_state) {
         case ESC_DISABLED:
             if(b_UTILS_flag_emergency_stop) {
-                rs_UTILS_pc.printf("EMERGENCY STOP\r\n");
+                rs_LOG_pc.printf("EMERGENCY STOP\r\n");
                 b_UTILS_flag_emergency_stop = false;
                 if(b_CHASSIS_with_pushbutton) {
                     relay_ESC = 1;
@@ -54,7 +53,7 @@
             break;
 
         case ESC_INIT:
-            rs_UTILS_pc.printf("Init esc\r\n");
+            rs_LOG_pc.printf("Init esc\r\n");
             b_UTILS_flag_button_ESC = false;
             pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
             pwm_ESC.pulsewidth_us(0);
@@ -72,8 +71,7 @@
             wait(1);
             pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
             wait(1);
-            t_emergency_brake.start();
-            t_emergency_brake.reset();
+            log_start(10000);
             break;
 
         case ESC_COMMAND:

--- a/sm_mpu.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_mpu.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -3,10 +3,6 @@
 #include "sm_esc.h"
 
 MPU6050 s_MPU_mpu;
-Timer t_mpuLoggingTimer;
-int i_mpuLogging_duration;
-bool b_MPU_logFlag = false;
-int mpu_init_try = 0;
 
 int16_t ax, ay, az;
 double ax_g,ay_g,az_g;
@@ -27,7 +23,7 @@
 
     switch(e_state_MPU) {
         case INIT_MPU:
-                e_next_state = RUNNING_MPU;
+            e_next_state = RUNNING_MPU;
             break;
         case RUNNING_MPU:
 
@@ -46,7 +42,7 @@
     switch(e_state_MPU) {
         case INIT_MPU:
             mpu6050TestResult = false;
-            rs_UTILS_pc.printf("MPU INITIALIZING");
+            rs_LOG_pc.printf("MPU INITIALIZING");
             s_MPU_mpu.initialize();
             rs_UTILS_odroid.printf("MPU6050 testConnection \r\n");
 
@@ -59,38 +55,19 @@
             break;
         case RUNNING_MPU:
             //logging mpu data
-            mpu_log_notify();
+            
+            /*s_MPU_mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+            //correction pour avoir une acceleration coef empirique
+            ax_g = ax /1658.0 ;
+            ay_g = ay /1658.0 ;
+            az_g = az /1658.0 ;
+
+            rs_LOG_pc.printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;gyr X =%d; gyr Y =%d; gyr Z=%d;speed: %f;odom section %f; odom global %f\n\r",ax_g,ay_g,az_g,gx,gy,gz,d_ODOM_speed_mps,d_ODOM_distFromSectionStart_m,d_ODOM_distFromGlobalStart_m);
+            */
             break;
 
         default:
             break;
     }
 
-}
-
-void mpu_log_start(int duration_ms)
-{
-    b_MPU_logFlag = true;
-    t_mpuLoggingTimer.start();
-    i_mpuLogging_duration = duration_ms;
-    t_mpuLoggingTimer.reset();
-}
-
-void mpu_log_notify()
-{
-    if(t_mpuLoggingTimer.read_ms() > i_mpuLogging_duration) {
-        b_MPU_logFlag = false;
-        t_mpuLoggingTimer.stop();
-        t_mpuLoggingTimer.reset();
-    }
-    if(b_MPU_logFlag && t_mpuLoggingTimer.read_ms() < i_mpuLogging_duration) {
-        s_MPU_mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-        //correction pour avoir une acceleration coef empirique
-        ax_g = ax /1658.0 ;
-        ay_g = ay /1658.0 ;
-        az_g = az /1658.0 ;
-
-        //rs_UTILS_pc.printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;gyr X =%d; gyr Y =%d; gyr Z=%d;speed: %f;odom section %f; odom global %f\n\r",ax_g,ay_g,az_g,gx,gy,gz,d_ODOM_speed_mps,d_ODOM_distFromSectionStart_m,d_ODOM_distFromGlobalStart_m);
-
-    }
 }
\ No newline at end of file

--- a/sm_mpu.h	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_mpu.h	Thu Oct 03 23:28:56 2019 +0000
@@ -11,9 +11,6 @@
     ERROR_MPU
 }E_STATE_MPU;
 
-void mpu_log_notify();
-void mpu_log_start(int duration_ms);
-
 void init_sm_mpu();
 void update_sm_mpu();
 void output_sm_mpu();

--- a/sm_sections.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_sections.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -18,18 +18,18 @@
 
 //section de départ
     s_section1.nextSection = &s_section2;
-    s_section1.consigne_position = 0.75;
+    s_section1.consigne_position = 0.5;
     s_section1.targetSpeed_mps = 1640;
-    s_section1.lng_section_m = 4.0;
+    s_section1.lng_section_m = 2.0;
     s_section1.coef_p = 1.0;
     s_section1.coef_d = 0.0;
     s_section1.coef_i = 0.000;
 
     //epingle 1
     s_section2.nextSection = NULL;
-    s_section2.consigne_position = 0.75;
+    s_section2.consigne_position = 0.5;
     s_section2.targetSpeed_mps = 1640;
-    s_section2.lng_section_m = 4.0;
+    s_section2.lng_section_m = 2.0;
     s_section2.coef_p = 1.0;
     s_section2.coef_d = 0.0;
     s_section2.coef_i = 0.000;
@@ -41,27 +41,27 @@
 
     E_STATE_SECTIONS e_next_state = e_SECTIONS_state;
 
-    //rs_UTILS_pc.printf("\r\nUpdate Section\r\n");
+    //rs_LOG_pc.printf("\r\nUpdate Section\r\n");
 
     if(b_UTILS_flag_emergency_stop) {
-        rs_UTILS_pc.printf("emergency stop -> STOPPED\r\n");
+        rs_LOG_pc.printf("emergency stop -> STOPPED\r\n");
         e_next_state = STOPPED;
     } else {
         switch (e_SECTIONS_state) {
             case INIT_SECTION:
                 e_next_state = RUNNING_SECTION;
-                //rs_UTILS_pc.printf("RUNNING SECTION\r\n");
+                //rs_LOG_pc.printf("RUNNING SECTION\r\n");
                 break;
             case RUNNING_SECTION:
                 if(d_ODOM_distFromSectionStart_m > s_UTILS_currentSection->lng_section_m) {
                     e_next_state = LOADING_SECTION;
-                    //rs_UTILS_pc.printf("LOADING SECTION !\r\n");
+                    //rs_LOG_pc.printf("LOADING SECTION !\r\n");
                 }
                 break;
             case LOADING_SECTION:
                 if(s_UTILS_currentSection != NULL) {
                     e_next_state = RUNNING_SECTION;
-                    //rs_UTILS_pc.printf("RUNNING SECTION !\r\n");
+                    //rs_LOG_pc.printf("RUNNING SECTION !\r\n");
                 } else {
                     b_UTILS_flag_emergency_stop = true;
                     e_next_state = STOPPED;
@@ -70,7 +70,7 @@
             case STOPPED:
                 if(b_UTILS_flag_button_SECTIONS) {
                     e_next_state = INIT_SECTION;
-                    rs_UTILS_pc.printf("INIT SECTION !\r\n");
+                    rs_LOG_pc.printf("INIT SECTION !\r\n");
                 }
                 break;
             default:
@@ -86,12 +86,11 @@
         case INIT_SECTION:
             b_UTILS_flag_button_SECTIONS = false;
             s_UTILS_currentSection = &s_section1;
-            rs_UTILS_pc.printf("sect 1 INITIALIZED\r\n");
-             mpu_log_start(10000);
+            rs_LOG_pc.printf("sect 1 INITIALIZED\r\n");
         case RUNNING_SECTION:
             break;
         case LOADING_SECTION:
-            //rs_UTILS_pc.printf("next section loading\r\n");
+            //rs_LOG_pc.printf("next section loading\r\n");
             s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
             d_ODOM_distFromSectionStart_m = 0.0;
             break;

--- a/sm_servo.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_servo.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -24,6 +24,13 @@
 double d_dist_left_90;
 double d_dist_right_90;
 
+double d_prev_dist_left_45;
+double d_prev_dist_right_45;
+double d_dist_left_45;
+double d_dist_right_45;
+
+double d_positionOnTrack;
+
 double coef_positionnement;
 double coef_angle_correction_bordure;
 double angle_correction_final ;
@@ -31,6 +38,9 @@
 double angle_correction_bordure;
 double distance_parcourue;
 
+double ponderation_angle[6];
+double d_mediane;
+double distCapt;
 
 void init_sm_servo()
 {
@@ -40,40 +50,46 @@
 double pwmFromAngle(double angleDeg)
 {
     //on a une regression linéaire entre l'angle et la pwm on centre sur 0 puis on applique
-    pulsewidth = (angleDeg * 10.0) + SERVO_PULSE_MIDDLE_US;
-    //rs_UTILS_pc.printf("pulsewidth = %d =(%f * 10.0) + 1500\r\n",pulsewidth,angleDeg);
-    rs_UTILS_pc.printf("TIME: %d\r\n",t_utils_timerSinceStart.read_ms());
+    pulsewidth = (d_CHASSIS_inversion*angleDeg * 11.0) + SERVO_PULSE_MIDDLE_US;
+    //rs_LOG_pc.printf("pulsewidth = %d =(%f * 11.0) + 1500\r\n",pulsewidth,angleDeg);
     if(pulsewidth > SERVO_PULSE_MAX_US) {
         pulsewidth =  SERVO_PULSE_MAX_US;
     } else if(pulsewidth < SERVO_PULSE_MIN_US) {
         pulsewidth = SERVO_PULSE_MIN_US;
     }
-    
+    s_LOG_history[i_LOG_index_data].pwm_dir = pulsewidth;
     return pulsewidth;
 }
 
 
 
-double distcapteur;
-
 double getDistCapteur(AnalogIn* p)
 {
-    double d = 0.182/(double)p->read() ;     
-    if (d > 1.5){
-        d = 1.5 ;
-        }
-    return d ;
+    distCapt = 0.182/(double)p->read() ;
+    if (distCapt > 1.5) {
+        distCapt = 1.5 ;
+    }
+    return distCapt ;
 }
 
+//renvoie un double entre 0 et 1 avec 0 position extrème gauche et 1 extrème droite
+double positionOnTrack()
+{
+    d_positionOnTrack = 0.25*(3*(d_dist_left_90/(d_dist_left_90 + d_dist_right_90)) + (d_dist_left_45/(d_dist_left_45 + d_dist_right_45) ));
 
+    return d_positionOnTrack;
+}
 
 double compute_angle_correction(double consignePos)
 {
     //aquisition
-    coef_positionnement = 0.01;
+    coef_positionnement = 0.1;//0.05
     coef_angle_correction_bordure = 1.0;
-    d_dist_left_90 = getDistCapteur(&ana_left_90);
-    d_dist_right_90 = getDistCapteur(&ana_right_90);
+    d_dist_left_90 = getDistCapteur(&ana_left_90) + 0.016;
+    d_dist_right_90 = getDistCapteur(&ana_right_90) + 0.016;
+    d_dist_left_45 = getDistCapteur(&ana_left_45) + 0.042;
+    d_dist_right_45 = getDistCapteur(&ana_right_45) + 0.042;
+
 
     angle_correction_final = 0.0;
     angle_correction_position = 0.0;
@@ -81,19 +97,68 @@
 
     update_speed();
 
-    distance_parcourue = d_ODOM_computed_pos_m - d_last_odom_asservissement + 0.001;
-    //rs_UTILS_pc.printf("distance_parcourue = %f = %f - %f + 0.001 \n\r",distance_parcourue,d_ODOM_computed_pos_m,d_last_odom_asservissement );
-    angle_correction_bordure = atan((d_prev_dist_left_90 - d_dist_left_90)/ distance_parcourue)*57.296;
-    //rs_UTILS_pc.printf("angle_correction_bordure = %f = -atan((%f - %f)/ %f) \n\r",angle_correction_bordure,d_prev_dist_left_90,d_dist_left_90,distance_parcourue) ;
-    angle_correction_position = atan((consignePos - d_dist_left_90)/ (0.04*d_ODOM_speed_mps))*57.296;
-    //rs_UTILS_pc.printf("angle_correction_position = %f = atan((%f - %f)/ (0.04* %f ))*57.296,##### TIME: %d\r\n",angle_correction_position,consignePos,d_dist_left_90,d_ODOM_speed_mps,t_utils_timerSinceStart.read_ms());
-    
+    distance_parcourue = d_ODOM_computed_pos_m - d_last_odom_asservissement ;
+
+    //left 90
+    ponderation_angle[0] = atan2( (d_prev_dist_left_90 - d_dist_left_90), distance_parcourue)*57.296 ;
+    //right 90
+    ponderation_angle[1] = -atan2( (d_prev_dist_right_90 - d_dist_right_90), distance_parcourue)*57.296 ;
+    //left 45
+    ponderation_angle[2] = atan2( ((d_prev_dist_left_45 - d_dist_left_45)*0.7071), distance_parcourue)*57.296 ;
+    //right 45
+    ponderation_angle[3] = -atan2( ((d_prev_dist_right_45 - d_dist_right_45)*0.7071), distance_parcourue)*57.296 ;
+
+    //angle detecte entre le 45 et le 90 gauche
+    ponderation_angle[4] = atan(1.4142*(d_prev_dist_left_90/d_prev_dist_left_45) - 1)*57.296;
+    //rs_LOG_pc.printf("d_prev_dist_left_90:%f,d_prev_dist_left_45:%f;angle cote gauche = %f;angle 90 gauche: %f\n\r",d_prev_dist_left_90,d_prev_dist_left_45,ponderation_angle[4],ponderation_angle[0]);
+
+    //angle detecte entre le 45 et le 90 droite
+    ponderation_angle[5] = -atan(1.4142*(d_prev_dist_right_90/d_prev_dist_right_45) - 1)*57.296;
+    //rs_LOG_pc.printf("angle cote droit = %f\n\r",ponderation_angle[5] );
+
+
+    //votation
+    bubbleSort(ponderation_angle,6);
+    d_mediane = (ponderation_angle[2] + ponderation_angle[3])*0.5;
+    //si la valeur la plus extreme est le min, on prend juste les 5 dernières valeurs
+    /*if((d_mediane - ponderation_angle[0]) > (ponderation_angle[5] - d_mediane)) {
+        for(int i=1; i<6; i++) {
+            angle_correction_bordure = ponderation_angle[i];
+        }
+    } else {
+        for(int i=0; i<5; i++) {
+            angle_correction_bordure = ponderation_angle[i];
+        }
+    }
+    */
+    angle_correction_bordure = d_mediane;
+
+
+    angle_correction_position = atan2((consignePos - positionOnTrack()), (0.04*d_ODOM_speed_mps))*57.296;
+
+    //rs_LOG_pc.printf("distance_parcourue = %f = %f - %f + 0.001 \n\r",distance_parcourue,d_ODOM_computed_pos_m,d_last_odom_asservissement );
+    //rs_LOG_pc.printf("angle_correction_bordure = %f = -atan((%f - %f)/ %f) \n\r",angle_correction_bordure,d_prev_dist_left_90,d_dist_left_90,distance_parcourue) ;
+    //rs_LOG_pc.printf("angle_correction_position = %f = atan((%f - %f)/ (0.04* %f ))*57.296,##### TIME: %d\r\n",angle_correction_position,consignePos,d_dist_left_90,d_ODOM_speed_mps,t_utils_timerSinceStart.read_ms());
+
     angle_correction_final = angle_correction_position * coef_positionnement + angle_correction_bordure*coef_angle_correction_bordure ;
-    //rs_UTILS_pc.printf("angle correction: %f;angle correction bordure: %f ;angle correction position: %f\r\n",angle_correction_final,angle_correction_bordure,angle_correction_position);
+    rs_LOG_pc.printf("angle correction: %f;angle correction bordure: %f ;angle correction position: %f\r\n",angle_correction_final,angle_correction_bordure,angle_correction_position);
 
     d_last_odom_asservissement = d_ODOM_computed_pos_m;
     d_prev_dist_left_90 = d_dist_left_90;
     d_prev_dist_right_90 = d_dist_right_90;
+    d_prev_dist_left_45 = d_dist_left_45;
+    d_prev_dist_right_45 = d_dist_right_45;
+
+    //logging
+    s_LOG_history[i_LOG_index_data].left_90 = d_dist_left_90;
+    s_LOG_history[i_LOG_index_data].right_90 = d_dist_right_90;
+    s_LOG_history[i_LOG_index_data].left_45 = d_dist_left_45;
+    s_LOG_history[i_LOG_index_data].right_45 = d_dist_right_45;
+    s_LOG_history[i_LOG_index_data].odom = d_ODOM_distFromGlobalStart_m;
+    s_LOG_history[i_LOG_index_data].speed = d_ODOM_speed_mps;
+    
+    log_check();
+
     return  angle_correction_final;
 }
 
@@ -111,10 +176,10 @@
         case SERVO_COMMAND:
             //si on detecte une nouvelle acquisition, on traite le signal
             d_dist_left_90 = getDistCapteur(&ana_left_90);
-            rs_UTILS_pc.printf("d_dist_left_90 : %f\r\n",d_dist_left_90);
+            //rs_LOG_pc.printf("d_dist_left_90 : %f;d_dist_right_90 : %f;d_positionOnTrack: %f\r\n",d_dist_left_90,d_dist_right_90,d_positionOnTrack);
             if(d_dist_left_90 != d_prev_dist_left_90) {
 
-                //rs_UTILS_pc.printf("compute angle et updateSpeed\r\n");
+                //rs_LOG_pc.printf("compute angle et updateSpeed\r\n");
                 pulsewidth = pwmFromAngle( compute_angle_correction(s_UTILS_currentSection->consigne_position) );
 
                 d_prev_dist_left_90 = d_dist_left_90;
@@ -135,13 +200,14 @@
 {
     switch(e_stateServo) {
         case SERVO_INIT:
-            rs_UTILS_pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
+            rs_LOG_pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
             pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
             pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
             directionCheck = true;
             break;
         case SERVO_COMMAND:
-            pwm_Servo.pulsewidth_us(pulsewidth);
+            //pwm_Servo.pulsewidth_us(pulsewidth);
+            pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
 
             break;
         default:

--- a/sm_servo.h	Wed Oct 02 22:25:12 2019 +0000
+++ b/sm_servo.h	Thu Oct 03 23:28:56 2019 +0000
@@ -9,6 +9,7 @@
 #define SERVO_PULSE_MAX_US 2000
 #define SERVO_PULSE_MIN_US 1050
 
+
 /*
 Enums
 */
@@ -33,5 +34,6 @@
 
 double compute_angle_correction(double consigne);
 double pwmFromAngle(double angleDeg);
+double positionOnTrack();
 
 #endif
\ No newline at end of file

--- a/utils.cpp	Wed Oct 02 22:25:12 2019 +0000
+++ b/utils.cpp	Thu Oct 03 23:28:56 2019 +0000
@@ -1,4 +1,5 @@
 #include "utils.h"
+#include "log.h"
 
 bool b_UTILS_flag_button_ESC = false;
 bool b_UTILS_flag_button_SECTIONS = false;
@@ -6,21 +7,26 @@
 Timer t_utils_timerSinceStart;
 s_Section* s_UTILS_currentSection = NULL;
 
-Serial rs_UTILS_pc(USBTX, USBRX);
+
 Serial rs_UTILS_odroid(PC_10, PC_11,115200);
 InterruptIn b_UTILS_button(USER_BUTTON);
 InterruptIn b_UTILS_emergency_signal(PD_0);
 
+int i,j;
 
 void it_pressed()
 {
-    rs_UTILS_pc.printf("t_utils_timerSinceStart.read_ms(): %ld\r\n", t_utils_timerSinceStart.read_ms());
+    rs_LOG_pc.printf("t_utils_timerSinceStart.read_ms(): %ld\r\n", t_utils_timerSinceStart.read_ms());
     if(t_utils_timerSinceStart.read_ms() > 500) {
-        t_utils_timerSinceStart.reset();
-        rs_UTILS_pc.printf("Button pressed\r\n");
-        b_UTILS_flag_button_SECTIONS = true;
-        b_UTILS_flag_button_ESC = true;
-        b_UTILS_flag_emergency_stop = false;
+        rs_LOG_pc.printf("Button pressed\r\n");
+        if(b_LOG_READY) {
+            b_LOG_READY = false;
+        } else {
+            t_utils_timerSinceStart.reset();
+            b_UTILS_flag_button_SECTIONS = true;
+            b_UTILS_flag_button_ESC = true;
+            b_UTILS_flag_emergency_stop = false;
+        }
     }
     return;
 }
@@ -28,4 +34,23 @@
 void it_emergency_signal_recieved()
 {
     b_UTILS_flag_emergency_stop = true;
+}
+
+void bubbleSort(double arr[], int n)
+{
+    for (i = 0; i < n-1; i++) {
+        // Last i elements are already in place
+        for (j = 0; j < n-i-1; j++) {
+            if (arr[j] > arr[j+1])  {
+                swap(&arr[j], &arr[j+1]);
+            }
+        }
     }
+}
+
+void swap(double *xp, double *yp)
+{
+    double temp = *xp;
+    *xp = *yp;
+    *yp = temp;
+}
\ No newline at end of file

--- a/utils.h	Wed Oct 02 22:25:12 2019 +0000
+++ b/utils.h	Thu Oct 03 23:28:56 2019 +0000
@@ -3,6 +3,7 @@
 
 #include "mbed.h"
 #include "chassis_mode.h"
+#include "log.h"
 
 /*
 Structs
@@ -18,7 +19,6 @@
    s_section* nextSection;
 }s_Section;
 
-
 /*
 Globals
 */
@@ -27,7 +27,6 @@
 extern bool b_UTILS_flag_button_SECTIONS;
 extern bool b_UTILS_flag_emergency_stop;
 
-extern Serial rs_UTILS_pc;
 extern Serial rs_UTILS_odroid;
 extern InterruptIn b_UTILS_button;
 extern InterruptIn b_UTILS_emergency_signal;
@@ -40,5 +39,7 @@
 */
 void it_pressed();
 void it_emergency_signal_recieved();
+void bubbleSort(double arr[], int n);
+void swap(double *xp, double *yp);
 
 #endif
\ No newline at end of file