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
This commit is contained in:
devdesk
2026-02-05 18:20:26 +02:00
parent 63ad41970c
commit 87a05fc80a
3 changed files with 66 additions and 17 deletions

View File

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

View File

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

View File

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