#include "BNO08X.h"
#include "LSM6DS3.h"
#include <array>
#include <vector>

#define MAC_ADDRESS 0x4b
#define SAMPLING_TIME_MS 2000

bno08X sensor(MAC_ADDRESS);
std::array<float, 3> linear_acc;

std::array<uint8_t, 4> flex_pins = {D0, D1, D2, D3};

LSM6DS3 myIMU(I2C_MODE, 0x6A);

std::vector<float> imu;
std::vector<uint8_t> flex;
std::vector<unsigned long> timestamps;

void setup()
{
    Serial.begin(115200);

    while (!Serial)
        delay(10);

    Serial.println("Iniciando...");

    if (sensor.try_connect())
    {
        if (!sensor.init_reports())
            Serial.println("Error init reports");
        else
            Serial.println("BN ready");
    }
    else
        Serial.println("Error connect");

    if (myIMU.begin() != 0)
        Serial.println("LSM error");
    else
        Serial.println("LSM ready");

    for (const uint8_t pin : flex_pins)
        pinMode(pin, INPUT);

    Serial.println("Todo ready");
}

uint8_t map_flex(short min_value, short max_value, int value)
{
    if (value < min_value)
        return 0;
    else if (value > max_value)
        return 2;
    else
        return 1;
}

void captureDataForDuration(std::vector<float> &imu, 
std::vector<uint8_t> &flex, std::vector<unsigned long> &time, 
unsigned long duration_ms)
{
    unsigned long start_time = millis();

    unsigned long current_time = start_time;

    while (current_time - start_time < duration_ms)
    {
        timestamps.push_back(current_time - start_time);

        for (const uint8_t pin : flex_pins)
        {
            int value = analogRead(pin);

            uint8_t mapped_value = 0;

            switch (pin)
            {
            case D0:
                mapped_value = 2.0;
                break;
            case D1:
                mapped_value = map_flex(300, 400, value);
                break;
            case D2:
                if (value > 860)
                    mapped_value = 0;
                else if (value < 800)
                    mapped_value = 2;
                else
                    mapped_value = 1;

                break;
            case D3:
                mapped_value = map_flex(290, 350, value);
                break;
            }

            flex.push_back(mapped_value);
        }

        bool success = sensor.get_info(&linear_acc);
        if (success)
        {
            // Orden x, y, z
            imu.push_back(linear_acc[0]);
            imu.push_back(linear_acc[1]);
            imu.push_back(linear_acc[2]);
        }

        imu.push_back(myIMU.readFloatAccelX());
        imu.push_back(myIMU.readFloatAccelY());
        imu.push_back(myIMU.readFloatAccelZ());
        imu.push_back(myIMU.readFloatGyroX());
        imu.push_back(myIMU.readFloatGyroY());
        imu.push_back(myIMU.readFloatGyroZ());

        current_time = millis();
    }
}

void loop()
{
    captureDataForDuration(imu, flex, timestamps, SAMPLING_TIME_MS);

    Serial.println("|");

    unsigned short i = 0;
    unsigned short j = 0;
    unsigned short k = 0;
    unsigned short h = 0;

    while (i < imu.size())
    {
        Serial.println(timestamps[h]);
        h++;

        for (k = 0; k < 4; k++)
        {
            Serial.println(flex[j]);
            j++;
        }

        for (k = 0; k < 9; k++)
        {
            Serial.println(imu[i]);
            i++;
        }
    }

    Serial.println("|");

    imu.clear();
    flex.clear();
    timestamps.clear();
}
Test
