project enzo
dit is em
Dependencies: mbed
Diff: main.cpp
- Revision:
- 11:aac90fca0290
- Parent:
- 10:18116d979d61
- Child:
- 12:2f3afa8d0ddc
--- a/main.cpp Wed May 03 10:12:04 2017 +0000
+++ b/main.cpp Tue May 09 13:59:06 2017 +0000
@@ -1,13 +1,14 @@
#include "mbed.h"
-DigitalOut Motor1 (D13);
-DigitalOut Motor2 (A0);
-DigitalOut Motor3 (D12);
-DigitalOut Motor4 (D11);
+PwmOut MotorL1 (D13);
+PwmOut MotorL2 (A0);
+PwmOut MotorR3 (D12);
+PwmOut MotorR4 (D11);
AnalogIn Sensor1 (A1);
AnalogIn Sensor2 (A2);
AnalogIn Sensor3 (A3);
AnalogIn Sensor4 (A4);
+AnalogIn zoeksensor (A5);
DigitalOut steppera (D2);
DigitalOut stepperb (D3);
DigitalOut stepperc (D4);
@@ -19,6 +20,8 @@
Serial pc(USBTX, USBRX);
+int afstand = zoeksensor;
+
int stapmode = 0;
int position = 0;
int pos;
@@ -88,60 +91,343 @@
serial.attach(&run_serial, 0.2);
while(1) {
if ( Sensor1 >= 1) {
- Motor1 = 0 ;
- Motor2 = 1 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (500);
- Motor1 = 1 ;
- Motor2 = 0 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (1000);
}
if (Sensor2 >= 1) {
- Motor1 = 0 ;
- Motor2 = 1 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (500);
- Motor1 = 1 ;
- Motor2 = 0 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (1000);
}
if (Sensor3 >= 1) {
- Motor1 = 0 ;
- Motor2 = 1 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (500);
- Motor1 = 1 ;
- Motor2 = 0 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (1000);
}
if (Sensor4 >= 1) {
- Motor1 = 0 ;
- Motor2 = 1 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 0 ;
+ MotorL2 = 1 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (500);
- Motor1 = 1 ;
- Motor2 = 0 ;
- Motor3 = 0 ;
- Motor4 = 1 ;
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 0 ;
+ MotorR4 = 1 ;
wait_ms (1000);
}
else {
- Motor1 = 1 ;
- Motor2 = 0 ;
- Motor3 = 1 ;
- Motor4 = 0 ;
+ MotorL1 = 1 ;
+ MotorL2 = 0 ;
+ MotorR3 = 1 ;
+ MotorR4 = 0 ;
}
-
+
}
+ while (1){
+
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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
+ MotorL1 = 1 ;
+ 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);
+ }
+ }
}