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
|
// Current logging interval for data collection
|
||||||
#define CURRENT_LOG_INTERVAL_MS 100 // Log current readings every 100ms
|
#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
|
// Web Server
|
||||||
#define HTTP_PORT 80
|
#define HTTP_PORT 80
|
||||||
|
|
||||||
|
|||||||
@@ -4,6 +4,9 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
|
|
||||||
|
// Stall callback function type
|
||||||
|
typedef void (*StallCallback)();
|
||||||
|
|
||||||
class MotorController {
|
class MotorController {
|
||||||
public:
|
public:
|
||||||
void begin();
|
void begin();
|
||||||
@@ -18,6 +21,11 @@ public:
|
|||||||
float getCurrentLeft(); // Current in amps (reverse direction)
|
float getCurrentLeft(); // Current in amps (reverse direction)
|
||||||
float getCurrentActive(); // Current from active direction
|
float getCurrentActive(); // Current from active direction
|
||||||
|
|
||||||
|
// Stall detection
|
||||||
|
bool isStalled();
|
||||||
|
void setStallCallback(StallCallback callback);
|
||||||
|
void resetStallDetection();
|
||||||
|
|
||||||
// Pingpong mode (time-based only)
|
// Pingpong mode (time-based only)
|
||||||
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent);
|
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent);
|
||||||
void stopPingpong();
|
void stopPingpong();
|
||||||
@@ -48,11 +56,18 @@ private:
|
|||||||
unsigned long _pingpongLastSwitch = 0;
|
unsigned long _pingpongLastSwitch = 0;
|
||||||
int _pingpongDirection = 1;
|
int _pingpongDirection = 1;
|
||||||
|
|
||||||
|
// Stall detection state
|
||||||
|
bool _stalled = false;
|
||||||
|
int _stallConfirmCount = 0;
|
||||||
|
StallCallback _stallCallback = nullptr;
|
||||||
|
unsigned long _lastDirectionChangeTime = 0;
|
||||||
|
|
||||||
void applyMotorState();
|
void applyMotorState();
|
||||||
float readCurrentSense(int pin);
|
float readCurrentSense(int pin);
|
||||||
void calibrateCurrentOffset();
|
void calibrateCurrentOffset();
|
||||||
void updatePingpong();
|
void updatePingpong();
|
||||||
int applyRandomness(int baseValue, int randomPercent);
|
int applyRandomness(int baseValue, int randomPercent);
|
||||||
|
void checkStall();
|
||||||
};
|
};
|
||||||
|
|
||||||
extern MotorController motor;
|
extern MotorController motor;
|
||||||
|
|||||||
10
src/main.cpp
10
src/main.cpp
@@ -4,6 +4,13 @@
|
|||||||
#include "motor.h"
|
#include "motor.h"
|
||||||
#include "webserver.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() {
|
void setupWiFi() {
|
||||||
Serial.println("Connecting to WiFi...");
|
Serial.println("Connecting to WiFi...");
|
||||||
|
|
||||||
@@ -39,11 +46,12 @@ void setup() {
|
|||||||
|
|
||||||
Serial.println("\n=============================");
|
Serial.println("\n=============================");
|
||||||
Serial.println(" BTS7960 Motor Controller");
|
Serial.println(" BTS7960 Motor Controller");
|
||||||
Serial.println(" (Stall Detection Removed)");
|
Serial.println(" Simple Stall Detection");
|
||||||
Serial.println("=============================\n");
|
Serial.println("=============================\n");
|
||||||
|
|
||||||
// Initialize motor controller
|
// Initialize motor controller
|
||||||
motor.begin();
|
motor.begin();
|
||||||
|
motor.setStallCallback(onMotorStall);
|
||||||
|
|
||||||
// Connect to WiFi
|
// Connect to WiFi
|
||||||
setupWiFi();
|
setupWiFi();
|
||||||
|
|||||||
@@ -49,7 +49,12 @@ void MotorController::setSpeed(int speed) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void MotorController::setDirection(int dir) {
|
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();
|
applyMotorState();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -58,6 +63,7 @@ void MotorController::stop() {
|
|||||||
_direction = 0;
|
_direction = 0;
|
||||||
ledcWrite(PWM_CHANNEL_R, 0);
|
ledcWrite(PWM_CHANNEL_R, 0);
|
||||||
ledcWrite(PWM_CHANNEL_L, 0);
|
ledcWrite(PWM_CHANNEL_L, 0);
|
||||||
|
resetStallDetection();
|
||||||
Serial.println("Motor: STOPPED (manual)");
|
Serial.println("Motor: STOPPED (manual)");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -76,6 +82,9 @@ void MotorController::update() {
|
|||||||
Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
|
Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
|
||||||
_currentRight, _currentLeft, _direction, _speed);
|
_currentRight, _currentLeft, _direction, _speed);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Check for stall condition
|
||||||
|
checkStall();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Update pingpong mode
|
// Update pingpong mode
|
||||||
@@ -107,6 +116,61 @@ float MotorController::getCurrentActive() {
|
|||||||
return 0.0f;
|
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() {
|
void MotorController::applyMotorState() {
|
||||||
// Apply minimum PWM when motor is running
|
// Apply minimum PWM when motor is running
|
||||||
int effectiveSpeed = _speed;
|
int effectiveSpeed = _speed;
|
||||||
|
|||||||
@@ -109,6 +109,19 @@ const char index_html[] PROGMEM = R"rawliteral(
|
|||||||
border-top: 2px solid #0f3460;
|
border-top: 2px solid #0f3460;
|
||||||
margin: 30px 0;
|
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 {
|
.pingpong-section {
|
||||||
background: #16213e;
|
background: #16213e;
|
||||||
padding: 20px;
|
padding: 20px;
|
||||||
@@ -169,6 +182,8 @@ const char index_html[] PROGMEM = R"rawliteral(
|
|||||||
<div class="container">
|
<div class="container">
|
||||||
<h1>Motor Control</h1>
|
<h1>Motor Control</h1>
|
||||||
|
|
||||||
|
<div class="stall-warning" id="stallWarning">⚠️ STALL DETECTED!</div>
|
||||||
|
|
||||||
<div class="current-display">
|
<div class="current-display">
|
||||||
<div>
|
<div>
|
||||||
<div class="current-label">CURRENT R</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('currentR').textContent = data.currentR.toFixed(2);
|
||||||
document.getElementById('currentL').textContent = data.currentL.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
|
// Update pingpong status
|
||||||
if (data.pingpong) {
|
if (data.pingpong) {
|
||||||
ppStatus.textContent = 'ACTIVE';
|
ppStatus.textContent = 'ACTIVE';
|
||||||
@@ -387,6 +410,7 @@ void handleStatus() {
|
|||||||
",\"direction\":" + String(motor.getDirection()) +
|
",\"direction\":" + String(motor.getDirection()) +
|
||||||
",\"currentR\":" + String(motor.getCurrentRight(), 2) +
|
",\"currentR\":" + String(motor.getCurrentRight(), 2) +
|
||||||
",\"currentL\":" + String(motor.getCurrentLeft(), 2) +
|
",\"currentL\":" + String(motor.getCurrentLeft(), 2) +
|
||||||
|
",\"stalled\":" + (motor.isStalled() ? "true" : "false") +
|
||||||
",\"pingpong\":" + (motor.isPingpongActive() ? "true" : "false") +
|
",\"pingpong\":" + (motor.isPingpongActive() ? "true" : "false") +
|
||||||
",\"ppSpeed\":" + String(motor.getPingpongSpeed()) +
|
",\"ppSpeed\":" + String(motor.getPingpongSpeed()) +
|
||||||
",\"ppTime\":" + String(motor.getPingpongTime()) +
|
",\"ppTime\":" + String(motor.getPingpongTime()) +
|
||||||
|
|||||||
Reference in New Issue
Block a user