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 }