File content as of revision 10:18116d979d61:

#include "mbed.h"

DigitalOut Motor1 (D13);
DigitalOut Motor2 (A0);
DigitalOut Motor3 (D12);
DigitalOut Motor4 (D11);
AnalogIn Sensor1 (A1);
AnalogIn Sensor2 (A2);
AnalogIn Sensor3 (A3);
AnalogIn Sensor4 (A4);
DigitalOut steppera (D2);
DigitalOut stepperb (D3);
DigitalOut stepperc (D4);
DigitalOut stepperd (D5);


Ticker lidar;
Ticker serial;
 
Serial pc(USBTX, USBRX);

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 > 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) {
          Motor1 = 0 ;
          Motor2 = 1 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (500);
          Motor1 = 1 ;
          Motor2 = 0 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (1000);
          }
        if (Sensor2 >= 1) {
          Motor1 = 0 ;
          Motor2 = 1 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (500);
          Motor1 = 1 ;
          Motor2 = 0 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (1000);
          }
        if (Sensor3 >= 1) {
          Motor1 = 0 ;
          Motor2 = 1 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (500);
          Motor1 = 1 ;
          Motor2 = 0 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (1000);
          }
        if (Sensor4 >= 1) {
          Motor1 = 0 ;
          Motor2 = 1 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (500);
          Motor1 = 1 ;
          Motor2 = 0 ;
          Motor3 = 0 ;
          Motor4 = 1 ;
          wait_ms (1000);
          }
        else {
          Motor1 = 1 ;
          Motor2 = 0 ;
          Motor3 = 1 ;
          Motor4 = 0 ;
          }
          
        
    }
}