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@12:6151eb503170, 2017-06-01 (annotated)

Committer:
syasya
Date:
Thu Jun 01 09:30:37 2017 +0000
Revision:
12:6151eb503170
Parent:
10:1a69a6f659bd
Child:
13:4fdd8ad91c69
probleme t3

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