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@12:58ad06f9847d, 2019-10-04 (annotated)

Committer:
GaspardD
Date:
Fri Oct 04 22:20:23 2019 +0000
Revision:
12:58ad06f9847d
Parent:
10:e63fe4080760
Child:
14:914467165f34
dernier code

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 12:58ad06f9847d 21 int i_lastTimeAsserv;
GaspardD 8:f23601373e8b 22
GaspardD 8:f23601373e8b 23 double d_prev_dist_left_90;
GaspardD 8:f23601373e8b 24 double d_prev_dist_right_90;
GaspardD 8:f23601373e8b 25 double d_dist_left_90;
GaspardD 8:f23601373e8b 26 double d_dist_right_90;
GaspardD 8:f23601373e8b 27
GaspardD 9:1b54bac6d9a7 28 double d_prev_dist_left_45;
GaspardD 9:1b54bac6d9a7 29 double d_prev_dist_right_45;
GaspardD 9:1b54bac6d9a7 30 double d_dist_left_45;
GaspardD 9:1b54bac6d9a7 31 double d_dist_right_45;
GaspardD 9:1b54bac6d9a7 32
GaspardD 9:1b54bac6d9a7 33 double d_positionOnTrack;
GaspardD 9:1b54bac6d9a7 34
GaspardD 8:f23601373e8b 35 double coef_positionnement;
GaspardD 8:f23601373e8b 36 double coef_angle_correction_bordure;
GaspardD 8:f23601373e8b 37 double angle_correction_final ;
GaspardD 8:f23601373e8b 38 double angle_correction_position;
GaspardD 8:f23601373e8b 39 double angle_correction_bordure;
GaspardD 8:f23601373e8b 40 double distance_parcourue;
GaspardD 8:f23601373e8b 41
GaspardD 9:1b54bac6d9a7 42 double ponderation_angle[6];
GaspardD 12:58ad06f9847d 43 double mask_table[6];
GaspardD 9:1b54bac6d9a7 44 double d_mediane;
GaspardD 12:58ad06f9847d 45 double d_moyenne;
GaspardD 12:58ad06f9847d 46 double d_sumMask;
GaspardD 9:1b54bac6d9a7 47 double distCapt;
GaspardD 12:58ad06f9847d 48 double d_SERVO_coefFreinage = 0.0;
GaspardD 8:f23601373e8b 49
GaspardD 2:e9d928fd327a 50 void init_sm_servo()
GaspardD 2:e9d928fd327a 51 {
GaspardD 2:e9d928fd327a 52 e_stateServo = SERVO_INIT;
GaspardD 2:e9d928fd327a 53 }
GaspardD 2:e9d928fd327a 54
GaspardD 8:f23601373e8b 55 double pwmFromAngle(double angleDeg)
GaspardD 8:f23601373e8b 56 {
GaspardD 8:f23601373e8b 57 //on a une regression linéaire entre l'angle et la pwm on centre sur 0 puis on applique
GaspardD 9:1b54bac6d9a7 58 pulsewidth = (d_CHASSIS_inversion*angleDeg * 11.0) + SERVO_PULSE_MIDDLE_US;
GaspardD 9:1b54bac6d9a7 59 //rs_LOG_pc.printf("pulsewidth = %d =(%f * 11.0) + 1500\r\n",pulsewidth,angleDeg);
GaspardD 8:f23601373e8b 60 if(pulsewidth > SERVO_PULSE_MAX_US) {
GaspardD 8:f23601373e8b 61 pulsewidth = SERVO_PULSE_MAX_US;
GaspardD 8:f23601373e8b 62 } else if(pulsewidth < SERVO_PULSE_MIN_US) {
GaspardD 8:f23601373e8b 63 pulsewidth = SERVO_PULSE_MIN_US;
GaspardD 8:f23601373e8b 64 }
GaspardD 12:58ad06f9847d 65
GaspardD 8:f23601373e8b 66 return pulsewidth;
GaspardD 8:f23601373e8b 67 }
GaspardD 8:f23601373e8b 68
GaspardD 8:f23601373e8b 69
GaspardD 8:f23601373e8b 70
GaspardD 8:f23601373e8b 71 double getDistCapteur(AnalogIn* p)
GaspardD 8:f23601373e8b 72 {
GaspardD 9:1b54bac6d9a7 73 distCapt = 0.182/(double)p->read() ;
GaspardD 9:1b54bac6d9a7 74 if (distCapt > 1.5) {
GaspardD 9:1b54bac6d9a7 75 distCapt = 1.5 ;
GaspardD 9:1b54bac6d9a7 76 }
GaspardD 9:1b54bac6d9a7 77 return distCapt ;
GaspardD 8:f23601373e8b 78 }
GaspardD 8:f23601373e8b 79
GaspardD 9:1b54bac6d9a7 80 //renvoie un double entre 0 et 1 avec 0 position extrème gauche et 1 extrème droite
GaspardD 9:1b54bac6d9a7 81 double positionOnTrack()
GaspardD 9:1b54bac6d9a7 82 {
GaspardD 9:1b54bac6d9a7 83 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) ));
GaspardD 8:f23601373e8b 84
GaspardD 9:1b54bac6d9a7 85 return d_positionOnTrack;
GaspardD 9:1b54bac6d9a7 86 }
GaspardD 8:f23601373e8b 87
GaspardD 8:f23601373e8b 88 double compute_angle_correction(double consignePos)
GaspardD 8:f23601373e8b 89 {
GaspardD 12:58ad06f9847d 90 if(t_utils_timerSinceStart.read_ms() - i_lastTimeAsserv > 40){
GaspardD 8:f23601373e8b 91 //aquisition
GaspardD 12:58ad06f9847d 92 coef_positionnement = s_UTILS_currentSection->coef_pos;
GaspardD 12:58ad06f9847d 93 coef_angle_correction_bordure = s_UTILS_currentSection->coef_bord;
GaspardD 12:58ad06f9847d 94
GaspardD 9:1b54bac6d9a7 95 d_dist_left_90 = getDistCapteur(&ana_left_90) + 0.016;
GaspardD 9:1b54bac6d9a7 96 d_dist_right_90 = getDistCapteur(&ana_right_90) + 0.016;
GaspardD 9:1b54bac6d9a7 97 d_dist_left_45 = getDistCapteur(&ana_left_45) + 0.042;
GaspardD 9:1b54bac6d9a7 98 d_dist_right_45 = getDistCapteur(&ana_right_45) + 0.042;
GaspardD 9:1b54bac6d9a7 99
GaspardD 8:f23601373e8b 100 angle_correction_final = 0.0;
GaspardD 8:f23601373e8b 101 angle_correction_position = 0.0;
GaspardD 8:f23601373e8b 102 angle_correction_bordure = 0.0;
GaspardD 8:f23601373e8b 103
GaspardD 8:f23601373e8b 104 update_speed();
GaspardD 8:f23601373e8b 105
GaspardD 9:1b54bac6d9a7 106 distance_parcourue = d_ODOM_computed_pos_m - d_last_odom_asservissement ;
GaspardD 9:1b54bac6d9a7 107
GaspardD 9:1b54bac6d9a7 108 //left 90
GaspardD 9:1b54bac6d9a7 109 ponderation_angle[0] = atan2( (d_prev_dist_left_90 - d_dist_left_90), distance_parcourue)*57.296 ;
GaspardD 9:1b54bac6d9a7 110 //right 90
GaspardD 9:1b54bac6d9a7 111 ponderation_angle[1] = -atan2( (d_prev_dist_right_90 - d_dist_right_90), distance_parcourue)*57.296 ;
GaspardD 9:1b54bac6d9a7 112 //left 45
GaspardD 9:1b54bac6d9a7 113 ponderation_angle[2] = atan2( ((d_prev_dist_left_45 - d_dist_left_45)*0.7071), distance_parcourue)*57.296 ;
GaspardD 9:1b54bac6d9a7 114 //right 45
GaspardD 9:1b54bac6d9a7 115 ponderation_angle[3] = -atan2( ((d_prev_dist_right_45 - d_dist_right_45)*0.7071), distance_parcourue)*57.296 ;
GaspardD 9:1b54bac6d9a7 116
GaspardD 9:1b54bac6d9a7 117 //angle detecte entre le 45 et le 90 gauche
GaspardD 9:1b54bac6d9a7 118 ponderation_angle[4] = atan(1.4142*(d_prev_dist_left_90/d_prev_dist_left_45) - 1)*57.296;
GaspardD 9:1b54bac6d9a7 119
GaspardD 9:1b54bac6d9a7 120 //angle detecte entre le 45 et le 90 droite
GaspardD 9:1b54bac6d9a7 121 ponderation_angle[5] = -atan(1.4142*(d_prev_dist_right_90/d_prev_dist_right_45) - 1)*57.296;
GaspardD 9:1b54bac6d9a7 122
GaspardD 12:58ad06f9847d 123 s_LOG_history[i_LOG_index_data].angle_bord0 = ponderation_angle[0];
GaspardD 12:58ad06f9847d 124 s_LOG_history[i_LOG_index_data].angle_bord1 = ponderation_angle[1];
GaspardD 12:58ad06f9847d 125 s_LOG_history[i_LOG_index_data].angle_bord2 = ponderation_angle[2];
GaspardD 12:58ad06f9847d 126 s_LOG_history[i_LOG_index_data].angle_bord3 = ponderation_angle[3];
GaspardD 12:58ad06f9847d 127 s_LOG_history[i_LOG_index_data].angle_bord4 = ponderation_angle[4];
GaspardD 12:58ad06f9847d 128 s_LOG_history[i_LOG_index_data].angle_bord5 = ponderation_angle[5];
GaspardD 12:58ad06f9847d 129 s_LOG_history[i_LOG_index_data].distParcourue = distance_parcourue;
GaspardD 9:1b54bac6d9a7 130
GaspardD 9:1b54bac6d9a7 131 //votation
GaspardD 12:58ad06f9847d 132 if(d_ODOM_speed_mps < 50.0 && d_ODOM_speed_mps > 0.01)
GaspardD 12:58ad06f9847d 133 {
GaspardD 12:58ad06f9847d 134 bubbleSort(ponderation_angle,6);
GaspardD 12:58ad06f9847d 135 d_mediane = (ponderation_angle[2] + ponderation_angle[3])*0.5;
GaspardD 9:1b54bac6d9a7 136 }
GaspardD 12:58ad06f9847d 137 else
GaspardD 12:58ad06f9847d 138 {
GaspardD 12:58ad06f9847d 139 d_mediane = (ponderation_angle[4] + ponderation_angle[5])*0.5;
GaspardD 12:58ad06f9847d 140 }
GaspardD 12:58ad06f9847d 141
GaspardD 9:1b54bac6d9a7 142 angle_correction_bordure = d_mediane;
GaspardD 9:1b54bac6d9a7 143
GaspardD 12:58ad06f9847d 144 angle_correction_position = (consignePos - positionOnTrack());
GaspardD 9:1b54bac6d9a7 145
GaspardD 9:1b54bac6d9a7 146 //rs_LOG_pc.printf("distance_parcourue = %f = %f - %f + 0.001 \n\r",distance_parcourue,d_ODOM_computed_pos_m,d_last_odom_asservissement );
GaspardD 9:1b54bac6d9a7 147 //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) ;
GaspardD 9:1b54bac6d9a7 148
GaspardD 8:f23601373e8b 149 angle_correction_final = angle_correction_position * coef_positionnement + angle_correction_bordure*coef_angle_correction_bordure ;
GaspardD 8:f23601373e8b 150
GaspardD 8:f23601373e8b 151 d_last_odom_asservissement = d_ODOM_computed_pos_m;
GaspardD 8:f23601373e8b 152 d_prev_dist_left_90 = d_dist_left_90;
GaspardD 8:f23601373e8b 153 d_prev_dist_right_90 = d_dist_right_90;
GaspardD 9:1b54bac6d9a7 154 d_prev_dist_left_45 = d_dist_left_45;
GaspardD 9:1b54bac6d9a7 155 d_prev_dist_right_45 = d_dist_right_45;
GaspardD 9:1b54bac6d9a7 156
GaspardD 12:58ad06f9847d 157
GaspardD 12:58ad06f9847d 158 //logging
GaspardD 12:58ad06f9847d 159 s_LOG_history[i_LOG_index_data].pwm_dir = pulsewidth;
GaspardD 12:58ad06f9847d 160 s_LOG_history[i_LOG_index_data].left_90 = d_dist_left_90;
GaspardD 12:58ad06f9847d 161 s_LOG_history[i_LOG_index_data].right_90 = d_dist_right_90;
GaspardD 12:58ad06f9847d 162 s_LOG_history[i_LOG_index_data].left_45 = d_dist_left_45;
GaspardD 12:58ad06f9847d 163 s_LOG_history[i_LOG_index_data].right_45 = d_dist_right_45;
GaspardD 12:58ad06f9847d 164 s_LOG_history[i_LOG_index_data].odom = d_ODOM_distFromGlobalStart_m;
GaspardD 12:58ad06f9847d 165 s_LOG_history[i_LOG_index_data].speed = d_ODOM_speed_mps;
GaspardD 12:58ad06f9847d 166
GaspardD 12:58ad06f9847d 167 s_LOG_history[i_LOG_index_data].angle_pos = angle_correction_position;
GaspardD 12:58ad06f9847d 168 s_LOG_history[i_LOG_index_data].angle = angle_correction_final;
GaspardD 12:58ad06f9847d 169
GaspardD 12:58ad06f9847d 170 log_check();
GaspardD 12:58ad06f9847d 171
GaspardD 12:58ad06f9847d 172
GaspardD 12:58ad06f9847d 173 d_SERVO_coefFreinage = (abs(angle_correction_final)/90);
GaspardD 12:58ad06f9847d 174 i_lastTimeAsserv = t_utils_timerSinceStart.read_ms();
GaspardD 12:58ad06f9847d 175 }
GaspardD 8:f23601373e8b 176 return angle_correction_final;
GaspardD 8:f23601373e8b 177 }
GaspardD 8:f23601373e8b 178
GaspardD 8:f23601373e8b 179
GaspardD 2:e9d928fd327a 180 void update_sm_servo()
GaspardD 2:e9d928fd327a 181 {
GaspardD 3:1b7eb426247e 182 E_STATE_SERVO next_state = e_stateServo;
GaspardD 2:e9d928fd327a 183
GaspardD 2:e9d928fd327a 184 switch(e_stateServo) {
GaspardD 2:e9d928fd327a 185 case SERVO_INIT:
GaspardD 3:1b7eb426247e 186 if(directionCheck) {
GaspardD 3:1b7eb426247e 187 next_state = SERVO_COMMAND;
GaspardD 3:1b7eb426247e 188 }
GaspardD 2:e9d928fd327a 189 break;
GaspardD 2:e9d928fd327a 190 case SERVO_COMMAND:
GaspardD 8:f23601373e8b 191 //si on detecte une nouvelle acquisition, on traite le signal
GaspardD 8:f23601373e8b 192 d_dist_left_90 = getDistCapteur(&ana_left_90);
GaspardD 9:1b54bac6d9a7 193 //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);
GaspardD 8:f23601373e8b 194 if(d_dist_left_90 != d_prev_dist_left_90) {
GaspardD 8:f23601373e8b 195
GaspardD 9:1b54bac6d9a7 196 //rs_LOG_pc.printf("compute angle et updateSpeed\r\n");
GaspardD 8:f23601373e8b 197 pulsewidth = pwmFromAngle( compute_angle_correction(s_UTILS_currentSection->consigne_position) );
GaspardD 8:f23601373e8b 198
GaspardD 8:f23601373e8b 199 d_prev_dist_left_90 = d_dist_left_90;
GaspardD 8:f23601373e8b 200
GaspardD 8:f23601373e8b 201 }
GaspardD 8:f23601373e8b 202
GaspardD 8:f23601373e8b 203
GaspardD 2:e9d928fd327a 204 break;
GaspardD 2:e9d928fd327a 205 default:
GaspardD 3:1b7eb426247e 206 break;
GaspardD 3:1b7eb426247e 207 }
GaspardD 2:e9d928fd327a 208
GaspardD 2:e9d928fd327a 209 e_stateServo = next_state;
GaspardD 2:e9d928fd327a 210 return;
GaspardD 2:e9d928fd327a 211 }
GaspardD 2:e9d928fd327a 212
GaspardD 2:e9d928fd327a 213 void output_sm_servo()
GaspardD 2:e9d928fd327a 214 {
GaspardD 3:1b7eb426247e 215 switch(e_stateServo) {
GaspardD 3:1b7eb426247e 216 case SERVO_INIT:
GaspardD 12:58ad06f9847d 217 rs_LOG.printf("init servo with pulse %d us\r\n",SERVO_PULSE_MIDDLE_US);
GaspardD 3:1b7eb426247e 218 pwm_Servo.period_ms(SERVO_PERIOD_DURATION_MS);
GaspardD 3:1b7eb426247e 219 pwm_Servo.pulsewidth_us(SERVO_PULSE_MIDDLE_US);
GaspardD 3:1b7eb426247e 220 directionCheck = true;
GaspardD 3:1b7eb426247e 221 break;
GaspardD 3:1b7eb426247e 222 case SERVO_COMMAND:
GaspardD 10:e63fe4080760 223 pwm_Servo.pulsewidth_us(pulsewidth);
GaspardD 4:efa207509f63 224
GaspardD 3:1b7eb426247e 225 break;
GaspardD 3:1b7eb426247e 226 default:
GaspardD 3:1b7eb426247e 227 break;
GaspardD 3:1b7eb426247e 228 }
GaspardD 3:1b7eb426247e 229 return;
GaspardD 2:e9d928fd327a 230 }