Depth Proximity Alert Logger

/*
 * Sonar-Haptic Diver Navigation Unit
 * ------------------------------------
 * Board:      ESP32-S3-DevKitC-1
 * Sensors:    JSN-SR04T waterproof ultrasonic sonar
 *             MS5837-30BA I2C depth/pressure sensor (Blue Robotics Bar30)
 * Actuators:  2x ERM haptic motors via 2N3904 transistors
 * Storage:    MicroSD card (SPI) for dive logging
 *
 * Haptic language (ERM1 = proximity, ERM2 = depth):
 *   Distance > 300 cm  : both silent
 *   150–300 cm         : ERM1 slow pulse (1Hz)
 *   75–150 cm          : ERM1 fast pulse (4Hz)
 *   25–75 cm           : ERM1 continuous ON
 *   Depth alert (>10m) : ERM2 slow pulse
 *   Depth alert (>20m) : ERM2 fast pulse
 *   Depth > 30m        : ERM2 continuous ON (emergency)
 *
 * Data logging: CSV every 500ms to /DIVE_LOG.CSV on SD card
 */

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <NewPing.h>
#include "MS5837.h"

// ── Pin Definitions ────────────────────────────────────────────────────────
#define TRIG_PIN    1
#define ECHO_PIN    2
#define ERM1_PIN    3   // Proximity haptic actuator
#define ERM2_PIN    4   // Depth haptic actuator
#define SD_CS_PIN   10
#define SD_MOSI     11
#define SD_SCK      12
#define SD_MISO     13
#define I2C_SDA     8
#define I2C_SCL     9

// ── Configuration ──────────────────────────────────────────────────────────
#define MAX_DISTANCE_CM       400    // JSN-SR04T max range
#define SONAR_INTERVAL_MS     100    // Sonar ping every 100ms
#define LOG_INTERVAL_MS       500    // SD card log every 500ms
#define PRESSURE_INTERVAL_MS  200    // MS5837 read every 200ms
#define SERIAL_BAUD           115200

// Proximity distance thresholds (cm)
#define DIST_FAR      300
#define DIST_MID      150
#define DIST_NEAR     75

// Depth thresholds (metres, seawater density 1025 kg/m³)
#define DEPTH_L1_M    10.0f
#define DEPTH_L2_M    20.0f
#define DEPTH_L3_M    30.0f

// Fluid density: 1025 for seawater, 997 for freshwater
#define FLUID_DENSITY 1025.0f

// ── Globals ────────────────────────────────────────────────────────────────

// Forward declarations
bool pulseHigh(uint16_t halfPeriodMs);
void updateHaptics();
void logToSD();

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE_CM);
MS5837 depthSensor;

float gDistanceCm   = 0.0f;
float gPressureMbar = 0.0f;
float gTempC        = 0.0f;
float gDepthM       = 0.0f;

bool  sdAvailable   = false;
bool  depthOk       = false;

unsigned long lastSonarMs    = 0;
unsigned long lastPressureMs = 0;
unsigned long lastLogMs      = 0;

// ── Haptic helpers ─────────────────────────────────────────────────────────
// Non-blocking pulse generator: returns true when the pin should be HIGH
// based on a symmetric on/off cycle at `halfPeriodMs` per half-cycle.
bool pulseHigh(uint16_t halfPeriodMs) {
    return ((millis() / halfPeriodMs) & 1) == 0;
}

// Set ERM actuators based on current sensor readings (call every loop)
void updateHaptics() {
    // ── ERM1: Proximity ──
    bool erm1State = false;
    if (gDistanceCm > 0 && gDistanceCm <= DIST_NEAR) {
        // Very close: continuous
        erm1State = true;
    } else if (gDistanceCm > DIST_NEAR && gDistanceCm <= DIST_MID) {
        // Medium: fast pulse 4 Hz (125ms half-period)
        erm1State = pulseHigh(125);
    } else if (gDistanceCm > DIST_MID && gDistanceCm <= DIST_FAR) {
        // Far: slow pulse 1 Hz (500ms half-period)
        erm1State = pulseHigh(500);
    }
    digitalWrite(ERM1_PIN, erm1State ? HIGH : LOW);

    // ── ERM2: Depth ──
    bool erm2State = false;
    if (depthOk) {
        if (gDepthM >= DEPTH_L3_M) {
            // Emergency depth: continuous
            erm2State = true;
        } else if (gDepthM >= DEPTH_L2_M) {
            // Depth alert level 2: fast 4 Hz
            erm2State = pulseHigh(125);
        } else if (gDepthM >= DEPTH_L1_M) {
            // Depth alert level 1: slow 1 Hz
            erm2State = pulseHigh(500);
        }
    }
    digitalWrite(ERM2_PIN, erm2State ? HIGH : LOW);
}

