Marco Mayer / Mbed OS Queue

Files at this revision

API Documentation at this revision

Comitter:
demayer
Date:
Wed Apr 22 11:50:00 2020 +0000
Parent:
1:b36bbc1c6d27
Commit message:
test

Changed in this revision

MPU9250.cpp Show annotated file Show diff for this revision Revisions of this file
MPU9250.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/MPU9250.cpp	Sat Apr 11 08:15:48 2020 +0000
+++ b/MPU9250.cpp	Wed Apr 22 11:50:00 2020 +0000
@@ -1,7 +1,6 @@
 #include "MPU9250.h"
 
 
-
 uint8_t Ascale = AFS_2G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
 uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
 uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
@@ -47,18 +46,28 @@
 float a_old[3] = {0.00f, 0.00f, 0.00f};
 float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
 
-
 accData_t myData;
-MPU9250 mpu9250;
 Timer t;
-Serial pc(USBTX, USBRX); // tx, rx
 
 #define SAMPLE_TIME 100
 
-
+#define STEP_NUMBER 5
 float sum = 0;
 uint32_t sumCount = 0;
 char buffer[14];
+float ax_sum = 0;
+static float vx_buffer[STEP_NUMBER];
+static int8_t stepCounter = 0;
+
+
+// MPU9250-Constructor
+MPU9250::MPU9250(Serial* serialPtr)
+{
+    pc = serialPtr;
+    imuSetup();
+    vx_old = 0;
+}
+
 
 
 //===================================================================================================================
@@ -611,192 +620,89 @@
 }
 
 
-
-// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
-// measured ones.
-void MPU9250::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
-{
-    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
-    float norm;
-    float hx, hy, bx, bz;
-    float vx, vy, vz, wx, wy, wz;
-    float ex, ey, ez;
-    float pa, pb, pc;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    float q1q1 = q1 * q1;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q1q4 = q1 * q4;
-    float q2q2 = q2 * q2;
-    float q2q3 = q2 * q3;
-    float q2q4 = q2 * q4;
-    float q3q3 = q3 * q3;
-    float q3q4 = q3 * q4;
-    float q4q4 = q4 * q4;
-
-    // Normalise accelerometer measurement
-    norm = sqrt(ax * ax + ay * ay + az * az);
-    if (norm == 0.0f) return; // handle NaN
-    norm = 1.0f / norm;        // use reciprocal for division
-    ax *= norm;
-    ay *= norm;
-    az *= norm;
-
-    // Normalise magnetometer measurement
-    norm = sqrt(mx * mx + my * my + mz * mz);
-    if (norm == 0.0f) return; // handle NaN
-    norm = 1.0f / norm;        // use reciprocal for division
-    mx *= norm;
-    my *= norm;
-    mz *= norm;
-
-    // Reference direction of Earth's magnetic field
-    hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
-    hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
-    bx = sqrt((hx * hx) + (hy * hy));
-    bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
-
-    // Estimated direction of gravity and magnetic field
-    vx = 2.0f * (q2q4 - q1q3);
-    vy = 2.0f * (q1q2 + q3q4);
-    vz = q1q1 - q2q2 - q3q3 + q4q4;
-    wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
-    wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
-    wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
-
-    // Error is cross product between estimated direction and measured direction of gravity
-    ex = (ay * vz - az * vy) + (my * wz - mz * wy);
-    ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
-    ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
-    if (Ki > 0.0f) {
-        eInt[0] += ex;      // accumulate integral error
-        eInt[1] += ey;
-        eInt[2] += ez;
-    } else {
-        eInt[0] = 0.0f;     // prevent integral wind up
-        eInt[1] = 0.0f;
-        eInt[2] = 0.0f;
-    }
-
-    // Apply feedback terms
-    gx = gx + Kp * ex + Ki * eInt[0];
-    gy = gy + Kp * ey + Ki * eInt[1];
-    gz = gz + Kp * ez + Ki * eInt[2];
-
-    // Integrate rate of change of quaternion
-    pa = q2;
-    pb = q3;
-    pc = q4;
-    q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
-    q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
-    q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
-    q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
-
-    // Normalise quaternion
-    norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    norm = 1.0f / norm;
-    q[0] = q1 * norm;
-    q[1] = q2 * norm;
-    q[2] = q3 * norm;
-    q[3] = q4 * norm;
-
-}
-
-// Integrates the acceleration to get the boards translative velocity
-void MPU9250::velocityUpdate(float ax, float ay, float az)
-{
-    // short name local variable for readability
-
-    v_trans[0] = deltat*0.5*(a_old[0] + ax*GRAVITATION) + v_trans[0];
-    v_trans[1] = deltat*0.5*(a_old[1] + ay*GRAVITATION) + v_trans[1];
-    v_trans[2] = deltat*0.5*(a_old[2] + az*GRAVITATION + GRAVITATION) + v_trans[2];
-
-    a_old[0] = ax*GRAVITATION;
-    a_old[1] = ay*GRAVITATION;
-    a_old[2] = az*GRAVITATION;
-}
-
 void MPU9250::readIMU()
 {
     // If intPin goes high, all data registers have new data
-    if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+    if(readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
 
-        mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
+        readAccelData(accelCount);  // Read the x/y/z adc values
         // Now we'll calculate the accleration value into actual g's
         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
         ay = (float)accelCount[1]*aRes - accelBias[1];
         az = (float)accelCount[2]*aRes - accelBias[2];
 
-        mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+        readGyroData(gyroCount);  // Read the x/y/z adc values
         // Calculate the gyro value into actual degrees per second
         gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
         gy = (float)gyroCount[1]*gRes - gyroBias[1];
         gz = (float)gyroCount[2]*gRes - gyroBias[2];
 
-        mpu9250.readMagData(magCount);  // Read the x/y/z adc values
+        readMagData(magCount);  // Read the x/y/z adc values
         // Calculate the magnetometer values in milliGauss
         // Include factory calibration per data sheet and user environmental corrections
         mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
         my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
         mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
     }
+    
 
-    pc.printf("ax, ay, az, delta_t;%f;%f;%f;%f\n\r", ax, ay, az*GRAVITATION + GRAVITATION, deltat);
+
+    //pc.printf("ax, ay, az, delta_t;%f;%f;%f;%f\n\r", ax, ay, az*GRAVITATION + GRAVITATION, deltat);
 
     Now = t.read_us();
 
     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
     lastUpdate = Now;
+    
+    //pc.printf("%f,%f,%f,%f\n\r",ax, ay, az, deltat);
+
 
     sum += deltat;
     sumCount++;
 
-
-    mpu9250.velocityUpdate(ax, ay, az);
-
     // Pass gyro rate as rad/s
-    mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
     //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - _count;
+    //pc.printf("Zeit intern: %d\n\r", t.read_ms());
+}
 
