main.cpp

#include <Arduino.h>
#include <Wire.h>
#include "mbed.h"

// ---------------------------------------------------------------------------
// Relaxometer — GSR biofeedback for the Raspberry Pi Pico
//
// A Seeed Grove GSR module measures galvanic skin response. As the wearer
// relaxes, skin resistance RISES. We give that back as sound: the buzzer
// emits a soft, pulsing beep whose PITCH falls and whose PULSE SLOWS as the
// wearer calms down. The exercise is to make the sound low and slow.
//
// The buzzer is driven by hardware PWM (mbed::PwmOut): the PWM period sets the
// pitch and the duty cycle sets the VOLUME, so we can keep it gentle. With no
// electrodes on skin the Grove output sits near mid-scale; readings only drop
// below that when worn, so a ceiling test detects contact and stays silent.
//
// Wiring:
//   Grove GSR signal  -> GP26 (ADC0)
//   Buzzer +          -> GP15
//   Buzzer -          -> GND
//   Both VCC          -> 3V3(OUT) ,  GND -> GND
// ---------------------------------------------------------------------------

// ---- Pins -----------------------------------------------------------------
const uint8_t PIN_GSR    = 26;   // ADC0
const uint8_t PIN_POT    = 27;   // ADC1 — trim pot wiper (live contact threshold)
const uint8_t PIN_BUZZER = 15;

// ---- Audio range ----------------------------------------------------------
// Calm (high resistance) -> low pitch ; aroused (low resistance) -> high pitch.
// Wide span so changes in state are heard as a big swing in pitch.
const uint16_t FREQ_LOW  = 150;  // Hz, most relaxed
const uint16_t FREQ_HIGH = 2200; // Hz, most aroused

// Per the Seeed reference relation a HIGHER raw reading means HIGHER skin
// resistance (more relaxed). If on your module the pitch moves the wrong way
// (rises as you relax), flip this to true.
const bool INVERT_RESPONSE = false;

// ---- Soft pulsing beep ----------------------------------------------------
// Short beeps with silent gaps are gentler than a continuous tone. The pulse
// PERIOD tracks arousal too: long and slow when calm, quick when tense — like
// a heartbeat settling.
const uint16_t BEEP_MS      = 120;  // length of each beep
const uint16_t PERIOD_CALM  = 4000; // ms between beeps, fully relaxed
const uint16_t PERIOD_TENSE = 1000; // ms between beeps, fully aroused

// Buzzer volume = PWM duty cycle (0.0 silent .. 0.5 loudest for a piezo).
// Start quiet; raise toward ~0.2 for louder.
const float    VOLUME = 0.05f;

// ---- Contact detection ----------------------------------------------------
// No electrodes on skin -> the Grove output sits near its open-circuit
// (maximum-resistance) level; wearing it pulls the reading BELOW that. So
// "worn" means raw is under the contact ceiling (and above a small floor that
// catches a disconnected/shorted lead).
// The ceiling is set live by the trim pot on PIN_POT: fully CCW = CEIL_MIN
// (very inclusive), fully CW = CEIL_MAX (just below the open-circuit level).
// Measured on this rig: not-worn ~2349, worn ~1835, so this span brackets the
// useful cutoff range. Dial it by ear; the active value prints to Serial.
const uint16_t CEIL_MIN      = 1700;
const uint16_t CEIL_MAX      = 2340;
const uint16_t CONTACT_FLOOR = 30;

// ---- Battery monitor (Waveshare Pico-UPS, INA219 over I2C) -----------------
// The UPS exposes an INA219 gauge on I2C. Pins/address below are Waveshare's
// defaults — verify against your module's wiki if the gauge doesn't respond.
// A single LiPo reads ~4.2 V full to ~3.0 V empty; percentage uses Waveshare's
// linear mapping. When charge drops below LOW_BATT_PCT we emit a distinctive
// double-chirp (different from the biofeedback beeps) every LOW_ALERT_MS.
const uint8_t  UPS_SDA       = 6;      // GP6  (Waveshare UPS I2C SDA)
const uint8_t  UPS_SCL       = 7;      // GP7  (Waveshare UPS I2C SCL)
const uint8_t  INA219_ADDR   = 0x43;   // Waveshare UPS default address
const uint8_t  REG_BUSVOLT   = 0x02;   // INA219 bus-voltage register
const float    BATT_EMPTY_V  = 3.0f;   // LiPo ~0%
const float    BATT_FULL_V   = 4.2f;   // LiPo ~100%
const uint8_t  LOW_BATT_PCT  = 15;     // alert below this charge level
const uint16_t ALERT_FREQ    = 2000;   // distinctive low-battery chirp pitch
const uint32_t BATT_CHECK_MS = 5000;   // how often to poll the gauge
const uint32_t LOW_ALERT_MS  = 20000;  // min spacing between low-battery alerts

// Diagnostic: scan I2C buses at startup and print any devices found, so you
// can confirm the UPS gauge's bus (GP6/GP7 vs GP4/GP5) and address. Set back
// to false for normal use (it adds a startup delay and only matters on USB).
const bool     I2C_SCAN      = false;