// ── SD card logger ─────────────────────────────────────────────────────────
void logToSD() {
    if (!sdAvailable) return;
    File f = SD.open("/DIVE_LOG.CSV", FILE_APPEND);
    if (!f) {
        Serial.println("[SD] Open failed");
        return;
    }
    // Columns: time_ms, dist_cm, pressure_mbar, temp_c, depth_m
    f.print(millis());        f.print(',');
    f.print(gDistanceCm, 1);  f.print(',');
    f.print(gPressureMbar, 2);f.print(',');
    f.print(gTempC, 2);       f.print(',');
    f.println(gDepthM, 2);
    f.close();
}

// ── Setup ──────────────────────────────────────────────────────────────────
void setup() {
    Serial.begin(SERIAL_BAUD);
    delay(500);
    Serial.println("\n=== Sonar-Haptic Diver Navigation Unit ===");

    // ERM motor output pins
    pinMode(ERM1_PIN, OUTPUT);
    pinMode(ERM2_PIN, OUTPUT);
    digitalWrite(ERM1_PIN, LOW);
    digitalWrite(ERM2_PIN, LOW);

    // ── I2C: MS5837 depth sensor ──
    Wire.begin(I2C_SDA, I2C_SCL);
    if (depthSensor.init()) {
        depthSensor.setModel(MS5837::MS5837_30BA);
        depthSensor.setFluidDensity(FLUID_DENSITY);
        depthOk = true;
        Serial.println("[MS5837] Depth sensor OK");
    } else {
        Serial.println("[MS5837] ERROR: Sensor not found — check SDA/SCL wiring.");
    }

    // ── SPI: MicroSD card ──
    SPI.begin(SD_SCK, SD_MISO, SD_MOSI, SD_CS_PIN);
    if (SD.begin(SD_CS_PIN)) {
        sdAvailable = true;
        Serial.println("[SD] Card mounted OK");
        // Write CSV header if file is new
        if (!SD.exists("/DIVE_LOG.CSV")) {
            File f = SD.open("/DIVE_LOG.CSV", FILE_WRITE);
            if (f) {
                f.println("time_ms,dist_cm,pressure_mbar,temp_c,depth_m");
                f.close();
            }
        }
    } else {
        Serial.println("[SD] No card detected — logging disabled.");
    }

    // ── Startup haptic pulse: confirm power-on ──
    digitalWrite(ERM1_PIN, HIGH);
    digitalWrite(ERM2_PIN, HIGH);
    delay(200);
    digitalWrite(ERM1_PIN, LOW);
    digitalWrite(ERM2_PIN, LOW);
    delay(100);
    digitalWrite(ERM1_PIN, HIGH);
    delay(200);
    digitalWrite(ERM1_PIN, LOW);

    Serial.println("[BOOT] Ready.");
}

// ── Loop ───────────────────────────────────────────────────────────────────
void loop() {
    unsigned long now = millis();

    // ── Sonar ping ──
    if (now - lastSonarMs >= SONAR_INTERVAL_MS) {
        lastSonarMs = now;
        uint16_t pingUs = sonar.ping();
        gDistanceCm = (pingUs == 0) ? 0.0f
                                    : sonar.convert_cm(pingUs);
        if (gDistanceCm > 0) {
            Serial.printf("[SONAR] Distance: %.1f cm\n", gDistanceCm);
        } else {
            Serial.println("[SONAR] No echo / out of range");
        }
    }

    // ── MS5837 read ──
    if (now - lastPressureMs >= PRESSURE_INTERVAL_MS) {
        lastPressureMs = now;
        if (depthOk) {
            depthSensor.read();
            gPressureMbar = depthSensor.pressure();
            gTempC        = depthSensor.temperature();
            gDepthM       = depthSensor.depth();
            Serial.printf("[DEPTH] %.2f m  |  %.1f mbar  |  %.2f °C\n",
                          gDepthM, gPressureMbar, gTempC);
        }
    }

    // ── Haptic actuators ──
    updateHaptics();

    // ── SD log ──
    if (now - lastLogMs >= LOG_INTERVAL_MS) {
        lastLogMs = now;
        logToSD();
    }
}