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@7:64bfecbb3000, 2017-05-31 (annotated)

Committer:
syasya
Date:
Wed May 31 12:17:59 2017 +0000
Revision:
7:64bfecbb3000
Parent:
5:3746060957fb
Child:
9:7f62c0329849
with bearing;

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