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@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?

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();
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 }