Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
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?
User | Revision | Line number | New 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 | } |