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