Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
Diff: fct.cpp
- Revision:
- 8:24a3fa0f912a
- Parent:
- 5:3746060957fb
--- a/fct.cpp Wed May 31 08:48:23 2017 +0000 +++ b/fct.cpp Wed Jun 07 16:14:27 2017 +0000 @@ -1,177 +1,2 @@ #include "mbed.h" #include "fct.h" -void contournement() -{ - sensMG.write(0); - sensMD.write(0); - cmdD=35; - cmdG=15; - MD.pulsewidth(vitesse(cmdD)); - MG.pulsewidth(vitesse(cmdG)); - -} -void stopMotor() -{ - sensMG.write(0); - sensMD.write(0); - MG.pulsewidth(vitesse(0)); - MD.pulsewidth(vitesse(0)); -} -void suivi_mur() -{ - E3=E2; - E2=E1; - E1=E0; - E0=US2-US1; - if((E0+E1)>0) { - cmdD=VMOY; - cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-45); - } else { - cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45); - cmdG=VMOY; - } - - MD.pulsewidth(vitesse(cmdD)); - MG.pulsewidth(vitesse(cmdG)); - - wait(0.001); -} -void rotation_horaire() -{ - sensMG.write(0); - sensMD.write(1); - MG.pulsewidth(vitesse(30)); - MD.pulsewidth(vitesse(30)); -} -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.035); - 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)>40)||((AN1-AN1_av)<-40)) { - 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)>40)||((AN2-AN2_av)<-40)) { - 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); -}