File content as of revision 0:c871d5355b99:

#include <mbed.h>
#include "Periphery/SupportSystem.h"
#include "Misc/SystemTimer.h"
#include "rtos.h"

#define PI 3.14159265

Serial serialXbee(p28,p27), serialMinnow(p13, p14);
PwmOut drivePWM(p22), steerPWM(p21);
I2C i2c(p9, p10);

DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4);

DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30);
DigitalIn redlight(p16);

IMU_RegisterDataBuffer_t *IMU_registerDataBuffer;

void hearbeat_thread(void const *args);

int main() {
    drivePWM.period_ms(20);
    steerPWM.period_ms(20);

    SystemTimer *millis = new SystemTimer();
    SupportSystem *supportSystem = new SupportSystem(0x80, &i2c);
    Thread hearbeatThread(hearbeat_thread);
    
    ledTwo =1;
    ledThree = 1;
    steerPWM.pulsewidth_us(1500);
    //float strecke_anfang, strecke_ende, size_parking_space;

    while(true){
        //IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
        if(buttonFour == 1){
        ledTwo = 0;
//-------   1   ----------------
  /*      drivePWM.pulsewidth_us(1700); //Vorwärts
        wait_ms(2500);
        drivePWM.pulsewidth_us(1500);   //Stehen
        wait_ms(200);
        drivePWM.pulsewidth_us(1325);   //1. Einschlag
        steerPWM.pulsewidth_us(2000);
        wait_ms(970);
        steerPWM.pulsewidth_us(1000);   //2. Einschlag
        wait_ms(950);
        drivePWM.pulsewidth_us(1700);   //Korrektur nach vorne
        steerPWM.pulsewidth_us(1700);
        wait_ms(420);
        drivePWM.pulsewidth_us(1500);   //Ende
        steerPWM.pulsewidth_us(1500);
        ledTwo = 1;*/
//-------   2   ----------------        
        drivePWM.pulsewidth_us(1800); //Vorwärts
        wait_ms(825);
        drivePWM.pulsewidth_us(1500);   //Stehen
        wait_ms(200);
        drivePWM.pulsewidth_us(1300);   //1. Einschlag
        steerPWM.pulsewidth_us(2000);
        wait_ms(680);
        steerPWM.pulsewidth_us(1000);   //2. Einschlag
        wait_ms(630);
        drivePWM.pulsewidth_us(1700);   //Korrektur nach vorne
        steerPWM.pulsewidth_us(1700);
        wait_ms(420);
        drivePWM.pulsewidth_us(1500);   //Ende
        steerPWM.pulsewidth_us(1500);       
        
        
        }
        if(buttonOne == 1){
            ledTwo = 0;
            ledThree = 0;
            drivePWM.pulsewidth_us(1500);
        }
    }
        
        
        /*if(buttonFour == 1){
            drivePWM.pulsewidth_us(1700);
        
            if(redlight == 1){
                ledTwo = 1;
                
                do{
                ledThree = 1;
                }while(redlight == 1);
                strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
                do{
                    ledThree = 0;
                }while(redlight == 0);
                strecke_ende = IMU_registerDataBuffer->distanceXRegister;
                size_parking_space = strecke_ende - strecke_anfang;
                
                }
        }
        */
}


void hearbeat_thread(void const *args) {
    while(true) {
        heartbeatLED = !heartbeatLED;
        Thread::wait(100);
    }
}