tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
_test_suivi_mur
by 
christophe vermaelen
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "fct.h" 00002 00003 int main() 00004 { 00005 00006 jack.mode(PullUp); 00007 int etat=0; 00008 init(); 00009 t3.reset(); 00010 t3.stop(); 00011 while(1) { 00012 //printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f jack=%d\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG,jack.read()); 00013 //wait(0.1); 00014 //printf("etat=%d set=%.1f CAP=%.1f t3=%f capt1=%d capt2=%d\n\r",etat,bearing_set,bearing,t3.read(),capt1.read(),capt2.read()); 00015 //wait(0.1); 00016 capt=capt2.read(); 00017 00018 switch(etat) { 00019 case 0 : 00020 if(jack.read()==1) { 00021 etat=1; 00022 t3.reset(); 00023 t3.start(); 00024 t5.reset(); 00025 t5.start(); 00026 } 00027 break; 00028 case 1 : 00029 00030 if(jack.read()!=1) etat=0; 00031 if((AN2>10 && AN2<40) || US3<18) { 00032 etat=2; 00033 stop(); 00034 } 00035 if(US2>100 && US1<40) { 00036 etat=3; 00037 stop(); 00038 } 00039 /*if(t3.read()>7) { 00040 etat=4; 00041 stop(); 00042 }*/ 00043 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00044 etat=9; 00045 t4.reset(); 00046 t4.start(); 00047 stop(); 00048 } 00049 break; 00050 case 2 : 00051 if(jack.read()!=1) etat=0; 00052 if(US2>100 && US1<40) { 00053 etat=3; 00054 stop(); 00055 } 00056 if(AN2>30) { 00057 etat=1; 00058 t2.start(); 00059 t2.reset(); 00060 stop(); 00061 } 00062 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00063 etat=9; 00064 t4.reset(); 00065 t4.start(); 00066 stop(); 00067 } 00068 break; 00069 case 3 : 00070 if(jack.read()!=1) etat=0; 00071 if((AN2>10 && AN2<30) || US3<18) { 00072 etat=2; 00073 stop(); 00074 } 00075 if(t2.read()>5) { 00076 etat=1; 00077 stop(); 00078 } 00079 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00080 etat=9; 00081 t4.reset(); 00082 t4.start(); 00083 stop(); 00084 } 00085 break; 00086 case 4 : 00087 etat=5; 00088 t3.reset(); 00089 stop(); 00090 00091 break; 00092 case 5 : 00093 if(US3<40 || US2<50) { 00094 etat=6; 00095 stop(); 00096 } 00097 break; 00098 case 6 : 00099 t3.start(); 00100 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00101 etat=9; 00102 t4.reset(); 00103 t4.start(); 00104 stop(); 00105 } 00106 if(jack.read()!=1) etat=0; 00107 if((AN2>10 && AN2<30) || US3<18) { 00108 etat=7; 00109 stop(); 00110 } 00111 if(US2>100 && US1<40) { 00112 etat=8; 00113 stop(); 00114 } 00115 if(t3.read()>58) { 00116 etat=9; 00117 } 00118 break; 00119 case 7 : 00120 if(jack.read()!=1) etat=0; 00121 if(US2>100 && US1<40) { 00122 etat=8; 00123 stop(); 00124 } 00125 if(AN2>30) { 00126 etat=6; 00127 t2.start(); 00128 t2.reset(); 00129 stop(); 00130 } 00131 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00132 etat=9; 00133 t4.reset(); 00134 t4.start(); 00135 stop(); 00136 } 00137 break; 00138 case 8 : 00139 if(jack.read()!=1) etat=0; 00140 if((AN2>10 && AN2<30) || US3<18) { 00141 etat=7; 00142 stop(); 00143 } 00144 if(t2.read()>4) { 00145 etat=6; 00146 stop(); 00147 } 00148 if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { 00149 etat=9; 00150 t4.reset(); 00151 t4.start(); 00152 stop(); 00153 } 00154 break; 00155 case 9 : 00156 if(t4.read()>2) etat=10; 00157 stop(); 00158 break; 00159 case 10 : 00160 if(jack.read()!=1) etat=0; 00161 stop(); 00162 break; 00163 00164 00165 } 00166 switch(etat) { 00167 case 0 : 00168 stop(); 00169 break; 00170 case 1 : 00171 suivi_mur(); 00172 break; 00173 case 2 : 00174 rotation_sharp(); 00175 break; 00176 case 3 : 00177 contournement(); 00178 break; 00179 case 4 : 00180 rotation_horaire(); 00181 wait_ms(700); 00182 break; 00183 case 5 : 00184 en_avant(); 00185 break; 00186 case 6 : 00187 suivi_mur_dist(); 00188 break; 00189 case 7 : 00190 rotation_sharp(); 00191 break; 00192 case 8 : 00193 contournement(); 00194 break; 00195 case 9 : 00196 ballon(); 00197 smoke.write(1); 00198 break; 00199 case 10 : 00200 stop(); 00201 if(t4.read()>3) { 00202 smoke.write(0); 00203 } 00204 break; 00205 } 00206 00207 } 00208 }
Generated on Wed Aug 3 2022 12:40:57 by
1.7.2