tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
main.cpp@11:cb477f6ec781, 2017-06-01 (annotated)
- Committer:
- syasya
- Date:
- Thu Jun 01 08:18:04 2017 +0000
- Revision:
- 11:cb477f6ec781
- Parent:
- 10:1a69a6f659bd
- Child:
- 12:6151eb503170
avec jack;
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 | { |
syasya | 11:cb477f6ec781 | 9 | |
syasya | 11:cb477f6ec781 | 10 | jack.mode(PullUp); |
vermaelen | 2:82b72fa8dbcd | 11 | int etat=0; |
vermaelen | 2:82b72fa8dbcd | 12 | init(); |
syasya | 9:7f62c0329849 | 13 | t3.reset(); |
syasya | 9:7f62c0329849 | 14 | t3.start(); |
vermaelen | 0:dcb865a03d57 | 15 | while(1) { |
syasya | 7:64bfecbb3000 | 16 | // 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 | 17 | //wait(0.05); |
syasya | 11:cb477f6ec781 | 18 | //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing); |
syasya | 7:64bfecbb3000 | 19 | //wait(0.1); |
syasya | 11:cb477f6ec781 | 20 | |
syasya | 7:64bfecbb3000 | 21 | bearing=boussole.readBearing()/10.0; |
syasya | 10:1a69a6f659bd | 22 | ledsetat.write(etat); |
syasya | 11:cb477f6ec781 | 23 | |
syasya | 10:1a69a6f659bd | 24 | if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) { |
syasya | 11:cb477f6ec781 | 25 | t3.start(); |
syasya | 11:cb477f6ec781 | 26 | } else { |
syasya | 10:1a69a6f659bd | 27 | t3.stop(); |
syasya | 10:1a69a6f659bd | 28 | } |
syasya | 11:cb477f6ec781 | 29 | |
vermaelen | 2:82b72fa8dbcd | 30 | switch(etat) { |
syasya | 11:cb477f6ec781 | 31 | case 0 : |
syasya | 11:cb477f6ec781 | 32 | if(jack.read()==0) etat=1; |
syasya | 11:cb477f6ec781 | 33 | break; |
syasya | 11:cb477f6ec781 | 34 | case 1 : |
syasya | 11:cb477f6ec781 | 35 | if(jack.read()!=0) etat=0; |
vermaelen | 3:b91371837109 | 36 | if((AN2>10 && AN2<20) || US3<13) { |
syasya | 11:cb477f6ec781 | 37 | etat=2; |
syasya | 7:64bfecbb3000 | 38 | stop(); |
vermaelen | 2:82b72fa8dbcd | 39 | } |
vermaelen | 3:b91371837109 | 40 | if(US2>100 && US1<40) { |
syasya | 11:cb477f6ec781 | 41 | etat=3; |
syasya | 7:64bfecbb3000 | 42 | stop(); |
vermaelen | 3:b91371837109 | 43 | } |
syasya | 11:cb477f6ec781 | 44 | if(t3.read()>t_set) { |
syasya | 11:cb477f6ec781 | 45 | etat=4; |
syasya | 9:7f62c0329849 | 46 | bearing_set=bearing_set+25.0; |
syasya | 9:7f62c0329849 | 47 | t3.reset(); |
syasya | 9:7f62c0329849 | 48 | t3.stop(); |
syasya | 9:7f62c0329849 | 49 | stop(); |
syasya | 9:7f62c0329849 | 50 | } |
vermaelen | 2:82b72fa8dbcd | 51 | break; |
syasya | 10:1a69a6f659bd | 52 | case 2 : |
syasya | 11:cb477f6ec781 | 53 | if(jack.read()!=0) etat=0; |
vermaelen | 4:78a9354fcee8 | 54 | if(US2>100 && US1<40) { |
syasya | 11:cb477f6ec781 | 55 | etat=3; |
syasya | 7:64bfecbb3000 | 56 | stop(); |
vermaelen | 4:78a9354fcee8 | 57 | } |
vermaelen | 2:82b72fa8dbcd | 58 | if(AN2>30) { |
syasya | 11:cb477f6ec781 | 59 | etat=1; |
vermaelen | 3:b91371837109 | 60 | t2.start(); |
vermaelen | 3:b91371837109 | 61 | t2.reset(); |
syasya | 7:64bfecbb3000 | 62 | stop(); |
vermaelen | 2:82b72fa8dbcd | 63 | } |
vermaelen | 2:82b72fa8dbcd | 64 | break; |
syasya | 10:1a69a6f659bd | 65 | case 3 : |
syasya | 11:cb477f6ec781 | 66 | if(jack.read()!=0) etat=0; |
vermaelen | 3:b91371837109 | 67 | if((AN2>10 && AN2<20) || US3<13) { |
syasya | 11:cb477f6ec781 | 68 | etat=2; |
syasya | 7:64bfecbb3000 | 69 | stop(); |
vermaelen | 3:b91371837109 | 70 | } |
syasya | 10:1a69a6f659bd | 71 | if(US1>100 && US2<10) { |
syasya | 11:cb477f6ec781 | 72 | etat=1; |
syasya | 10:1a69a6f659bd | 73 | stop(); |
vermaelen | 3:b91371837109 | 74 | } |
vermaelen | 3:b91371837109 | 75 | break; |
syasya | 10:1a69a6f659bd | 76 | case 4 : |
syasya | 11:cb477f6ec781 | 77 | if(jack.read()!=0) etat=0; |
syasya | 9:7f62c0329849 | 78 | if(bearing>bearing_set) { |
syasya | 11:cb477f6ec781 | 79 | etat=5; |
syasya | 9:7f62c0329849 | 80 | t3.reset(); |
syasya | 9:7f62c0329849 | 81 | stop(); |
syasya | 9:7f62c0329849 | 82 | } |
syasya | 9:7f62c0329849 | 83 | break; |
syasya | 10:1a69a6f659bd | 84 | case 5 : |
syasya | 11:cb477f6ec781 | 85 | if(jack.read()!=0) etat=0; |
syasya | 9:7f62c0329849 | 86 | if((AN2>10 && AN2<22) || US3<22) { |
syasya | 11:cb477f6ec781 | 87 | etat=2; |
syasya | 11:cb477f6ec781 | 88 | t_set=8; |
syasya | 9:7f62c0329849 | 89 | stop(); |
syasya | 9:7f62c0329849 | 90 | } |
syasya | 9:7f62c0329849 | 91 | break; |
syasya | 7:64bfecbb3000 | 92 | |
vermaelen | 2:82b72fa8dbcd | 93 | } |
vermaelen | 2:82b72fa8dbcd | 94 | switch(etat) { |
syasya | 11:cb477f6ec781 | 95 | case 0 : |
syasya | 10:1a69a6f659bd | 96 | stop(); |
vermaelen | 2:82b72fa8dbcd | 97 | break; |
vermaelen | 2:82b72fa8dbcd | 98 | case 1 : |
syasya | 10:1a69a6f659bd | 99 | suivi_mur(); |
vermaelen | 2:82b72fa8dbcd | 100 | break; |
vermaelen | 3:b91371837109 | 101 | case 2 : |
syasya | 10:1a69a6f659bd | 102 | rotation_horaire(); |
vermaelen | 3:b91371837109 | 103 | break; |
syasya | 9:7f62c0329849 | 104 | case 3 : |
syasya | 10:1a69a6f659bd | 105 | contournement(); |
syasya | 10:1a69a6f659bd | 106 | break; |
syasya | 10:1a69a6f659bd | 107 | case 4 : |
syasya | 9:7f62c0329849 | 108 | rotation_horaire(); |
syasya | 9:7f62c0329849 | 109 | break; |
syasya | 10:1a69a6f659bd | 110 | case 5 : |
syasya | 9:7f62c0329849 | 111 | en_avant(); |
syasya | 9:7f62c0329849 | 112 | break; |
vermaelen | 2:82b72fa8dbcd | 113 | } |
vermaelen | 1:714fd6b732be | 114 | |
vermaelen | 0:dcb865a03d57 | 115 | } |
vermaelen | 0:dcb865a03d57 | 116 | } |