TriSense Pro: gyroskop a akcelerometr

/*
 * Zakladni cteni pres I2C — knihovna Voltino TriSense 1.5.0
 * Pouziva zjednodusene beginI2C() a univerzalni readIMU().
 * Ve vychozim stavu bezi FIFO_NONE (prime cteni registru).
 */

#include <TriSense.h>

ICM42688P IMU;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  Serial.println("Starting I2C communication...");

  // Zapne varování knihovny (např. při překročení šířky pásma)
  IMU.setDebug(true);

  // Inicializace I2C. Bez argumentu se pouzije vychozi rychlost
  // knihovny a adresa ICM_ADDR_PRIMARY (0x68).
  // Pro ESP32 s vlastnimi piny:
  //   IMU.beginI2C(400000, ICM_ADDR_PRIMARY, SDA_PIN, SCL_PIN);
  if (!IMU.beginI2C()) {
    Serial.println("Sensor not found on I2C (check wiring or address)!");
    while (1);
  }

  IMU.setODR(ODR_500HZ);   // vzorkovací frekvence

  // --- SEM VLOŽTE KALIBRAČNÍ HODNOTY (až je změříte) ---
  // IMU.setAccelOffset(0.02, -0.01, 0.05);
  // IMU.setAccelScale(1.001, 0.999, 1.002);
  // IMU.setGyroOffset(0.5, -0.2, 0.1);

  Serial.println("Sensor OK. Reading data...");
}

void loop() {
  float ax, ay, az, gx, gy, gz;

  if (IMU.readIMU(ax, ay, az, gx, gy, gz)) {
    Serial.print("Accel [g]: ");
    Serial.print(ax, 3); Serial.print(", ");
    Serial.print(ay, 3); Serial.print(", ");
    Serial.print(az, 3);

    Serial.print(" | Gyro [dps]: ");
    Serial.print(gx, 2); Serial.print(", ");
    Serial.print(gy, 2); Serial.print(", ");
    Serial.println(gz, 2);
  }

  delay(10);
}

/*
 * Kalibracni nastroj — knihovna Voltino TriSense 1.5.0
 * Změří biasy a škálovací chyby konkrétního kusu čipu.
 * Výsledné hodnoty zkopírujte do setup() svého hlavního programu.
 *
 * OVLÁDÁNÍ (přes Serial Monitor):
 *   'g' -> kalibrace gyroskopu (senzor musí být zcela v klidu)
 *   'a' -> šestibodová kalibrace akcelerometru
 *   'x' -> vynulování offsetů v RAM
 */

#include <TriSense.h>

const uint8_t  CS_PIN   = 17;
const uint32_t SPI_FREQ = 10000000;

ICM42688P IMU;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  // Používáte-li I2C, nahraďte tento řádek za IMU.beginI2C();
  if (!IMU.beginSPI(CS_PIN, SPI_FREQ)) {
    Serial.println("IMU not found!");
    while (1);
  }

  Serial.println("ICM-42688-P Connected.");

  // Vypnuté FIFO -> kalibrace běží synchronně v reálném čase
  IMU.setFIFOMode(FIFO_NONE);
  IMU.setODR(ODR_500HZ);

  Serial.println("--------------------------------------");
  Serial.println("Ready to calibrate.");
  Serial.println("Send 'g' to calibrate GYRO.");
  Serial.println("Send 'a' to calibrate ACCELEROMETER.");
  Serial.println("Send 'x' to RESET sensor offsets in RAM.");
  Serial.println("--------------------------------------");
}

void loop() {
  if (Serial.available()) {
    char c = Serial.read();

    if (c == 'g') {
      IMU.autoCalibrateGyro(1000);   // 1000 vzorků
    }
    else if (c == 'a') {
      IMU.autoCalibrateAccel();      // interaktivní 6bodová kalibrace
    }
    else if (c == 'x') {
      IMU.setGyroOffset(0, 0, 0);
      IMU.setAccelOffset(0, 0, 0);
      IMU.setAccelScale(1, 1, 1);
      Serial.println("All offsets reset to zero.");
    }
  }
}

/*
 * SPI s 20bitovym HIRES FIFO — knihovna Voltino TriSense 1.5.0
 * Ukazuje zapnutí režimu vysokého rozlišení a efektivní
 * vyprazdňování hardwarového bufferu.
 */

#include <TriSense.h>

// CS pin podle desky (17 pro Raspberry Pi Pico, 10 pro Arduino Uno)
const uint8_t CS_PIN = 17;

// 10 MHz vyhovuje ESP32/RP2040. Pro klasická AVR Arduina použijte 4000000.
const uint32_t SPI_FREQ = 10000000;

ICM42688P IMU;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  IMU.setDebug(true);

  if (!IMU.beginSPI(CS_PIN, SPI_FREQ)) {
    Serial.println("SPI Communication Error! Check connections.");
    while (1);
  }

  // 20bitové FIFO s vysokým rozlišením
  // Pozor: automaticky zamkne rozsahy na 16 g a 2000 dps
  IMU.setFIFOMode(FIFO_20BIT_HIRES);

  // Je-li požadované ODR nad kapacitu sběrnice,
  // knihovna ho sama sníží na bezpečnou hodnotu.
  IMU.setODR(ODR_1KHZ);

  Serial.print("Actual Output Data Rate applied: ");
  Serial.print(IMU.getODRHz());
  Serial.println(" Hz");
  Serial.println("Setup complete. Starting loop.");
}

void loop() {
  float ax, ay, az, gx, gy, gz;

  // Smyčka while je u FIFO zásadní — rychle vyprázdní buffer,
  // pokud byl mikrokontrolér chvíli zaneprázdněný.
  while (IMU.readIMU(ax, ay, az, gx, gy, gz)) {
    static unsigned long lastPrint = 0;

    // Tiskneme 10x za sekundu, aby se Serial Monitor nezahltil
    if (millis() - lastPrint >= 100) {
      lastPrint = millis();

      Serial.print("A: ");
      Serial.print(ax, 5); Serial.print(", ");   // 5 desetinných míst = 20bit přesnost
      Serial.print(ay, 5); Serial.print(", ");
      Serial.print(az, 5);

      Serial.print(" | G: ");
      Serial.print(gx, 3); Serial.print(", ");
      Serial.print(gy, 3); Serial.print(", ");
      Serial.println(gz, 3);
    }
  }
}