christophe vermaelen / Mbed 2 deprecated _test_suivi_mur

...

Dependencies:   mbed

fct.cpp@5:3746060957fb, 2017-05-29 (annotated)

Committer:
vermaelen
Date:
Mon May 29 16:52:08 2017 +0000
Revision:
5:3746060957fb
Parent:
4:78a9354fcee8
Child:
7:24a3fa0f912a
CHANGEMENT DE COMMANDE ASSERVISSEMENT (VMAX)

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