File content as of revision 0:29223e7b313d:

#include "mbed.h"

Ticker lidar;
Ticker serial;

Serial pc(USBTX, USBRX);

DigitalOut led1(LED1);
DigitalOut steppera (D2);
DigitalOut stepperb (D3);
DigitalOut stepperc (D4);
DigitalOut stepperd (D5);

int stapmode = 0;
int position = 0;
int pos;

// poar neemn
// twee poar neemn
// twee tettn in n envelop

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 > 360) {
            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 (true) {
        //doe maar wat leuks
        led1 = !led1;
        wait_ms (500);
    }


}