From 241d1ae4579119700b4f5425bda2d5fc71f59bfe Mon Sep 17 00:00:00 2001 From: devdesk Date: Thu, 5 Feb 2026 20:49:59 +0200 Subject: [PATCH] Fix stall oscillation loop in pingpong mode When pingpong detected a stall and switched direction, only _stalled and _stallStartTime were reset, leaving _stallCandidateCount and _motorStartTime unchanged. This caused motor inrush current after direction change to immediately trigger another stall, creating an infinite oscillation loop. Now calls resetStallDetection() which properly resets all stall state including triggering the STALL_STABILIZE_MS grace period to ignore inrush current. --- include/config.h | 26 +++++++++++------ include/motor.h | 7 +++++ src/main.cpp | 6 ++++ src/motor.cpp | 75 +++++++++++++++++++++++++++++++++++++++--------- 4 files changed, 93 insertions(+), 21 deletions(-) diff --git a/include/config.h b/include/config.h index 1a7e2e9..845c534 100644 --- a/include/config.h +++ b/include/config.h @@ -12,13 +12,14 @@ #define DNS IPAddress(10, 81, 2, 1) // BTS7960 Pin Definitions -#define RPWM_PIN 25 // Right PWM (Forward) -#define LPWM_PIN 26 // Left PWM (Reverse) -#define R_EN_PIN 27 // Right Enable #define L_EN_PIN 14 // Left Enable -#define R_IS_PIN 34 // Right Current Sense (ADC input only) +#define LPWM_PIN 26 // Left PWM (Reverse) #define L_IS_PIN 35 // Left Current Sense (ADC input only) +#define R_EN_PIN 27 // Right Enable +#define RPWM_PIN 25 // Right PWM (Forward) +#define R_IS_PIN 34 // Right Current Sense (ADC input only) + // PWM Configuration #define PWM_FREQ 20000 // 20kHz - reduces motor noise #define PWM_RESOLUTION 8 // 8-bit resolution (0-255) @@ -34,16 +35,25 @@ #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 CURRENT_CALIBRATION 1.0f // 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_DELTA_THRESHOLD 1.2f // 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 STALL_EMA_BASELINE 2.0f // Expected running current baseline for EMA seeding (amps) +#define STALL_STABILIZE_MS 300 // Ignore stalls for this long after motor starts/changes +#define STALL_CONFIRM_MS 50 // Current must exceed threshold for this long to confirm stall #define DISABLE_STALL_DETECT false // Set to true to disable stall detection +// Under-current stall detection: motor commanded but drawing no current +#define STALL_UNDERCURRENT_THRESHOLD 0.5f // If current below this for too long = stall (amps) +#define STALL_UNDERCURRENT_MS 1000 // Under-current must persist this long to confirm stall + +// Repeated spike stall detection: catches oscillating stall (driver protection cycling) +#define STALL_CANDIDATE_COUNT 5 // Number of stall candidates in window to confirm stall +#define STALL_CANDIDATE_WINDOW_MS 2000 // Time window for counting stall candidates + // Web Server #define HTTP_PORT 80 diff --git a/include/motor.h b/include/motor.h index 94f04b0..4b4ae58 100644 --- a/include/motor.h +++ b/include/motor.h @@ -50,6 +50,13 @@ private: unsigned long _motorStartTime = 0; // When motor started (for stabilization period) bool _emaInitialized = false; // EMA needs seeding on first reading + // Under-current stall detection + unsigned long _undercurrentStartTime = 0; // When under-current condition started + + // Repeated spike detection (oscillating stall pattern) + int _stallCandidateCount = 0; // Number of stall candidates in current window + unsigned long _stallCandidateWindowStart = 0; // When current counting window started + // Pingpong state bool _pingpongActive = false; int _pingpongBaseSpeed = 50; diff --git a/src/main.cpp b/src/main.cpp index ce0c250..8b1f95b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -34,7 +34,13 @@ void setupWiFi() { } // Stall protection callback - stops motor immediately when stall detected +// (unless pingpong with stallReturn is active - it handles stall by switching direction) void onMotorStall(float current) { + if (motor.isPingpongActive() && motor.getPingpongStallReturn()) { + // Pingpong will handle stall by switching direction + Serial.printf("STALL DETECTED: Pingpong will switch direction (current: %.2fA)\n", current); + return; + } Serial.printf("STALL PROTECTION: Stopping motor (current: %.2fA)\n", current); motor.stop(); } diff --git a/src/motor.cpp b/src/motor.cpp index f0f505c..921f4f0 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -66,6 +66,9 @@ void MotorController::stop() { void MotorController::resetStallDetection() { _stalled = false; _stallStartTime = 0; + _undercurrentStartTime = 0; + _stallCandidateCount = 0; + _stallCandidateWindowStart = 0; _emaInitialized = false; // Re-seed EMA on motor state change _motorStartTime = millis(); } @@ -284,9 +287,9 @@ void MotorController::updatePingpong() { // Switch direction _pingpongDirection = -_pingpongDirection; - // Reset stall state for new direction - _stalled = false; - _stallStartTime = 0; + // Full stall detection reset for new direction + // This triggers STALL_STABILIZE_MS grace period to ignore motor inrush current + resetStallDetection(); // Apply randomness for next cycle _pingpongCurrentSpeed = applyRandomness(_pingpongBaseSpeed, _pingpongSpeedRandomPercent); @@ -324,6 +327,7 @@ void MotorController::checkStall() { if (_direction == 0 || _speed == 0) { _stalled = false; _stallStartTime = 0; + _undercurrentStartTime = 0; _emaInitialized = false; return; } @@ -331,16 +335,15 @@ void MotorController::checkStall() { unsigned long now = millis(); float activeCurrent = getCurrentActive(); - // Initialize EMA on first reading + // Initialize EMA at expected baseline (prevents inrush from looking like stall) if (!_emaInitialized) { - _currentEMA = activeCurrent; + _currentEMA = STALL_EMA_BASELINE; _emaInitialized = true; } - // During stabilization: update EMA but don't detect stalls - // This lets EMA track normal running current before detection starts + // During stabilization: DON'T update EMA - keep baseline to avoid drift toward 0 + // This lets stall detection work correctly even if starting from stalled position if ((now - _motorStartTime) < STALL_STABILIZE_MS) { - _currentEMA = (STALL_EMA_ALPHA * activeCurrent) + ((1.0f - STALL_EMA_ALPHA) * _currentEMA); return; } @@ -360,10 +363,34 @@ void MotorController::checkStall() { if (_stallStartTime == 0) { // Start timing potential stall _stallStartTime = now; - Serial.printf("Stall candidate: current=%.2fA, avg=%.2fA, delta=%.2fA\n", - activeCurrent, _currentEMA, delta); + + // Count this as a stall candidate for repeated-spike detection + if (_stallCandidateWindowStart == 0) { + _stallCandidateWindowStart = now; + _stallCandidateCount = 1; + } else if ((now - _stallCandidateWindowStart) > STALL_CANDIDATE_WINDOW_MS) { + // Window expired, start new window + _stallCandidateWindowStart = now; + _stallCandidateCount = 1; + } else { + _stallCandidateCount++; + } + + Serial.printf("Stall candidate #%d: current=%.2fA, avg=%.2fA, delta=%.2fA\n", + _stallCandidateCount, activeCurrent, _currentEMA, delta); + + // Check if enough candidates to confirm stall (oscillating pattern) + if (_stallCandidateCount >= STALL_CANDIDATE_COUNT && !_stalled) { + _stalled = true; + Serial.printf("STALL DETECTED (repeated spikes)! %d candidates in %lums\n", + _stallCandidateCount, now - _stallCandidateWindowStart); + + if (_stallCallback != nullptr) { + _stallCallback(activeCurrent); + } + } } else if ((now - _stallStartTime) > STALL_CONFIRM_MS) { - // Stall confirmed + // Sustained spike - stall confirmed if (!_stalled) { _stalled = true; Serial.printf("STALL DETECTED! Current: %.2fA (avg: %.2fA, delta: %.2fA)\n", @@ -376,13 +403,35 @@ void MotorController::checkStall() { } } } else { - // Current normal, reset stall timing (but keep EMA updating) + // Current normal, reset spike 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; + // Don't clear _stalled here - under-current/repeated-spike detection may have set it + } + + // Under-current stall detection: motor commanded but not drawing current + // This catches stalls where the motor can't even start (e.g., already at end stop) + if (!_stalled && activeCurrent < STALL_UNDERCURRENT_THRESHOLD) { + if (_undercurrentStartTime == 0) { + _undercurrentStartTime = now; + } else if ((now - _undercurrentStartTime) > STALL_UNDERCURRENT_MS) { + _stalled = true; + Serial.printf("STALL DETECTED (under-current)! Current: %.2fA (threshold: %.2fA)\n", + activeCurrent, STALL_UNDERCURRENT_THRESHOLD); + + if (_stallCallback != nullptr) { + _stallCallback(activeCurrent); + } + } + } else if (activeCurrent >= STALL_UNDERCURRENT_THRESHOLD) { + // Current is normal, reset under-current timer and clear stall + _undercurrentStartTime = 0; + if (!spikeDetected) { + _stalled = false; + } } #endif }