feat: Enhance motion sensor functionality with I2C error handling and display updates

2026-02-22 15:39:45 -05:00
parent df9bd461d1
commit 9decccdce5
7 changed files with 147 additions and 32 deletions
--- a/src/app/App.cpp
+++ b/src/app/App.cpp
@@ -331,15 +331,15 @@ void App::loop() {

  motion.loop();
  if (motion.available()) {
-    face.setTiltEffects(motion.eyeOffsetX(), motion.pupilSizeDelta());
-    if (motion.consumePickupEvent()) {
-      face.triggerSurprised();
-    }
+    face.setTiltEffects(0, 0);              // disable old motion-based eye deformation
+    face.setFaceSlideX(motion.eyeOffsetX()); // roll -> slide whole face left/right
+    (void)motion.consumePickupEvent();      // consume pickup events so they don't accumulate
    if (motion.isMoving()) {
      motionWakeUntilMs = millis() + MOTION_WAKE_MS;
    }
  } else {
    face.setTiltEffects(0, 0);
+    face.setFaceSlideX(0);
  }

  updateScheduleState();
@@ -362,8 +362,8 @@ void App::loop() {
    if (currentConnected) {
      Serial.println("[App] WiFi connected - showing on display");
      if (!displaySleeping) {
-        display.showStatus("WiFi Connected!", wifi.ssid().c_str());
-        delay(2000);
+        display.showStatus("WiFi Connected!", wifi.ip().toString());
+        delay(3000);
      }
    } else if (wifi.mode() == NET_STA) {
      Serial.println("[App] WiFi disconnected");
--- a/src/sensors/MotionSensor.cpp
+++ b/src/sensors/MotionSensor.cpp
@@ -1,9 +1,11 @@
 #include "MotionSensor.h"
 #include <math.h>
+#include <Wire.h>

 static constexpr float GRAVITY_MS2 = 9.80665f;
 static constexpr float ROLL_DEADZONE_DEG = 6.0f;
 static constexpr float PITCH_DEADZONE_DEG = 6.0f;
+static constexpr uint8_t MAX_I2C_ERRORS_BEFORE_DISABLE = 8;

 static float applyDeadzone(float value, float deadzone) {
  if (fabsf(value) <= deadzone) return 0.0f;
@@ -16,6 +18,16 @@ float MotionSensor::clampf(float v, float lo, float hi) {
  return v;
 }

+bool MotionSensor::pingMpu(uint8_t attempts) {
+  for (uint8_t i = 0; i < attempts; i++) {
+    Wire.beginTransmission(_i2cAddr);
+    uint8_t err = Wire.endTransmission();
+    if (err == 0) return true;
+    delay(2);
+  }
+  return false;
+}
+
 void MotionSensor::begin() {
  _ok = _mpu.begin();
  if (!_ok) {
@@ -27,8 +39,17 @@ void MotionSensor::begin() {
  _mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  _mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

+  // Determine active address for later health pings (most modules use 0x68).
+  _i2cAddr = 0x68;
+  if (!pingMpu(1)) {
+    _i2cAddr = 0x69;
+    if (!pingMpu(1)) _i2cAddr = 0x68;
+  }
+
  _nextMs = millis();
-  Serial.println("[Motion] MPU6050 ready");
+  Serial.print("[Motion] MPU6050 ready @ 0x");
+  if (_i2cAddr < 16) Serial.print('0');
+  Serial.println(_i2cAddr, HEX);
 }

 void MotionSensor::loop() {
@@ -36,8 +57,34 @@ void MotionSensor::loop() {

  unsigned long now = millis();
  if ((long)(now - _nextMs) < 0) return;
+  if (_i2cBackoffUntilMs != 0 && (long)(now - _i2cBackoffUntilMs) < 0) return;
  _nextMs = now + INTERVAL_MS;

+  if (!pingMpu(2)) {
+    _consecutiveI2cErrors++;
+    unsigned long backoffMs = (unsigned long)(_consecutiveI2cErrors * 50U);
+    if (backoffMs > 1000UL) backoffMs = 1000UL;
+    _i2cBackoffUntilMs = now + backoffMs;
+
+    Serial.print("[Motion] I2C ping failed (");
+    Serial.print(_consecutiveI2cErrors);
+    Serial.print("/");
+    Serial.print(MAX_I2C_ERRORS_BEFORE_DISABLE);
+    Serial.println(") - backing off");
+
+    if (_consecutiveI2cErrors >= MAX_I2C_ERRORS_BEFORE_DISABLE) {
+      _ok = false;
+      Serial.println("[Motion] Too many I2C failures - disabling MPU6050 motion features");
+    }
+    return;
+  }
+
+  if (_consecutiveI2cErrors > 0) {
+    Serial.println("[Motion] I2C recovered");
+    _consecutiveI2cErrors = 0;
+  }
+  _i2cBackoffUntilMs = 0;
+
  sensors_event_t a, g, t;
  _mpu.getEvent(&a, &g, &t);

--- a/src/sensors/MotionSensor.h
+++ b/src/sensors/MotionSensor.h
@@ -42,6 +42,10 @@ private:
  bool _pickupLatched = false;
  float _rollZeroDeg = 0.0f;
  float _pitchZeroDeg = 0.0f;
+  uint8_t _i2cAddr = 0x68;
+  uint8_t _consecutiveI2cErrors = 0;
+  unsigned long _i2cBackoffUntilMs = 0;

  static float clampf(float v, float lo, float hi);
+  bool pingMpu(uint8_t attempts = 2);
 };
--- a/src/settings/Settings.cpp
+++ b/src/settings/Settings.cpp
@@ -53,7 +53,10 @@ void Settings::setKidsMode(bool v) { _prefs.putBool("kids_mode", v); }
 bool Settings::webhookEnabled() const { return _prefs.getBool("wh_en", false); }
 void Settings::setWebhookEnabled(bool v) { _prefs.putBool("wh_en", v); }

-String Settings::webhookUrl() const { return _prefs.getString("wh_url", ""); }
+String Settings::webhookUrl() const {
+  if (!_prefs.isKey("wh_url")) return "";
+  return _prefs.getString("wh_url", "");
+}
 void Settings::setWebhookUrl(const String& url) { _prefs.putString("wh_url", url); }

 String Settings::timezone() const {
@@ -85,8 +88,8 @@ void Settings::setWakeTime(const String& hhmm) {

 MotionCalibration Settings::motionCalibration() const {
  MotionCalibration c{};
-  c.rollZeroDeg = _prefs.getFloat("accel_r0", 0.0f);
-  c.pitchZeroDeg = _prefs.getFloat("accel_p0", 0.0f);
+  c.rollZeroDeg = _prefs.isKey("accel_r0") ? _prefs.getFloat("accel_r0", 0.0f) : 0.0f;
+  c.pitchZeroDeg = _prefs.isKey("accel_p0") ? _prefs.getFloat("accel_p0", 0.0f) : 0.0f;
  return c;
 }

--- a/src/ui/Display.cpp
+++ b/src/ui/Display.cpp
@@ -35,7 +35,9 @@ void Display::begin() {
  Serial.println(OLED_ADDR, HEX);

  Wire.begin(PIN_SDA, PIN_SCL);
+  Wire.setClock(100000); // More tolerant for multi-device sensor bus wiring
  Wire.setTimeOut(20);
+  Serial.println("[Display] I2C clock set to 100kHz");
  delay(20);
  logExpectedI2CDevices();

--- a/src/ui/FaceRenderer.cpp
+++ b/src/ui/FaceRenderer.cpp
@@ -35,15 +35,15 @@ int FaceRenderer::eyeCy() const {
 }

 int FaceRenderer::leftEyeCxBase() const {
-  return screenW() * 5 / 16; // 40 on 128px
+  return screenW() * 5 / 16 + _faceSlideX; // 40 on 128px
 }

 int FaceRenderer::rightEyeCxBase() const {
-  return screenW() * 11 / 16; // 88 on 128px
+  return screenW() * 11 / 16 + _faceSlideX; // 88 on 128px
 }

 int FaceRenderer::mouthCx() const {
-  return screenW() / 2;
+  return screenW() / 2 + _faceSlideX;
 }

 int FaceRenderer::mouthY() const {
@@ -65,6 +65,10 @@ void FaceRenderer::triggerSurprised(unsigned long durationMs) {
  _surprisedUntilMs = now + durationMs;
 }

+void FaceRenderer::setFaceSlideX(int8_t x) {
+  _faceSlideTargetX = clampInt(x, -12, 12);
+}
+
 void FaceRenderer::begin(Display& display, Settings& settings) {
  _display = &display;
  _settings = &settings;
@@ -87,6 +91,7 @@ void FaceRenderer::loop(const MoistureSensor& moisture, const BatterySensor& bat
  updateMood(moisture.percent());
  updateDeathMode(now);
  updateBatteryLowMode(battery.percent());
+  _faceSlideX += clampInt(_faceSlideTargetX - _faceSlideX, -1, 1);

  if (_routineAnim != ROUTINE_NONE) {
    renderRoutine(now, battery);
@@ -199,6 +204,41 @@ void FaceRenderer::updateTooWet(unsigned long now) {
    _tooWetMouthOpenUntilMs = now + randRange(1200, 2200);
    _nextTooWetMouthEventMs = now + randRange(45000, 90000);
  }
+
+  stepTooWetBubbles(now);
+}
+
+void FaceRenderer::stepTooWetBubbles(unsigned long now) {
+  if (!_display) return;
+  const int w = screenW();
+  const int h = screenH();
+
+  auto respawnBubble = [&](uint8_t i, bool randomY) {
+    _tooWetBubbleR[i] = (uint8_t)random(2, 6); // 2..5 px
+    _tooWetBubbleX[i] = (float)random(_tooWetBubbleR[i], w - _tooWetBubbleR[i]);
+    _tooWetBubbleSpeed[i] = ((float)random(10, 28)) / 10.0f; // 1.0..2.7 px per step
+    _tooWetBubbleY[i] = randomY ? (float)random(-h, h + 8) : (float)(h + _tooWetBubbleR[i] + random(0, 12));
+  };
+
+  if (!_tooWetBubblesInit) {
+    for (uint8_t i = 0; i < TOO_WET_BUBBLE_COUNT; i++) respawnBubble(i, true);
+    _tooWetBubblesInit = true;
+    _lastTooWetBubbleStepMs = now;
+    return;
+  }
+
+  if (_lastTooWetBubbleStepMs == 0) _lastTooWetBubbleStepMs = now;
+  unsigned long dtMs = now - _lastTooWetBubbleStepMs;
+  if (dtMs < 50) return;
+  _lastTooWetBubbleStepMs = now;
+
+  float dtScale = dtMs / 50.0f;
+  for (uint8_t i = 0; i < TOO_WET_BUBBLE_COUNT; i++) {
+    _tooWetBubbleY[i] -= _tooWetBubbleSpeed[i] * dtScale;
+    if (_tooWetBubbleY[i] < -(float)_tooWetBubbleR[i] - 2.0f) {
+      respawnBubble(i, false); // restart below bottom and rise all the way back up
+    }
+  }
 }

 void FaceRenderer::renderDead(unsigned long now, const BatterySensor& battery) {
@@ -291,12 +331,12 @@ void FaceRenderer::renderNormal(unsigned long now, const BatterySensor& battery)
    drawBigWaterText(now);
  }
  else {
+    drawBubbles(0);
    if (_blinking) drawEyesClosed();
    else drawEyesOpen(0, 0, 4);
    bool mouthOpen = ((long)(now - _tooWetMouthOpenUntilMs) < 0);
    if (mouthOpen) drawMouthSurprised();
    else drawMouthFlat();
-    drawBubbles((now / 120) % 36);
    if (mouthOpen) drawMouthBubbles(now);
  }

@@ -367,12 +407,21 @@ void FaceRenderer::renderSurprised(unsigned long now, const BatterySensor& batte
  auto &d = _display->oled();
  d.clearDisplay();

+  // Keep "too wet" bubbles active even while the pickup surprise face is showing.
+  if (_mood == TOO_WET) {
+    drawBubbles(0);
+  }
+
  int pulse = ((now / 180UL) % 2UL == 0UL) ? 1 : 0;
  drawEyesOpen(0, 0, 5 + pulse);
  d.drawLine(leftEyeCxBase() - 14, eyeCy() - 8, leftEyeCxBase() + 8, eyeCy() - 14, 1);
  d.drawLine(rightEyeCxBase() - 8, eyeCy() - 14, rightEyeCxBase() + 14, eyeCy() - 8, 1);
  drawMouthSurprised();

+  if (_mood == TOO_WET && ((long)(now - _tooWetMouthOpenUntilMs) < 0)) {
+    drawMouthBubbles(now);
+  }
+
  _display->drawBatteryIcon(batteryIconX(), 2, battery.percent(), battery.shouldBlink(), battery.isCharging());
  d.display();
 }
@@ -381,11 +430,11 @@ void FaceRenderer::renderSurprised(unsigned long now, const BatterySensor& batte

 void FaceRenderer::drawEyesOpen(int dx, int dy, int pupilRadius) {
  auto &d = _display->oled();
-  int eyeR = clampInt(12 + _tiltPupilSizeDelta, 6, 18);
-  int pupilR = clampInt(pupilRadius + (_tiltPupilSizeDelta / 2), 1, eyeR - 2);
+  int eyeR = 12;
+  int pupilR = clampInt(pupilRadius, 1, eyeR - 2);
  int cy = eyeCy();
-  int leftCx = clampInt(leftEyeCxBase() + _tiltEyeDx, eyeR, d.width() - eyeR - 1);
-  int rightCx = clampInt(rightEyeCxBase() + _tiltEyeDx, eyeR, d.width() - eyeR - 1);
+  int leftCx = clampInt(leftEyeCxBase(), eyeR, d.width() - eyeR - 1);
+  int rightCx = clampInt(rightEyeCxBase(), eyeR, d.width() - eyeR - 1);
  d.fillCircle(leftCx, cy, eyeR, 1);
  d.fillCircle(rightCx, cy, eyeR, 1);
  d.fillCircle(leftCx + dx, cy + dy, pupilR, 0);
@@ -401,11 +450,11 @@ void FaceRenderer::drawEyesClosed() {

 void FaceRenderer::drawEyesSmallPupils(int dy, int sx, int sy, int pupilRadius) {
  auto &d = _display->oled();
-  int eyeR = clampInt(12 + _tiltPupilSizeDelta, 6, 18);
-  int r = clampInt(pupilRadius + (_tiltPupilSizeDelta / 2), 1, 7);
+  int eyeR = 12;
+  int r = clampInt(pupilRadius, 1, 7);
  int cy = eyeCy();
-  int leftCx = clampInt(leftEyeCxBase() + _tiltEyeDx, eyeR, d.width() - eyeR - 1);
-  int rightCx = clampInt(rightEyeCxBase() + _tiltEyeDx, eyeR, d.width() - eyeR - 1);
+  int leftCx = clampInt(leftEyeCxBase(), eyeR, d.width() - eyeR - 1);
+  int rightCx = clampInt(rightEyeCxBase(), eyeR, d.width() - eyeR - 1);
  d.fillCircle(leftCx, cy, eyeR, 1);
  d.fillCircle(rightCx, cy, eyeR, 1);
  d.fillCircle(leftCx + sx, cy + dy + sy, r, 0);
@@ -471,16 +520,15 @@ void FaceRenderer::drawMouthSurprised() {

 void FaceRenderer::drawBubbles(int off) {
  auto &d = _display->oled();
-  int rise = off % 36;  // 0..35
-
-  // Upward float with staggered phases so bubbles continuously rise.
-  int y1 = 58 - rise;
-  int y2 = 58 - ((rise + 12) % 36);
-  int y3 = 58 - ((rise + 24) % 36);
-
-  d.drawCircle(12, y1, 4, 1);
-  d.drawCircle(24, y2, 3, 1);
-  d.drawCircle(34, y3, 5, 1);
+  (void)off;
+  for (uint8_t i = 0; i < TOO_WET_BUBBLE_COUNT; i++) {
+    int r = _tooWetBubbleR[i];
+    int x = (int)_tooWetBubbleX[i];
+    int y = (int)_tooWetBubbleY[i];
+    if (r <= 0) continue;
+    if (y < -r || y > d.height() + r) continue;
+    d.drawCircle(x, y, r, 1);
+  }
 }

 void FaceRenderer::drawMouthBubbles(unsigned long now) {
--- a/src/ui/FaceRenderer.h
+++ b/src/ui/FaceRenderer.h
@@ -15,6 +15,7 @@ public:
  void setRoutineAnimation(RoutineAnim anim, uint16_t progressPermille);
  void setTiltEffects(int8_t eyeDx, int8_t pupilSizeDelta);
  void triggerSurprised(unsigned long durationMs = 1500);
+  void setFaceSlideX(int8_t x);

  bool isDeadMode() const { return _deadMode; }
  Mood mood() const { return _mood; }
@@ -47,6 +48,13 @@ private:

  unsigned long _tooWetMouthOpenUntilMs = 0;
  unsigned long _nextTooWetMouthEventMs = 0;
+  bool _tooWetBubblesInit = false;
+  static constexpr uint8_t TOO_WET_BUBBLE_COUNT = 8;
+  float _tooWetBubbleX[TOO_WET_BUBBLE_COUNT]{};
+  float _tooWetBubbleY[TOO_WET_BUBBLE_COUNT]{};
+  float _tooWetBubbleSpeed[TOO_WET_BUBBLE_COUNT]{};
+  uint8_t _tooWetBubbleR[TOO_WET_BUBBLE_COUNT]{};
+  unsigned long _lastTooWetBubbleStepMs = 0;

  bool _silly = false;
  uint8_t _sillyVariant = 0;
@@ -63,6 +71,8 @@ private:
  int8_t _tiltEyeDx = 0;
  int8_t _tiltPupilSizeDelta = 0;
  unsigned long _surprisedUntilMs = 0;
+  int8_t _faceSlideX = 0;
+  int8_t _faceSlideTargetX = 0;

  void updateMood(int moisturePct);
  void updateDeathMode(unsigned long now);
@@ -70,6 +80,7 @@ private:
  void updateHappy(unsigned long now);
  void updateDry(unsigned long now);
  void updateTooWet(unsigned long now);
+  void stepTooWetBubbles(unsigned long now);

  void renderDead(unsigned long now, const BatterySensor& battery);
  void renderBatteryLow(unsigned long now, const BatterySensor& battery);