tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
fct.cpp
- Committer:
- syasya
- Date:
- 2017-06-01
- Revision:
- 12:6151eb503170
- Parent:
- 10:1a69a6f659bd
- Child:
- 13:4fdd8ad91c69
File content as of revision 12:6151eb503170:
#include "mbed.h" #include "fct.h" #include "CMPS03.h" void rotation_sharp() { sensMG.write(0); sensMD.write(1); cmdD=30; cmdG=30; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } void en_avant() { sensMG.write(0); sensMD.write(0); cmdD=35; cmdG=35; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } void contournement() { sensMG.write(0); sensMD.write(0); cmdD=35; cmdG=15; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } void rotation_horaire() { sensMG.write(0); sensMD.write(0); cmdD=15; cmdG=35; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } void stop() { sensMG.write(0); sensMD.write(0); cmdD=0; cmdG=0; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } void suivi_mur() { E1=E0; E0=US2-US1; if((E0+E1)>0) { cmdD=VMAX; cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-45); } else { cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-45); cmdG=VMAX; } MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); wait(0.001); } 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_trig); echo.fall(&stop_trig); wait(0.1); bearing_set=boussole.readBearing()/10.0; wait(0.1); } 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_trig() { temp.reset(); temp.start(); } void stop_trig() { 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>VLIMIT) vit=VLIMIT; return ((vit/100.0)*PERIOD); }