feat: add simple stall detection with threshold + debounce
- Add STALL_THRESHOLD (8A) and STALL_CONFIRM_SAMPLES (3) config - Add 500ms stabilization delay after direction change to prevent false positives - Add stall warning banner in web UI - Stop motor and pingpong when stall detected Algorithm: If active current > 8A for 3 consecutive samples (300ms), and motor has been running for at least 500ms, trigger stall callback.
This commit is contained in:
@@ -40,6 +40,12 @@
|
||||
// Current logging interval for data collection
|
||||
#define CURRENT_LOG_INTERVAL_MS 100 // Log current readings every 100ms
|
||||
|
||||
// Simple Stall Detection
|
||||
// Based on clean data: Running ~2A, Stall ~17A (8.5x difference)
|
||||
#define STALL_THRESHOLD 8.0f // Amps - midpoint between 2A run and 17A stall
|
||||
#define STALL_CONFIRM_SAMPLES 3 // Debounce: 3 samples = 300ms at 100ms interval
|
||||
#define STALL_STABILIZE_MS 500 // Ignore current spikes for 500ms after direction change
|
||||
|
||||
// Web Server
|
||||
#define HTTP_PORT 80
|
||||
|
||||
|
||||
@@ -4,6 +4,9 @@
|
||||
#include <Arduino.h>
|
||||
#include "config.h"
|
||||
|
||||
// Stall callback function type
|
||||
typedef void (*StallCallback)();
|
||||
|
||||
class MotorController {
|
||||
public:
|
||||
void begin();
|
||||
@@ -18,6 +21,11 @@ public:
|
||||
float getCurrentLeft(); // Current in amps (reverse direction)
|
||||
float getCurrentActive(); // Current from active direction
|
||||
|
||||
// Stall detection
|
||||
bool isStalled();
|
||||
void setStallCallback(StallCallback callback);
|
||||
void resetStallDetection();
|
||||
|
||||
// Pingpong mode (time-based only)
|
||||
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent);
|
||||
void stopPingpong();
|
||||
@@ -48,11 +56,18 @@ private:
|
||||
unsigned long _pingpongLastSwitch = 0;
|
||||
int _pingpongDirection = 1;
|
||||
|
||||
// Stall detection state
|
||||
bool _stalled = false;
|
||||
int _stallConfirmCount = 0;
|
||||
StallCallback _stallCallback = nullptr;
|
||||
unsigned long _lastDirectionChangeTime = 0;
|
||||
|
||||
void applyMotorState();
|
||||
float readCurrentSense(int pin);
|
||||
void calibrateCurrentOffset();
|
||||
void updatePingpong();
|
||||
int applyRandomness(int baseValue, int randomPercent);
|
||||
void checkStall();
|
||||
};
|
||||
|
||||
extern MotorController motor;
|
||||
|
||||
10
src/main.cpp
10
src/main.cpp
@@ -4,6 +4,13 @@
|
||||
#include "motor.h"
|
||||
#include "webserver.h"
|
||||
|
||||
// Called when motor stall is detected
|
||||
void onMotorStall() {
|
||||
Serial.println("Stall callback triggered - stopping motor!");
|
||||
motor.stop();
|
||||
motor.stopPingpong();
|
||||
}
|
||||
|
||||
void setupWiFi() {
|
||||
Serial.println("Connecting to WiFi...");
|
||||
|
||||
@@ -39,11 +46,12 @@ void setup() {
|
||||
|
||||
Serial.println("\n=============================");
|
||||
Serial.println(" BTS7960 Motor Controller");
|
||||
Serial.println(" (Stall Detection Removed)");
|
||||
Serial.println(" Simple Stall Detection");
|
||||
Serial.println("=============================\n");
|
||||
|
||||
// Initialize motor controller
|
||||
motor.begin();
|
||||
motor.setStallCallback(onMotorStall);
|
||||
|
||||
// Connect to WiFi
|
||||
setupWiFi();
|
||||
|
||||
@@ -49,7 +49,12 @@ void MotorController::setSpeed(int speed) {
|
||||
}
|
||||
|
||||
void MotorController::setDirection(int dir) {
|
||||
_direction = constrain(dir, -1, 1);
|
||||
int newDir = constrain(dir, -1, 1);
|
||||
if (newDir != _direction) {
|
||||
_lastDirectionChangeTime = millis();
|
||||
resetStallDetection();
|
||||
}
|
||||
_direction = newDir;
|
||||
applyMotorState();
|
||||
}
|
||||
|
||||
@@ -58,6 +63,7 @@ void MotorController::stop() {
|
||||
_direction = 0;
|
||||
ledcWrite(PWM_CHANNEL_R, 0);
|
||||
ledcWrite(PWM_CHANNEL_L, 0);
|
||||
resetStallDetection();
|
||||
Serial.println("Motor: STOPPED (manual)");
|
||||
}
|
||||
|
||||
@@ -76,6 +82,9 @@ void MotorController::update() {
|
||||
Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
|
||||
_currentRight, _currentLeft, _direction, _speed);
|
||||
}
|
||||
|
||||
// Check for stall condition
|
||||
checkStall();
|
||||
#endif
|
||||
|
||||
// Update pingpong mode
|
||||
@@ -107,6 +116,61 @@ float MotorController::getCurrentActive() {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// Stall detection
|
||||
bool MotorController::isStalled() {
|
||||
return _stalled;
|
||||
}
|
||||
|
||||
void MotorController::setStallCallback(StallCallback callback) {
|
||||
_stallCallback = callback;
|
||||
}
|
||||
|
||||
void MotorController::resetStallDetection() {
|
||||
_stalled = false;
|
||||
_stallConfirmCount = 0;
|
||||
}
|
||||
|
||||
void MotorController::checkStall() {
|
||||
// Only check when motor is running
|
||||
if (_direction == 0 || _speed == 0) {
|
||||
resetStallDetection();
|
||||
return;
|
||||
}
|
||||
|
||||
// Skip stall check during stabilization period after direction change
|
||||
// (prevents false positives from inrush current spikes)
|
||||
if ((millis() - _lastDirectionChangeTime) < STALL_STABILIZE_MS) {
|
||||
return;
|
||||
}
|
||||
|
||||
float activeCurrent = getCurrentActive();
|
||||
|
||||
// Simple threshold-based stall detection with debounce
|
||||
if (activeCurrent > STALL_THRESHOLD) {
|
||||
_stallConfirmCount++;
|
||||
|
||||
if (_stallConfirmCount >= STALL_CONFIRM_SAMPLES && !_stalled) {
|
||||
_stalled = true;
|
||||
Serial.printf("STALL DETECTED! Current=%.2fA (threshold=%.1fA)\n",
|
||||
activeCurrent, STALL_THRESHOLD);
|
||||
|
||||
// Call stall callback if registered
|
||||
if (_stallCallback != nullptr) {
|
||||
_stallCallback();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Reset counter if current drops below threshold
|
||||
_stallConfirmCount = 0;
|
||||
|
||||
// Clear stall flag if current returns to normal
|
||||
if (_stalled) {
|
||||
_stalled = false;
|
||||
Serial.println("Stall condition cleared");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MotorController::applyMotorState() {
|
||||
// Apply minimum PWM when motor is running
|
||||
int effectiveSpeed = _speed;
|
||||
|
||||
@@ -109,6 +109,19 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
border-top: 2px solid #0f3460;
|
||||
margin: 30px 0;
|
||||
}
|
||||
.stall-warning {
|
||||
background: #ff5252;
|
||||
color: white;
|
||||
padding: 15px;
|
||||
border-radius: 10px;
|
||||
margin-bottom: 20px;
|
||||
font-weight: bold;
|
||||
font-size: 18px;
|
||||
display: none;
|
||||
}
|
||||
.stall-warning.active {
|
||||
display: block;
|
||||
}
|
||||
.pingpong-section {
|
||||
background: #16213e;
|
||||
padding: 20px;
|
||||
@@ -169,6 +182,8 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
<div class="container">
|
||||
<h1>Motor Control</h1>
|
||||
|
||||
<div class="stall-warning" id="stallWarning">⚠️ STALL DETECTED!</div>
|
||||
|
||||
<div class="current-display">
|
||||
<div>
|
||||
<div class="current-label">CURRENT R</div>
|
||||
@@ -323,6 +338,14 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
document.getElementById('currentR').textContent = data.currentR.toFixed(2);
|
||||
document.getElementById('currentL').textContent = data.currentL.toFixed(2);
|
||||
|
||||
// Update stall warning
|
||||
const stallWarning = document.getElementById('stallWarning');
|
||||
if (data.stalled) {
|
||||
stallWarning.classList.add('active');
|
||||
} else {
|
||||
stallWarning.classList.remove('active');
|
||||
}
|
||||
|
||||
// Update pingpong status
|
||||
if (data.pingpong) {
|
||||
ppStatus.textContent = 'ACTIVE';
|
||||
@@ -387,6 +410,7 @@ void handleStatus() {
|
||||
",\"direction\":" + String(motor.getDirection()) +
|
||||
",\"currentR\":" + String(motor.getCurrentRight(), 2) +
|
||||
",\"currentL\":" + String(motor.getCurrentLeft(), 2) +
|
||||
",\"stalled\":" + (motor.isStalled() ? "true" : "false") +
|
||||
",\"pingpong\":" + (motor.isPingpongActive() ? "true" : "false") +
|
||||
",\"ppSpeed\":" + String(motor.getPingpongSpeed()) +
|
||||
",\"ppTime\":" + String(motor.getPingpongTime()) +
|
||||
|
||||
Reference in New Issue
Block a user