tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
main.cpp@13:4fdd8ad91c69, 2018-01-22 (annotated)
- Committer:
- syasya
- Date:
- Mon Jan 22 19:32:00 2018 +0000
- Revision:
- 13:4fdd8ad91c69
- Parent:
- 12:6151eb503170
ROBOTIQUE COMPETITION PARIS
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vermaelen | 2:82b72fa8dbcd | 1 | #include "fct.h" |
syasya | 7:64bfecbb3000 | 2 | |
vermaelen | 0:dcb865a03d57 | 3 | int main() |
vermaelen | 0:dcb865a03d57 | 4 | { |
syasya | 11:cb477f6ec781 | 5 | |
syasya | 11:cb477f6ec781 | 6 | jack.mode(PullUp); |
vermaelen | 2:82b72fa8dbcd | 7 | int etat=0; |
vermaelen | 2:82b72fa8dbcd | 8 | init(); |
syasya | 9:7f62c0329849 | 9 | t3.reset(); |
syasya | 13:4fdd8ad91c69 | 10 | t3.stop(); |
vermaelen | 0:dcb865a03d57 | 11 | while(1) { |
syasya | 13:4fdd8ad91c69 | 12 | //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()); |
syasya | 7:64bfecbb3000 | 13 | //wait(0.1); |
syasya | 13:4fdd8ad91c69 | 14 | //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()); |
syasya | 13:4fdd8ad91c69 | 15 | //wait(0.1); |
syasya | 13:4fdd8ad91c69 | 16 | capt=capt2.read(); |
syasya | 11:cb477f6ec781 | 17 | |
vermaelen | 2:82b72fa8dbcd | 18 | switch(etat) { |
syasya | 11:cb477f6ec781 | 19 | case 0 : |
syasya | 13:4fdd8ad91c69 | 20 | if(jack.read()==1) { |
syasya | 12:6151eb503170 | 21 | etat=1; |
syasya | 12:6151eb503170 | 22 | t3.reset(); |
syasya | 13:4fdd8ad91c69 | 23 | t3.start(); |
syasya | 13:4fdd8ad91c69 | 24 | t5.reset(); |
syasya | 13:4fdd8ad91c69 | 25 | t5.start(); |
syasya | 12:6151eb503170 | 26 | } |
syasya | 11:cb477f6ec781 | 27 | break; |
syasya | 12:6151eb503170 | 28 | case 1 : |
syasya | 13:4fdd8ad91c69 | 29 | |
syasya | 13:4fdd8ad91c69 | 30 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 31 | if((AN2>10 && AN2<40) || US3<18) { |
syasya | 11:cb477f6ec781 | 32 | etat=2; |
syasya | 7:64bfecbb3000 | 33 | stop(); |
vermaelen | 2:82b72fa8dbcd | 34 | } |
syasya | 13:4fdd8ad91c69 | 35 | if(US2>100 && US1<40) { |
syasya | 11:cb477f6ec781 | 36 | etat=3; |
syasya | 7:64bfecbb3000 | 37 | stop(); |
vermaelen | 3:b91371837109 | 38 | } |
syasya | 13:4fdd8ad91c69 | 39 | /*if(t3.read()>7) { |
syasya | 11:cb477f6ec781 | 40 | etat=4; |
syasya | 13:4fdd8ad91c69 | 41 | stop(); |
syasya | 13:4fdd8ad91c69 | 42 | }*/ |
syasya | 13:4fdd8ad91c69 | 43 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 44 | etat=9; |
syasya | 13:4fdd8ad91c69 | 45 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 46 | t4.start(); |
syasya | 9:7f62c0329849 | 47 | stop(); |
syasya | 9:7f62c0329849 | 48 | } |
vermaelen | 2:82b72fa8dbcd | 49 | break; |
syasya | 10:1a69a6f659bd | 50 | case 2 : |
syasya | 13:4fdd8ad91c69 | 51 | if(jack.read()!=1) etat=0; |
vermaelen | 4:78a9354fcee8 | 52 | if(US2>100 && US1<40) { |
syasya | 11:cb477f6ec781 | 53 | etat=3; |
syasya | 7:64bfecbb3000 | 54 | stop(); |
vermaelen | 4:78a9354fcee8 | 55 | } |
vermaelen | 2:82b72fa8dbcd | 56 | if(AN2>30) { |
syasya | 11:cb477f6ec781 | 57 | etat=1; |
vermaelen | 3:b91371837109 | 58 | t2.start(); |
vermaelen | 3:b91371837109 | 59 | t2.reset(); |
syasya | 7:64bfecbb3000 | 60 | stop(); |
vermaelen | 2:82b72fa8dbcd | 61 | } |
syasya | 13:4fdd8ad91c69 | 62 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 63 | etat=9; |
syasya | 13:4fdd8ad91c69 | 64 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 65 | t4.start(); |
syasya | 13:4fdd8ad91c69 | 66 | stop(); |
syasya | 13:4fdd8ad91c69 | 67 | } |
vermaelen | 2:82b72fa8dbcd | 68 | break; |
syasya | 10:1a69a6f659bd | 69 | case 3 : |
syasya | 13:4fdd8ad91c69 | 70 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 71 | if((AN2>10 && AN2<30) || US3<18) { |
syasya | 11:cb477f6ec781 | 72 | etat=2; |
syasya | 7:64bfecbb3000 | 73 | stop(); |
vermaelen | 3:b91371837109 | 74 | } |
syasya | 13:4fdd8ad91c69 | 75 | if(t2.read()>5) { |
syasya | 11:cb477f6ec781 | 76 | etat=1; |
syasya | 10:1a69a6f659bd | 77 | stop(); |
vermaelen | 3:b91371837109 | 78 | } |
syasya | 13:4fdd8ad91c69 | 79 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 80 | etat=9; |
syasya | 13:4fdd8ad91c69 | 81 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 82 | t4.start(); |
syasya | 13:4fdd8ad91c69 | 83 | stop(); |
syasya | 13:4fdd8ad91c69 | 84 | } |
vermaelen | 3:b91371837109 | 85 | break; |
syasya | 10:1a69a6f659bd | 86 | case 4 : |
syasya | 13:4fdd8ad91c69 | 87 | etat=5; |
syasya | 13:4fdd8ad91c69 | 88 | t3.reset(); |
syasya | 13:4fdd8ad91c69 | 89 | stop(); |
syasya | 13:4fdd8ad91c69 | 90 | |
syasya | 13:4fdd8ad91c69 | 91 | break; |
syasya | 13:4fdd8ad91c69 | 92 | case 5 : |
syasya | 13:4fdd8ad91c69 | 93 | if(US3<40 || US2<50) { |
syasya | 13:4fdd8ad91c69 | 94 | etat=6; |
syasya | 9:7f62c0329849 | 95 | stop(); |
syasya | 9:7f62c0329849 | 96 | } |
syasya | 9:7f62c0329849 | 97 | break; |
syasya | 13:4fdd8ad91c69 | 98 | case 6 : |
syasya | 13:4fdd8ad91c69 | 99 | t3.start(); |
syasya | 13:4fdd8ad91c69 | 100 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 101 | etat=9; |
syasya | 13:4fdd8ad91c69 | 102 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 103 | t4.start(); |
syasya | 13:4fdd8ad91c69 | 104 | stop(); |
syasya | 13:4fdd8ad91c69 | 105 | } |
syasya | 13:4fdd8ad91c69 | 106 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 107 | if((AN2>10 && AN2<30) || US3<18) { |
syasya | 13:4fdd8ad91c69 | 108 | etat=7; |
syasya | 13:4fdd8ad91c69 | 109 | stop(); |
syasya | 13:4fdd8ad91c69 | 110 | } |
syasya | 13:4fdd8ad91c69 | 111 | if(US2>100 && US1<40) { |
syasya | 13:4fdd8ad91c69 | 112 | etat=8; |
syasya | 13:4fdd8ad91c69 | 113 | stop(); |
syasya | 13:4fdd8ad91c69 | 114 | } |
syasya | 13:4fdd8ad91c69 | 115 | if(t3.read()>58) { |
syasya | 13:4fdd8ad91c69 | 116 | etat=9; |
syasya | 13:4fdd8ad91c69 | 117 | } |
syasya | 13:4fdd8ad91c69 | 118 | break; |
syasya | 13:4fdd8ad91c69 | 119 | case 7 : |
syasya | 13:4fdd8ad91c69 | 120 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 121 | if(US2>100 && US1<40) { |
syasya | 13:4fdd8ad91c69 | 122 | etat=8; |
syasya | 13:4fdd8ad91c69 | 123 | stop(); |
syasya | 13:4fdd8ad91c69 | 124 | } |
syasya | 13:4fdd8ad91c69 | 125 | if(AN2>30) { |
syasya | 13:4fdd8ad91c69 | 126 | etat=6; |
syasya | 13:4fdd8ad91c69 | 127 | t2.start(); |
syasya | 13:4fdd8ad91c69 | 128 | t2.reset(); |
syasya | 13:4fdd8ad91c69 | 129 | stop(); |
syasya | 13:4fdd8ad91c69 | 130 | } |
syasya | 13:4fdd8ad91c69 | 131 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 132 | etat=9; |
syasya | 13:4fdd8ad91c69 | 133 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 134 | t4.start(); |
syasya | 9:7f62c0329849 | 135 | stop(); |
syasya | 9:7f62c0329849 | 136 | } |
syasya | 9:7f62c0329849 | 137 | break; |
syasya | 13:4fdd8ad91c69 | 138 | case 8 : |
syasya | 13:4fdd8ad91c69 | 139 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 140 | if((AN2>10 && AN2<30) || US3<18) { |
syasya | 13:4fdd8ad91c69 | 141 | etat=7; |
syasya | 13:4fdd8ad91c69 | 142 | stop(); |
syasya | 13:4fdd8ad91c69 | 143 | } |
syasya | 13:4fdd8ad91c69 | 144 | if(t2.read()>4) { |
syasya | 13:4fdd8ad91c69 | 145 | etat=6; |
syasya | 13:4fdd8ad91c69 | 146 | stop(); |
syasya | 13:4fdd8ad91c69 | 147 | } |
syasya | 13:4fdd8ad91c69 | 148 | if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { |
syasya | 13:4fdd8ad91c69 | 149 | etat=9; |
syasya | 13:4fdd8ad91c69 | 150 | t4.reset(); |
syasya | 13:4fdd8ad91c69 | 151 | t4.start(); |
syasya | 13:4fdd8ad91c69 | 152 | stop(); |
syasya | 13:4fdd8ad91c69 | 153 | } |
syasya | 13:4fdd8ad91c69 | 154 | break; |
syasya | 13:4fdd8ad91c69 | 155 | case 9 : |
syasya | 13:4fdd8ad91c69 | 156 | if(t4.read()>2) etat=10; |
syasya | 13:4fdd8ad91c69 | 157 | stop(); |
syasya | 13:4fdd8ad91c69 | 158 | break; |
syasya | 13:4fdd8ad91c69 | 159 | case 10 : |
syasya | 13:4fdd8ad91c69 | 160 | if(jack.read()!=1) etat=0; |
syasya | 13:4fdd8ad91c69 | 161 | stop(); |
syasya | 13:4fdd8ad91c69 | 162 | break; |
syasya | 13:4fdd8ad91c69 | 163 | |
syasya | 7:64bfecbb3000 | 164 | |
vermaelen | 2:82b72fa8dbcd | 165 | } |
vermaelen | 2:82b72fa8dbcd | 166 | switch(etat) { |
syasya | 11:cb477f6ec781 | 167 | case 0 : |
syasya | 10:1a69a6f659bd | 168 | stop(); |
vermaelen | 2:82b72fa8dbcd | 169 | break; |
vermaelen | 2:82b72fa8dbcd | 170 | case 1 : |
syasya | 10:1a69a6f659bd | 171 | suivi_mur(); |
vermaelen | 2:82b72fa8dbcd | 172 | break; |
vermaelen | 3:b91371837109 | 173 | case 2 : |
syasya | 12:6151eb503170 | 174 | rotation_sharp(); |
vermaelen | 3:b91371837109 | 175 | break; |
syasya | 9:7f62c0329849 | 176 | case 3 : |
syasya | 10:1a69a6f659bd | 177 | contournement(); |
syasya | 10:1a69a6f659bd | 178 | break; |
syasya | 10:1a69a6f659bd | 179 | case 4 : |
syasya | 13:4fdd8ad91c69 | 180 | rotation_horaire(); |
syasya | 13:4fdd8ad91c69 | 181 | wait_ms(700); |
syasya | 9:7f62c0329849 | 182 | break; |
syasya | 10:1a69a6f659bd | 183 | case 5 : |
syasya | 9:7f62c0329849 | 184 | en_avant(); |
syasya | 9:7f62c0329849 | 185 | break; |
syasya | 13:4fdd8ad91c69 | 186 | case 6 : |
syasya | 13:4fdd8ad91c69 | 187 | suivi_mur_dist(); |
syasya | 13:4fdd8ad91c69 | 188 | break; |
syasya | 13:4fdd8ad91c69 | 189 | case 7 : |
syasya | 13:4fdd8ad91c69 | 190 | rotation_sharp(); |
syasya | 13:4fdd8ad91c69 | 191 | break; |
syasya | 13:4fdd8ad91c69 | 192 | case 8 : |
syasya | 13:4fdd8ad91c69 | 193 | contournement(); |
syasya | 13:4fdd8ad91c69 | 194 | break; |
syasya | 13:4fdd8ad91c69 | 195 | case 9 : |
syasya | 13:4fdd8ad91c69 | 196 | ballon(); |
syasya | 13:4fdd8ad91c69 | 197 | smoke.write(1); |
syasya | 13:4fdd8ad91c69 | 198 | break; |
syasya | 13:4fdd8ad91c69 | 199 | case 10 : |
syasya | 13:4fdd8ad91c69 | 200 | stop(); |
syasya | 13:4fdd8ad91c69 | 201 | if(t4.read()>3) { |
syasya | 13:4fdd8ad91c69 | 202 | smoke.write(0); |
syasya | 13:4fdd8ad91c69 | 203 | } |
syasya | 13:4fdd8ad91c69 | 204 | break; |
vermaelen | 2:82b72fa8dbcd | 205 | } |
vermaelen | 1:714fd6b732be | 206 | |
vermaelen | 0:dcb865a03d57 | 207 | } |
vermaelen | 0:dcb865a03d57 | 208 | } |