Arduino-Code für Co-Drink

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_NeoPixel.h>
#include <WiFi.h>
#include <WiFiManager.h>     
#include <PubSubClient.h>

// ================= USER CONFIG =================
#define DEVICE_NAME   "CoDrink01"                // Set device name. e.g. for Device02 >> CoDrink02
const char* MQTT_SERVER = "broker.emqx.io";
const uint16_t MQTT_PORT = 1883;

// Device pairing (set opposites on the two bottles)
const char* MY_ID       = "B";                 // your Bottle
const char* PARTNER_ID  = "A";                 // pair Bottle
const char* PAIR_ID     = "YOUR_UNIQUE_NAME"; // <<< Replace YOUR_UNIQUE_NAME with your own, preferably unique, name.
// =================================================

// ================== SETTINGS ====================
#define LED_PIN       D10
#define LED_COUNT     1
#define LED_BRIGHT    40

// Tilt hysteresis (drinking detection)
#define TILT_ON_DEG     15.0
#define TILT_OFF_DEG     8.0

// Stillness thresholds (for OFF decision)
#define GYRO_STILL_MAG   0.20   // rad/s
#define LIN_STILL_TH     0.15   // m/s^2

// Dwell (debounce)
#define ON_DWELL_MS      200
#define OFF_DWELL_MS     1200

// Filtering
#define LP_ALPHA         0.90
#define G_ALPHA          0.98
// =================================================

Adafruit_MPU6050 mpu;
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

// Networking/MQTT
WiFiClient espClient;
PubSubClient mqtt(espClient);

// Topics (publish mine, subscribe partner)
String topicMine;     // e.g., codrink/<PAIR_ID>/A
String topicPartner;  // e.g., codrink/<PAIR_ID>/B

enum BottleState { ON_DESK, IN_HAND };
BottleState state = ON_DESK;

bool pending = false;
unsigned long pendingSince = 0;

// Upright calibration ref
float refx=0, refy=0, refz=0;
float refG=9.81;

// Gravity estimate
float gX=0, gY=0, gZ=0;

// Filtered mags
float fLinMag = 0;
float fGyroMag = 0;

// Partner state + timeout failsafe
volatile bool partnerDrinking = false;
const unsigned long PARTNER_TIMEOUT_MS = 8000;
volatile unsigned long lastPartnerMsgMs = 0;

// Reliability: re-assert current state
unsigned long lastAssertMs = 0;
unsigned long stateEnteredMs = 0;
int assertBurstsRemaining = 0;
const unsigned long ASSERT_PERIOD_MS   = 1200; // periodic while IN_HAND
const unsigned long ASSERT_BURST_GAP_MS= 180;  // gap for burst repeats
const int ASSERT_BURST_COUNT           = 2;    // send +2 after change

// ----------------- Helpers -----------------
void setLED(bool on) {
  // Change color here (R,G,B)
  strip.setPixelColor(0, on ? strip.Color(255,20,0) : strip.Color(0,0,0));
  strip.show();
}

void readIMU(float &ax, float &ay, float &az, float &gx, float &gy, float &gz) {
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);
  ax = a.acceleration.x; ay = a.acceleration.y; az = a.acceleration.z;
  gx = g.gyro.x;         gy = g.gyro.y;         gz = g.gyro.z;
}

void calibrateUpright() {
  Serial.println("Calibration: keep STILL on desk for ~2s...");
  const int N = 200, d = 10;
  float sx=0, sy=0, sz=0;
  for (int i=0; i<N; i++) {
    float ax,ay,az,gx,gy,gz;
    readIMU(ax,ay,az,gx,gy,gz);
    sx += ax; sy += ay; sz += az;
    delay(d);
  }
  refx = sx/N; refy = sy/N; refz = sz/N;
  refG = sqrt(refx*refx + refy*refy + refz*refz);
  Serial.print("refG = "); Serial.println(refG, 3);
  Serial.print("ref = "); Serial.print(refx,3); Serial.print(", ");
  Serial.print(refy,3); Serial.print(", "); Serial.println(refz,3);
}

float tiltFromReferenceDeg(float ax, float ay, float az) {
  float magA = sqrt(ax*ax + ay*ay + az*az);
  if (magA < 1e-6 || refG < 1e-6) return 0;
  float dot = ax*refx + ay*refy + az*refz;
  float cosT = dot / (magA * refG);
  if (cosT > 1) cosT = 1;
  if (cosT < -1) cosT = -1;
  return acos(cosT) * 180.0 / PI;
}

