tami/walker_control
6
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

Fix stall oscillation loop in pingpong mode

Browse Source 
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.
This commit is contained in:
devdesk
2026-02-05 20:49:59 +02:00
parent 568339367e
commit 241d1ae457
4 changed files with 93 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
include/config.h
View File

@@ -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

7
include/motor.h
View File

@@ -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;

6
src/main.cpp
View File

@@ -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();
}

75
src/motor.cpp
View File

@@ -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
}