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@10:1a69a6f659bd, 2017-06-01 (annotated)

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