// ----------------- MQTT -----------------
void handleMqttMessage(char* topic, byte* payload, unsigned int length) {
  if (String(topic) == topicPartner) {
    bool val = false;
    if (length >= 1) {
      if (payload[0] == '1') val = true;
      else if (payload[0] == '0') val = false;
    }
    partnerDrinking = val;
    lastPartnerMsgMs = millis();
    setLED(partnerDrinking); // local LED mirrors partner only
    Serial.print("[MQTT] Partner -> ");
    Serial.println(partnerDrinking ? "DRINKING (LED ON)" : "ON_DESK (LED OFF)");
  }
}

void tuneMqttParams() {
  mqtt.setKeepAlive(12);     // seconds
  mqtt.setSocketTimeout(3);  // seconds
  mqtt.setBufferSize(256);
}

void connectMQTT() {
  if (mqtt.connected()) return;

  uint8_t mac[6];
  WiFi.macAddress(mac);
  char clientId[64];
  snprintf(clientId, sizeof(clientId), "codrink-%s-%s-%02X%02X%02X",
           PAIR_ID, MY_ID, mac[3], mac[4], mac[5]);

  mqtt.setServer(MQTT_SERVER, MQTT_PORT);
  tuneMqttParams();
  mqtt.setCallback(handleMqttMessage);

  // Connect with Last Will: "0" (retained) on my topic
  topicMine    = String("codrink/") + PAIR_ID + "/" + MY_ID;
  topicPartner = String("codrink/") + PAIR_ID + "/" + PARTNER_ID;

  Serial.printf("Connecting MQTT to %s:%u ...\n", MQTT_SERVER, MQTT_PORT);
  if (mqtt.connect(clientId, topicMine.c_str(), 0, true, "0")) {
    Serial.println("MQTT connected");
    mqtt.subscribe(topicPartner.c_str());
    Serial.print("Subscribed: "); Serial.println(topicPartner);

    // On (re)connect, publish my current retained state
    bool myDrinking = (state == IN_HAND);
    mqtt.publish(topicMine.c_str(), myDrinking ? "1" : "0", true);

    // Start a short assert burst to increase delivery odds
    assertBurstsRemaining = ASSERT_BURST_COUNT;
    lastAssertMs = millis();

    Serial.print("[MQTT] topicMine: ");    Serial.println(topicMine);
    Serial.print("[MQTT] topicPartner: "); Serial.println(topicPartner);
  } else {
    Serial.print("MQTT connect failed, rc="); Serial.println(mqtt.state());
  }
}

void publishMyStateRetained(bool drinking) {
  const char* msg = drinking ? "1" : "0";
  bool ok = mqtt.publish(topicMine.c_str(), msg, true); // retained
  if (!ok) {
    connectMQTT();
    mqtt.loop();
    ok = mqtt.publish(topicMine.c_str(), msg, true);
  }
  Serial.print("[MQTT] Publish(retained) "); Serial.print(topicMine);
  Serial.print(" = "); Serial.print(msg);
  Serial.println(ok ? " (ok)" : " (fail)");
}

