Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 2:c7897a3f5f11, committed 2020-04-22
- Comitter:
- demayer
- Date:
- Wed Apr 22 11:50:00 2020 +0000
- Parent:
- 1:b36bbc1c6d27
- Commit message:
- test
Changed in this revision
--- 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