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

Committer:
syasya
Date:
Wed May 31 16:30:10 2017 +0000
Revision:
9:7f62c0329849
Parent:
7:64bfecbb3000
Child:
10:1a69a6f659bd
suive du bord probleme timer et contournement

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