TRR 2018 / Mbed 2 deprecated biniou-TRR2019-DLVV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050

sm_servo.cpp@8:f23601373e8b, 2019-10-02 (annotated)

Committer:
GaspardD
Date:
Wed Oct 02 22:25:12 2019 +0000
Revision:
8:f23601373e8b
Parent:
6:ab9f3695633f
Child:
9:1b54bac6d9a7
angle correction working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 2:e9d928fd327a 1 #include "sm_servo.h"
GaspardD 2:e9d928fd327a 2
GaspardD 2:e9d928fd327a 3 PwmOut pwm_Servo(PE_9);
GaspardD 8:f23601373e8b 4
GaspardD 8:f23601373e8b 5 // declaration des entrées analogiques
GaspardD 8:f23601373e8b 6 AnalogIn front(PA_5);
GaspardD 8:f23601373e8b 7 AnalogIn ana_left_90(PA_4);
GaspardD 8:f23601373e8b 8 AnalogIn ana_right_90(PF_10);
GaspardD 8:f23601373e8b 9 AnalogIn ana_left_45(PC_0);
GaspardD 8:f23601373e8b 10 AnalogIn ana_right_45(PF_4);
GaspardD 8:f23601373e8b 11 AnalogIn ana_left_5(PF_9);
GaspardD 8:f23601373e8b 12 AnalogIn ana_right_5(PA_6);
GaspardD 8:f23601373e8b 13
GaspardD 8:f23601373e8b 14 AnalogIn analog_value_PF3(PF_3);
GaspardD 8:f23601373e8b 15
GaspardD 2:e9d928fd327a 16 E_STATE_SERVO e_stateServo;
GaspardD 3:1b7eb426247e 17 bool directionCheck = false;
GaspardD 6:ab9f3695633f 18 int pulsewidth = SERVO_PULSE_MIDDLE_US;//SERVO_PULSE_MAX_US;
GaspardD 2:e9d928fd327a 19
GaspardD 8:f23601373e8b 20 double d_last_odom_asservissement = 0;
GaspardD 8:f23601373e8b 21
GaspardD 8:f23601373e8b 22 double d_prev_dist_left_90;
GaspardD 8:f23601373e8b 23 double d_prev_dist_right_90;
GaspardD 8:f23601373e8b 24 double d_dist_left_90;
GaspardD 8:f23601373e8b 25 double d_dist_right_90;
GaspardD 8:f23601373e8b 26
GaspardD 8:f23601373e8b 27 double coef_positionnement;
GaspardD 8:f23601373e8b 28 double coef_angle_correction_bordure;
GaspardD 8:f23601373e8b 29 double angle_correction_final ;
GaspardD 8:f23601373e8b 30 double angle_correction_position;
GaspardD 8:f23601373e8b 31 double angle_correction_bordure;
GaspardD 8:f23601373e8b 32 double distance_parcourue;
GaspardD 8:f23601373e8b 33
GaspardD 8:f23601373e8b 34
GaspardD 2:e9d928fd327a 35 void init_sm_servo()
GaspardD 2:e9d928fd327a 36 {
GaspardD 2:e9d928fd327a 37 e_stateServo = SERVO_INIT;
GaspardD 2:e9d928fd327a 38 }
GaspardD 2:e9d928fd327a 39
GaspardD 8:f23601373e8b 40 double pwmFromAngle(double angleDeg)
GaspardD 8:f23601373e8b 41 {
GaspardD 8:f23601373e8b 42 //on a une regression linéaire entre l'angle et la pwm on centre sur 0 puis on applique
GaspardD 8:f23601373e8b 43 pulsewidth = (angleDeg * 10.0) + SERVO_PULSE_MIDDLE_US;
GaspardD 8:f23601373e8b 44 //rs_UTILS_pc.printf("pulsewidth = %d =(%f * 10.0) + 1500\r\n",pulsewidth,angleDeg);
GaspardD 8:f23601373e8b 45 rs_UTILS_pc.printf("TIME: %d\r\n",t_utils_timerSinceStart.read_ms());
GaspardD 8:f23601373e8b 46 if(pulsewidth > SERVO_PULSE_MAX_US) {
GaspardD 8:f23601373e8b 47 pulsewidth = SERVO_PULSE_MAX_US;
GaspardD 8:f23601373e8b 48 } else if(pulsewidth < SERVO_PULSE_MIN_US) {
GaspardD 8:f23601373e8b 49 pulsewidth = SERVO_PULSE_MIN_US;
GaspardD 8:f23601373e8b 50 }
GaspardD 8:f23601373e8b 51
GaspardD 8:f23601373e8b 52 return pulsewidth;
GaspardD 8:f23601373e8b 53 }
GaspardD 8:f23601373e8b 54
GaspardD 8:f23601373e8b 55
GaspardD 8:f23601373e8b 56
GaspardD 8:f23601373e8b 57 double distcapteur;
GaspardD 8:f23601373e8b 58
GaspardD 8:f23601373e8b 59 double getDistCapteur(AnalogIn* p)
GaspardD 8:f23601373e8b 60 {
GaspardD 8:f23601373e8b 61 double d = 0.182/(double)p->read() ;
GaspardD 8:f23601373e8b 62 if (d > 1.5){
GaspardD 8:f23601373e8b 63 d = 1.5 ;
GaspardD 8:f23601373e8b 64 }
GaspardD 8:f23601373e8b 65 return d ;
GaspardD 8:f23601373e8b 66 }
GaspardD 8:f23601373e8b 67
GaspardD 8:f23601373e8b 68
GaspardD 8:f23601373e8b 69
GaspardD 8:f23601373e8b 70 double compute_angle_correction(double consignePos)
GaspardD 8:f23601373e8b 71 {
GaspardD 8:f23601373e8b 72 //aquisition
GaspardD 8:f23601373e8b 73 coef_positionnement = 0.01;
GaspardD 8:f23601373e8b 74 coef_angle_correction_bordure = 1.0;
GaspardD 8:f23601373e8b 75 d_dist_left_90 = getDistCapteur(&ana_left_90);
GaspardD 8:f23601373e8b 76 d_dist_right_90 = getDistCapteur(&ana_right_90);
GaspardD 8:f23601373e8b 77
GaspardD 8:f23601373e8b 78 angle_correction_final = 0.0;
GaspardD 8:f23601373e8b 79 angle_correction_position = 0.0;
GaspardD 8:f23601373e8b 80 angle_correction_bordure = 0.0;
GaspardD 8:f23601373e8b 81
GaspardD 8:f23601373e8b 82 update_speed();
GaspardD 8:f23601373e8b 83
GaspardD 8:f23601373e8b 84 distance_parcourue = d_ODOM_computed_pos_m - d_last_odom_asservissement + 0.001;
GaspardD 8:f23601373e8b 85 //rs_UTILS_pc.printf("distance_parcourue = %f = %f - %f + 0.001 \n\r",distance_parcourue,d_ODOM_computed_pos_m,d_last_odom_asservissement );
GaspardD 8:f23601373e8b 86 angle_correction_bordure = atan((d_prev_dist_left_90 - d_dist_left_90)/ distance_parcourue)*57.296;
GaspardD 8:f23601373e8b 87 //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) ;
GaspardD 8:f23601373e8b 88 angle_correction_position = atan((consignePos - d_dist_left_90)/ (0.04*d_ODOM_speed_mps))*57.296;
GaspardD 8:f23601373e8b 89 //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());
GaspardD 8:f23601373e8b 90
GaspardD 8:f23601373e8b 91 angle_correction_final = angle_correction_position * coef_positionnement + angle_correction_bordure*coef_angle_correction_bordure ;
GaspardD 8:f23601373e8b 92 //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);
GaspardD 8:f23601373e8b 93
GaspardD 8:f23601373e8b 94 d_last_odom_asservissement = d_ODOM_computed_pos_m;
GaspardD 8:f23601373e8b 95 d_prev_dist_left_90 = d_dist_left_90;
GaspardD 8:f23601373e8b 96 d_prev_dist_right_90 = d_dist_right_90;
GaspardD 8:f23601373e8b 97 return angle_correction_final;
GaspardD 8:f23601373e8b 98 }
GaspardD 8:f23601373e8b 99
GaspardD 8:f23601373e8b 100
GaspardD 2:e9d928fd327a 101 void update_sm_servo()
GaspardD 2:e9d928fd327a 102 {
GaspardD 3:1b7eb426247e 103 E_STATE_SERVO next_state = e_stateServo;
GaspardD 2:e9d928fd327a 104
GaspardD 2:e9d928fd327a 105 switch(e_stateServo) {
GaspardD 2:e9d928fd327a 106 case SERVO_INIT:
GaspardD 3:1b7eb426247e 107 if(directionCheck) {
GaspardD 3:1b7eb426247e 108 next_state = SERVO_COMMAND;
GaspardD 3:1b7eb426247e 109 }
GaspardD 2:e9d928fd327a 110 break;
GaspardD 2:e9d928fd327a 111 case SERVO_COMMAND:
GaspardD 8:f23601373e8b 112 //si on detecte une nouvelle acquisition, on traite le signal
GaspardD 8:f23601373e8b 113 d_dist_left_90 = getDistCapteur(&ana_left_90);
GaspardD 8:f23601373e8b 114 rs_UTILS_pc.printf("d_dist_left_90 : %f\r\n",d_dist_left_90);
GaspardD 8:f23601373e8b 115 if(d_dist_left_90 != d_prev_dist_left_90) {
GaspardD 8:f23601373e8b 116
GaspardD 8:f23601373e8b 117 //rs_UTILS_pc.printf("compute angle et updateSpeed\r\n");
GaspardD 8:f23601373e8b 118 pulsewidth = pwmFromAngle( compute_angle_correction(s_UTILS_currentSection->consigne_position) );
GaspardD 8:f23601373e8b 119
GaspardD 8:f23601373e8b 120 d_prev_dist_left_90 = d_dist_left_90;
GaspardD 8:f23601373e8b 121
GaspardD 8:f23601373e8b 122 }
GaspardD 8:f23601373e8b 123
GaspardD 8:f23601373e8b 124
GaspardD 2:e9d928fd327a 125 break;
GaspardD 2:e9d928fd327a 126 default:
GaspardD 3:1b7eb426247e 127 break;
GaspardD 3:1b7eb426247e 128 }
GaspardD 2:e9d928fd327a 129
GaspardD 2:e9d928fd327a 130 e_stateServo = next_state;
GaspardD 2:e9d928fd327a 131 return;
GaspardD 2:e9d928fd327a 132 }
GaspardD 2:e9d928fd327a 133
GaspardD 2:e9d928fd327a 134 void output_sm_servo()
GaspardD 2:e9d928fd327a 135 {
GaspardD 3:1b7eb426247e 136 switch(e_stateServo) {
GaspardD 3:1b7eb426247e 137 case SERVO_INIT:
GaspardD 8:f23601373e8b 138 rs_UTILS_pc.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
GaspardD 3:1b7eb426247e 139 pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
GaspardD 3:1b7eb426247e 140 pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
GaspardD 3:1b7eb426247e 141 directionCheck = true;
GaspardD 3:1b7eb426247e 142 break;
GaspardD 3:1b7eb426247e 143 case SERVO_COMMAND:
GaspardD 4:efa207509f63 144 pwm_Servo.pulsewidth_us(pulsewidth);
GaspardD 4:efa207509f63 145
GaspardD 3:1b7eb426247e 146 break;
GaspardD 3:1b7eb426247e 147 default:
GaspardD 3:1b7eb426247e 148 break;
GaspardD 3:1b7eb426247e 149 }
GaspardD 3:1b7eb426247e 150 return;
GaspardD 2:e9d928fd327a 151 }