Alexandre Pirotte / Mbed 2 deprecated 0concours_cachan_programme_ok

ok

Dependencies:   mbed

Fork of _test_suivi_mur by christophe vermaelen

fct.cpp@7:2f4660e9cf92, 2017-06-23 (annotated)

Committer:
pirottealex
Date:
Fri Jun 23 11:19:58 2017 +0000
Revision:
7:2f4660e9cf92
Parent:
5:3746060957fb 
pixy_ok2;

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 3:b91371837109 3
vermaelen 2:82b72fa8dbcd 4 void stopMotor()
vermaelen 2:82b72fa8dbcd 5 {
pirottealex 7:2f4660e9cf92 6 vitesse(0,0);
vermaelen 2:82b72fa8dbcd 7 }
vermaelen 3:b91371837109 8
vermaelen 2:82b72fa8dbcd 9 void init()
vermaelen 2:82b72fa8dbcd 10 {
pirottealex 7:2f4660e9cf92 11 capb1.mode(PullUp);
pirottealex 7:2f4660e9cf92 12 capb2.mode(PullUp);
pirottealex 7:2f4660e9cf92 13 servo_stop();
pirottealex 7:2f4660e9cf92 14 jack.mode(PullUp);
vermaelen 2:82b72fa8dbcd 15 MG.period(PERIOD);
vermaelen 2:82b72fa8dbcd 16 MD.period(PERIOD);
pirottealex 7:2f4660e9cf92 17 vitesse(0,0);
vermaelen 3:b91371837109 18 tic1.attach(&fcttrig,0.035);
vermaelen 2:82b72fa8dbcd 19 tic2.attach(&mesAN,0.01);
vermaelen 2:82b72fa8dbcd 20 echo.rise(&start);
vermaelen 2:82b72fa8dbcd 21 echo.fall(&stop);
pirottealex 7:2f4660e9cf92 22
vermaelen 2:82b72fa8dbcd 23 }
vermaelen 2:82b72fa8dbcd 24 void mesAN()
vermaelen 2:82b72fa8dbcd 25 {
vermaelen 2:82b72fa8dbcd 26 if(flag4==0) {
vermaelen 2:82b72fa8dbcd 27 AN1_av=AN1;
vermaelen 2:82b72fa8dbcd 28 }
pirottealex 7:2f4660e9cf92 29 AN1=a/(3.3*AnaG.read()-b);
pirottealex 7:2f4660e9cf92 30 if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 31 float temp=AN1;
vermaelen 2:82b72fa8dbcd 32 AN1=AN1_av;
vermaelen 2:82b72fa8dbcd 33 AN1_av=temp;
vermaelen 2:82b72fa8dbcd 34 flag4=1;
vermaelen 2:82b72fa8dbcd 35 } else {
vermaelen 2:82b72fa8dbcd 36 flag4=0;
vermaelen 2:82b72fa8dbcd 37 }
pirottealex 7:2f4660e9cf92 38 if((AN1<0)||(AN1>120))AN1=120;
pirottealex 7:2f4660e9cf92 39
vermaelen 2:82b72fa8dbcd 40
vermaelen 2:82b72fa8dbcd 41
vermaelen 2:82b72fa8dbcd 42 if(flag5==0) {
vermaelen 2:82b72fa8dbcd 43 AN2_av=AN2;
vermaelen 2:82b72fa8dbcd 44 }
pirottealex 7:2f4660e9cf92 45 AN2=a/(3.3*AnaAV.read()-b);
vermaelen 2:82b72fa8dbcd 46
pirottealex 7:2f4660e9cf92 47 if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 48 float temp=AN2;
vermaelen 2:82b72fa8dbcd 49 AN2=AN2_av;
vermaelen 2:82b72fa8dbcd 50 AN2_av=temp;
vermaelen 2:82b72fa8dbcd 51 flag5=1;
vermaelen 2:82b72fa8dbcd 52 } else {
vermaelen 2:82b72fa8dbcd 53 flag5=0;
vermaelen 2:82b72fa8dbcd 54 }
pirottealex 7:2f4660e9cf92 55 if((AN2<0)||(AN2>120))AN2=120;
pirottealex 7:2f4660e9cf92 56 capt_d=AN1;
pirottealex 7:2f4660e9cf92 57 capt_g=AN2;
vermaelen 2:82b72fa8dbcd 58 }
vermaelen 2:82b72fa8dbcd 59
pirottealex 7:2f4660e9cf92 60
vermaelen 2:82b72fa8dbcd 61 void fcttrig()
vermaelen 2:82b72fa8dbcd 62 {
vermaelen 2:82b72fa8dbcd 63 switch(drap) {
vermaelen 2:82b72fa8dbcd 64 case 1 :
vermaelen 2:82b72fa8dbcd 65 trigger2.write(1);
vermaelen 2:82b72fa8dbcd 66 wait_us(10);
vermaelen 2:82b72fa8dbcd 67 trigger2.write(0);
vermaelen 2:82b72fa8dbcd 68 drap=2;
vermaelen 2:82b72fa8dbcd 69 break;
vermaelen 2:82b72fa8dbcd 70 case 2 :
vermaelen 2:82b72fa8dbcd 71 trigger3.write(1);
vermaelen 2:82b72fa8dbcd 72 wait_us(10);
vermaelen 2:82b72fa8dbcd 73 trigger3.write(0);
vermaelen 2:82b72fa8dbcd 74 drap=3;
vermaelen 2:82b72fa8dbcd 75 break;
vermaelen 2:82b72fa8dbcd 76 case 3 :
vermaelen 2:82b72fa8dbcd 77 trigger1.write(1);
vermaelen 2:82b72fa8dbcd 78 wait_us(10);
vermaelen 2:82b72fa8dbcd 79 trigger1.write(0);
vermaelen 2:82b72fa8dbcd 80 drap=1;
vermaelen 2:82b72fa8dbcd 81 break;
vermaelen 2:82b72fa8dbcd 82 }
vermaelen 2:82b72fa8dbcd 83
vermaelen 2:82b72fa8dbcd 84 }
vermaelen 2:82b72fa8dbcd 85 void start()
vermaelen 2:82b72fa8dbcd 86 {
vermaelen 2:82b72fa8dbcd 87 temp.reset();
vermaelen 2:82b72fa8dbcd 88 temp.start();
vermaelen 2:82b72fa8dbcd 89 }
vermaelen 2:82b72fa8dbcd 90 void stop()
vermaelen 2:82b72fa8dbcd 91 {
vermaelen 2:82b72fa8dbcd 92 temp.stop();
vermaelen 2:82b72fa8dbcd 93 switch(drap) {
vermaelen 2:82b72fa8dbcd 94 case 1 :
vermaelen 2:82b72fa8dbcd 95 if(flag3==0) {
vermaelen 2:82b72fa8dbcd 96 US3_av=US3;
vermaelen 2:82b72fa8dbcd 97 }
vermaelen 2:82b72fa8dbcd 98 US3=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 99 if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
vermaelen 2:82b72fa8dbcd 100 float temp=US3;
vermaelen 2:82b72fa8dbcd 101 US3=US3_av;
vermaelen 2:82b72fa8dbcd 102 US3_av=temp;
vermaelen 2:82b72fa8dbcd 103 flag3=1;
vermaelen 2:82b72fa8dbcd 104 } else {
vermaelen 2:82b72fa8dbcd 105 flag3=0;
vermaelen 2:82b72fa8dbcd 106 }
pirottealex 7:2f4660e9cf92 107 if(US3>capt_max)US3=capt_max;
pirottealex 7:2f4660e9cf92 108 if(US3<capt_min)US3=capt_min;
vermaelen 2:82b72fa8dbcd 109 break;
vermaelen 2:82b72fa8dbcd 110 case 2 :
vermaelen 2:82b72fa8dbcd 111 if(flag2==0) {
vermaelen 2:82b72fa8dbcd 112 US2_av=US2;
vermaelen 2:82b72fa8dbcd 113 }
vermaelen 2:82b72fa8dbcd 114 US2=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 115 if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 116 float temp=US2;
vermaelen 2:82b72fa8dbcd 117 US2=US2_av;
vermaelen 2:82b72fa8dbcd 118 US2_av=temp;
vermaelen 2:82b72fa8dbcd 119 flag2=1;
vermaelen 2:82b72fa8dbcd 120 } else {
vermaelen 2:82b72fa8dbcd 121 flag2=0;
vermaelen 2:82b72fa8dbcd 122 }
pirottealex 7:2f4660e9cf92 123 if(US2>capt_max)US2=capt_max;
pirottealex 7:2f4660e9cf92 124 if(US2<capt_min)US2=capt_min;
vermaelen 2:82b72fa8dbcd 125 break;
vermaelen 2:82b72fa8dbcd 126 case 3 :
vermaelen 2:82b72fa8dbcd 127 if(flag1==0) {
vermaelen 2:82b72fa8dbcd 128 US1_av=US1;
vermaelen 2:82b72fa8dbcd 129 }
vermaelen 2:82b72fa8dbcd 130 US1=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 131 if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 132 float temp=US1;
vermaelen 2:82b72fa8dbcd 133 US1=US1_av;
vermaelen 2:82b72fa8dbcd 134 US1_av=temp;
vermaelen 2:82b72fa8dbcd 135 flag1=1;
vermaelen 2:82b72fa8dbcd 136 } else {
vermaelen 2:82b72fa8dbcd 137 flag1=0;
vermaelen 2:82b72fa8dbcd 138 }
pirottealex 7:2f4660e9cf92 139 if(US1>capt_max)US1=capt_max;
pirottealex 7:2f4660e9cf92 140 if(US1<capt_min)US1=capt_min;
vermaelen 2:82b72fa8dbcd 141 break;
vermaelen 2:82b72fa8dbcd 142 }
pirottealex 7:2f4660e9cf92 143 capt_ed=US1;
pirottealex 7:2f4660e9cf92 144 capt_eg=US3;
pirottealex 7:2f4660e9cf92 145 capt_m=US2;
vermaelen 2:82b72fa8dbcd 146 }
pirottealex 7:2f4660e9cf92 147 void vitesse(float vitG, float vitD)
vermaelen 2:82b72fa8dbcd 148 {
pirottealex 7:2f4660e9cf92 149 if(vitG<0) {
pirottealex 7:2f4660e9cf92 150 vitG=-1*vitG;
pirottealex 7:2f4660e9cf92 151 sensMG.write(1);
pirottealex 7:2f4660e9cf92 152 } else sensMG.write(0);
pirottealex 7:2f4660e9cf92 153 MG.pulsewidth(((vitG)/100.0)*PERIOD);
pirottealex 7:2f4660e9cf92 154 if(vitD<0) {
pirottealex 7:2f4660e9cf92 155 vitD=-1*vitD;
pirottealex 7:2f4660e9cf92 156 sensMD.write(1);
pirottealex 7:2f4660e9cf92 157 } else sensMD.write(0);
pirottealex 7:2f4660e9cf92 158 MD.pulsewidth(((vitD)/100.0)*PERIOD);
vermaelen 2:82b72fa8dbcd 159 }
pirottealex 7:2f4660e9cf92 160
pirottealex 7:2f4660e9cf92 161 void servo_start(void)
pirottealex 7:2f4660e9cf92 162 {
pirottealex 7:2f4660e9cf92 163 servo.period(periode);
pirottealex 7:2f4660e9cf92 164 turn=18.8;
pirottealex 7:2f4660e9cf92 165 servo.pulsewidth_ms(turn);
pirottealex 7:2f4660e9cf92 166 wait(0.25);
pirottealex 7:2f4660e9cf92 167 turn=19.8;
pirottealex 7:2f4660e9cf92 168 servo.pulsewidth_ms(turn);
pirottealex 7:2f4660e9cf92 169 wait(0.25);
pirottealex 7:2f4660e9cf92 170 }
pirottealex 7:2f4660e9cf92 171
pirottealex 7:2f4660e9cf92 172 void servo_stop(void)
pirottealex 7:2f4660e9cf92 173 {
pirottealex 7:2f4660e9cf92 174 servo.period(periode);
pirottealex 7:2f4660e9cf92 175 turn=18.8;
pirottealex 7:2f4660e9cf92 176 servo.pulsewidth_ms(turn);
pirottealex 7:2f4660e9cf92 177 }