Revision 0:c871d5355b99, committed 2016-02-07

Comitter:

becheo

Date:

Sun Feb 07 06:14:32 2016 +0000

Child:

1:719ac5a42c19

Commit message:

Einparken mit verschiedenen Geschwindigkeiten

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SerialOutputLogger.cpp	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,34 @@
+#include "Misc/SerialOutputLogger.h"
+
+SerialOutputLogger::SerialOutputLogger(Serial *serialPort, IMU_RegisterDataBuffer_t *imuRegisterDataBuffer, RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer) {
+    this->serialPort = serialPort;
+    this->imuRegisterDataBuffer = imuRegisterDataBuffer;
+    this->radioDecoderRegisterDataBuffer = radioDecoderRegisterDataBuffer;
+}
+
+void SerialOutputLogger::writeRadioDecoderDataLog(bool csv) {
+    if (csv) {
+        for(uint8_t i=0; i<3; i++) {
+            serialPort->printf("%d, ", radioDecoderRegisterDataBuffer->channelValid[i]);
+            serialPort->printf("%d, ", radioDecoderRegisterDataBuffer->channelActiveTime[i]);
+            if (i == 2) {
+                serialPort->printf("%d\r\n", radioDecoderRegisterDataBuffer->channelPercent[i]);
+            }
+        }
+    } else {
+        for(uint8_t i=0; i<3; i++) {
+            serialPort->printf("RadioDecoder - Ch[%d] Valid:      %d\r\n",i,radioDecoderRegisterDataBuffer->channelValid[i]);
+            serialPort->printf("RadioDecoder - Ch[%d] ActiveTime: %d\r\n",i,radioDecoderRegisterDataBuffer->channelActiveTime[i]);
+            serialPort->printf("RadioDecoder - Ch[%d] Percentage: %d\r\n\r\n",i,radioDecoderRegisterDataBuffer->channelPercent[i]);
+        }
+    }
+}
+
+
+void SerialOutputLogger::writeImuDataLog(bool csv) {
+    if (csv) {
+        
+    } else {
+        
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SerialOutputLogger.h	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,19 @@
+#ifndef SERIAL_OUTPUT_LOGGER_H_
+#define SERIAL_OUTPUT_LOGGER_H_
+
+#include <mbed.h>
+#include "Periphery/SupportSystem.h"
+
+class SerialOutputLogger {
+
+private:
+    Serial *serialPort;
+    IMU_RegisterDataBuffer_t *imuRegisterDataBuffer;
+    RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer;
+public:
+    SerialOutputLogger(Serial *serialPort, IMU_RegisterDataBuffer_t *imuRegisterDataBuffer, RadioDecoder_RegisterDataBuffer_t *radioDecoderRegisterDataBuffer);
+    void writeRadioDecoderDataLog(bool csv);    
+    void writeImuDataLog(bool csv);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SystemTimer.cpp	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,33 @@
+#include <Misc/SystemTimer.h>
+
+
+SystemTimer::SystemTimer(){
+
+    systemUptimeMillis = 0;
+
+    systemTicker.attach_us(this,&SystemTimer::systemTickerHandler,1000);
+
+}
+
+void SystemTimer::systemTickerHandler(){
+
+    systemUptimeMillis++;
+
+}
+
+
+uint64_t SystemTimer::getUptimeMillis(){
+    return systemUptimeMillis;
+}
+
+
+bool SystemTimer::isTimeoutPassed(uint64_t *timestampMillis, uint32_t timeoutMillis){
+
+    if((systemUptimeMillis - *timestampMillis) > timeoutMillis){
+        *timestampMillis = systemUptimeMillis;
+        return true;
+    }else{
+        return false;
+    }
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Misc/SystemTimer.h	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,29 @@
+#ifndef SYSTEMTIMER_H_
+#define SYSTEMTIMER_H_
+
+#include "mbed.h"
+
+class SystemTimer {
+
+private:
+
+    uint64_t systemUptimeMillis;
+
+    Ticker systemTicker;
+
+public:
+
+    SystemTimer();
+
+    void systemTickerHandler();
+
+    uint64_t getUptimeMillis();
+
+    bool isTimeoutPassed(uint64_t *timestampMillis, uint32_t timeoutMillis);
+
+
+};
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Periphery/SupportSystem.cpp	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,274 @@
+#include <Periphery/SupportSystem.h>
+
+
+SupportSystem::SupportSystem(){
+
+    init(0,0);
+
+    transmissionErrorCount = 0;
+    lightManagerCommand = 0;
+}
+
+SupportSystem::SupportSystem(uint8_t i2cAddress, I2C *i2c){
+
+    init(i2cAddress, i2c);
+
+    transmissionErrorCount = 0;
+    lightManagerCommand = 0;
+}
+
+void SupportSystem::init(uint8_t i2cAddress, I2C *i2c){
+
+    this->i2cAddress = i2cAddress;
+    this->i2c = i2c;
+
+}
+
+
+
+void SupportSystem::readData(uint8_t registerAddress, void *buffer, size_t length){
+
+
+    i2c->write(i2cAddress,(const char*)&registerAddress,sizeof(uint8_t),false);
+    i2c->read(i2cAddress,(char*)buffer,length,false);
+
+}
+
+void SupportSystem::writeData(uint8_t registerAddress, void *buffer, size_t length){
+
+
+    i2c->start();
+
+
+    if(!i2c->write(i2cAddress & 0xfe)){
+        i2c->stop();
+        return;
+    }
+
+    if(!i2c->write(registerAddress)){
+        i2c->stop();
+        return;
+    }
+
+    for(uint32_t i=0; i<length; i++){
+        if(!i2c->write(*((uint8_t*)buffer))){
+            *(uint8_t*)buffer += 1;
+            i2c->stop();
+            return;
+        }
+    }
+
+    i2c->stop();
+
+
+}
+
+
+uint16_t SupportSystem::crc16_update(uint16_t crc, uint8_t a) {
+    int i;
+
+    crc ^= a;
+    for (i = 0; i < 8; ++i) {
+        if (crc & 1)
+            crc = (crc >> 1) ^ 0xA001;
+        else
+            crc = (crc >> 1);
+    }
+
+    return crc;
+}
+
+uint16_t SupportSystem::generateChecksum(void *data, size_t len){
+
+    uint16_t i, crc = 0;
+
+    char *baseAddress = (char*)(data);
+
+    for(i=0;i<len;i++){
+        char c = baseAddress[i];
+        crc = crc16_update(crc, c);
+    }
+
+    return crc;
+
+}
+
+uint32_t SupportSystem::getTransmissionErrorCount(){
+    return transmissionErrorCount;
+}
+
+/************************************************************
+ *
+ * Maintenance
+ *
+ ***********************************************************/
+
+uint32_t SupportSystem::readMaintenanceUptimeMillis(){
+
+    uint32_t uptimeMillis;
+
+    readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_UPTIME,(void*)&uptimeMillis,sizeof(uint32_t));
+
+    return uptimeMillis;
+}
+
+/************************************************************
+ *
+ * IMU
+ *
+ ***********************************************************/
+
+//float SupportSystem::readImuDistanceX(){
+//
+//  float traveledDistance;
+//
+//  readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_DISTANCE_X,(void*)&traveledDistance,sizeof(float));
+//
+//  return traveledDistance;
+//
+//}
+//
+//
+//void SupportSystem::writeImuCommandResetDistanceX(){
+//
+//  uint8_t command = (1<<1);
+//
+//  writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND,(void*)&command,sizeof(uint8_t));
+//
+//}
+
+void SupportSystem::writeImuConfig(float *config_register, size_t length) {
+    writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR, config_register, sizeof(float)*length);
+    
+    uint8_t command = 1<<2;
+    writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND, &command, sizeof(uint8_t));
+    
+    wait(0.1);
+}
+
+
+IMU_RegisterDataBuffer_t *SupportSystem::getImuRegisterDataBuffer(){
+
+    uint8_t tries = 0;
+    uint16_t checksum;
+
+    do{
+
+        readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET,(void*)&IMU_registerDataBuffer,sizeof(IMU_RegisterDataBuffer_t));
+        checksum = generateChecksum(&IMU_registerDataBuffer,(sizeof(IMU_RegisterDataBuffer_t) - sizeof(uint16_t)));
+
+    }while(tries++ < 5 && checksum != IMU_registerDataBuffer.completeChecksum);
+
+    transmissionErrorCount += tries-1;
+
+    return &IMU_registerDataBuffer;
+
+}
+
+
+/************************************************************
+ *
+ * LightManager
+ *
+ ***********************************************************/
+
+void SupportSystem::writeLightManagerCommand(){
+
+    uint8_t tries = 0;
+    uint8_t control;
+
+    do{
+
+        writeData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&lightManagerCommand,sizeof(uint8_t));
+        readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND,(void*)&control,sizeof(uint8_t));
+
+    }while(tries++ < 5 && control != lightManagerCommand);
+
+    transmissionErrorCount += tries-1;
+
+}
+
+void SupportSystem::setLightManagerCommandBit(bool value, uint8_t bit){
+
+    if(value){
+        lightManagerCommand |= (1<<bit);
+    }else{
+        lightManagerCommand &= ~(1<<bit);
+    }
+}
+
+void SupportSystem::setLightManagerBrakeLight(bool active, bool writeOut){
+
+    setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT);
+
+    if(writeOut){
+        writeLightManagerCommand();
+    }
+
+}
+
+void SupportSystem::setLightManagerRemoteLight(bool active, bool writeOut){
+
+    setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT);
+
+    if(writeOut){
+        writeLightManagerCommand();
+    }
+
+}
+
+void SupportSystem::setLightManagerSignalLeft(bool active, bool writeOut){
+
+    setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT);
+
+    if(writeOut){
+        writeLightManagerCommand();
+    }
+
+}
+
+void SupportSystem::setLightManagerSignalRight(bool active, bool writeOut){
+
+    setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT);
+
+    if(writeOut){
+        writeLightManagerCommand();
+    }
+
+}
+
+void SupportSystem::setLightManagerSignalBoth(bool active, bool writeOut){
+
+
+    setLightManagerCommandBit(active,SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH);
+
+    if(writeOut){
+        writeLightManagerCommand();
+    }
+
+}
+
+
+/************************************************************
+ *
+ * RadioDecoder
+ *
+ ***********************************************************/
+
+RadioDecoder_RegisterDataBuffer_t *SupportSystem::getRadioDecoderRegisterDataBuffer(){
+
+    uint8_t tries = 0;
+    uint16_t checksum;
+
+    do{
+
+        readData(SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET,(void*)&RadioDecoder_registerDataBuffer,sizeof(RadioDecoder_RegisterDataBuffer_t));
+        checksum = generateChecksum(&RadioDecoder_registerDataBuffer,sizeof(uint8_t)*12);
+
+    }while(tries++ < 5 && checksum != RadioDecoder_registerDataBuffer.checksum);
+
+    transmissionErrorCount += tries-1;
+
+    return &RadioDecoder_registerDataBuffer;
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Periphery/SupportSystem.h	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,219 @@
+#ifndef SUPPORTSYSTEM_H_
+#define SUPPORTSYSTEM_H_
+
+#include <mbed.h>
+#include <I2C.h>
+
+/*
+ * Necessary for strcut sizes
+ */
+#pragma pack (1)
+
+
+/*
+ * Data container
+ */
+
+typedef struct {
+    uint8_t i2cDeviceAddress;
+    uint32_t uptimeMillis;
+} Main_MaintenanceRegisterDataBuffer_t;
+
+typedef struct {
+
+    uint8_t commandRegister;
+
+    int32_t sensorRawXRegister;
+    int32_t sensorRawYRegister;
+
+    float velocityXRegister;
+    float velocityXFilteredRegister;
+    float velocityAngularRegister;
+    float velocityAngularFilteredRegister;
+
+    float positionXRegister;
+    float positionYRegister;
+    float positionAngleRegister;
+
+    float conversionFactorRegister;
+
+    float sensorPositionXRegister;
+    float sensorPositionYRegister;
+    float sensorPositionAngleRegister;
+
+    int32_t curveRadiusRegister;
+
+    float distanceXRegister;
+
+    uint16_t squal;
+
+    uint16_t completeChecksum;
+//  uint16_t velocityChecksumRegister;
+//  uint16_t positionChecksumRegister;
+
+} IMU_RegisterDataBuffer_t;
+
+typedef struct {
+    uint8_t commands;
+} LightManager_RegisterDataBuffer_t;
+
+typedef struct {
+
+    uint8_t  channelValid[3];
+    uint16_t channelActiveTime[3];
+    int8_t   channelPercent[3];
+    uint16_t checksum;
+
+} RadioDecoder_RegisterDataBuffer_t;
+
+
+
+
+/*
+ * Support system - software register
+ */
+
+// Maintenance
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_OFFSET                  0
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_ADDRESS                 (SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_OFFSET + 0)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_UPTIME                  (SUPPORT_SYSTEM_REGISTER_ADDRESS_MAINTENANCE_OFFSET + 1)
+
+// IMU
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET                          sizeof(Main_MaintenanceRegisterDataBuffer_t)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMMAND                         (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 0)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SENSOR_RAW_X                    (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 1)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SENSOR_RAW_Y                    (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 5)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_VELOCITY_X                      (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 9)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_VELOCITY_X_FILTERED             (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 13)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_VELOCITY_ANGULAR                (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 17)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_VELOCITY_ANGULAR_FILTERED       (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 21)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_POSITION_X                      (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 25)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_POSITION_Y                      (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 29)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_POSITION_ANGLE                  (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 33)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CONVERSION_FACTOR               (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 37)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SENSOR_POSITION_X               (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 41)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SENSOR_POSITION_Y               (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 45)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SENSOR_POSITION_ANGLE           (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 49)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_CURVE_RADIUS                    (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 53)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_DISTANCE_X                      (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 57)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_SQUAL                           (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 61)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_COMPLETE_CHECKSUM               (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 63)
+
+//#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_VELOCITY_CHECKSUM             (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 63)
+//#define SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_POSITION_CHECKSUM             (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + 65)
+
+// Light manager
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_OFFSET                 (SUPPORT_SYSTEM_REGISTER_ADDRESS_IMU_OFFSET + sizeof(IMU_RegisterDataBuffer_t))
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_COMMAND                (SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_OFFSET + 0)
+
+// Radio decoder
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET                     (SUPPORT_SYSTEM_REGISTER_ADDRESS_LIGHTMANAGER_OFFSET + sizeof(LightManager_RegisterDataBuffer_t))
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_ONE_VALID          (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 0)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_TWO_VALID          (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 1)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_THREE_VALID        (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 2)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_ONE_ACTIVE_TIME    (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 3)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_TWO_ACTIVE_TIME    (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 5)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_THREE_ACTIVE_TIME  (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 7)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_ONE_PERCENTAGE     (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 9)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_TWO_PERCENTAGE     (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 10)
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHANNEL_THREE_PERCENTAGE   (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 11)
+
+#define SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_CHECKSUM                   (SUPPORT_SYSTEM_REGISTER_ADDRESS_RADIODECODER_OFFSET + 12)
+
+
+
+/*
+ * LightManager
+ */
+#define SUPPORT_SYSTEM_LIGHTMANAGER_BIT_REMOTE_LIGHT    0
+#define SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_LEFT     1
+#define SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_RIGHT    2
+#define SUPPORT_SYSTEM_LIGHTMANAGER_BIT_SIGNAL_BOTH     3
+#define SUPPORT_SYSTEM_LIGHTMANAGER_BIT_BRAKE_LIGHT     4
+
+
+class SupportSystem{
+
+
+private:
+
+    uint8_t i2cAddress;
+    I2C *i2c;
+    uint32_t transmissionErrorCount;
+
+    uint16_t crc16_update(uint16_t crc, uint8_t a);
+    uint16_t generateChecksum(void *data, size_t len);
+
+    /*
+     * IMU
+     */
+    uint8_t imuCommand;
+    IMU_RegisterDataBuffer_t IMU_registerDataBuffer;
+
+    /*
+     * LightManager
+     */
+    uint8_t lightManagerCommand;
+    void setLightManagerCommandBit(bool value, uint8_t bit);
+
+    /*
+     * RadioDecoder
+     */
+    RadioDecoder_RegisterDataBuffer_t RadioDecoder_registerDataBuffer;
+
+public:
+
+    SupportSystem();
+    SupportSystem(uint8_t i2cAddress, I2C *i2c);
+
+    void init(uint8_t i2cAddress, I2C *i2c);
+
+    void readData(uint8_t registerAddress, void *buffer, size_t length);
+    void writeData(uint8_t registerAddress, void *buffer, size_t length);
+
+    uint32_t getTransmissionErrorCount();
+
+    /*
+     * Maintenance
+     */
+    uint32_t readMaintenanceUptimeMillis();
+
+
+    /*
+     * IMU
+     */
+    IMU_RegisterDataBuffer_t *getImuRegisterDataBuffer();
+    void writeImuConfig(float *config_register, size_t length=4);
+
+
+
+    /*
+     * LightManager
+     */
+    void writeLightManagerCommand();
+    void setLightManagerBrakeLight(bool active, bool writeOut=true);
+    void setLightManagerRemoteLight(bool active, bool writeOut=true);
+    void setLightManagerSignalLeft(bool active, bool writeOut=true);
+    void setLightManagerSignalRight(bool active, bool writeOut=true);
+    void setLightManagerSignalBoth(bool active, bool writeOut=true);
+
+    /*
+     * RadioDecoder
+     */
+    RadioDecoder_RegisterDataBuffer_t *getRadioDecoderRegisterDataBuffer();
+
+};
+
+
+
+#endif /* SUPPORTSYSTEM_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,107 @@
+#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);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#3d9d2b8b8f17

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Feb 07 06:14:32 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/f141b2784e32
\ No newline at end of file