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.
This commit is contained in:
@@ -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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user