-    //-----------------------------------------
-    // Update displayed value
-    if (delt_t > SAMPLE_TIME) {
+
+
+/*//-----------------------------------------
+// Update displayed value
+if (delt_t > SAMPLE_TIME) {
 
 
 
-        //pc.printf("vx, vy, vz: %f %f %f\n\r", v_trans[0], v_trans[1], v_trans[2]);
+    //pc.printf("vx, vy, vz: %f %f %f\n\r", v_trans[0], v_trans[1], v_trans[2]);
+
+
+    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+    pitch *= 180.0f / PI;
+    yaw   *= 180.0f / PI;
+    yaw   -= 2.93f; // Declination at 8572 Berg TG: +2° 56'
+    roll  *= 180.0f / PI;
+
 
 
-        yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-        roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-        pitch *= 180.0f / PI;
-        yaw   *= 180.0f / PI;
-        yaw   -= 2.93f; // Declination at 8572 Berg TG: +2° 56'
-        roll  *= 180.0f / PI;
-        
-        myData.ax = yaw;
-        myData.ay = pitch;
-        myData.az = roll;
-
+    myled= !myled;
+    _count = t.read_ms();
 
-        myled= !myled;
-        _count = t.read_ms();
+    if(_count > 1<<21) {
+        t.start(); // start the timer over again if ~30 minutes has passed
+        _count = 0;
+        deltat= 0;
+        lastUpdate = t.read_us();
+    }
+    sum = 0;
+    sumCount = 0;
+}*/
 
-        if(_count > 1<<21) {
-            t.start(); // start the timer over again if ~30 minutes has passed
-            _count = 0;
-            deltat= 0;
-            lastUpdate = t.read_us();
-        }
-        sum = 0;
-        sumCount = 0;
-    }
-}
 
 
 void MPU9250::imuSetup()
@@ -804,74 +710,122 @@
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
 
-    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+    pc->printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
 
     t.start();
 //  lcd.setBrightness(0.05);
 
 
     // Read the WHO_AM_I register, this is a good test of communication
-    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x71\n\r");
+    uint8_t whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
+    pc->printf("I AM 0x%x\n\r", whoami);
+    pc->printf("I SHOULD BE 0x71\n\r");
 
     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
