Diff: sm_servo.cpp

Revision:

8:f23601373e8b

Parent:

6:ab9f3695633f

Child:

9:1b54bac6d9a7

--- a/sm_servo.cpp	Sun Sep 29 20:22:43 2019 +0000
+++ b/sm_servo.cpp	Wed Oct 02 22:25:12 2019 +0000
@@ -1,15 +1,103 @@
 #include "sm_servo.h"
 
 PwmOut pwm_Servo(PE_9);
+
+// declaration des entrées analogiques
+AnalogIn front(PA_5);
+AnalogIn ana_left_90(PA_4);
+AnalogIn ana_right_90(PF_10);
+AnalogIn ana_left_45(PC_0);
+AnalogIn ana_right_45(PF_4);
+AnalogIn ana_left_5(PF_9);
+AnalogIn ana_right_5(PA_6);
+
+AnalogIn analog_value_PF3(PF_3);
+
 E_STATE_SERVO e_stateServo;
 bool directionCheck = false;
 int pulsewidth = SERVO_PULSE_MIDDLE_US;//SERVO_PULSE_MAX_US;
 
+double d_last_odom_asservissement = 0;
+
+double d_prev_dist_left_90;
+double d_prev_dist_right_90;
+double d_dist_left_90;
+double d_dist_right_90;
+
+double coef_positionnement;
+double coef_angle_correction_bordure;
+double angle_correction_final ;
+double angle_correction_position;
+double angle_correction_bordure;
+double distance_parcourue;
+
+
 void init_sm_servo()
 {
     e_stateServo = SERVO_INIT;
 }
 
+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());
+    if(pulsewidth > SERVO_PULSE_MAX_US) {
+        pulsewidth =  SERVO_PULSE_MAX_US;
+    } else if(pulsewidth < SERVO_PULSE_MIN_US) {
+        pulsewidth = SERVO_PULSE_MIN_US;
+    }
+    
+    return pulsewidth;
+}
+
+
+
+double distcapteur;
+
+double getDistCapteur(AnalogIn* p)
+{
+    double d = 0.182/(double)p->read() ;     
+    if (d > 1.5){
+        d = 1.5 ;
+        }
+    return d ;
+}
+
+
+
+double compute_angle_correction(double consignePos)
+{
+    //aquisition
+    coef_positionnement = 0.01;
+    coef_angle_correction_bordure = 1.0;
+    d_dist_left_90 = getDistCapteur(&ana_left_90);
+    d_dist_right_90 = getDistCapteur(&ana_right_90);
+
+    angle_correction_final = 0.0;
+    angle_correction_position = 0.0;
+    angle_correction_bordure = 0.0;
+
+    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());
+    
+    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);
+
+    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;
+    return  angle_correction_final;
+}
+
+
 void update_sm_servo()
 {
     E_STATE_SERVO next_state = e_stateServo;
@@ -21,6 +109,19 @@
             }
             break;
         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);
+            if(d_dist_left_90 != d_prev_dist_left_90) {
+
+                //rs_UTILS_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;
+
+            }
+
+
             break;
         default:
             break;
@@ -34,28 +135,12 @@
 {
     switch(e_stateServo) {
         case SERVO_INIT:
-            wait(1);
-            pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
+            rs_UTILS_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:
-        /*
-            if(pulsewidth < SERVO_PULSE_MAX_US && directionCheck) {
-                pulsewidth += 1;
-                if (pulsewidth >= SERVO_PULSE_MAX_US) {
-                    directionCheck = false;
-                }
-            }  else if (pulsewidth > SERVO_PULSE_MIN_US && !directionCheck) {
-                pulsewidth -= 1;
-                if (pulsewidth <= SERVO_PULSE_MIN_US) {
-                    directionCheck = true;
-                }
-            }
-            pc.printf("send pulse %d to servo\r\n",pulsewidth);
-            */
-            
             pwm_Servo.pulsewidth_us(pulsewidth);
 
             break;