Alexandre Pirotte / Mbed 2 deprecated 0concours_cachan_programme_ok

ok

Dependencies:   mbed

Fork of _test_suivi_mur by christophe vermaelen

fct.cpp@2:82b72fa8dbcd, 2017-05-28 (annotated)

Committer:
vermaelen
Date:
Sun May 28 17:36:57 2017 +0000
Revision:
2:82b72fa8dbcd
Child:
3:b91371837109
V1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 2:82b72fa8dbcd 1 #include "mbed.h"
vermaelen 2:82b72fa8dbcd 2 #include "fct.h"
vermaelen 2:82b72fa8dbcd 3 void stopMotor()
vermaelen 2:82b72fa8dbcd 4 {
vermaelen 2:82b72fa8dbcd 5 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 6 sensMD.write(0);
vermaelen 2:82b72fa8dbcd 7 MG.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 8 MD.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 9 }
vermaelen 2:82b72fa8dbcd 10 void suivi_mur()
vermaelen 2:82b72fa8dbcd 11 {
vermaelen 2:82b72fa8dbcd 12 cmdD=1.05*VMOY+Kp_E*(US2-US1);
vermaelen 2:82b72fa8dbcd 13 cmdG=VMOY-Kp_E*(US2-US1);
vermaelen 2:82b72fa8dbcd 14
vermaelen 2:82b72fa8dbcd 15 MD.pulsewidth(vitesse(cmdD));
vermaelen 2:82b72fa8dbcd 16 MG.pulsewidth(vitesse(cmdG));
vermaelen 2:82b72fa8dbcd 17 }
vermaelen 2:82b72fa8dbcd 18 void rotation_horaire()
vermaelen 2:82b72fa8dbcd 19 {
vermaelen 2:82b72fa8dbcd 20 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 21 sensMD.write(1);
vermaelen 2:82b72fa8dbcd 22 MG.pulsewidth(vitesse(20));
vermaelen 2:82b72fa8dbcd 23 MD.pulsewidth(vitesse(20));
vermaelen 2:82b72fa8dbcd 24 }
vermaelen 2:82b72fa8dbcd 25 void init()
vermaelen 2:82b72fa8dbcd 26 {
vermaelen 2:82b72fa8dbcd 27 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 28 sensMD.write(0);
vermaelen 2:82b72fa8dbcd 29 MG.period(PERIOD);
vermaelen 2:82b72fa8dbcd 30 MD.period(PERIOD);
vermaelen 2:82b72fa8dbcd 31 MG.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 32 MD.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 33 tic1.attach(&fcttrig,0.033);
vermaelen 2:82b72fa8dbcd 34 tic2.attach(&mesAN,0.01);
vermaelen 2:82b72fa8dbcd 35 echo.rise(&start);
vermaelen 2:82b72fa8dbcd 36 echo.fall(&stop);
vermaelen 2:82b72fa8dbcd 37 }
vermaelen 2:82b72fa8dbcd 38 void mesAN()
vermaelen 2:82b72fa8dbcd 39 {
vermaelen 2:82b72fa8dbcd 40 if(flag4==0) {
vermaelen 2:82b72fa8dbcd 41 AN1_av=AN1;
vermaelen 2:82b72fa8dbcd 42 }
vermaelen 2:82b72fa8dbcd 43 AN1=0.82*a/(3.3*AnaG.read()-b);
vermaelen 2:82b72fa8dbcd 44 if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 45 float temp=AN1;
vermaelen 2:82b72fa8dbcd 46 AN1=AN1_av;
vermaelen 2:82b72fa8dbcd 47 AN1_av=temp;
vermaelen 2:82b72fa8dbcd 48 flag4=1;
vermaelen 2:82b72fa8dbcd 49 } else {
vermaelen 2:82b72fa8dbcd 50 flag4=0;
vermaelen 2:82b72fa8dbcd 51 }
vermaelen 2:82b72fa8dbcd 52 if(AN1<0||AN1>150)AN1=150;
vermaelen 2:82b72fa8dbcd 53
vermaelen 2:82b72fa8dbcd 54
vermaelen 2:82b72fa8dbcd 55 if(flag5==0) {
vermaelen 2:82b72fa8dbcd 56 AN2_av=AN2;
vermaelen 2:82b72fa8dbcd 57 }
vermaelen 2:82b72fa8dbcd 58 AN2=0.82*a/(3.3*AnaAV.read()-b);
vermaelen 2:82b72fa8dbcd 59
vermaelen 2:82b72fa8dbcd 60 if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 61 float temp=AN2;
vermaelen 2:82b72fa8dbcd 62 AN2=AN2_av;
vermaelen 2:82b72fa8dbcd 63 AN2_av=temp;
vermaelen 2:82b72fa8dbcd 64 flag5=1;
vermaelen 2:82b72fa8dbcd 65 } else {
vermaelen 2:82b72fa8dbcd 66 flag5=0;
vermaelen 2:82b72fa8dbcd 67 }
vermaelen 2:82b72fa8dbcd 68 if(AN2<0||AN2>150)AN2=150;
vermaelen 2:82b72fa8dbcd 69 }
vermaelen 2:82b72fa8dbcd 70
vermaelen 2:82b72fa8dbcd 71 void fcttrig()
vermaelen 2:82b72fa8dbcd 72 {
vermaelen 2:82b72fa8dbcd 73 switch(drap) {
vermaelen 2:82b72fa8dbcd 74 case 1 :
vermaelen 2:82b72fa8dbcd 75 trigger2.write(1);
vermaelen 2:82b72fa8dbcd 76 wait_us(10);
vermaelen 2:82b72fa8dbcd 77 trigger2.write(0);
vermaelen 2:82b72fa8dbcd 78 drap=2;
vermaelen 2:82b72fa8dbcd 79 break;
vermaelen 2:82b72fa8dbcd 80 case 2 :
vermaelen 2:82b72fa8dbcd 81 trigger3.write(1);
vermaelen 2:82b72fa8dbcd 82 wait_us(10);
vermaelen 2:82b72fa8dbcd 83 trigger3.write(0);
vermaelen 2:82b72fa8dbcd 84 drap=3;
vermaelen 2:82b72fa8dbcd 85 break;
vermaelen 2:82b72fa8dbcd 86 case 3 :
vermaelen 2:82b72fa8dbcd 87 trigger1.write(1);
vermaelen 2:82b72fa8dbcd 88 wait_us(10);
vermaelen 2:82b72fa8dbcd 89 trigger1.write(0);
vermaelen 2:82b72fa8dbcd 90 drap=1;
vermaelen 2:82b72fa8dbcd 91 break;
vermaelen 2:82b72fa8dbcd 92 }
vermaelen 2:82b72fa8dbcd 93
vermaelen 2:82b72fa8dbcd 94 }
vermaelen 2:82b72fa8dbcd 95 void start()
vermaelen 2:82b72fa8dbcd 96 {
vermaelen 2:82b72fa8dbcd 97 temp.reset();
vermaelen 2:82b72fa8dbcd 98 temp.start();
vermaelen 2:82b72fa8dbcd 99 }
vermaelen 2:82b72fa8dbcd 100 void stop()
vermaelen 2:82b72fa8dbcd 101 {
vermaelen 2:82b72fa8dbcd 102 temp.stop();
vermaelen 2:82b72fa8dbcd 103 switch(drap) {
vermaelen 2:82b72fa8dbcd 104 case 1 :
vermaelen 2:82b72fa8dbcd 105 if(flag3==0) {
vermaelen 2:82b72fa8dbcd 106 US3_av=US3;
vermaelen 2:82b72fa8dbcd 107 }
vermaelen 2:82b72fa8dbcd 108 US3=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 109 if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
vermaelen 2:82b72fa8dbcd 110 float temp=US3;
vermaelen 2:82b72fa8dbcd 111 US3=US3_av;
vermaelen 2:82b72fa8dbcd 112 US3_av=temp;
vermaelen 2:82b72fa8dbcd 113 flag3=1;
vermaelen 2:82b72fa8dbcd 114 } else {
vermaelen 2:82b72fa8dbcd 115 flag3=0;
vermaelen 2:82b72fa8dbcd 116 }
vermaelen 2:82b72fa8dbcd 117 if(US3<0||US3>150)US3=150;
vermaelen 2:82b72fa8dbcd 118 break;
vermaelen 2:82b72fa8dbcd 119 case 2 :
vermaelen 2:82b72fa8dbcd 120 if(flag2==0) {
vermaelen 2:82b72fa8dbcd 121 US2_av=US2;
vermaelen 2:82b72fa8dbcd 122 }
vermaelen 2:82b72fa8dbcd 123 US2=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 124 if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 125 float temp=US2;
vermaelen 2:82b72fa8dbcd 126 US2=US2_av;
vermaelen 2:82b72fa8dbcd 127 US2_av=temp;
vermaelen 2:82b72fa8dbcd 128 flag2=1;
vermaelen 2:82b72fa8dbcd 129 } else {
vermaelen 2:82b72fa8dbcd 130 flag2=0;
vermaelen 2:82b72fa8dbcd 131 }
vermaelen 2:82b72fa8dbcd 132 if(US2<0||US2>150)US2=150;
vermaelen 2:82b72fa8dbcd 133 break;
vermaelen 2:82b72fa8dbcd 134 case 3 :
vermaelen 2:82b72fa8dbcd 135 if(flag1==0) {
vermaelen 2:82b72fa8dbcd 136 US1_av=US1;
vermaelen 2:82b72fa8dbcd 137 }
vermaelen 2:82b72fa8dbcd 138 US1=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 139 if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 140 float temp=US1;
vermaelen 2:82b72fa8dbcd 141 US1=US1_av;
vermaelen 2:82b72fa8dbcd 142 US1_av=temp;
vermaelen 2:82b72fa8dbcd 143 flag1=1;
vermaelen 2:82b72fa8dbcd 144 } else {
vermaelen 2:82b72fa8dbcd 145 flag1=0;
vermaelen 2:82b72fa8dbcd 146 }
vermaelen 2:82b72fa8dbcd 147 if(US1<0||US1>150)US1=150;
vermaelen 2:82b72fa8dbcd 148 break;
vermaelen 2:82b72fa8dbcd 149 }
vermaelen 2:82b72fa8dbcd 150 }
vermaelen 2:82b72fa8dbcd 151 float vitesse(float vit)
vermaelen 2:82b72fa8dbcd 152 {
vermaelen 2:82b72fa8dbcd 153 if(vit<0) vit=0;
vermaelen 2:82b72fa8dbcd 154 if(vit>VMAX) vit=VMAX;
vermaelen 2:82b72fa8dbcd 155 return ((vit/100.0)*PERIOD);
vermaelen 2:82b72fa8dbcd 156 }