// ---- ADC ------------------------------------------------------------------
const uint8_t  ADC_BITS  = 12;     // 0..4095 on the Pico
const uint16_t ADC_MAX   = 4095;
const uint8_t  N_SAMPLES = 64;     // oversample to suppress noise

// ---- Signal smoothing -----------------------------------------------------
const float    EMA_ALPHA = 0.05f;  // smaller = smoother but slower

// ---- Adaptive auto-calibration --------------------------------------------
// Absolute GSR level varies per person/electrode/trim-pot, so we map the
// wearer's own slowly-moving min/max window onto the full pitch range. The
// window gently contracts toward the signal so it re-centres over time.
const float    WINDOW_DECAY = 0.0005f;
const float    MIN_SPAN     = 80.0f;

// ---- Timing ---------------------------------------------------------------
const uint16_t SAMPLE_MS = 20;     // ~50 Hz sensor update

// ---- State ----------------------------------------------------------------
float ema       = -1.0f;
float adaptLow  = 0.0f;
float adaptHigh = ADC_MAX;
bool  inContact = false;

uint16_t curFreq        = FREQ_LOW;
uint16_t curPeriod      = PERIOD_CALM;
uint16_t contactCeiling = CEIL_MAX;   // set live from the trim pot

uint32_t lastSampleMs = 0;
uint32_t beepPhaseMs  = 0;
bool     beeping      = false;

// Hardware PWM buzzer driver (created in setup once the pin is known).
mbed::PwmOut *buzzer = nullptr;

// Dedicated I2C bus for the Waveshare UPS gauge (separate from default Wire).
MbedI2C upsI2C(UPS_SDA, UPS_SCL);
uint32_t lastBattMs  = 0;
uint32_t lastAlertMs = 0;
float    battPct     = 100.0f;
bool     battOk      = false;   // did the gauge respond?

// Average several ADC reads for a stable sample.
uint16_t readGsr() {
  uint32_t sum = 0;
  for (uint8_t i = 0; i < N_SAMPLES; i++) {
    sum += analogRead(PIN_GSR);
  }
  return (uint16_t)(sum / N_SAMPLES);
}

// Read the trim pot (averaged) and map it to the live contact ceiling.
uint16_t readContactCeiling() {
  uint32_t sum = 0;
  for (uint8_t i = 0; i < 16; i++) sum += analogRead(PIN_POT);
  uint16_t pot = sum / 16;
  return map(pot, 0, ADC_MAX, CEIL_MIN, CEIL_MAX);
}

// (Re)centre the adaptive window and smoother on a fresh reading.
void seed(uint16_t v) {
  ema       = v;
  adaptLow  = v - MIN_SPAN * 0.5f;
  adaptHigh = v + MIN_SPAN * 0.5f;
}

// Play a tone: PWM period sets pitch, duty cycle sets volume.
void buzzerTone(uint16_t freq) {
  buzzer->period(1.0f / (float)freq);
  buzzer->write(VOLUME);
}

// Silence the buzzer by parking the duty cycle at zero (pin held low).
void silenceBuzzer() {
  buzzer->write(0.0f);
}

// Distinctive low-battery signal: two short high chirps. Brief and infrequent,
// so the simple blocking delays here are fine.
void lowBatteryChirp() {
  for (uint8_t i = 0; i < 2; i++) {
    buzzerTone(ALERT_FREQ);
    delay(80);
    silenceBuzzer();
    delay(120);
  }
}

// Read the INA219 bus voltage (battery rail). Returns false if the gauge
// doesn't acknowledge (e.g. UPS not fitted), leaving the alert disabled.
bool readBattVoltage(float &volts) {
  upsI2C.beginTransmission(INA219_ADDR);
  upsI2C.write(REG_BUSVOLT);
  if (upsI2C.endTransmission(false) != 0) return false;   // repeated start
  if (upsI2C.requestFrom(INA219_ADDR, (size_t)2) != 2) return false;
  uint16_t hi = upsI2C.read();
  uint16_t lo = upsI2C.read();
  // Bus voltage sits in bits [15:3]; LSB = 4 mV.
  uint16_t raw = (hi << 8) | lo;
  volts = (raw >> 3) * 0.004f;
  return true;
}

// Diagnostic: probe every address on a bus and print those that acknowledge.
void scanI2C(MbedI2C &bus, const char *pins) {
  bus.begin();
  Serial.print("I2C scan on "); Serial.print(pins); Serial.println(":");
  uint8_t found = 0;
  for (uint8_t addr = 1; addr < 127; addr++) {
    bus.beginTransmission(addr);
    if (bus.endTransmission() == 0) {
      Serial.print("  device at 0x");
      if (addr < 16) Serial.print('0');
      Serial.println(addr, HEX);
      found++;
    }
  }
  if (!found) Serial.println("  (none found)");
}

