Mirte H / Mbed 2 deprecated Lijn_code_af_robotzwenkwiel

leuk

Dependencies:   mbed HCSR04

main.cpp@1:a8c5e92c6cbf, 2019-05-29 (annotated)

Committer:
mirteholm
Date:
Wed May 29 15:28:52 2019 +0000
Revision:
1:a8c5e92c6cbf
Parent:
0:7d87166a1170 
leuk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harveys 0:7d87166a1170 1 // Initialize a pins to perform Serial Communication for receive
harveys 0:7d87166a1170 2 // the result of the printf on PC (USB Virtual Com)
harveys 0:7d87166a1170 3 // I suggest to use TeraTerm on PC.
harveys 0:7d87166a1170 4 // TeraTerm configuration must be: 9600-8-N-1 FlowControl None
mirteholm 1:a8c5e92c6cbf 5
harveys 0:7d87166a1170 6 /* EXAMPLE */
harveys 0:7d87166a1170 7 #include "mbed.h"
harveys 0:7d87166a1170 8 #include "hcsr04.h"
harveys 0:7d87166a1170 9
harveys 0:7d87166a1170 10 Serial pc(USBTX, USBRX);
mirteholm 1:a8c5e92c6cbf 11
mirteholm 1:a8c5e92c6cbf 12
mirteholm 1:a8c5e92c6cbf 13 DigitalOut linksachter(D9);
mirteholm 1:a8c5e92c6cbf 14 DigitalOut rechtsachter(A6);
mirteholm 1:a8c5e92c6cbf 15 DigitalOut rechtsvoor(D7);
mirteholm 1:a8c5e92c6cbf 16 DigitalOut linksvoor(D10);
mirteholm 1:a8c5e92c6cbf 17 PwmOut pwm(D6); // links
mirteholm 1:a8c5e92c6cbf 18 PwmOut pwm2(D11);
mirteholm 1:a8c5e92c6cbf 19
mirteholm 1:a8c5e92c6cbf 20 HCSR04 sensor(D4, A0);
mirteholm 1:a8c5e92c6cbf 21
mirteholm 1:a8c5e92c6cbf 22
mirteholm 1:a8c5e92c6cbf 23
mirteholm 1:a8c5e92c6cbf 24 DigitalIn lijnsensor4(D1);
mirteholm 1:a8c5e92c6cbf 25 DigitalIn lijnsensor3(D0);
mirteholm 1:a8c5e92c6cbf 26 DigitalIn lijnsensor1(D2);
mirteholm 1:a8c5e92c6cbf 27 DigitalIn lijnsensor2(D3);
mirteholm 1:a8c5e92c6cbf 28
mirteholm 1:a8c5e92c6cbf 29
mirteholm 1:a8c5e92c6cbf 30 HCSR04 sensor2(D4, A2);
mirteholm 1:a8c5e92c6cbf 31 //HCSR04 sensor2(D4, A0);
mirteholm 1:a8c5e92c6cbf 32 //HCSR04 sensor3(D4, A0);
mirteholm 1:a8c5e92c6cbf 33 //HCSR04 sensor4(D4, A0);
mirteholm 1:a8c5e92c6cbf 34
mirteholm 1:a8c5e92c6cbf 35
mirteholm 1:a8c5e92c6cbf 36
mirteholm 1:a8c5e92c6cbf 37 Timer timer;
mirteholm 1:a8c5e92c6cbf 38 Timer timer2;
mirteholm 1:a8c5e92c6cbf 39 Timer timer3;
mirteholm 1:a8c5e92c6cbf 40 Timer timer4;
mirteholm 1:a8c5e92c6cbf 41 Timer timer5;
mirteholm 1:a8c5e92c6cbf 42 Timer timer6;
mirteholm 1:a8c5e92c6cbf 43
mirteholm 1:a8c5e92c6cbf 44
mirteholm 1:a8c5e92c6cbf 45 enum ROBOTSTATE {vooruit, lijn_detect_linksvoor, lijn_detect_rechtsvoor, lijn_detect_linksachter, lijn_detect_rechtsachter, lijn_detect_voor, lijn_detect_achter, robot_detect_voor, robot_detect_rechts, robot_detect_links, robot_detect_achter, start,};
mirteholm 1:a8c5e92c6cbf 46 ROBOTSTATE myState;
mirteholm 1:a8c5e92c6cbf 47 int main()
harveys 0:7d87166a1170 48 {
mirteholm 1:a8c5e92c6cbf 49 pwm.period_ms(10/6);
mirteholm 1:a8c5e92c6cbf 50 pwm.pulsewidth_ms(1);
mirteholm 1:a8c5e92c6cbf 51 pwm2.period_ms(10/6);
mirteholm 1:a8c5e92c6cbf 52 pwm2.pulsewidth_ms(1);
mirteholm 1:a8c5e92c6cbf 53
mirteholm 1:a8c5e92c6cbf 54 while(1) {
mirteholm 1:a8c5e92c6cbf 55
mirteholm 1:a8c5e92c6cbf 56
mirteholm 1:a8c5e92c6cbf 57
mirteholm 1:a8c5e92c6cbf 58 switch(myState) {
mirteholm 1:a8c5e92c6cbf 59
mirteholm 1:a8c5e92c6cbf 60
mirteholm 1:a8c5e92c6cbf 61
mirteholm 1:a8c5e92c6cbf 62
mirteholm 1:a8c5e92c6cbf 63
mirteholm 1:a8c5e92c6cbf 64 case vooruit:
mirteholm 1:a8c5e92c6cbf 65 pwm.period_ms(10/6);
mirteholm 1:a8c5e92c6cbf 66 pwm.pulsewidth_ms(1);
mirteholm 1:a8c5e92c6cbf 67 pwm2.period_ms(10/6);
mirteholm 1:a8c5e92c6cbf 68 pwm2.pulsewidth_ms(1);
mirteholm 1:a8c5e92c6cbf 69
mirteholm 1:a8c5e92c6cbf 70 pwm=0.4;
mirteholm 1:a8c5e92c6cbf 71 pwm2=0.4;
mirteholm 1:a8c5e92c6cbf 72 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 73 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 74 linksachter=0;
mirteholm 1:a8c5e92c6cbf 75 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 76 if(lijnsensor1==0) {
mirteholm 1:a8c5e92c6cbf 77 myState = lijn_detect_linksvoor;
mirteholm 1:a8c5e92c6cbf 78 }
mirteholm 1:a8c5e92c6cbf 79 if(lijnsensor2==0) {
mirteholm 1:a8c5e92c6cbf 80 myState=lijn_detect_rechtsvoor;
mirteholm 1:a8c5e92c6cbf 81 }
mirteholm 1:a8c5e92c6cbf 82 if (lijnsensor3 ==0) {
mirteholm 1:a8c5e92c6cbf 83 myState=lijn_detect_linksachter;
mirteholm 1:a8c5e92c6cbf 84 }
mirteholm 1:a8c5e92c6cbf 85 if (lijnsensor4==0) {
mirteholm 1:a8c5e92c6cbf 86 myState = lijn_detect_rechtsachter;
mirteholm 1:a8c5e92c6cbf 87 }
mirteholm 1:a8c5e92c6cbf 88 if (sensor.distance()<50) { myState = robot_detect_voor; }
mirteholm 1:a8c5e92c6cbf 89 if (sensor2.distance()<30) {myState = robot_detect_achter;}
mirteholm 1:a8c5e92c6cbf 90 break;
mirteholm 1:a8c5e92c6cbf 91
mirteholm 1:a8c5e92c6cbf 92
mirteholm 1:a8c5e92c6cbf 93
mirteholm 1:a8c5e92c6cbf 94 case lijn_detect_linksvoor:
mirteholm 1:a8c5e92c6cbf 95
mirteholm 1:a8c5e92c6cbf 96 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 97 rechtsvoor=0;
mirteholm 1:a8c5e92c6cbf 98 linksachter=0;
mirteholm 1:a8c5e92c6cbf 99 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 100 pwm=0.9;
mirteholm 1:a8c5e92c6cbf 101 pwm2=0.9;
mirteholm 1:a8c5e92c6cbf 102 wait_ms(2000);
mirteholm 1:a8c5e92c6cbf 103 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 104 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 105 wait_ms(500);
mirteholm 1:a8c5e92c6cbf 106 myState = vooruit;
mirteholm 1:a8c5e92c6cbf 107 break;
mirteholm 1:a8c5e92c6cbf 108
mirteholm 1:a8c5e92c6cbf 109 case lijn_detect_rechtsvoor:
mirteholm 1:a8c5e92c6cbf 110
mirteholm 1:a8c5e92c6cbf 111
mirteholm 1:a8c5e92c6cbf 112 linksvoor=0;
mirteholm 1:a8c5e92c6cbf 113 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 114 linksachter=0;
mirteholm 1:a8c5e92c6cbf 115 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 116 pwm=0.9;
mirteholm 1:a8c5e92c6cbf 117 pwm2=0.9;
mirteholm 1:a8c5e92c6cbf 118 wait_ms(2000);
mirteholm 1:a8c5e92c6cbf 119 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 120 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 121 wait_ms(500);
mirteholm 1:a8c5e92c6cbf 122 myState= vooruit;
mirteholm 1:a8c5e92c6cbf 123 break;
mirteholm 1:a8c5e92c6cbf 124
mirteholm 1:a8c5e92c6cbf 125
mirteholm 1:a8c5e92c6cbf 126 case lijn_detect_linksachter:
mirteholm 1:a8c5e92c6cbf 127
mirteholm 1:a8c5e92c6cbf 128 timer3.start();
mirteholm 1:a8c5e92c6cbf 129 linksvoor=0;
mirteholm 1:a8c5e92c6cbf 130 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 131 linksachter=0;
mirteholm 1:a8c5e92c6cbf 132 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 133 pwm=0.7;
mirteholm 1:a8c5e92c6cbf 134 pwm2=0.8;
mirteholm 1:a8c5e92c6cbf 135 if(timer3.read_ms() >= 500 ) {
mirteholm 1:a8c5e92c6cbf 136 timer3.stop();
mirteholm 1:a8c5e92c6cbf 137 timer3.reset();
mirteholm 1:a8c5e92c6cbf 138 myState = vooruit;
mirteholm 1:a8c5e92c6cbf 139 }
mirteholm 1:a8c5e92c6cbf 140 break;
mirteholm 1:a8c5e92c6cbf 141
mirteholm 1:a8c5e92c6cbf 142 case lijn_detect_rechtsachter:
mirteholm 1:a8c5e92c6cbf 143
mirteholm 1:a8c5e92c6cbf 144 timer4.start();
mirteholm 1:a8c5e92c6cbf 145 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 146 rechtsvoor=0;
mirteholm 1:a8c5e92c6cbf 147 linksachter=0;
mirteholm 1:a8c5e92c6cbf 148 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 149 pwm=0.7;
mirteholm 1:a8c5e92c6cbf 150 pwm2=0.8;
mirteholm 1:a8c5e92c6cbf 151 if(timer4.read_ms() >= 500 ) { // voor twee seconden draait hij
mirteholm 1:a8c5e92c6cbf 152 timer4.stop();
mirteholm 1:a8c5e92c6cbf 153 timer4.reset();
mirteholm 1:a8c5e92c6cbf 154 myState = vooruit;
mirteholm 1:a8c5e92c6cbf 155 }
mirteholm 1:a8c5e92c6cbf 156 break;
mirteholm 1:a8c5e92c6cbf 157
mirteholm 1:a8c5e92c6cbf 158
mirteholm 1:a8c5e92c6cbf 159 case robot_detect_voor:
mirteholm 1:a8c5e92c6cbf 160
mirteholm 1:a8c5e92c6cbf 161
mirteholm 1:a8c5e92c6cbf 162 timer5.start();
mirteholm 1:a8c5e92c6cbf 163 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 164 rechtsvoor=1;
mirteholm 1:a8c5e92c6cbf 165 linksachter=0;
mirteholm 1:a8c5e92c6cbf 166 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 167 pwm=1;
mirteholm 1:a8c5e92c6cbf 168 pwm2=1;
mirteholm 1:a8c5e92c6cbf 169
mirteholm 1:a8c5e92c6cbf 170 if(lijnsensor1==0) {
mirteholm 1:a8c5e92c6cbf 171 timer5.stop(); timer5.reset(); myState = lijn_detect_linksvoor;
mirteholm 1:a8c5e92c6cbf 172 }
mirteholm 1:a8c5e92c6cbf 173 if(lijnsensor2==0) {
mirteholm 1:a8c5e92c6cbf 174 timer5.stop(); timer5.reset(); myState=lijn_detect_rechtsvoor;
mirteholm 1:a8c5e92c6cbf 175 }
mirteholm 1:a8c5e92c6cbf 176 if (lijnsensor3 ==0) {
mirteholm 1:a8c5e92c6cbf 177 timer5.stop(); timer5.reset(); myState=lijn_detect_linksachter;
mirteholm 1:a8c5e92c6cbf 178 }
mirteholm 1:a8c5e92c6cbf 179 if (lijnsensor4==0) {
mirteholm 1:a8c5e92c6cbf 180 timer5.stop(); timer5.reset(); myState = lijn_detect_rechtsachter;
mirteholm 1:a8c5e92c6cbf 181 }
mirteholm 1:a8c5e92c6cbf 182 if (timer5.read_ms() >= 2000) { timer5.stop(); timer5.reset(); myState = vooruit; }
mirteholm 1:a8c5e92c6cbf 183 break;
mirteholm 1:a8c5e92c6cbf 184
mirteholm 1:a8c5e92c6cbf 185 case robot_detect_achter:
mirteholm 1:a8c5e92c6cbf 186
mirteholm 1:a8c5e92c6cbf 187
mirteholm 1:a8c5e92c6cbf 188 timer6.start();
mirteholm 1:a8c5e92c6cbf 189 pwm=1;
mirteholm 1:a8c5e92c6cbf 190 pwm2=1;
mirteholm 1:a8c5e92c6cbf 191 linksvoor=1;
mirteholm 1:a8c5e92c6cbf 192 rechtsvoor=0;
mirteholm 1:a8c5e92c6cbf 193 linksachter=0;
mirteholm 1:a8c5e92c6cbf 194 rechtsachter=0;
mirteholm 1:a8c5e92c6cbf 195
mirteholm 1:a8c5e92c6cbf 196 if(lijnsensor1==0) {
mirteholm 1:a8c5e92c6cbf 197 timer6.stop(); timer6.reset(); myState = lijn_detect_linksvoor;
mirteholm 1:a8c5e92c6cbf 198 }
mirteholm 1:a8c5e92c6cbf 199 if(lijnsensor2==0) {
mirteholm 1:a8c5e92c6cbf 200 timer6.stop(); timer6.reset(); myState=lijn_detect_rechtsvoor;
mirteholm 1:a8c5e92c6cbf 201 }
mirteholm 1:a8c5e92c6cbf 202 if (lijnsensor3 ==0) {
mirteholm 1:a8c5e92c6cbf 203 timer6.stop(); timer6.reset(); myState=lijn_detect_linksachter;
mirteholm 1:a8c5e92c6cbf 204 }
mirteholm 1:a8c5e92c6cbf 205 if (lijnsensor4==0) {
mirteholm 1:a8c5e92c6cbf 206 timer6.stop(); timer6.reset(); myState = lijn_detect_rechtsachter;
mirteholm 1:a8c5e92c6cbf 207 }
mirteholm 1:a8c5e92c6cbf 208 if (timer6.read_ms() >= 1000) { timer6.stop(); timer6.reset(); myState = vooruit; }
mirteholm 1:a8c5e92c6cbf 209
mirteholm 1:a8c5e92c6cbf 210
mirteholm 1:a8c5e92c6cbf 211
mirteholm 1:a8c5e92c6cbf 212
mirteholm 1:a8c5e92c6cbf 213
mirteholm 1:a8c5e92c6cbf 214
mirteholm 1:a8c5e92c6cbf 215
mirteholm 1:a8c5e92c6cbf 216
harveys 0:7d87166a1170 217 }
harveys 0:7d87166a1170 218 }
harveys 0:7d87166a1170 219 }