Alexandre Pirotte / Mbed 2 deprecated 0concours_cachan_programme_ok

ok

Dependencies:   mbed

Fork of _test_suivi_mur by christophe vermaelen

fct.cpp@3:b91371837109, 2017-05-29 (annotated)

Committer:
vermaelen
Date:
Mon May 29 12:01:43 2017 +0000
Revision:
3:b91371837109
Parent:
2:82b72fa8dbcd
Child:
4:78a9354fcee8
suivi avec etat de contournement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 2:82b72fa8dbcd 1 #include "mbed.h"
vermaelen 2:82b72fa8dbcd 2 #include "fct.h"
vermaelen 3:b91371837109 3 void contournement()
vermaelen 3:b91371837109 4 {
vermaelen 3:b91371837109 5 sensMG.write(0);
vermaelen 3:b91371837109 6 sensMD.write(0);
vermaelen 3:b91371837109 7 cmdD=30;
vermaelen 3:b91371837109 8 cmdG=10;
vermaelen 3:b91371837109 9 MD.pulsewidth(vitesse(cmdD));
vermaelen 3:b91371837109 10 MG.pulsewidth(vitesse(cmdG));
vermaelen 3:b91371837109 11
vermaelen 3:b91371837109 12 }
vermaelen 2:82b72fa8dbcd 13 void stopMotor()
vermaelen 2:82b72fa8dbcd 14 {
vermaelen 2:82b72fa8dbcd 15 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 16 sensMD.write(0);
vermaelen 2:82b72fa8dbcd 17 MG.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 18 MD.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 19 }
vermaelen 2:82b72fa8dbcd 20 void suivi_mur()
vermaelen 2:82b72fa8dbcd 21 {
vermaelen 3:b91371837109 22 E3=E2;
vermaelen 3:b91371837109 23 E2=E1;
vermaelen 3:b91371837109 24 E1=E0;
vermaelen 3:b91371837109 25 E0=US2-US1;
vermaelen 3:b91371837109 26 cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-20);
vermaelen 3:b91371837109 27 cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-20);
vermaelen 3:b91371837109 28
vermaelen 2:82b72fa8dbcd 29 MD.pulsewidth(vitesse(cmdD));
vermaelen 2:82b72fa8dbcd 30 MG.pulsewidth(vitesse(cmdG));
vermaelen 3:b91371837109 31
vermaelen 3:b91371837109 32 wait(0.001);
vermaelen 2:82b72fa8dbcd 33 }
vermaelen 2:82b72fa8dbcd 34 void rotation_horaire()
vermaelen 2:82b72fa8dbcd 35 {
vermaelen 2:82b72fa8dbcd 36 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 37 sensMD.write(1);
vermaelen 2:82b72fa8dbcd 38 MG.pulsewidth(vitesse(20));
vermaelen 2:82b72fa8dbcd 39 MD.pulsewidth(vitesse(20));
vermaelen 2:82b72fa8dbcd 40 }
vermaelen 2:82b72fa8dbcd 41 void init()
vermaelen 2:82b72fa8dbcd 42 {
vermaelen 2:82b72fa8dbcd 43 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 44 sensMD.write(0);
vermaelen 2:82b72fa8dbcd 45 MG.period(PERIOD);
vermaelen 2:82b72fa8dbcd 46 MD.period(PERIOD);
vermaelen 2:82b72fa8dbcd 47 MG.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 48 MD.pulsewidth(vitesse(0));
vermaelen 3:b91371837109 49 tic1.attach(&fcttrig,0.035);
vermaelen 2:82b72fa8dbcd 50 tic2.attach(&mesAN,0.01);
vermaelen 2:82b72fa8dbcd 51 echo.rise(&start);
vermaelen 2:82b72fa8dbcd 52 echo.fall(&stop);
vermaelen 2:82b72fa8dbcd 53 }
vermaelen 2:82b72fa8dbcd 54 void mesAN()
vermaelen 2:82b72fa8dbcd 55 {
vermaelen 2:82b72fa8dbcd 56 if(flag4==0) {
vermaelen 2:82b72fa8dbcd 57 AN1_av=AN1;
vermaelen 2:82b72fa8dbcd 58 }
vermaelen 2:82b72fa8dbcd 59 AN1=0.82*a/(3.3*AnaG.read()-b);
vermaelen 3:b91371837109 60 if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
vermaelen 2:82b72fa8dbcd 61 float temp=AN1;
vermaelen 2:82b72fa8dbcd 62 AN1=AN1_av;
vermaelen 2:82b72fa8dbcd 63 AN1_av=temp;
vermaelen 2:82b72fa8dbcd 64 flag4=1;
vermaelen 2:82b72fa8dbcd 65 } else {
vermaelen 2:82b72fa8dbcd 66 flag4=0;
vermaelen 2:82b72fa8dbcd 67 }
vermaelen 2:82b72fa8dbcd 68 if(AN1<0||AN1>150)AN1=150;
vermaelen 2:82b72fa8dbcd 69
vermaelen 2:82b72fa8dbcd 70
vermaelen 2:82b72fa8dbcd 71 if(flag5==0) {
vermaelen 2:82b72fa8dbcd 72 AN2_av=AN2;
vermaelen 2:82b72fa8dbcd 73 }
vermaelen 2:82b72fa8dbcd 74 AN2=0.82*a/(3.3*AnaAV.read()-b);
vermaelen 2:82b72fa8dbcd 75
vermaelen 3:b91371837109 76 if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
vermaelen 2:82b72fa8dbcd 77 float temp=AN2;
vermaelen 2:82b72fa8dbcd 78 AN2=AN2_av;
vermaelen 2:82b72fa8dbcd 79 AN2_av=temp;
vermaelen 2:82b72fa8dbcd 80 flag5=1;
vermaelen 2:82b72fa8dbcd 81 } else {
vermaelen 2:82b72fa8dbcd 82 flag5=0;
vermaelen 2:82b72fa8dbcd 83 }
vermaelen 2:82b72fa8dbcd 84 if(AN2<0||AN2>150)AN2=150;
vermaelen 2:82b72fa8dbcd 85 }
vermaelen 2:82b72fa8dbcd 86
vermaelen 2:82b72fa8dbcd 87 void fcttrig()
vermaelen 2:82b72fa8dbcd 88 {
vermaelen 2:82b72fa8dbcd 89 switch(drap) {
vermaelen 2:82b72fa8dbcd 90 case 1 :
vermaelen 2:82b72fa8dbcd 91 trigger2.write(1);
vermaelen 2:82b72fa8dbcd 92 wait_us(10);
vermaelen 2:82b72fa8dbcd 93 trigger2.write(0);
vermaelen 2:82b72fa8dbcd 94 drap=2;
vermaelen 2:82b72fa8dbcd 95 break;
vermaelen 2:82b72fa8dbcd 96 case 2 :
vermaelen 2:82b72fa8dbcd 97 trigger3.write(1);
vermaelen 2:82b72fa8dbcd 98 wait_us(10);
vermaelen 2:82b72fa8dbcd 99 trigger3.write(0);
vermaelen 2:82b72fa8dbcd 100 drap=3;
vermaelen 2:82b72fa8dbcd 101 break;
vermaelen 2:82b72fa8dbcd 102 case 3 :
vermaelen 2:82b72fa8dbcd 103 trigger1.write(1);
vermaelen 2:82b72fa8dbcd 104 wait_us(10);
vermaelen 2:82b72fa8dbcd 105 trigger1.write(0);
vermaelen 2:82b72fa8dbcd 106 drap=1;
vermaelen 2:82b72fa8dbcd 107 break;
vermaelen 2:82b72fa8dbcd 108 }
vermaelen 2:82b72fa8dbcd 109
vermaelen 2:82b72fa8dbcd 110 }
vermaelen 2:82b72fa8dbcd 111 void start()
vermaelen 2:82b72fa8dbcd 112 {
vermaelen 2:82b72fa8dbcd 113 temp.reset();
vermaelen 2:82b72fa8dbcd 114 temp.start();
vermaelen 2:82b72fa8dbcd 115 }
vermaelen 2:82b72fa8dbcd 116 void stop()
vermaelen 2:82b72fa8dbcd 117 {
vermaelen 2:82b72fa8dbcd 118 temp.stop();
vermaelen 2:82b72fa8dbcd 119 switch(drap) {
vermaelen 2:82b72fa8dbcd 120 case 1 :
vermaelen 2:82b72fa8dbcd 121 if(flag3==0) {
vermaelen 2:82b72fa8dbcd 122 US3_av=US3;
vermaelen 2:82b72fa8dbcd 123 }
vermaelen 2:82b72fa8dbcd 124 US3=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 125 if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
vermaelen 2:82b72fa8dbcd 126 float temp=US3;
vermaelen 2:82b72fa8dbcd 127 US3=US3_av;
vermaelen 2:82b72fa8dbcd 128 US3_av=temp;
vermaelen 2:82b72fa8dbcd 129 flag3=1;
vermaelen 2:82b72fa8dbcd 130 } else {
vermaelen 2:82b72fa8dbcd 131 flag3=0;
vermaelen 2:82b72fa8dbcd 132 }
vermaelen 2:82b72fa8dbcd 133 if(US3<0||US3>150)US3=150;
vermaelen 2:82b72fa8dbcd 134 break;
vermaelen 2:82b72fa8dbcd 135 case 2 :
vermaelen 2:82b72fa8dbcd 136 if(flag2==0) {
vermaelen 2:82b72fa8dbcd 137 US2_av=US2;
vermaelen 2:82b72fa8dbcd 138 }
vermaelen 2:82b72fa8dbcd 139 US2=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 140 if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 141 float temp=US2;
vermaelen 2:82b72fa8dbcd 142 US2=US2_av;
vermaelen 2:82b72fa8dbcd 143 US2_av=temp;
vermaelen 2:82b72fa8dbcd 144 flag2=1;
vermaelen 2:82b72fa8dbcd 145 } else {
vermaelen 2:82b72fa8dbcd 146 flag2=0;
vermaelen 2:82b72fa8dbcd 147 }
vermaelen 2:82b72fa8dbcd 148 if(US2<0||US2>150)US2=150;
vermaelen 2:82b72fa8dbcd 149 break;
vermaelen 2:82b72fa8dbcd 150 case 3 :
vermaelen 2:82b72fa8dbcd 151 if(flag1==0) {
vermaelen 2:82b72fa8dbcd 152 US1_av=US1;
vermaelen 2:82b72fa8dbcd 153 }
vermaelen 2:82b72fa8dbcd 154 US1=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 155 if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 156 float temp=US1;
vermaelen 2:82b72fa8dbcd 157 US1=US1_av;
vermaelen 2:82b72fa8dbcd 158 US1_av=temp;
vermaelen 2:82b72fa8dbcd 159 flag1=1;
vermaelen 2:82b72fa8dbcd 160 } else {
vermaelen 2:82b72fa8dbcd 161 flag1=0;
vermaelen 2:82b72fa8dbcd 162 }
vermaelen 2:82b72fa8dbcd 163 if(US1<0||US1>150)US1=150;
vermaelen 2:82b72fa8dbcd 164 break;
vermaelen 2:82b72fa8dbcd 165 }
vermaelen 2:82b72fa8dbcd 166 }
vermaelen 2:82b72fa8dbcd 167 float vitesse(float vit)
vermaelen 2:82b72fa8dbcd 168 {
vermaelen 2:82b72fa8dbcd 169 if(vit<0) vit=0;
vermaelen 2:82b72fa8dbcd 170 if(vit>VMAX) vit=VMAX;
vermaelen 2:82b72fa8dbcd 171 return ((vit/100.0)*PERIOD);
vermaelen 2:82b72fa8dbcd 172 }