From dffb859826fd3188dc05087f3d3486e57ca4afae Mon Sep 17 00:00:00 2001 From: devdesk Date: Thu, 5 Feb 2026 21:29:38 +0200 Subject: [PATCH] feat: add simple stall detection with threshold + debounce - Add STALL_THRESHOLD (8A) and STALL_CONFIRM_SAMPLES (3) config - Add 500ms stabilization delay after direction change to prevent false positives - Add stall warning banner in web UI - Stop motor and pingpong when stall detected Algorithm: If active current > 8A for 3 consecutive samples (300ms), and motor has been running for at least 500ms, trigger stall callback. --- include/config.h | 6 +++++ include/motor.h | 15 +++++++++++ src/main.cpp | 10 ++++++- src/motor.cpp | 66 ++++++++++++++++++++++++++++++++++++++++++++++- src/webserver.cpp | 24 +++++++++++++++++ 5 files changed, 119 insertions(+), 2 deletions(-) diff --git a/include/config.h b/include/config.h index f166813..6ae6458 100644 --- a/include/config.h +++ b/include/config.h @@ -40,6 +40,12 @@ // Current logging interval for data collection #define CURRENT_LOG_INTERVAL_MS 100 // Log current readings every 100ms +// Simple Stall Detection +// Based on clean data: Running ~2A, Stall ~17A (8.5x difference) +#define STALL_THRESHOLD 8.0f // Amps - midpoint between 2A run and 17A stall +#define STALL_CONFIRM_SAMPLES 3 // Debounce: 3 samples = 300ms at 100ms interval +#define STALL_STABILIZE_MS 500 // Ignore current spikes for 500ms after direction change + // Web Server #define HTTP_PORT 80 diff --git a/include/motor.h b/include/motor.h index 891e9f3..467c22c 100644 --- a/include/motor.h +++ b/include/motor.h @@ -4,6 +4,9 @@ #include #include "config.h" +// Stall callback function type +typedef void (*StallCallback)(); + class MotorController { public: void begin(); @@ -18,6 +21,11 @@ public: float getCurrentLeft(); // Current in amps (reverse direction) float getCurrentActive(); // Current from active direction + // Stall detection + bool isStalled(); + void setStallCallback(StallCallback callback); + void resetStallDetection(); + // Pingpong mode (time-based only) void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent); void stopPingpong(); @@ -48,11 +56,18 @@ private: unsigned long _pingpongLastSwitch = 0; int _pingpongDirection = 1; + // Stall detection state + bool _stalled = false; + int _stallConfirmCount = 0; + StallCallback _stallCallback = nullptr; + unsigned long _lastDirectionChangeTime = 0; + void applyMotorState(); float readCurrentSense(int pin); void calibrateCurrentOffset(); void updatePingpong(); int applyRandomness(int baseValue, int randomPercent); + void checkStall(); }; extern MotorController motor; diff --git a/src/main.cpp b/src/main.cpp index b6d6d40..4ece2de 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,6 +4,13 @@ #include "motor.h" #include "webserver.h" +// Called when motor stall is detected +void onMotorStall() { + Serial.println("Stall callback triggered - stopping motor!"); + motor.stop(); + motor.stopPingpong(); +} + void setupWiFi() { Serial.println("Connecting to WiFi..."); @@ -39,11 +46,12 @@ void setup() { Serial.println("\n============================="); Serial.println(" BTS7960 Motor Controller"); - Serial.println(" (Stall Detection Removed)"); + Serial.println(" Simple Stall Detection"); Serial.println("=============================\n"); // Initialize motor controller motor.begin(); + motor.setStallCallback(onMotorStall); // Connect to WiFi setupWiFi(); diff --git a/src/motor.cpp b/src/motor.cpp index b37bfd2..76b2364 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -49,7 +49,12 @@ void MotorController::setSpeed(int speed) { } void MotorController::setDirection(int dir) { - _direction = constrain(dir, -1, 1); + int newDir = constrain(dir, -1, 1); + if (newDir != _direction) { + _lastDirectionChangeTime = millis(); + resetStallDetection(); + } + _direction = newDir; applyMotorState(); } @@ -58,6 +63,7 @@ void MotorController::stop() { _direction = 0; ledcWrite(PWM_CHANNEL_R, 0); ledcWrite(PWM_CHANNEL_L, 0); + resetStallDetection(); Serial.println("Motor: STOPPED (manual)"); } @@ -76,6 +82,9 @@ void MotorController::update() { Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n", _currentRight, _currentLeft, _direction, _speed); } + + // Check for stall condition + checkStall(); #endif // Update pingpong mode @@ -107,6 +116,61 @@ float MotorController::getCurrentActive() { return 0.0f; } +// Stall detection +bool MotorController::isStalled() { + return _stalled; +} + +void MotorController::setStallCallback(StallCallback callback) { + _stallCallback = callback; +} + +void MotorController::resetStallDetection() { + _stalled = false; + _stallConfirmCount = 0; +} + +void MotorController::checkStall() { + // Only check when motor is running + if (_direction == 0 || _speed == 0) { + resetStallDetection(); + return; + } + + // Skip stall check during stabilization period after direction change + // (prevents false positives from inrush current spikes) + if ((millis() - _lastDirectionChangeTime) < STALL_STABILIZE_MS) { + return; + } + + float activeCurrent = getCurrentActive(); + + // Simple threshold-based stall detection with debounce + if (activeCurrent > STALL_THRESHOLD) { + _stallConfirmCount++; + + if (_stallConfirmCount >= STALL_CONFIRM_SAMPLES && !_stalled) { + _stalled = true; + Serial.printf("STALL DETECTED! Current=%.2fA (threshold=%.1fA)\n", + activeCurrent, STALL_THRESHOLD); + + // Call stall callback if registered + if (_stallCallback != nullptr) { + _stallCallback(); + } + } + } else { + // Reset counter if current drops below threshold + _stallConfirmCount = 0; + + // Clear stall flag if current returns to normal + if (_stalled) { + _stalled = false; + Serial.println("Stall condition cleared"); + } + } +} + void MotorController::applyMotorState() { // Apply minimum PWM when motor is running int effectiveSpeed = _speed; diff --git a/src/webserver.cpp b/src/webserver.cpp index d195aab..a3088db 100644 --- a/src/webserver.cpp +++ b/src/webserver.cpp @@ -109,6 +109,19 @@ const char index_html[] PROGMEM = R"rawliteral( border-top: 2px solid #0f3460; margin: 30px 0; } + .stall-warning { + background: #ff5252; + color: white; + padding: 15px; + border-radius: 10px; + margin-bottom: 20px; + font-weight: bold; + font-size: 18px; + display: none; + } + .stall-warning.active { + display: block; + } .pingpong-section { background: #16213e; padding: 20px; @@ -169,6 +182,8 @@ const char index_html[] PROGMEM = R"rawliteral(

Motor Control

+
⚠️ STALL DETECTED!
+
CURRENT R
@@ -323,6 +338,14 @@ const char index_html[] PROGMEM = R"rawliteral( document.getElementById('currentR').textContent = data.currentR.toFixed(2); document.getElementById('currentL').textContent = data.currentL.toFixed(2); + // Update stall warning + const stallWarning = document.getElementById('stallWarning'); + if (data.stalled) { + stallWarning.classList.add('active'); + } else { + stallWarning.classList.remove('active'); + } + // Update pingpong status if (data.pingpong) { ppStatus.textContent = 'ACTIVE'; @@ -387,6 +410,7 @@ void handleStatus() { ",\"direction\":" + String(motor.getDirection()) + ",\"currentR\":" + String(motor.getCurrentRight(), 2) + ",\"currentL\":" + String(motor.getCurrentLeft(), 2) + + ",\"stalled\":" + (motor.isStalled() ? "true" : "false") + ",\"pingpong\":" + (motor.isPingpongActive() ? "true" : "false") + ",\"ppSpeed\":" + String(motor.getPingpongSpeed()) + ",\"ppTime\":" + String(motor.getPingpongTime()) +