christophe vermaelen
/
_test_suivi_mur
...
Diff: fct.cpp
- Revision:
- 2:82b72fa8dbcd
- Child:
- 3:b91371837109
diff -r 714fd6b732be -r 82b72fa8dbcd fct.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fct.cpp Sun May 28 17:36:57 2017 +0000 @@ -0,0 +1,156 @@ +#include "mbed.h" +#include "fct.h" +void stopMotor() +{ + sensMG.write(0); + sensMD.write(0); + MG.pulsewidth(vitesse(0)); + MD.pulsewidth(vitesse(0)); +} +void suivi_mur() +{ + cmdD=1.05*VMOY+Kp_E*(US2-US1); + cmdG=VMOY-Kp_E*(US2-US1); + + MD.pulsewidth(vitesse(cmdD)); + MG.pulsewidth(vitesse(cmdG)); +} +void rotation_horaire() +{ + sensMG.write(0); + sensMD.write(1); + MG.pulsewidth(vitesse(20)); + MD.pulsewidth(vitesse(20)); +} +void init() +{ + sensMG.write(0); + sensMD.write(0); + MG.period(PERIOD); + MD.period(PERIOD); + MG.pulsewidth(vitesse(0)); + MD.pulsewidth(vitesse(0)); + tic1.attach(&fcttrig,0.033); + tic2.attach(&mesAN,0.01); + echo.rise(&start); + echo.fall(&stop); +} +void mesAN() +{ + if(flag4==0) { + AN1_av=AN1; + } + AN1=0.82*a/(3.3*AnaG.read()-b); + if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) { + float temp=AN1; + AN1=AN1_av; + AN1_av=temp; + flag4=1; + } else { + flag4=0; + } + if(AN1<0||AN1>150)AN1=150; + + + if(flag5==0) { + AN2_av=AN2; + } + AN2=0.82*a/(3.3*AnaAV.read()-b); + + if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) { + float temp=AN2; + AN2=AN2_av; + AN2_av=temp; + flag5=1; + } else { + flag5=0; + } + if(AN2<0||AN2>150)AN2=150; +} + +void fcttrig() +{ + switch(drap) { + case 1 : + trigger2.write(1); + wait_us(10); + trigger2.write(0); + drap=2; + break; + case 2 : + trigger3.write(1); + wait_us(10); + trigger3.write(0); + drap=3; + break; + case 3 : + trigger1.write(1); + wait_us(10); + trigger1.write(0); + drap=1; + break; + } + +} +void start() +{ + temp.reset(); + temp.start(); +} +void stop() +{ + temp.stop(); + switch(drap) { + case 1 : + if(flag3==0) { + US3_av=US3; + } + US3=temp.read_us()/58.31; + if(((US3-US3_av)>50)||((US3-US3_av)<-50)) { + float temp=US3; + US3=US3_av; + US3_av=temp; + flag3=1; + } else { + flag3=0; + } + if(US3<0||US3>150)US3=150; + break; + case 2 : + if(flag2==0) { + US2_av=US2; + } + US2=temp.read_us()/58.31; + if(((US2-US2_av)>50)||((US2-US2_av)<-50)) { + float temp=US2; + US2=US2_av; + US2_av=temp; + flag2=1; + } else { + flag2=0; + } + if(US2<0||US2>150)US2=150; + break; + case 3 : + if(flag1==0) { + US1_av=US1; + } + US1=temp.read_us()/58.31; + if(((US1-US1_av)>50)||((US1-US1_av)<-50)) { + float temp=US1; + US1=US1_av; + US1_av=temp; + flag1=1; + } else { + flag1=0; + } + if(US1<0||US1>150)US1=150; + break; + } +} +float vitesse(float vit) +{ + if(vit<0) vit=0; + if(vit>VMAX) vit=VMAX; + return ((vit/100.0)*PERIOD); +}