Detekce pohybu s MPU-6050, aneb zkus mě šlohnout

#include "stdio.h"
#include <math.h>
#include "pico/stdlib.h"
#include "haw/MPU6050.h"

// pokud se čísla h1 a h2 neliší od sebe o toleranci, vrací false, pokud se liší vrací true
bool porovnej_s_toleranci( int h1, int h2, int tolerance )
{
    if( h1 == h2) return false;
    if( h1+tolerance >= h2 && h1-tolerance <= h2 ) return false;
    return true;
}

int main()
{
bool pohyb = false;
int x,y,z, ox,oy,oz;

    stdio_init_all();

    gpio_init(22);
    gpio_set_dir(22,GPIO_OUT);
    gpio_put(22,1);
    sleep_ms(200);
    gpio_put(22,0);

    // Setup I2C properly
    gpio_init(PICO_DEFAULT_I2C_SDA_PIN);
    gpio_init(PICO_DEFAULT_I2C_SCL_PIN);
    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
    // Don't forget the pull ups! | Or use external ones
    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);

    // Pass in the I2C driver (Important for dual-core operations). The second parameter is the address,
    // which can change if you connect pin A0 to GND or to VCC.
    mpu6050_t mpu6050 = mpu6050_init(i2c_default, MPU6050_ADDRESS_A0_VCC);

    // Check if the MPU6050 can initialize
    if (mpu6050_begin(&mpu6050))
    {
        // Set scale of gyroscope
        mpu6050_set_scale(&mpu6050, MPU6050_SCALE_250DPS);
        // Set range of accelerometer
        mpu6050_set_range(&mpu6050, MPU6050_RANGE_2G); (1)

        // digital highpass filter
        mpu6050_set_dlpf_mode(&mpu6050, MPU6050_DHPF_0_63HZ);

        // Enable temperature, gyroscope and accelerometer readings
        mpu6050_set_temperature_measuring(&mpu6050, false);
        mpu6050_set_gyroscope_measuring(&mpu6050, false);
        mpu6050_set_accelerometer_measuring(&mpu6050, true);

        // Enable free fall, motion and zero motion interrupt flags
        mpu6050_set_int_free_fall(&mpu6050, true);
        mpu6050_set_int_motion(&mpu6050, true);
        mpu6050_set_int_zero_motion(&mpu6050, true);

        // Set motion detection threshold and duration
        mpu6050_set_motion_detection_threshold(&mpu6050, 1);
        mpu6050_set_motion_detection_duration(&mpu6050, 20);

        // Set zero motion detection threshold and duration
        mpu6050_set_zero_motion_detection_threshold(&mpu6050, 4);
        mpu6050_set_zero_motion_detection_duration(&mpu6050, 2);
    }
    else
    {
        while (1)
        {
            // Endless loop
            printf("Error! MPU6050 could not be initialized. Make sure you've entered the correct address. And double check your connections.\n");
            sleep_ms(1000);
        }
    }

    uint i = 0;
    uint j = 0;
    while (1)
    {
        // Fetch all data from the sensor | I2C is only used here
        mpu6050_event(&mpu6050);

        // Pointers to float vectors with all the results
        mpu6050_vectorf_t *accel = mpu6050_get_accelerometer(&mpu6050);
        mpu6050_vectorf_t *gyro = mpu6050_get_gyroscope(&mpu6050);

        // Activity struct holding all interrupt flags
        mpu6050_activity_t *activities = mpu6050_read_activities(&mpu6050);

        // Rough temperatures as float -- Keep in mind, this is not a temperature sensor!!!
        float tempC = mpu6050_get_temperature_c(&mpu6050);
        float tempF = mpu6050_get_temperature_f(&mpu6050);

        x = round(accel->x*10);
        y = round(accel->y*10);
        z = round(accel->z*10);
        // Print all the measurements
//        printf("Accelerometer: %f, %f, %f - Gyroscope: %f, %f, %f - Temperature: %f°C - Temperature: %f°F\n", accel->x, accel->y, accel->z, gyro->x, gyro->y, gyro->z, tempC, tempF);
        printf("Accelerometer: %d, %d, %d Overflow: %d - Freefall: %d - Inactivity: %d, Activity: %d, DataReady: %d PosX: %d - NegX: %d -- PosY: %d - NegY: %d -- PosZ: %d - NegZ: %d 22=%d , i=%d\r", x, y, z,
               activities->isOverflow,
               activities->isFreefall,
               activities->isInactivity,
               activities->isActivity,
               activities->isDataReady,
               activities->isPosActivityOnX,
               activities->isNegActivityOnX,
               activities->isPosActivityOnY,
               activities->isNegActivityOnY,
               activities->isPosActivityOnZ,
               activities->isNegActivityOnZ,
               pohyb,
               i);
        // I když je čidlo v klidu, tak se hodnoty naměřené akcelerometrem malinko od sebe liší
        // Toto malinko je tolerance.
        // Porovnáváme předchozí hodnotu akcelerometru se současnou hodnotou
        // i > 20 dělá prodlevu při zapnutí
        if(i>20 && ( porovnej_s_toleranci(x, ox, 2) || porovnej_s_toleranci(y, oy, 2) || porovnej_s_toleranci(z, oz, 2)) ) { (2)
            pohyb = true;  // zaznamenali jsme pohyb předmětu
            j = i;
        } else {
            pohyb = false; // předmět je v klidu
        }
        gpio_put(22,pohyb);
        mpu6050_set_sleep_enabled(&mpu6050, false);
        sleep_ms(180);
        mpu6050_set_sleep_enabled(&mpu6050, true);
        sleep_ms(20);
        ox = x; oy = y; oz = z;
        i++;
    }

    return 0;
}