File content as of revision 1:719ac5a42c19:

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

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


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;
Timer timer;

void hearbeat_thread(void const *args);

int main() {
    serialMinnow.baud(115200);
    I2C *i2c = new I2C(p9, p10);
    
    drivePWM.period_ms(20);
    steerPWM.period_ms(20);

    SystemTimer *millis = new SystemTimer();
    SupportSystem *supportSystem = new SupportSystem(0x80, i2c);
    
    Thread hearbeatThread(hearbeat_thread);

    steerPWM.pulsewidth_us(1500);
    float strecke_anfang, strecke_ende, size_parking_space, zeit_anfang, zeit_ende, zeit_parking_space;
    timer.start();
    
    // Konfiguration AMF-IMU
    // [0]: Conversation Factor
    // [1]: Sensor Position X
    // [2]: Sensor Position Y
    // [3]: Sensor Position Angle
    //float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f};
    //supportSystem->writeImuConfig(configData);

/*    while(true){
    
        ledTwo = !ledTwo;
        IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
        
        
        serialMinnow.printf("X %f\n\r", IMU_registerDataBuffer->distanceXRegister); 
        serialMinnow.printf("S %f\n\r", IMU_registerDataBuffer->squal);     
        serialMinnow.printf("Uptime %d\n\r", supportSystem->readMaintenanceUptimeMillis());
        serialMinnow.printf("Error: %d\n\r", supportSystem->getTransmissionErrorCount());
        
        wait(0.100);
    }
*/

    while(true){
        ledTwo = !ledTwo;
        IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer();
        
        if(buttonFour == 1){
            
            drivePWM.pulsewidth_us(1700);
            while(true){
                if(redlight == 1){
                    ledThree = 1;
                
                    /*do{
                    
                    }while(redlight == 1);
                    strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
                    ledThree = 1;
                    do{

                    }while(redlight == 0);*/
                    
                    /*strecke_anfang = IMU_registerDataBuffer->distanceXRegister;
                    wait_ms(2000);
                    strecke_ende = IMU_registerDataBuffer->distanceXRegister;
                    size_parking_space = strecke_ende - strecke_anfang;
                    serialMinnow.printf("%f, ", IMU_registerDataBuffer->distanceXRegister); 
                    serialMinnow.printf("%f\r\n", size_parking_space);*/ 
                    
                    do{
                    
                    }while(redlight == 1);
                    zeit_anfang = timer.read_ms();
                    do{

                    }while(redlight == 0);
                    zeit_ende = timer.read_ms();
                    zeit_parking_space = zeit_ende - zeit_anfang;
                    serialMinnow.printf("Differenz: %f\r\n", zeit_parking_space);
                }
                if(zeit_parking_space < 1350 && zeit_parking_space > 1100){
                    
                    drivePWM.pulsewidth_us(1700);   //Stück nach vorne fahren
                    wait_ms(200);
                    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);
                    break;
                }
            }
        }
        if(buttonOne == 1){
            ledTwo = 0;
            ledThree = 0;
            drivePWM.pulsewidth_us(1500);
        }
        wait_ms(100);
    }
}


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


        
//-------   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);       
        */