Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
Diff: fct.cpp
- Revision:
- 7:2f4660e9cf92
- Parent:
- 5:3746060957fb
--- a/fct.cpp Wed May 31 08:48:23 2017 +0000 +++ b/fct.cpp Fri Jun 23 11:19:58 2017 +0000 @@ -1,68 +1,33 @@ #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)); + vitesse(0,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); + capb1.mode(PullUp); + capb2.mode(PullUp); + servo_stop(); + jack.mode(PullUp); MG.period(PERIOD); MD.period(PERIOD); - MG.pulsewidth(vitesse(0)); - MD.pulsewidth(vitesse(0)); + vitesse(0,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)) { + AN1=a/(3.3*AnaG.read()-b); + if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) { float temp=AN1; AN1=AN1_av; AN1_av=temp; @@ -70,15 +35,16 @@ } else { flag4=0; } - if(AN1<0||AN1>150)AN1=150; + if((AN1<0)||(AN1>120))AN1=120; + if(flag5==0) { AN2_av=AN2; } - AN2=0.82*a/(3.3*AnaAV.read()-b); + AN2=a/(3.3*AnaAV.read()-b); - if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) { + if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) { float temp=AN2; AN2=AN2_av; AN2_av=temp; @@ -86,9 +52,12 @@ } else { flag5=0; } - if(AN2<0||AN2>150)AN2=150; + if((AN2<0)||(AN2>120))AN2=120; + capt_d=AN1; + capt_g=AN2; } + void fcttrig() { switch(drap) { @@ -135,7 +104,8 @@ } else { flag3=0; } - if(US3<0||US3>150)US3=150; + if(US3>capt_max)US3=capt_max; + if(US3<capt_min)US3=capt_min; break; case 2 : if(flag2==0) { @@ -150,7 +120,8 @@ } else { flag2=0; } - if(US2<0||US2>150)US2=150; + if(US2>capt_max)US2=capt_max; + if(US2<capt_min)US2=capt_min; break; case 3 : if(flag1==0) { @@ -165,13 +136,42 @@ } else { flag1=0; } - if(US1<0||US1>150)US1=150; + if(US1>capt_max)US1=capt_max; + if(US1<capt_min)US1=capt_min; break; } + capt_ed=US1; + capt_eg=US3; + capt_m=US2; } -float vitesse(float vit) +void vitesse(float vitG, float vitD) { - if(vit<0) vit=0; - if(vit>VMAX) vit=VMAX; - return ((vit/100.0)*PERIOD); + if(vitG<0) { + vitG=-1*vitG; + sensMG.write(1); + } else sensMG.write(0); + MG.pulsewidth(((vitG)/100.0)*PERIOD); + if(vitD<0) { + vitD=-1*vitD; + sensMD.write(1); + } else sensMD.write(0); + MD.pulsewidth(((vitD)/100.0)*PERIOD); } + +void servo_start(void) +{ + servo.period(periode); + turn=18.8; + servo.pulsewidth_ms(turn); + wait(0.25); + turn=19.8; + servo.pulsewidth_ms(turn); + wait(0.25); +} + +void servo_stop(void) +{ + servo.period(periode); + turn=18.8; + servo.pulsewidth_ms(turn); +}