Diff: sm_servo.cpp

Revision:

12:58ad06f9847d

Parent:

10:e63fe4080760

Child:

14:914467165f34

--- a/sm_servo.cpp	Fri Oct 04 09:58:51 2019 +0000
+++ b/sm_servo.cpp	Fri Oct 04 22:20:23 2019 +0000
@@ -18,6 +18,7 @@
 int pulsewidth = SERVO_PULSE_MIDDLE_US;//SERVO_PULSE_MAX_US;
 
 double d_last_odom_asservissement = 0;
+int i_lastTimeAsserv;
 
 double d_prev_dist_left_90;
 double d_prev_dist_right_90;
@@ -39,8 +40,12 @@
 double distance_parcourue;
 
 double ponderation_angle[6];
+double mask_table[6];
 double d_mediane;
+double d_moyenne;
+double d_sumMask;
 double distCapt;
+double d_SERVO_coefFreinage = 0.0;
 
 void init_sm_servo()
 {
@@ -57,18 +62,7 @@
     } 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;
 }
 
@@ -93,15 +87,16 @@
 
 double compute_angle_correction(double consignePos)
 {
+    if(t_utils_timerSinceStart.read_ms() - i_lastTimeAsserv  > 40){
     //aquisition
-    coef_positionnement = 0.1;//0.05
-    coef_angle_correction_bordure = 1.5;
+    coef_positionnement = s_UTILS_currentSection->coef_pos;
+    coef_angle_correction_bordure = s_UTILS_currentSection->coef_bord;
+
     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;
     angle_correction_bordure = 0.0;
@@ -121,38 +116,37 @@
 
     //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] );
 
+    s_LOG_history[i_LOG_index_data].angle_bord0 = ponderation_angle[0];
+    s_LOG_history[i_LOG_index_data].angle_bord1 = ponderation_angle[1];
+    s_LOG_history[i_LOG_index_data].angle_bord2 = ponderation_angle[2];
+    s_LOG_history[i_LOG_index_data].angle_bord3 = ponderation_angle[3];
+    s_LOG_history[i_LOG_index_data].angle_bord4 = ponderation_angle[4];
+    s_LOG_history[i_LOG_index_data].angle_bord5 = ponderation_angle[5];
+    s_LOG_history[i_LOG_index_data].distParcourue = distance_parcourue;
 
     //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];
-        }
+    if(d_ODOM_speed_mps < 50.0 && d_ODOM_speed_mps > 0.01)
+    {
+        bubbleSort(ponderation_angle,6);
+        d_mediane = (ponderation_angle[2] + ponderation_angle[3])*0.5;
     }
-    */
+    else
+    {
+        d_mediane = (ponderation_angle[4] + ponderation_angle[5])*0.5; 
+    }
+
     angle_correction_bordure = d_mediane;
 
-
-    angle_correction_position = atan2((consignePos - positionOnTrack()), (0.04*d_ODOM_speed_mps))*57.296;
+    angle_correction_position = (consignePos - positionOnTrack());
 
     //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_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;
@@ -160,6 +154,25 @@
     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].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;
+
+    s_LOG_history[i_LOG_index_data].angle_pos = angle_correction_position;
+    s_LOG_history[i_LOG_index_data].angle = angle_correction_final;
+
+    log_check();
+
+    
+    d_SERVO_coefFreinage = (abs(angle_correction_final)/90);
+    i_lastTimeAsserv = t_utils_timerSinceStart.read_ms();
+    }
     return  angle_correction_final;
 }
 
@@ -201,14 +214,13 @@
 {
     switch(e_stateServo) {
         case SERVO_INIT:
-            rs_LOG_pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
+            rs_LOG.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(SERVO_PULSE_MIDDLE_US);
 
             break;
         default: