tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
main.cpp@7:64bfecbb3000, 2017-05-31 (annotated)
- Committer:
- syasya
- Date:
- Wed May 31 12:17:59 2017 +0000
- Revision:
- 7:64bfecbb3000
- Parent:
- 6:5f7df5c74a77
- Child:
- 8:72d684a69957
- Child:
- 9:7f62c0329849
with bearing;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
syasya | 7:64bfecbb3000 | 1 | |
vermaelen | 2:82b72fa8dbcd | 2 | #include "fct.h" |
syasya | 7:64bfecbb3000 | 3 | |
syasya | 7:64bfecbb3000 | 4 | |
vermaelen | 3:b91371837109 | 5 | BusOut ledsetat(p12,p13); |
vermaelen | 0:dcb865a03d57 | 6 | |
vermaelen | 0:dcb865a03d57 | 7 | int main() |
vermaelen | 0:dcb865a03d57 | 8 | { |
vermaelen | 2:82b72fa8dbcd | 9 | int etat=0; |
vermaelen | 2:82b72fa8dbcd | 10 | init(); |
vermaelen | 0:dcb865a03d57 | 11 | while(1) { |
syasya | 7:64bfecbb3000 | 12 | // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG); |
vermaelen | 2:82b72fa8dbcd | 13 | //wait(0.05); |
syasya | 7:64bfecbb3000 | 14 | //printf("set=%.1f CAP=%.1f\n\r",bearing_set,bearing); |
syasya | 7:64bfecbb3000 | 15 | //wait(0.1); |
syasya | 7:64bfecbb3000 | 16 | |
syasya | 7:64bfecbb3000 | 17 | bearing=boussole.readBearing()/10.0; |
syasya | 7:64bfecbb3000 | 18 | t3.reset(); |
syasya | 7:64bfecbb3000 | 19 | t3.start(); |
vermaelen | 3:b91371837109 | 20 | ledsetat.write(etat); |
vermaelen | 2:82b72fa8dbcd | 21 | switch(etat) { |
vermaelen | 2:82b72fa8dbcd | 22 | case 0 : |
syasya | 7:64bfecbb3000 | 23 | if(t3.read()>4) { |
syasya | 7:64bfecbb3000 | 24 | etat=1; |
syasya | 7:64bfecbb3000 | 25 | t3.reset(); |
syasya | 7:64bfecbb3000 | 26 | stop(); |
syasya | 7:64bfecbb3000 | 27 | } |
vermaelen | 3:b91371837109 | 28 | if((AN2>10 && AN2<20) || US3<13) { |
vermaelen | 2:82b72fa8dbcd | 29 | etat=1; |
syasya | 7:64bfecbb3000 | 30 | stop(); |
vermaelen | 2:82b72fa8dbcd | 31 | } |
vermaelen | 3:b91371837109 | 32 | if(US2>100 && US1<40) { |
vermaelen | 3:b91371837109 | 33 | etat=2; |
syasya | 7:64bfecbb3000 | 34 | stop(); |
vermaelen | 3:b91371837109 | 35 | } |
vermaelen | 2:82b72fa8dbcd | 36 | break; |
vermaelen | 2:82b72fa8dbcd | 37 | case 1 : |
vermaelen | 4:78a9354fcee8 | 38 | if(US2>100 && US1<40) { |
vermaelen | 4:78a9354fcee8 | 39 | etat=2; |
syasya | 7:64bfecbb3000 | 40 | stop(); |
vermaelen | 4:78a9354fcee8 | 41 | } |
vermaelen | 2:82b72fa8dbcd | 42 | if(AN2>30) { |
vermaelen | 2:82b72fa8dbcd | 43 | etat=0; |
vermaelen | 3:b91371837109 | 44 | t2.start(); |
vermaelen | 3:b91371837109 | 45 | t2.reset(); |
syasya | 7:64bfecbb3000 | 46 | stop(); |
vermaelen | 2:82b72fa8dbcd | 47 | } |
syasya | 7:64bfecbb3000 | 48 | if(bearing>(bearing-90.0)) { |
syasya | 7:64bfecbb3000 | 49 | etat=0; |
syasya | 7:64bfecbb3000 | 50 | stop(); |
syasya | 7:64bfecbb3000 | 51 | } |
vermaelen | 2:82b72fa8dbcd | 52 | break; |
vermaelen | 3:b91371837109 | 53 | case 2 : |
vermaelen | 3:b91371837109 | 54 | if((AN2>10 && AN2<20) || US3<13) { |
vermaelen | 3:b91371837109 | 55 | etat=1; |
syasya | 7:64bfecbb3000 | 56 | stop(); |
vermaelen | 3:b91371837109 | 57 | } |
vermaelen | 6:5f7df5c74a77 | 58 | if(t2.read()>2.5) { |
vermaelen | 3:b91371837109 | 59 | etat=0; |
vermaelen | 3:b91371837109 | 60 | } |
vermaelen | 3:b91371837109 | 61 | break; |
syasya | 7:64bfecbb3000 | 62 | |
vermaelen | 2:82b72fa8dbcd | 63 | } |
vermaelen | 2:82b72fa8dbcd | 64 | switch(etat) { |
vermaelen | 2:82b72fa8dbcd | 65 | case 0 : |
vermaelen | 2:82b72fa8dbcd | 66 | suivi_mur(); |
vermaelen | 2:82b72fa8dbcd | 67 | break; |
vermaelen | 2:82b72fa8dbcd | 68 | case 1 : |
vermaelen | 2:82b72fa8dbcd | 69 | rotation_horaire(); |
vermaelen | 2:82b72fa8dbcd | 70 | break; |
vermaelen | 3:b91371837109 | 71 | case 2 : |
vermaelen | 3:b91371837109 | 72 | contournement(); |
vermaelen | 3:b91371837109 | 73 | break; |
vermaelen | 2:82b72fa8dbcd | 74 | } |
vermaelen | 1:714fd6b732be | 75 | |
vermaelen | 0:dcb865a03d57 | 76 | } |
vermaelen | 0:dcb865a03d57 | 77 | } |