tuan din engku syasya syazwani / Mbed 2 deprecated CACHAN_D_DAY_2

ROBOTIC COMPETITION PARIS 2017

Dependencies:   CMPS03 mbed

Fork of _test_suivi_mur by christophe vermaelen

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?

UserRevisionLine numberNew 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 }