Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
fct.cpp
- Committer:
- pirottealex
- Date:
- 2017-06-23
- Revision:
- 7:2f4660e9cf92
- Parent:
- 5:3746060957fb
File content as of revision 7:2f4660e9cf92:
#include "mbed.h" #include "fct.h" void stopMotor() { vitesse(0,0); } void init() { capb1.mode(PullUp); capb2.mode(PullUp); servo_stop(); jack.mode(PullUp); MG.period(PERIOD); MD.period(PERIOD); 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=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>120))AN1=120; if(flag5==0) { AN2_av=AN2; } AN2=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>120))AN2=120; capt_d=AN1; capt_g=AN2; } 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>capt_max)US3=capt_max; if(US3<capt_min)US3=capt_min; 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>capt_max)US2=capt_max; if(US2<capt_min)US2=capt_min; 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>capt_max)US1=capt_max; if(US1<capt_min)US1=capt_min; break; } capt_ed=US1; capt_eg=US3; capt_m=US2; } void vitesse(float vitG, float vitD) { 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); }