// Diagnostic mode: scan both candidate buses and print devices found. Runs on
// its own (no signal flood) so the results stay readable in the monitor.
void runI2CScan() {
  Serial.println("=== I2C scan (set I2C_SCAN=false when done) ===");
  MbedI2C altI2C(4, 5);
  scanI2C(altI2C, "GP4/GP5");
  scanI2C(upsI2C, "GP6/GP7 (UPS default)");
  Serial.println("=== end scan; INA219 is usually 0x40-0x4F ===\n");
}

// Poll the gauge periodically and chirp when charge runs low.
void updateBattery() {
  uint32_t now = millis();
  if (now - lastBattMs < BATT_CHECK_MS) return;
  lastBattMs = now;

  float v;
  battOk = readBattVoltage(v);
  if (!battOk) return;   // no UPS detected -> no alerts

  battPct = (v - BATT_EMPTY_V) / (BATT_FULL_V - BATT_EMPTY_V) * 100.0f;
  if (battPct < 0.0f)   battPct = 0.0f;
  if (battPct > 100.0f) battPct = 100.0f;

  Serial.print("batt:"); Serial.print(v, 2);
  Serial.print("V ");    Serial.print(battPct, 0); Serial.println("%");

  if (battPct < LOW_BATT_PCT && now - lastAlertMs >= LOW_ALERT_MS) {
    lastAlertMs = now;
    lowBatteryChirp();
  }
}

void setup() {
  Serial.begin(115200);
  analogReadResolution(ADC_BITS);
  buzzer = new mbed::PwmOut(digitalPinToPinName(PIN_BUZZER));
  silenceBuzzer();
  upsI2C.begin();
  seed(readGsr());
}

void updateSensor() {
  uint16_t raw = readGsr();
  contactCeiling = readContactCeiling();

  bool contactNow = (raw < contactCeiling && raw > CONTACT_FLOOR);

  // On regaining contact, re-seed so the tone doesn't jump from open-circuit
  // noise picked up while unworn.
  if (contactNow && !inContact) seed(raw);
  inContact = contactNow;

  if (!inContact) {
    Serial.print("raw:");      Serial.print(raw);
    Serial.print("\tceil:");   Serial.print(contactCeiling);
    Serial.println("\t-- no contact (silent) --");
    return;
  }

  // Smooth.
  ema += EMA_ALPHA * (raw - ema);

  // Adaptive window.
  if (ema < adaptLow)  adaptLow  = ema;
  if (ema > adaptHigh) adaptHigh = ema;
  adaptLow  += (ema - adaptLow)  * WINDOW_DECAY;
  adaptHigh += (ema - adaptHigh) * WINDOW_DECAY;
  if (adaptHigh - adaptLow < MIN_SPAN) {
    float mid = 0.5f * (adaptLow + adaptHigh);
    adaptLow  = mid - MIN_SPAN * 0.5f;
    adaptHigh = mid + MIN_SPAN * 0.5f;
  }

  // Normalised position in the window: high = high resistance (relaxed).
  float pos = (ema - adaptLow) / (adaptHigh - adaptLow);
  if (pos < 0.0f) pos = 0.0f;
  if (pos > 1.0f) pos = 1.0f;

  // Arousal drives pitch & pulse rate: relaxed -> 0, tense -> 1.
  float arousal = INVERT_RESPONSE ? pos : (1.0f - pos);

  curFreq   = (uint16_t)(FREQ_LOW + arousal * (FREQ_HIGH - FREQ_LOW));
  curPeriod = (uint16_t)(PERIOD_CALM - arousal * (PERIOD_CALM - PERIOD_TENSE));

  Serial.print("raw:");    Serial.print(raw);
  Serial.print("\tceil:"); Serial.print(contactCeiling);
  Serial.print("\tema:");  Serial.print(ema, 1);
  Serial.print("\tHz:");   Serial.print(curFreq);
  Serial.print("\tgap:");  Serial.print(curPeriod);
  Serial.print("\tarsl:"); Serial.println(arousal, 2);
}

// Non-blocking beep: ON for BEEP_MS, then silent for (period - BEEP_MS).
void updateBeep() {
  if (!inContact) {
    if (beeping) { silenceBuzzer(); beeping = false; }
    return;
  }

  uint32_t now = millis();
  if (beeping) {
    if (now - beepPhaseMs >= BEEP_MS) {
      silenceBuzzer();
      beeping = false;
      beepPhaseMs = now;
    }
  } else {
    uint16_t gap = (curPeriod > BEEP_MS) ? (curPeriod - BEEP_MS) : 0;
    if (now - beepPhaseMs >= gap) {
      buzzerTone(curFreq);  // pitch fixed for the duration of this beep
      beeping = true;
      beepPhaseMs = now;
    }
  }
}

void loop() {
  // Diagnostic: while scanning, do nothing else so the monitor stays clean.
  if (I2C_SCAN) {
    runI2CScan();
    delay(3000);
    return;
  }

  uint32_t now = millis();
  if (now - lastSampleMs >= SAMPLE_MS) {
    lastSampleMs = now;
    updateSensor();
  }
  updateBeep();
  updateBattery();
}