Progetto sensori max30102 non mi fa vedere più sulla serial monitor i risultati

[andrecampa]

Sto effettuando un progetto con arduino mkr zero board e due sensori max30102. Devo ottenere delle misurazioni su BPM, SPO2 ,IR e Red da salvare su scheda sd in modo da trasferirle su matlab tramite .cvs). Ho risolto il problema dell’indirizzo univoco utilizzando una sercom di default e una aggiuntiva sui pin 0(PA22) e 1 (PA23) ho ridotto il clock a 100kHz e mi vengono fuori i valori desiderati.
Il problema adesso è che i miei sensori non sono sincronizzati quindi ho pensato di utilizzare Millis () in modo da avere una sincronizzazione e prelevando campioni ogni 30 hz. Però adesso mi fa il setup (inizializzando sensori e scheda sd) ma non mi fa vedere più i valori a monitor
ecco il codice:

#include <Wire.h>
#include "wiring_private.h"
#include "MAX30105.h"
#include "heartRate.h"
#include "spo2_algorithm.h"
#include <SD.h>
#include <SPI.h>

// ===============================
// CONFIGURAZIONE PIN E COSTANTI
// ===============================
#define SDA1_PIN 0
#define SCL1_PIN 1
#define BUFFER_LENGTH 128          // Lunghezza buffer algoritmo Maxim
#define SAMPLE_INTERVAL 33         // Intervallo scrittura su Serial / SD in ms
#define SDCARD_SS_PIN              // Pin CS SD card (modificare se necessario)

// ===============================
// BUS I2C SECONDARIO (SERCOM3)
// ===============================
TwoWire Wire1(&sercom3, SDA1_PIN, SCL1_PIN);
void SERCOM3_Handler() { Wire1.onService(); }

// ===============================
// SENSORI MAX30102
// ===============================
MAX30105 sensor1;
MAX30105 sensor2;
bool sensor1Ready = false;
bool sensor2Ready = false;

// ===============================
// SD CARD
// ===============================
File logfile;

// ===============================
// BUFFER CIRCOLARI
// ===============================
uint32_t irBuffer1[BUFFER_LENGTH];
uint32_t redBuffer1[BUFFER_LENGTH];
uint32_t irBuffer2[BUFFER_LENGTH];
uint32_t redBuffer2[BUFFER_LENGTH];

// ===============================
// VARIABILI HR/SPO2
// ===============================
int32_t spo2_1, spo2_2;
int8_t validSPO2_1, validSPO2_2;
int32_t heartRate1, heartRate2;
int8_t validHeartRate1, validHeartRate2;

// ===============================
// INDICI BUFFER CIRCOLARE
// ===============================
int index1 = 0;
int index2 = 0;
bool buffer1Full = false;
bool buffer2Full = false;

// ===============================
// TIMER NON BLOCCANTE
// ===============================
unsigned long previousMillis = 0;

// ===============================
// SETUP
// ===============================
void setup() {
  Serial.begin(115200);
  while (!Serial);
  delay(1000);
  Serial.println("Avvio doppio MAX30102 + SD (buffer circolare)");

  // -------------------------------
  // SD CARD
  // -------------------------------
  if (!SD.begin(SDCARD_SS_PIN)) {
    Serial.println("Nessuna SD trovata, continuo senza salvataggio.");
  } else {
    logfile = SD.open("bio.csv", FILE_WRITE);
    if (logfile) {
      logfile.println("Time,IR1,RED1,BPM1,SpO2_1,IR2,RED2,BPM2,SpO2_2");
      logfile.flush();
      Serial.println("SD pronta, salvataggio attivo su bio.csv");
    }
  }

  // -------------------------------
  // PRIMO BUS I2C
  // -------------------------------
  Wire.begin();
  Wire.setClock(100000);
  delay(100);

  // -------------------------------
  // SECONDO BUS I2C (SERCOM3)
  // -------------------------------
  pinPeripheral(SDA1_PIN, PIO_SERCOM);
  pinPeripheral(SCL1_PIN, PIO_SERCOM);
  Wire1.begin();
  Wire1.setClock(100000);

  // -------------------------------
  // INIZIALIZZAZIONE SENSORE 1
  // -------------------------------
  if (sensor1.begin(Wire, I2C_SPEED_STANDARD)) {
    sensor1.setup(); // valori di default
    sensor1Ready = true;
    Serial.println("Sensore 1 inizializzato con valori di default.");
  }

  // -------------------------------
  // INIZIALIZZAZIONE SENSORE 2
  // -------------------------------
  if (sensor2.begin(Wire1, I2C_SPEED_STANDARD)) {
    sensor2.setup(); // valori di default
    sensor2Ready = true;
    Serial.println("Sensore 2 inizializzato con valori di default.");
  }
}

// ===============================
// LOOP PRINCIPALE
// ===============================
void loop() {
  unsigned long currentMillis = millis();

  // ---------- SENSORE 1 ----------
  if(sensor1Ready && sensor1.available()) {
    irBuffer1[index1] = sensor1.getIR();
    redBuffer1[index1] = sensor1.getRed();
    int currIndex1 = index1;
    index1 = (index1 + 1) % BUFFER_LENGTH;
    if(index1 == 0) buffer1Full = true;

    // Stampa su Serial sempre
    Serial.print("S1 index:"); Serial.print(currIndex1);
    Serial.print(" | IR:"); Serial.print(irBuffer1[currIndex1]);
    Serial.print(" RED:"); Serial.println(redBuffer1[currIndex1]);
  }

  // ---------- SENSORE 2 ----------
  if(sensor2Ready && sensor2.available()) {
    irBuffer2[index2] = sensor2.getIR();
    redBuffer2[index2] = sensor2.getRed();
    int currIndex2 = index2;
    index2 = (index2 + 1) % BUFFER_LENGTH;
    if(index2 == 0) buffer2Full = true;

    // Stampa su Serial sempre
    Serial.print("S2 index:"); Serial.print(currIndex2);
    Serial.print(" | IR:"); Serial.print(irBuffer2[currIndex2]);
    Serial.print(" RED:"); Serial.println(redBuffer2[currIndex2]);
  }

  // ---------- CALCOLO BPM/SPO2 E SALVATAGGIO SD ----------
  if(currentMillis - previousMillis >= SAMPLE_INTERVAL) {
    previousMillis = currentMillis;

    // Calcolo HR/SpO2 solo se buffer pieno
    if(buffer1Full) {
      maxim_heart_rate_and_oxygen_saturation(
        irBuffer1, BUFFER_LENGTH, redBuffer1,
        &spo2_1, &validSPO2_1,
        &heartRate1, &validHeartRate1
      );
    }
    if(buffer2Full) {
      maxim_heart_rate_and_oxygen_saturation(
        irBuffer2, BUFFER_LENGTH, redBuffer2,
        &spo2_2, &validSPO2_2,
        &heartRate2, &validHeartRate2
      );
    }

    // ---------- Salvataggio SD solo istante selezionato ----------
    if(logfile && buffer1Full && buffer2Full) {
      int lastIndex1 = (index1 + BUFFER_LENGTH - 1) % BUFFER_LENGTH;
      int lastIndex2 = (index2 + BUFFER_LENGTH - 1) % BUFFER_LENGTH;

      logfile.print(currentMillis / 1000.0, 2); logfile.print(",");
      logfile.print(irBuffer1[lastIndex1]); logfile.print(",");
      logfile.print(redBuffer1[lastIndex1]); logfile.print(",");
      logfile.print(heartRate1); logfile.print(",");
      logfile.print(spo2_1); logfile.print(",");
      logfile.print(irBuffer2[lastIndex2]); logfile.print(",");
      logfile.print(redBuffer2[lastIndex2]); logfile.print(",");
      logfile.print(heartRate2); logfile.print(",");
      logfile.println(spo2_2);
      logfile.flush();
    }
  }
  
}