project enzo
dit is em
Dependencies: mbed
Diff: main.cpp
- Revision:
- 13:02e56051501a
- Parent:
- 12:2f3afa8d0ddc
- Child:
- 14:878d20f36e99
--- a/main.cpp Mon May 15 08:55:31 2017 +0000
+++ b/main.cpp Mon May 15 11:07:06 2017 +0000
@@ -1,7 +1,7 @@
#include "mbed.h"
PwmOut MotorL1 (D13);
-PwmOut MotorL2 (A0);
+PwmOut MotorL2 (D0);
PwmOut MotorR3 (D12);
PwmOut MotorR4 (D11);
AnalogIn Sensor1 (A1);
@@ -30,6 +30,55 @@
// twee poar neemn
// twee tettn in n envelop
+int afstandzoeker ()
+{
+ bool a ;
+ if (zoeksensor > 0.25) {
+ a = 1;
+ }
+ if (zoeksensor > 0.25) {
+ a=0 ;
+ }
+ return a;
+}
+
+int lijnsensor ()
+
+{
+ bool a = 0;
+ bool b = 0;
+ bool c = 0;
+ bool d = 0;
+ bool e = 0;
+ bool f = 0;
+ int output;
+ if (Sensor1 > 0.01) {
+ a = 1;
+ } else {
+ a = 0;
+ }
+ if (Sensor2 > 0.01) {
+ b = 1<<1;
+ } else {
+ b = 0<<1;
+ }
+ if (Sensor3 > 0.01) {
+ c = 1<<2;
+ } else {
+ c = 0<<2;
+ }
+ if (Sensor4 > 0.01) {
+ d = 1<<3;
+ } else {
+ d = 0<<3;
+ }
+
+output = a || b || c || d;
+}
+
+
+
+
int stepper(int stapmode)
{
switch (stapmode) {
@@ -64,18 +113,18 @@
;
}
if (pos > 1000) { //na volledige rotatie ga naar nul
- pos = 0;
- }
+ pos = 0;
+ }
return pos;
-
+
}
-
+
void run_serial() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
{
-
+
pc.printf("%d\n",position);
}
-
+
void run_lidar() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
{
stapmode ++;
@@ -84,354 +133,350 @@
}
position = stepper(stapmode);
}
-
+
-int main() {
+int main()
+{
lidar.attach(&run_lidar, 0.0001);
serial.attach(&run_serial, 0.2);
- T.start;
- wait_ms (1);
- T.stop;
- Time = Time + (stop s- start)
-
+
while(1) {
if ( Sensor1 >= 1) {
- MotorL1 = 0 ;
- MotorL2 = 1 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (500);
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (1000);
- }
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (500);
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (1000);
+ }
if (Sensor2 >= 1) {
- MotorL1 = 0 ;
- MotorL2 = 1 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (500);
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (1000);
- }
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (500);
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (1000);
+ }
if (Sensor3 >= 1) {
- MotorL1 = 0 ;
- MotorL2 = 1 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (500);
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (1000);
- }
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (500);
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (1000);
+ }
if (Sensor4 >= 1) {
- MotorL1 = 0 ;
- MotorL2 = 1 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (500);
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0 ;
- MotorR4 = 1 ;
- wait_ms (1000);
- }
- else {
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 1 ;
- MotorR4 = 0 ;
- }
-
- // tweede loop
-
-
- if (afstand > 0.25 && pos > 17 && pos < 45) {
- // 10 graden
- MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.95 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 45&& pos < 72) {
- // 20 graden
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (500);
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
+ wait_ms (1000);
+ } else {
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.9 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 72 && pos < 100) {
- // 30 graden
+ MotorL2 = 0 ;
+ MotorR3 = 1 ;
+ MotorR4 = 0 ;
+ }
+
+ // tweede loop
+
+
+ if (afstand > 0.25 && pos > 17 && pos < 45) {
+ // 10 graden
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0.95 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 45&& pos < 72) {
+ // 20 graden
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0.9 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 72 && pos < 100) {
+ // 30 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.85 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 100 && pos < 128) {
- // 40 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.85 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 100 && pos < 128) {
+ // 40 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.8 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 128 && pos < 156) {
- // 50 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.8 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 128 && pos < 156) {
+ // 50 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.75 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 156 && pos < 183) {
- // 60 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.75 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 156 && pos < 183) {
+ // 60 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.7 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 183 && pos < 211) {
- // 70 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.7 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 183 && pos < 211) {
+ // 70 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.65 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 211 && pos < 239) {
- // 80 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.65 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 211 && pos < 239) {
+ // 80 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.6 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 239 && pos < 266) {
- // 90 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.6 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 239 && pos < 266) {
+ // 90 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.55 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 266 && pos < 294) {
- // 100 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.55 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 266 && pos < 294) {
+ // 100 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.5 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 294 && pos < 322) {
- // 110 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.5 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 294 && pos < 322) {
+ // 110 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.45 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 322 && pos < 349) {
- // 120 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.45 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 322 && pos < 349) {
+ // 120 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.4 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 349 && pos < 377) {
- // 130 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.4 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 349 && pos < 377) {
+ // 130 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.35 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 377 && pos < 405) {
- // 140 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.35 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 377 && pos < 405) {
+ // 140 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.3 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 405 && pos < 433) {
- // 150 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.3 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 405 && pos < 433) {
+ // 150 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.25 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 433 && pos < 460) {
- // 160 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.25 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 433 && pos < 460) {
+ // 160 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.2 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 460 && pos < 488) {
- // 170 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.2 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 460 && pos < 488) {
+ // 170 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.15 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 488 && pos < 516) {
- // 180 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.15 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 488 && pos < 516) {
+ // 180 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.1 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 516 && pos < 543) {
- // 190 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.1 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 516 && pos < 543) {
+ // 190 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.15 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 543 && pos < 571) {
- // 200 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.15 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 543 && pos < 571) {
+ // 200 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.2 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 571 && pos < 599) {
- // 210 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.2 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 571 && pos < 599) {
+ // 210 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.25 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 599 && pos < 626) {
- // 220 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.25 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 599 && pos < 626) {
+ // 220 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.30 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 626 && pos < 654) {
- // 230 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.30 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 626 && pos < 654) {
+ // 230 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.35 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 654 && pos < 682) {
- // 240 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.35 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 654 && pos < 682) {
+ // 240 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.40 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 682 && pos < 710) {
- // 250 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.40 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 682 && pos < 710) {
+ // 250 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.45 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 710 && pos < 737) {
- // 260 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.45 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 710 && pos < 737) {
+ // 260 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.50 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 737 && pos < 765) {
- // 270 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.50 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 737 && pos < 765) {
+ // 270 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.55 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 765 && pos < 793) {
- // 280 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.55 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 765 && pos < 793) {
+ // 280 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.60 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 793 && pos < 820) {
- // 290 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.60 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 793 && pos < 820) {
+ // 290 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.65 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 820 && pos < 848) {
- // 300 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.65 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 820 && pos < 848) {
+ // 300 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.70 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 848 && pos < 876) {
- // 310 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.70 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 848 && pos < 876) {
+ // 310 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.75 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 876 && pos < 903) {
- // 320 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.75 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 876 && pos < 903) {
+ // 320 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.80 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 903 && pos < 931) {
- // 330 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.80 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 903 && pos < 931) {
+ // 330 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.85 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 931 && pos < 956) {
- // 340 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.85 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 931 && pos < 956) {
+ // 340 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.90 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- if (afstand > 0.25 && pos >= 956 && pos < 987) {
- // 350 graden
+ MotorL2 = 0 ;
+ MotorR3 = 0.90 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ if (afstand > 0.25 && pos >= 956 && pos < 987) {
+ // 350 graden
MotorL1 = 1 ;
- MotorL2 = 0 ;
- MotorR3 = 0.95 ;
- MotorR4 = 0 ;
- wait_ms (1000);
- }
- }
+ MotorL2 = 0 ;
+ MotorR3 = 0.95 ;
+ MotorR4 = 0 ;
+ wait_ms (1000);
+ }
+ }
}