Diff: sm_servo.cpp

Revision:

9:1b54bac6d9a7

Parent:

8:f23601373e8b

Child:

10:e63fe4080760

--- 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: