fct.cpp

00001 #include "mbed.h"
00002 #include "fct.h"
00003 #include "CMPS03.h"
00004 
00005 void ballon()
00006 {
00007     stop();
00008     servo.period(0.02);
00009     servo.pulsewidth_ms(17);
00010     wait(0.25);
00011     servo.pulsewidth_ms(19.5);
00012     wait(0.25);
00013 }
00014 void suivi_mur_dist()
00015 {
00016     E1=E0;
00017     E0=US2-US1;
00018     if((E0+E1)>0) {
00019         cmdD=VMAX;
00020         cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-30);
00021     } else {
00022         cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-30);
00023         cmdG=VMAX;
00024     }
00025 
00026     MD.pulsewidth(vitesse(cmdD));
00027     MG.pulsewidth(vitesse(cmdG));
00028 
00029     wait(0.001);
00030 
00031 }
00032 void rotation_sharp()
00033 {
00034     sensMG.write(0);
00035     sensMD.write(1);
00036     cmdD=30;
00037     cmdG=30;
00038     MD.pulsewidth(vitesse(cmdD));
00039     MG.pulsewidth(vitesse(cmdG));
00040 }
00041 void en_avant()
00042 {
00043     sensMG.write(0);
00044     sensMD.write(0);
00045     cmdD=60;
00046     cmdG=60;
00047     MD.pulsewidth(vitesse(cmdD));
00048     MG.pulsewidth(vitesse(cmdG));
00049 }
00050 void contournement()
00051 {
00052     sensMG.write(0);
00053     sensMD.write(0);
00054     cmdD=40;
00055     cmdG=22;
00056     MD.pulsewidth(vitesse(cmdD));
00057     MG.pulsewidth(vitesse(cmdG));
00058 
00059 }
00060 void rotation_horaire()
00061 {
00062     sensMG.write(0);
00063     sensMD.write(0);
00064     cmdD=30;
00065     cmdG=40;
00066     MD.pulsewidth(vitesse(cmdD));
00067     MG.pulsewidth(vitesse(cmdG));
00068 }
00069 void stop()
00070 {
00071     sensMG.write(0);
00072     sensMD.write(0);
00073     cmdD=0;
00074     cmdG=0;
00075     MD.pulsewidth(vitesse(cmdD));
00076     MG.pulsewidth(vitesse(cmdG));
00077 }
00078 void suivi_mur()
00079 {
00080     E1=E0;
00081     E0=US2-US1;
00082     if((E0+E1)>0) {
00083         cmdD=VMAX;
00084         cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
00085     } else {
00086         cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
00087         cmdG=VMAX;
00088     }
00089 
00090     MD.pulsewidth(vitesse(cmdD));
00091     MG.pulsewidth(vitesse(cmdG));
00092 
00093     wait(0.001);
00094 
00095 }
00096 void init()
00097 {   
00098     smoke.write(0);
00099     sensMG.write(0);
00100     sensMD.write(0);
00101     MG.period(PERIOD);
00102     MD.period(PERIOD);
00103     MG.pulsewidth(vitesse(0));
00104     MD.pulsewidth(vitesse(0));
00105     tic1.attach(&fcttrig,0.035);
00106     tic2.attach(&mesAN,0.01);
00107     echo.rise(&start_trig);
00108     echo.fall(&stop_trig);
00109     wait(0.1);
00110     bearing_set=boussole.readBearing()/10.0;
00111     wait(0.1);
00112 }
00113 void mesAN()
00114 {
00115     if(flag4==0) {
00116         AN1_av=AN1;
00117     }
00118     AN1=0.82*a/(3.3*AnaG.read()-b);
00119     if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
00120         float temp=AN1;
00121         AN1=AN1_av;
00122         AN1_av=temp;
00123         flag4=1;
00124     } else {
00125         flag4=0;
00126     }
00127     if(AN1<0||AN1>150)AN1=150;
00128 
00129 
00130     if(flag5==0) {
00131         AN2_av=AN2;
00132     }
00133     AN2=0.82*a/(3.3*AnaAV.read()-b);
00134 
00135     if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
00136         float temp=AN2;
00137         AN2=AN2_av;
00138         AN2_av=temp;
00139         flag5=1;
00140     } else {
00141         flag5=0;
00142     }
00143     if(AN2<0||AN2>150)AN2=150;
00144 }
00145 
00146 void fcttrig()
00147 {
00148     switch(drap) {
00149         case 1 :
00150             trigger2.write(1);
00151             wait_us(10);
00152             trigger2.write(0);
00153             drap=2;
00154             break;
00155         case 2 :
00156             trigger3.write(1);
00157             wait_us(10);
00158             trigger3.write(0);
00159             drap=3;
00160             break;
00161         case 3 :
00162             trigger1.write(1);
00163             wait_us(10);
00164             trigger1.write(0);
00165             drap=1;
00166             break;
00167     }
00168 
00169 }
00170 void start_trig()
00171 {
00172     temp.reset();
00173     temp.start();
00174 }
00175 void stop_trig()
00176 {
00177     temp.stop();
00178     switch(drap) {
00179         case 1 :
00180             if(flag3==0) {
00181                 US3_av=US3;
00182             }
00183             US3=temp.read_us()/58.31;
00184             if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
00185                 float temp=US3;
00186                 US3=US3_av;
00187                 US3_av=temp;
00188                 flag3=1;
00189             } else {
00190                 flag3=0;
00191             }
00192             if(US3<0||US3>150)US3=150;
00193             break;
00194         case 2 :
00195             if(flag2==0) {
00196                 US2_av=US2;
00197             }
00198             US2=temp.read_us()/58.31;
00199             if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
00200                 float temp=US2;
00201                 US2=US2_av;
00202                 US2_av=temp;
00203                 flag2=1;
00204             } else {
00205                 flag2=0;
00206             }
00207             if(US2<0||US2>150)US2=150;
00208             break;
00209         case 3 :
00210             if(flag1==0) {
00211                 US1_av=US1;
00212             }
00213             US1=temp.read_us()/58.31;
00214             if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
00215                 float temp=US1;
00216                 US1=US1_av;
00217                 US1_av=temp;
00218                 flag1=1;
00219             } else {
00220                 flag1=0;
00221             }
00222             if(US1<0||US1>150)US1=150;
00223             break;
00224     }
00225 }
00226 float vitesse(float vit)
00227 {
00228     if(vit<0) vit=0;
00229     if(vit>VLIMIT) vit=VLIMIT;
00230     return ((vit/100.0)*PERIOD);
00231 }