File content as of revision 14:878d20f36e99:

#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;
    }
return a ;
return b;
return c;
return d;
    output = a || b || c || d;
}

int vooruit() {
     MotorL1 = 1;
     MotorL2 = 0;
     MotorR1 = 1;
     MotorR2 = 0;
     
}
int achteruit () {
    MotorL1 = 0;
     MotorL2 = 1;
     MotorR1 = 0;
     MotorR2 = 1;
    }
int Hardebochtlinksom (){
    MotorL1 = 1;
     MotorL2 = 0;
     MotorR1 = 0;
     MotorR2 = 1;
    }
int Hardebochtrechtsom() {
    MotorL1 = 0;
     MotorL2 = 1;
     MotorR1 = 1;
     MotorR2 = 0;
    }

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);
        }
    }
}