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@12:6151eb503170, 2017-06-01 (annotated)

Committer:
syasya
Date:
Thu Jun 01 09:30:37 2017 +0000
Revision:
12:6151eb503170
Parent:
11:cb477f6ec781
Child:
13:4fdd8ad91c69
probleme t3

Who changed what in which revision?

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