dit is em
Dependencies: mbed
main.cpp
- Committer:
- bjorntukkertje
- Date:
- 2017-05-15
- Revision:
- 13:02e56051501a
- Parent:
- 12:2f3afa8d0ddc
- Child:
- 14:878d20f36e99
File content as of revision 13:02e56051501a:
#include "mbed.h" PwmOut MotorL1 (D13); PwmOut MotorL2 (D0); 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); DigitalOut stepperd (D5); Ticker lidar; Ticker serial; Timer T; Serial pc(USBTX, USBRX); int afstand = zoeksensor; int stapmode = 0; int position = 0; int pos; // poar neemn // 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) { case 0: steppera = 1; stepperb = 0; stepperc = 1; stepperd = 0; pos ++; break; case 1: steppera = 1; stepperb = 0; stepperc = 0; stepperd = 1; pos ++; break; case 2: steppera = 0; stepperb = 1; stepperc = 0; stepperd = 1; pos ++; break; case 3: steppera = 0; stepperb = 1; stepperc = 1; stepperd = 0; pos ++; break; ; } if (pos > 1000) { //na volledige rotatie ga naar nul 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 ++; if (stapmode > 3) { stapmode = 0; } position = stepper(stapmode); } int main() { lidar.attach(&run_lidar, 0.0001); serial.attach(&run_serial, 0.2); 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); } 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); } 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); } 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 = 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); } } }