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

fct.cpp@13:4fdd8ad91c69, 2018-01-22 (annotated)

Committer:
syasya
Date:
Mon Jan 22 19:32:00 2018 +0000
Revision:
13:4fdd8ad91c69
Parent:
12:6151eb503170 
ROBOTIQUE COMPETITION PARIS

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"
syasya 7:64bfecbb3000 3 #include "CMPS03.h"
syasya 13:4fdd8ad91c69 4
syasya 13:4fdd8ad91c69 5 void ballon()
syasya 13:4fdd8ad91c69 6 {
syasya 13:4fdd8ad91c69 7 stop();
syasya 13:4fdd8ad91c69 8 servo.period(0.02);
syasya 13:4fdd8ad91c69 9 servo.pulsewidth_ms(17);
syasya 13:4fdd8ad91c69 10 wait(0.25);
syasya 13:4fdd8ad91c69 11 servo.pulsewidth_ms(19.5);
syasya 13:4fdd8ad91c69 12 wait(0.25);
syasya 13:4fdd8ad91c69 13 }
syasya 13:4fdd8ad91c69 14 void suivi_mur_dist()
syasya 13:4fdd8ad91c69 15 {
syasya 13:4fdd8ad91c69 16 E1=E0;
syasya 13:4fdd8ad91c69 17 E0=US2-US1;
syasya 13:4fdd8ad91c69 18 if((E0+E1)>0) {
syasya 13:4fdd8ad91c69 19 cmdD=VMAX;
syasya 13:4fdd8ad91c69 20 cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-30);
syasya 13:4fdd8ad91c69 21 } else {
syasya 13:4fdd8ad91c69 22 cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-30);
syasya 13:4fdd8ad91c69 23 cmdG=VMAX;
syasya 13:4fdd8ad91c69 24 }
syasya 13:4fdd8ad91c69 25
syasya 13:4fdd8ad91c69 26 MD.pulsewidth(vitesse(cmdD));
syasya 13:4fdd8ad91c69 27 MG.pulsewidth(vitesse(cmdG));
syasya 13:4fdd8ad91c69 28
syasya 13:4fdd8ad91c69 29 wait(0.001);
syasya 13:4fdd8ad91c69 30
syasya 13:4fdd8ad91c69 31 }
syasya 12:6151eb503170 32 void rotation_sharp()
syasya 12:6151eb503170 33 {
syasya 12:6151eb503170 34 sensMG.write(0);
syasya 12:6151eb503170 35 sensMD.write(1);
syasya 12:6151eb503170 36 cmdD=30;
syasya 12:6151eb503170 37 cmdG=30;
syasya 12:6151eb503170 38 MD.pulsewidth(vitesse(cmdD));
syasya 12:6151eb503170 39 MG.pulsewidth(vitesse(cmdG));
syasya 12:6151eb503170 40 }
syasya 9:7f62c0329849 41 void en_avant()
syasya 9:7f62c0329849 42 {
syasya 9:7f62c0329849 43 sensMG.write(0);
syasya 9:7f62c0329849 44 sensMD.write(0);
syasya 13:4fdd8ad91c69 45 cmdD=60;
syasya 13:4fdd8ad91c69 46 cmdG=60;
syasya 9:7f62c0329849 47 MD.pulsewidth(vitesse(cmdD));
syasya 9:7f62c0329849 48 MG.pulsewidth(vitesse(cmdG));
syasya 9:7f62c0329849 49 }
vermaelen 3:b91371837109 50 void contournement()
vermaelen 3:b91371837109 51 {
syasya 12:6151eb503170 52 sensMG.write(0);
vermaelen 3:b91371837109 53 sensMD.write(0);
syasya 13:4fdd8ad91c69 54 cmdD=40;
syasya 13:4fdd8ad91c69 55 cmdG=22;
vermaelen 3:b91371837109 56 MD.pulsewidth(vitesse(cmdD));
vermaelen 3:b91371837109 57 MG.pulsewidth(vitesse(cmdG));
vermaelen 3:b91371837109 58
vermaelen 3:b91371837109 59 }
syasya 7:64bfecbb3000 60 void rotation_horaire()
syasya 7:64bfecbb3000 61 {
syasya 7:64bfecbb3000 62 sensMG.write(0);
syasya 12:6151eb503170 63 sensMD.write(0);
syasya 13:4fdd8ad91c69 64 cmdD=30;
syasya 13:4fdd8ad91c69 65 cmdG=40;
syasya 7:64bfecbb3000 66 MD.pulsewidth(vitesse(cmdD));
syasya 7:64bfecbb3000 67 MG.pulsewidth(vitesse(cmdG));
syasya 7:64bfecbb3000 68 }
syasya 7:64bfecbb3000 69 void stop()
vermaelen 2:82b72fa8dbcd 70 {
vermaelen 2:82b72fa8dbcd 71 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 72 sensMD.write(0);
syasya 7:64bfecbb3000 73 cmdD=0;
syasya 7:64bfecbb3000 74 cmdG=0;
syasya 7:64bfecbb3000 75 MD.pulsewidth(vitesse(cmdD));
syasya 7:64bfecbb3000 76 MG.pulsewidth(vitesse(cmdG));
vermaelen 2:82b72fa8dbcd 77 }
vermaelen 2:82b72fa8dbcd 78 void suivi_mur()
vermaelen 2:82b72fa8dbcd 79 {
vermaelen 3:b91371837109 80 E1=E0;
vermaelen 3:b91371837109 81 E0=US2-US1;
vermaelen 5:3746060957fb 82 if((E0+E1)>0) {
syasya 7:64bfecbb3000 83 cmdD=VMAX;
syasya 7:64bfecbb3000 84 cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
vermaelen 5:3746060957fb 85 } else {
syasya 7:64bfecbb3000 86 cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
syasya 7:64bfecbb3000 87 cmdG=VMAX;
vermaelen 5:3746060957fb 88 }
vermaelen 3:b91371837109 89
vermaelen 2:82b72fa8dbcd 90 MD.pulsewidth(vitesse(cmdD));
vermaelen 2:82b72fa8dbcd 91 MG.pulsewidth(vitesse(cmdG));
vermaelen 3:b91371837109 92
vermaelen 3:b91371837109 93 wait(0.001);
syasya 7:64bfecbb3000 94
vermaelen 2:82b72fa8dbcd 95 }
vermaelen 2:82b72fa8dbcd 96 void init()
syasya 13:4fdd8ad91c69 97 {
syasya 13:4fdd8ad91c69 98 smoke.write(0);
vermaelen 2:82b72fa8dbcd 99 sensMG.write(0);
vermaelen 2:82b72fa8dbcd 100 sensMD.write(0);
vermaelen 2:82b72fa8dbcd 101 MG.period(PERIOD);
vermaelen 2:82b72fa8dbcd 102 MD.period(PERIOD);
vermaelen 2:82b72fa8dbcd 103 MG.pulsewidth(vitesse(0));
vermaelen 2:82b72fa8dbcd 104 MD.pulsewidth(vitesse(0));
vermaelen 3:b91371837109 105 tic1.attach(&fcttrig,0.035);
vermaelen 2:82b72fa8dbcd 106 tic2.attach(&mesAN,0.01);
syasya 7:64bfecbb3000 107 echo.rise(&start_trig);
syasya 7:64bfecbb3000 108 echo.fall(&stop_trig);
syasya 9:7f62c0329849 109 wait(0.1);
syasya 7:64bfecbb3000 110 bearing_set=boussole.readBearing()/10.0;
syasya 9:7f62c0329849 111 wait(0.1);
vermaelen 2:82b72fa8dbcd 112 }
vermaelen 2:82b72fa8dbcd 113 void mesAN()
vermaelen 2:82b72fa8dbcd 114 {
vermaelen 2:82b72fa8dbcd 115 if(flag4==0) {
vermaelen 2:82b72fa8dbcd 116 AN1_av=AN1;
vermaelen 2:82b72fa8dbcd 117 }
vermaelen 2:82b72fa8dbcd 118 AN1=0.82*a/(3.3*AnaG.read()-b);
vermaelen 3:b91371837109 119 if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
vermaelen 2:82b72fa8dbcd 120 float temp=AN1;
vermaelen 2:82b72fa8dbcd 121 AN1=AN1_av;
vermaelen 2:82b72fa8dbcd 122 AN1_av=temp;
vermaelen 2:82b72fa8dbcd 123 flag4=1;
vermaelen 2:82b72fa8dbcd 124 } else {
vermaelen 2:82b72fa8dbcd 125 flag4=0;
vermaelen 2:82b72fa8dbcd 126 }
vermaelen 2:82b72fa8dbcd 127 if(AN1<0||AN1>150)AN1=150;
vermaelen 2:82b72fa8dbcd 128
vermaelen 2:82b72fa8dbcd 129
vermaelen 2:82b72fa8dbcd 130 if(flag5==0) {
vermaelen 2:82b72fa8dbcd 131 AN2_av=AN2;
vermaelen 2:82b72fa8dbcd 132 }
vermaelen 2:82b72fa8dbcd 133 AN2=0.82*a/(3.3*AnaAV.read()-b);
vermaelen 2:82b72fa8dbcd 134
vermaelen 3:b91371837109 135 if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
vermaelen 2:82b72fa8dbcd 136 float temp=AN2;
vermaelen 2:82b72fa8dbcd 137 AN2=AN2_av;
vermaelen 2:82b72fa8dbcd 138 AN2_av=temp;
vermaelen 2:82b72fa8dbcd 139 flag5=1;
vermaelen 2:82b72fa8dbcd 140 } else {
vermaelen 2:82b72fa8dbcd 141 flag5=0;
vermaelen 2:82b72fa8dbcd 142 }
vermaelen 2:82b72fa8dbcd 143 if(AN2<0||AN2>150)AN2=150;
vermaelen 2:82b72fa8dbcd 144 }
vermaelen 2:82b72fa8dbcd 145
vermaelen 2:82b72fa8dbcd 146 void fcttrig()
vermaelen 2:82b72fa8dbcd 147 {
vermaelen 2:82b72fa8dbcd 148 switch(drap) {
vermaelen 2:82b72fa8dbcd 149 case 1 :
vermaelen 2:82b72fa8dbcd 150 trigger2.write(1);
vermaelen 2:82b72fa8dbcd 151 wait_us(10);
vermaelen 2:82b72fa8dbcd 152 trigger2.write(0);
vermaelen 2:82b72fa8dbcd 153 drap=2;
vermaelen 2:82b72fa8dbcd 154 break;
vermaelen 2:82b72fa8dbcd 155 case 2 :
vermaelen 2:82b72fa8dbcd 156 trigger3.write(1);
vermaelen 2:82b72fa8dbcd 157 wait_us(10);
vermaelen 2:82b72fa8dbcd 158 trigger3.write(0);
vermaelen 2:82b72fa8dbcd 159 drap=3;
vermaelen 2:82b72fa8dbcd 160 break;
vermaelen 2:82b72fa8dbcd 161 case 3 :
vermaelen 2:82b72fa8dbcd 162 trigger1.write(1);
vermaelen 2:82b72fa8dbcd 163 wait_us(10);
vermaelen 2:82b72fa8dbcd 164 trigger1.write(0);
vermaelen 2:82b72fa8dbcd 165 drap=1;
vermaelen 2:82b72fa8dbcd 166 break;
vermaelen 2:82b72fa8dbcd 167 }
vermaelen 2:82b72fa8dbcd 168
vermaelen 2:82b72fa8dbcd 169 }
syasya 7:64bfecbb3000 170 void start_trig()
vermaelen 2:82b72fa8dbcd 171 {
vermaelen 2:82b72fa8dbcd 172 temp.reset();
vermaelen 2:82b72fa8dbcd 173 temp.start();
vermaelen 2:82b72fa8dbcd 174 }
syasya 7:64bfecbb3000 175 void stop_trig()
vermaelen 2:82b72fa8dbcd 176 {
vermaelen 2:82b72fa8dbcd 177 temp.stop();
vermaelen 2:82b72fa8dbcd 178 switch(drap) {
vermaelen 2:82b72fa8dbcd 179 case 1 :
vermaelen 2:82b72fa8dbcd 180 if(flag3==0) {
vermaelen 2:82b72fa8dbcd 181 US3_av=US3;
vermaelen 2:82b72fa8dbcd 182 }
vermaelen 2:82b72fa8dbcd 183 US3=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 184 if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
vermaelen 2:82b72fa8dbcd 185 float temp=US3;
vermaelen 2:82b72fa8dbcd 186 US3=US3_av;
vermaelen 2:82b72fa8dbcd 187 US3_av=temp;
vermaelen 2:82b72fa8dbcd 188 flag3=1;
vermaelen 2:82b72fa8dbcd 189 } else {
vermaelen 2:82b72fa8dbcd 190 flag3=0;
vermaelen 2:82b72fa8dbcd 191 }
vermaelen 2:82b72fa8dbcd 192 if(US3<0||US3>150)US3=150;
vermaelen 2:82b72fa8dbcd 193 break;
vermaelen 2:82b72fa8dbcd 194 case 2 :
vermaelen 2:82b72fa8dbcd 195 if(flag2==0) {
vermaelen 2:82b72fa8dbcd 196 US2_av=US2;
vermaelen 2:82b72fa8dbcd 197 }
vermaelen 2:82b72fa8dbcd 198 US2=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 199 if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
vermaelen 2:82b72fa8dbcd 200 float temp=US2;
vermaelen 2:82b72fa8dbcd 201 US2=US2_av;
vermaelen 2:82b72fa8dbcd 202 US2_av=temp;
vermaelen 2:82b72fa8dbcd 203 flag2=1;
vermaelen 2:82b72fa8dbcd 204 } else {
vermaelen 2:82b72fa8dbcd 205 flag2=0;
vermaelen 2:82b72fa8dbcd 206 }
vermaelen 2:82b72fa8dbcd 207 if(US2<0||US2>150)US2=150;
vermaelen 2:82b72fa8dbcd 208 break;
vermaelen 2:82b72fa8dbcd 209 case 3 :
vermaelen 2:82b72fa8dbcd 210 if(flag1==0) {
vermaelen 2:82b72fa8dbcd 211 US1_av=US1;
vermaelen 2:82b72fa8dbcd 212 }
vermaelen 2:82b72fa8dbcd 213 US1=temp.read_us()/58.31;
vermaelen 2:82b72fa8dbcd 214 if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
vermaelen 2:82b72fa8dbcd 215 float temp=US1;
vermaelen 2:82b72fa8dbcd 216 US1=US1_av;
vermaelen 2:82b72fa8dbcd 217 US1_av=temp;
vermaelen 2:82b72fa8dbcd 218 flag1=1;
vermaelen 2:82b72fa8dbcd 219 } else {
vermaelen 2:82b72fa8dbcd 220 flag1=0;
vermaelen 2:82b72fa8dbcd 221 }
vermaelen 2:82b72fa8dbcd 222 if(US1<0||US1>150)US1=150;
vermaelen 2:82b72fa8dbcd 223 break;
vermaelen 2:82b72fa8dbcd 224 }
vermaelen 2:82b72fa8dbcd 225 }
vermaelen 2:82b72fa8dbcd 226 float vitesse(float vit)
vermaelen 2:82b72fa8dbcd 227 {
vermaelen 2:82b72fa8dbcd 228 if(vit<0) vit=0;
syasya 7:64bfecbb3000 229 if(vit>VLIMIT) vit=VLIMIT;
vermaelen 2:82b72fa8dbcd 230 return ((vit/100.0)*PERIOD);
vermaelen 2:82b72fa8dbcd 231 }