Diff: sm_servo.cpp

Revision:

10:e63fe4080760

Parent:

9:1b54bac6d9a7

Child:

12:58ad06f9847d

diff -r 1b54bac6d9a7 -r e63fe4080760 sm_servo.cpp
--- a/sm_servo.cpp	Thu Oct 03 23:28:56 2019 +0000
+++ b/sm_servo.cpp	Fri Oct 04 09:46:00 2019 +0000
@@ -57,7 +57,18 @@
     } else if(pulsewidth < SERVO_PULSE_MIN_US) {
         pulsewidth = SERVO_PULSE_MIN_US;
     }
+    
+    //logging
     s_LOG_history[i_LOG_index_data].pwm_dir = pulsewidth;
+    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 pulsewidth;
 }
 
@@ -84,7 +95,7 @@
 {
     //aquisition
     coef_positionnement = 0.1;//0.05
-    coef_angle_correction_bordure = 1.0;
+    coef_angle_correction_bordure = 1.5;
     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;
@@ -141,7 +152,7 @@
     //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_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);
+    //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;
@@ -149,16 +160,6 @@
     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;
 }
 
@@ -206,8 +207,8 @@
             directionCheck = true;
             break;
         case SERVO_COMMAND:
-            //pwm_Servo.pulsewidth_us(pulsewidth);
-            pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
+            pwm_Servo.pulsewidth_us(pulsewidth);
+            //pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
 
             break;
         default: