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:
@@ -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
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user