dit is em
Dependencies: mbed
Diff: main.cpp
- Revision:
- 11:aac90fca0290
- Parent:
- 10:18116d979d61
- Child:
- 12:2f3afa8d0ddc
diff -r 18116d979d61 -r aac90fca0290 main.cpp --- 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); + } + } }