-        pc.printf("MPU9250 is online...\n\r");
+        pc->printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+        pc->printf("MPU9250 is online...\n\r");
         sprintf(buffer, "0x%x", whoami);
         wait(1);
 
-        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
-        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
-        pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
-        pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
-        pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
-        pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
-        pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
-        pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
-        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-        pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-        pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-        pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-        pc.printf("x accel bias = %f\n\r", accelBias[0]);
-        pc.printf("y accel bias = %f\n\r", accelBias[1]);
-        pc.printf("z accel bias = %f\n\r", accelBias[2]);
+        resetMPU9250(); // Reset registers to default in preparation for device calibration
+        MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+        pc->printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
+        pc->printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
+        pc->printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
+        pc->printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
+        pc->printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
+        pc->printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
+        calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+        pc->printf("x gyro bias = %f\n\r", gyroBias[0]);
+        pc->printf("y gyro bias = %f\n\r", gyroBias[1]);
+        pc->printf("z gyro bias = %f\n\r", gyroBias[2]);
+        pc->printf("x accel bias = %f\n\r", accelBias[0]);
+        pc->printf("y accel bias = %f\n\r", accelBias[1]);
+        pc->printf("z accel bias = %f\n\r", accelBias[2]);
         wait(2);
-        mpu9250.initMPU9250();
-        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-        mpu9250.initAK8963(magCalibration);
-        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
-        pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
-        pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
-        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
-        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
-        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
-        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+        initMPU9250();
+        pc->printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        initAK8963(magCalibration);
+        pc->printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        pc->printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
+        pc->printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
+        if(Mscale == 0) pc->printf("Magnetometer resolution = 14  bits\n\r");
+        if(Mscale == 1) pc->printf("Magnetometer resolution = 16  bits\n\r");
+        if(Mmode == 2) pc->printf("Magnetometer ODR = 8 Hz\n\r");
+        if(Mmode == 6) pc->printf("Magnetometer ODR = 100 Hz\n\r");
         wait(1);
     } else {
-        pc.printf("Could not connect to MPU9250: \n\r");
-        pc.printf("%#x \n",  whoami);
+        pc->printf("Could not connect to MPU9250: \n\r");
+        pc->printf("%#x \n",  whoami);
         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
 
         while(1) {
             // Loop forever if communication doesn't happen
-            pc.printf("no IMU detected (verify if it's plugged in)\n\r");
+            pc->printf("no IMU detected (verify if it's plugged in)\n\r");
         }
     }
 
-    mpu9250.getAres(); // Get accelerometer sensitivity
-    mpu9250.getGres(); // Get gyro sensitivity
-    mpu9250.getMres(); // Get magnetometer sensitivity
-    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+    getAres(); // Get accelerometer sensitivity
+    getGres(); // Get gyro sensitivity
+    getMres(); // Get magnetometer sensitivity
+    pc->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
 }
 
-accData_t MPU9250::getVelocityFromIMU(){
-    readIMU();
+
+// N Werte sammeln, Integrale aufsummieren
+/*float MPU9250::getMedianAcc(){
+    int8_t i;
+    ax_sum = 0;
+    for (i=0; i<STEP_NUMBER; i++) {
+        readIMU();
+        //pc.printf("Zeit extern: %d\n\n\r", t.read_ms());
+        pc.printf("i=%d: ax=%f\n\r", i, ax);
+        ax_sum += ax;
+    }
+    pc.printf("Summe: %f\n\n\r", ax_sum/steps);
+    return ax_sum/steps;
+}*/
+
+
+// BufferArray-Variante
+float MPU9250::getVBuffer()
+{
+    // Save the last ax value before readIMU() is executed
+    float ax_old = ax;
     
-    return myData;
-}
\ No newline at end of file
+    // Save the last vx value
+    if (stepCounter == 0) {
+        vx_old = vx_buffer[STEP_NUMBER-1];
+    }
+    else{
+        vx_old = vx_buffer[stepCounter - 1];
+    }
+    
+    
+    // Sets new ax value
+    readIMU();
+
+
+    // Calculate Integration Step of new and old value
+    vx_buffer[stepCounter] = vx_old + deltat*0.5f*(ax + ax_old)/GRAVITATION;
+
+    int i;
+    float vx_median = 0;
+    for (i=0; i<STEP_NUMBER; i++) {
+        vx_median += vx_buffer[i];
+        //pc.printf("%d: %f\n\r", i, vx_buffer[i]);
+    }
+    //pc.printf("\n\r");
+    //pc.printf("%f,%f,%f,%f\n\r", ax, vx_median/STEP_NUMBER, vx_buffer[stepCounter], deltat);
+
+    stepCounter++;
+    if (stepCounter >= STEP_NUMBER) {
+        stepCounter = 0;
+    }
+    return vx_median/STEP_NUMBER;
+}
--- a/MPU9250.h	Sat Apr 11 08:15:48 2020 +0000
+++ b/MPU9250.h	Wed Apr 22 11:50:00 2020 +0000
@@ -181,14 +181,9 @@
   float az;
 }accData_t;
 