// ----------------- Arduino -----------------
void setup() {
  Serial.begin(115200);
  delay(300);
  Serial.println("\n=== CoDrink (Captive Portal + MQTT) ===");

  // I2C + IMU
  Wire.begin();
  if (!mpu.begin()) {
    Serial.println("ERROR: MPU6050 not found.");
    while(1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // LED
  strip.begin();
  strip.setBrightness(LED_BRIGHT);
  setLED(false);

  // -------- WiFi via WiFiManager (Captive Portal) ---------------------
  WiFi.mode(WIFI_STA);
  WiFi.setSleep(false); // stability on ESP32-C3

  WiFiManager wm;
  wm.setCaptivePortalEnable(true);
  wm.setConfigPortalBlocking(true);
  wm.setAPClientCheck(false);
  wm.setBreakAfterConfig(true);

  String apName = String(DEVICE_NAME) + "-Config";
  Serial.println("[WiFi] Checking stored WiFi...");

  // Try to connect using saved credentials (non-blocking wait)
  WiFi.begin();
  unsigned long startAttempt = millis();
  while (WiFi.status() != WL_CONNECTED && millis() - startAttempt < 30000) {
    delay(250);
    Serial.print(".");
  }
  Serial.println();

  // If not connected after 30 s, open portal automatically
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("[WiFi] No connection – starting Captive Portal.");
    if (!wm.startConfigPortal(apName.c_str())) {
      Serial.println("[WiFi] Configuration failed or timed out. Restarting...");
      delay(1000);
      ESP.restart();
    }
  }

  Serial.println("[WiFi] Connected!");
  Serial.print("[WiFi] SSID: "); Serial.println(WiFi.SSID());
  Serial.print("[WiFi] IP: ");   Serial.println(WiFi.localIP());

  // -------------------------------------------------------

  // MQTT connect (after Wi-Fi ready)
  connectMQTT();

  // Calibration (keep bottle still on desk)
  calibrateUpright();
  gX = refx; gY = refy; gZ = refz;
  state = ON_DESK;
  pending = false;

  // Initial announce & timers
  publishMyStateRetained(false);
  lastPartnerMsgMs = millis();
  stateEnteredMs   = millis();
  lastAssertMs     = millis();

  Serial.println("Ready (paired mode with captive portal, retained state, timeout, re-assert).");
}

void loop() {
  // Keep MQTT alive
  if (WiFi.status() == WL_CONNECTED && !mqtt.connected()) connectMQTT();
  mqtt.loop();

  // IMU processing
  float ax,ay,az,gx,gy,gz;
  readIMU(ax,ay,az,gx,gy,gz);

  float tiltDeg = tiltFromReferenceDeg(ax,ay,az);

  float gyroMag = sqrt(gx*gx + gy*gy + gz*gz);
  fGyroMag = LP_ALPHA*fGyroMag + (1.0f-LP_ALPHA)*gyroMag;

  gX = G_ALPHA*gX + (1.0f-G_ALPHA)*ax;
  gY = G_ALPHA*gY + (1.0f-G_ALPHA)*ay;
  gZ = G_ALPHA*gZ + (1.0f-G_ALPHA)*az;

  float lx = ax - gX, ly = ay - gY, lz = az - gZ;
  float linMag = sqrt(lx*lx + ly*ly + lz*lz);
  fLinMag = LP_ALPHA*fLinMag + (1.0f-LP_ALPHA)*linMag;

  bool wantOn = (tiltDeg > TILT_ON_DEG);
  bool stillOnDesk = (tiltDeg < TILT_OFF_DEG) &&
                     (fLinMag < LIN_STILL_TH) &&
                     (fGyroMag < GYRO_STILL_MAG);

  BottleState target = state;
  if (state == ON_DESK) {
    if (wantOn) target = IN_HAND;
  } else {
    if (stillOnDesk) target = ON_DESK;
  }

  unsigned long now = millis();

  // Dwell/debounce + publish on confirmed change
  if (target != state) {
    unsigned long dwell = (target == IN_HAND) ? ON_DWELL_MS : OFF_DWELL_MS;
    if (!pending) {
      pending = true;
      pendingSince = now;
    } else if (now - pendingSince >= dwell) {
      state = target;
      pending = false;

      publishMyStateRetained(state == IN_HAND);
      // Start a short burst of re-asserts
      assertBurstsRemaining = ASSERT_BURST_COUNT;
      lastAssertMs = now;
      stateEnteredMs = now;
    }
  } else {
    pending = false;
  }

  // Partner timeout failsafe
  if (partnerDrinking && (now - lastPartnerMsgMs > PARTNER_TIMEOUT_MS)) {
    partnerDrinking = false;
    setLED(false);
    Serial.println("[MQTT] Partner timeout -> assume OFF, LED OFF");
  }

  // Re-assert my current state to survive packet loss
  if (mqtt.connected()) {
    bool myDrinking = (state == IN_HAND);

    // Quick post-change burst
    if (assertBurstsRemaining > 0 && (now - lastAssertMs >= ASSERT_BURST_GAP_MS)) {
      publishMyStateRetained(myDrinking);
      lastAssertMs = now;
      assertBurstsRemaining--;
    }

    // Periodic while drinking
    if (myDrinking && (now - lastAssertMs >= ASSERT_PERIOD_MS)) {
      publishMyStateRetained(true);
      lastAssertMs = now;
    }
  }

  // Debug
  static unsigned long lastPrint = 0;
  if (now - lastPrint >= 250) {
    lastPrint = now;
    Serial.print("Tilt=");  Serial.print(tiltDeg,1);
    Serial.print(" lin=");  Serial.print(fLinMag,2);
    Serial.print(" gyro="); Serial.print(fGyroMag,2);
    Serial.print(" | local=");
    Serial.print(state==IN_HAND ? "DRINKING" : "ON_DESK");
    Serial.print(" | partnerLED=");
    Serial.println(partnerDrinking ? "ON" : "OFF");
  }

  delay(10);
}