From 87a05fc80a4fb0faed02bf1c3018b63096638839 Mon Sep 17 00:00:00 2001 From: devdesk Date: Thu, 5 Feb 2026 18:20:26 +0200 Subject: [PATCH] Replace absolute stall threshold with delta-based detection - Use exponential moving average (EMA) to track normal running current - Detect stall when current spikes above average by STALL_DELTA_THRESHOLD (2.0A) - Add stabilization period (500ms) after motor start to let EMA settle - Stall confirmation requires spike to persist for STALL_CONFIRM_MS (100ms) - EMA stops updating during stall to prevent threshold creep - Removes dependency on absolute current threshold that varied with speed --- include/config.h | 11 +++++--- include/motor.h | 6 +++++ src/motor.cpp | 66 ++++++++++++++++++++++++++++++++++++++---------- 3 files changed, 66 insertions(+), 17 deletions(-) diff --git a/include/config.h b/include/config.h index 4117e2e..1a7e2e9 100644 --- a/include/config.h +++ b/include/config.h @@ -34,9 +34,14 @@ #define SENSE_RESISTOR 1000.0f // 1kΩ sense resistor (ohms) #define ADC_MAX 4095.0f // 12-bit ADC max value #define ADC_VREF 3.3f // ADC reference voltage -#define CURRENT_CALIBRATION 0.33f // Calibration factor (measured current / reported current) -#define STALL_CURRENT_THRESHOLD 7.0f // Current (amps) that indicates stall -#define STALL_DETECT_TIME_MS 1000 // Time to confirm stall (ms) - accounts for startup inrush +#define CURRENT_CALIBRATION 0.33f // Calibration factor (measured current / reported current) + +// Stall Detection Configuration (delta-based) +// Detects sudden current spikes above the rolling average +#define STALL_DELTA_THRESHOLD 2.0f // Current spike above average that indicates stall (amps) +#define STALL_EMA_ALPHA 0.1f // EMA smoothing factor (0.1 = slow, 0.5 = fast response) +#define STALL_STABILIZE_MS 500 // Ignore stalls for this long after motor starts/changes +#define STALL_CONFIRM_MS 100 // Current must exceed threshold for this long to confirm stall #define DISABLE_STALL_DETECT false // Set to true to disable stall detection // Web Server diff --git a/include/motor.h b/include/motor.h index d1275dd..94f04b0 100644 --- a/include/motor.h +++ b/include/motor.h @@ -45,6 +45,11 @@ private: int _adcOffsetRight = 0; int _adcOffsetLeft = 0; + // Delta-based stall detection (rolling average tracking) + float _currentEMA = 0; // Exponential moving average of current + unsigned long _motorStartTime = 0; // When motor started (for stabilization period) + bool _emaInitialized = false; // EMA needs seeding on first reading + // Pingpong state bool _pingpongActive = false; int _pingpongBaseSpeed = 50; @@ -61,6 +66,7 @@ private: float readCurrentSense(int pin); void calibrateCurrentOffset(); void checkStall(); + void resetStallDetection(); void updatePingpong(); int applyRandomness(int baseValue, int randomPercent); }; diff --git a/src/motor.cpp b/src/motor.cpp index ca2d886..a89555b 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -45,27 +45,31 @@ void MotorController::begin() { void MotorController::setSpeed(int speed) { _speed = constrain(speed, 0, 100); - _stalled = false; // Reset stall state on new command - _stallStartTime = 0; + resetStallDetection(); applyMotorState(); } void MotorController::setDirection(int dir) { _direction = constrain(dir, -1, 1); - _stalled = false; // Reset stall state on new command - _stallStartTime = 0; + resetStallDetection(); applyMotorState(); } void MotorController::stop() { // Don't reset _speed - keep last speed setting _direction = 0; - _stalled = false; - _stallStartTime = 0; + resetStallDetection(); ledcWrite(PWM_CHANNEL_R, 0); ledcWrite(PWM_CHANNEL_L, 0); } +void MotorController::resetStallDetection() { + _stalled = false; + _stallStartTime = 0; + _emaInitialized = false; // Re-seed EMA on motor state change + _motorStartTime = millis(); +} + void MotorController::update() { #if CURRENT_SENSING_ENABLED static unsigned long lastPrintTime = 0; @@ -77,8 +81,10 @@ void MotorController::update() { // Log current readings every 500ms when motor is running if ((_direction != 0 || _speed != 0) && (millis() - lastPrintTime > 500)) { lastPrintTime = millis(); - Serial.printf("Current: R=%.2fA L=%.2fA Active=%.2fA (threshold=%.1fA)\n", - _currentRight, _currentLeft, getCurrentActive(), STALL_CURRENT_THRESHOLD); + float activeCurrent = getCurrentActive(); + float delta = activeCurrent - _currentEMA; + Serial.printf("Current: R=%.2fA L=%.2fA Active=%.2fA (avg=%.2fA, delta=%.2fA, thresh=%.1fA)\n", + _currentRight, _currentLeft, activeCurrent, _currentEMA, delta, STALL_DELTA_THRESHOLD); } // Check for stall condition @@ -312,25 +318,53 @@ int MotorController::applyRandomness(int baseValue, int randomPercent) { void MotorController::checkStall() { #if CURRENT_SENSING_ENABLED + if (DISABLE_STALL_DETECT) return; + // Only check stall when motor should be running if (_direction == 0 || _speed == 0) { _stalled = false; _stallStartTime = 0; + _emaInitialized = false; + return; + } + + unsigned long now = millis(); + + // Skip stall detection during stabilization period after motor start/change + if ((now - _motorStartTime) < STALL_STABILIZE_MS) { return; } float activeCurrent = getCurrentActive(); - // Check if current exceeds stall threshold - if (activeCurrent > STALL_CURRENT_THRESHOLD) { + // Initialize or update EMA (exponential moving average) + if (!_emaInitialized) { + _currentEMA = activeCurrent; // Seed with first reading + _emaInitialized = true; + return; // Need more samples before detecting + } + + // Calculate delta from average + float delta = activeCurrent - _currentEMA; + + // Update EMA (only when not in stall to prevent average rising during stall) + if (!_stalled) { + _currentEMA = (STALL_EMA_ALPHA * activeCurrent) + ((1.0f - STALL_EMA_ALPHA) * _currentEMA); + } + + // Check if current spike exceeds delta threshold + if (delta > STALL_DELTA_THRESHOLD) { if (_stallStartTime == 0) { // Start timing potential stall - _stallStartTime = millis(); - } else if ((millis() - _stallStartTime) > STALL_DETECT_TIME_MS) { + _stallStartTime = now; + Serial.printf("Stall candidate: current=%.2fA, avg=%.2fA, delta=%.2fA\n", + activeCurrent, _currentEMA, delta); + } else if ((now - _stallStartTime) > STALL_CONFIRM_MS) { // Stall confirmed if (!_stalled) { _stalled = true; - Serial.printf("STALL DETECTED! Current: %.2fA\n", activeCurrent); + Serial.printf("STALL DETECTED! Current: %.2fA (avg: %.2fA, delta: %.2fA)\n", + activeCurrent, _currentEMA, delta); // Call callback if registered if (_stallCallback != nullptr) { @@ -339,7 +373,11 @@ void MotorController::checkStall() { } } } else { - // Current normal, reset stall detection + // Current normal, reset stall timing (but keep EMA updating) + if (_stallStartTime != 0) { + Serial.printf("Stall candidate cleared: current=%.2fA, avg=%.2fA\n", + activeCurrent, _currentEMA); + } _stallStartTime = 0; _stalled = false; }