-
 class MPU9250 {
-
-
-public:
-    //------------------------------------------------------------------------------
-    // Function prototypes
-
+    
+private:
     void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
 
     char readByte(uint8_t address, uint8_t subAddress);
@@ -220,16 +215,25 @@
     void MPU9250SelfTest(float * destination);
 
     void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
-
-    void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
-
-    void velocityUpdate(float ax, float ay, float az);
     
     void readIMU();
     
     void imuSetup();
+
+    Serial* pc;
     
-    accData_t getVelocityFromIMU();
+public:
+    //--------------------------------------------------------------------------
+    // Constructor
+    MPU9250(Serial* serialPtr);
+    
+    //--------------------------------------------------------------------------
+    // Only functions that are needed to get values from IMU
+    float getVBuffer();
+    
+    float getV();
+    
+    float vx_old;
 };
 
 #endif
\ No newline at end of file
--- a/main.cpp	Sat Apr 11 08:15:48 2020 +0000
+++ b/main.cpp	Wed Apr 22 11:50:00 2020 +0000
@@ -7,55 +7,84 @@
 DigitalOut led1(LED1);
 InterruptIn sw(USER_BUTTON);
 
+
+// Sensor Interrupts
+InterruptIn button1(D2);
+InterruptIn button2(D3);
+InterruptIn button3(D4);
+InterruptIn button4(D5);
+InterruptIn ir1(D6);
+
+
 Thread eventthread;
 Thread imuthread;
 bool read_imu_isrunning;
 
-
 // Pin defines
 DigitalOut led_green(D4);
 
-//-----------------------------------------------------
-//IMU
-//-----------------------------------------------------
+Serial* pc;
+MPU9250* mpu9250;
+
 
-void rise_handler(void)
+void button1Event()
+{
+    pc->printf("Button 1\n\r");
+}
+
+void button2Event()
+{
+    pc->printf("Button 2\n\r");
+}
+
+void button3Event()
 {
-    printf("rise_handler in context %p\r\n", Thread::gettid());
-    // Toggle LED
-    led1 = !led1;
-    for (int i = 0; i<10; i++) {
-        led_green = !led_green;
-        wait(0.5);
+    pc->printf("Button 3\n\r");
+}
+
+void button4Event()
+{
+    pc->printf("Button 4\n\r");
+}
+
+void ir1Event()
+{
+    pc->printf("IR-1\n\r");
+    mpu9250->vx_old = 10.5;
+}
+
+void readIMUThread()
+{
+    pc->printf("in IMU-Thread\n\r");
+    while(1) {
+        pc->printf("vx: %f\n\r", mpu9250->getVBuffer());
+        wait(0.4);
     }
 }
 
-void fall_handler(void)
-{
-    printf("fall_handler in context %p\r\n", Thread::gettid());
-    // Toggle LED
-    led1 = !led1;
-}
-
-
-
-
 
 
 int main()
 {
-    //pc.baud(9600);
-    //imuSetup();
-    //imuthread.start(readIMU);
+    pc = new Serial(USBTX, USBRX);
+    pc->baud(9600);
+
+    // Setup of IMU (Constructor calls setup-function)
+    mpu9250 = new MPU9250(pc);
+
+    imuthread.start(readIMUThread);
 
     // Request the shared queue
     EventQueue *queue = mbed_event_queue();
-    //printf("Starting in context %p\r\n", Thread::gettid());
+    pc->printf("Starting in context %p\r\n", Thread::gettid());
+
 
-    // The 'rise' handler will execute in IRQ context
-    sw.rise(queue->event(rise_handler));
-    // The 'fall' handler will execute in the context of the shared queue (actually the main thread)
-    sw.fall(queue->event(fall_handler));
+    button1.rise(queue->event(button1Event));
+    button2.rise(queue->event(button2Event));
+    button3.rise(queue->event(button3Event));
+    button4.rise(queue->event(button4Event));
+    ir1.rise(queue->event(ir1Event));
+    
     // Setup complete, so we now dispatch the shared queue from main
-    queue->dispatch_forever();
+    queue->dispatch();
 }
\ No newline at end of file