Merge feature/simple-stall-detection: simple threshold-based stall detection
This commit is contained in:
devdesk
@@ -37,22 +37,14 @@
|
||||
#define ADC_VREF 3.3f // ADC reference voltage
|
||||
#define CURRENT_CALIBRATION 1.0f // Calibration factor (measured current / reported current)
|
||||
|
||||
// Stall Detection Configuration (delta-based)
|
||||
// Detects sudden current spikes above the rolling average
|
||||
#define STALL_DELTA_THRESHOLD 1.2f // 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_EMA_BASELINE 2.0f // Expected running current baseline for EMA seeding (amps)
|
||||
#define STALL_STABILIZE_MS 300 // Ignore stalls for this long after motor starts/changes
|
||||
#define STALL_CONFIRM_MS 50 // Current must exceed threshold for this long to confirm stall
|
||||
#define DISABLE_STALL_DETECT false // Set to true to disable stall detection
|
||||
// Current logging interval for data collection
|
||||
#define CURRENT_LOG_INTERVAL_MS 100 // Log current readings every 100ms
|
||||
|
||||
// Under-current stall detection: motor commanded but drawing no current
|
||||
#define STALL_UNDERCURRENT_THRESHOLD 0.5f // If current below this for too long = stall (amps)
|
||||
#define STALL_UNDERCURRENT_MS 1000 // Under-current must persist this long to confirm stall
|
||||
|
||||
// Repeated spike stall detection: catches oscillating stall (driver protection cycling)
|
||||
#define STALL_CANDIDATE_COUNT 5 // Number of stall candidates in window to confirm stall
|
||||
#define STALL_CANDIDATE_WINDOW_MS 2000 // Time window for counting stall candidates
|
||||
// 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,59 +4,47 @@
|
||||
#include <Arduino.h>
|
||||
#include "config.h"
|
||||
|
||||
// Stall callback function type
|
||||
typedef void (*StallCallback)();
|
||||
|
||||
class MotorController {
|
||||
public:
|
||||
void begin();
|
||||
void setSpeed(int speed); // 0-100 percentage
|
||||
void setDirection(int dir); // -1=reverse, 0=stop, 1=forward
|
||||
void stop();
|
||||
void update(); // Call in loop() for stall detection
|
||||
void update(); // Call in loop() for current monitoring
|
||||
|
||||
int getSpeed();
|
||||
int getDirection();
|
||||
float getCurrentRight(); // Current in amps (forward direction)
|
||||
float getCurrentLeft(); // Current in amps (reverse direction)
|
||||
float getCurrentActive(); // Current from active direction
|
||||
bool isStalled(); // True if stall detected
|
||||
|
||||
// Callback for stall events
|
||||
void setStallCallback(void (*callback)(float current));
|
||||
// Stall detection
|
||||
bool isStalled();
|
||||
void setStallCallback(StallCallback callback);
|
||||
void resetStallDetection();
|
||||
|
||||
// Pingpong mode
|
||||
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent, bool useStallReturn);
|
||||
// Pingpong mode (time-based only)
|
||||
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent);
|
||||
void stopPingpong();
|
||||
bool isPingpongActive();
|
||||
int getPingpongSpeed();
|
||||
int getPingpongTime();
|
||||
int getPingpongSpeedRandom();
|
||||
int getPingpongTimeRandom();
|
||||
bool getPingpongStallReturn();
|
||||
|
||||
private:
|
||||
int _speed = 0;
|
||||
int _direction = 0;
|
||||
float _currentRight = 0;
|
||||
float _currentLeft = 0;
|
||||
bool _stalled = false;
|
||||
unsigned long _stallStartTime = 0;
|
||||
void (*_stallCallback)(float current) = nullptr;
|
||||
|
||||
// ADC zero-current offset (calibrated at startup)
|
||||
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
|
||||
|
||||
// Under-current stall detection
|
||||
unsigned long _undercurrentStartTime = 0; // When under-current condition started
|
||||
|
||||
// Repeated spike detection (oscillating stall pattern)
|
||||
int _stallCandidateCount = 0; // Number of stall candidates in current window
|
||||
unsigned long _stallCandidateWindowStart = 0; // When current counting window started
|
||||
|
||||
// Pingpong state
|
||||
bool _pingpongActive = false;
|
||||
int _pingpongBaseSpeed = 50;
|
||||
@@ -67,15 +55,19 @@ private:
|
||||
int _pingpongCurrentTime = 2000;
|
||||
unsigned long _pingpongLastSwitch = 0;
|
||||
int _pingpongDirection = 1;
|
||||
bool _pingpongUseStallReturn = false; // Return only after stall detection
|
||||
|
||||
// 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 checkStall();
|
||||
void resetStallDetection();
|
||||
void updatePingpong();
|
||||
int applyRandomness(int baseValue, int randomPercent);
|
||||
void checkStall();
|
||||
};
|
||||
|
||||
extern MotorController motor;
|
||||
|
||||
104
plans/remove-stall-detection-branch.md
Normal file
104
plans/remove-stall-detection-branch.md
Normal file
@@ -0,0 +1,104 @@
|
||||
# Branch: feature/remove-stall-detection
|
||||
|
||||
## Summary
|
||||
|
||||
Removed all stall detection logic to collect clean current data with new beefy PSU before reimplmenting simpler stall detection.
|
||||
|
||||
## Problem
|
||||
|
||||
The original stall detection was overly complex with:
|
||||
- Delta-based spike detection with EMA averaging
|
||||
- Under-current detection
|
||||
- Repeated spike detection for oscillating patterns
|
||||
- Multiple timing thresholds and stabilization periods
|
||||
- 16 configuration parameters
|
||||
|
||||
This complexity was trying to compensate for a weak PSU that couldn't provide enough current, causing unreliable stall detection.
|
||||
|
||||
## What Was Removed
|
||||
|
||||
### config.h
|
||||
- `STALL_DELTA_THRESHOLD`
|
||||
- `STALL_EMA_ALPHA`
|
||||
- `STALL_EMA_BASELINE`
|
||||
- `STALL_STABILIZE_MS`
|
||||
- `STALL_CONFIRM_MS`
|
||||
- `DISABLE_STALL_DETECT`
|
||||
- `STALL_UNDERCURRENT_THRESHOLD`
|
||||
- `STALL_UNDERCURRENT_MS`
|
||||
- `STALL_CANDIDATE_COUNT`
|
||||
- `STALL_CANDIDATE_WINDOW_MS`
|
||||
|
||||
### motor.h / motor.cpp
|
||||
- `checkStall()` function (~115 lines)
|
||||
- `resetStallDetection()` function
|
||||
- `isStalled()` method
|
||||
- `setStallCallback()` method
|
||||
- `getPingpongStallReturn()` method
|
||||
- All stall-related member variables
|
||||
|
||||
### webserver.cpp
|
||||
- Stall warning UI banner
|
||||
- "Return on stall only" checkbox
|
||||
- `stalled` and `ppStallReturn` from status JSON
|
||||
|
||||
### main.cpp
|
||||
- `onMotorStall()` callback function
|
||||
|
||||
## What Was Added
|
||||
|
||||
- `CURRENT_LOG_INTERVAL_MS` config (100ms)
|
||||
- Frequent current logging: `CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d`
|
||||
- "Motor: STOPPED (manual)" message
|
||||
|
||||
## Current Data Analysis
|
||||
|
||||
See [src/current_loggin.log](../src/current_loggin.log) for sample data.
|
||||
|
||||
### Running Current (motor moving freely)
|
||||
- **Range:** 0.67A - 2.64A
|
||||
- **Average:** ~1.5-2.0A
|
||||
|
||||
### Stall Current (motor blocked at end stop)
|
||||
- **Range:** 16.8A - 21.0A
|
||||
- **Average:** ~17-18A
|
||||
|
||||
### Key Observations
|
||||
1. **Clear separation:** Running ~2A vs Stall ~17A = 8.5x difference
|
||||
2. **Fast transition:** Stall spike happens within 1-2 samples (100-200ms)
|
||||
3. **Initial spike:** Can see 3.77A or 5.35A briefly before full stall current
|
||||
4. **Gradual decay:** Stall current trends down slightly over time (~21A → ~17A)
|
||||
|
||||
## Next Step: Simple Stall Detection
|
||||
|
||||
Based on the clean data, implement a simple threshold-based stall detection:
|
||||
|
||||
### Proposed Algorithm
|
||||
```
|
||||
IF motor is running (dir != 0 AND speed > 0):
|
||||
IF active_current > STALL_THRESHOLD for STALL_CONFIRM_SAMPLES:
|
||||
STALL DETECTED
|
||||
```
|
||||
|
||||
### Proposed Config (much simpler)
|
||||
```c
|
||||
#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
|
||||
```
|
||||
|
||||
### Why This Should Work
|
||||
- With beefy PSU, running current is stable ~2A
|
||||
- Stall current is clearly ~17A+
|
||||
- No need for EMA averaging or complex spike detection
|
||||
- Simple threshold + debounce is enough
|
||||
|
||||
## Files Changed
|
||||
|
||||
| File | Lines Removed | Lines Added |
|
||||
|------|---------------|-------------|
|
||||
| config.h | 16 | 2 |
|
||||
| motor.h | 26 | 0 |
|
||||
| motor.cpp | 180 | 1 |
|
||||
| webserver.cpp | 50 | 5 |
|
||||
| main.cpp | 12 | 1 |
|
||||
| **Total** | **284** | **9** |
|
||||
128
src/current_loggin.log
Normal file
128
src/current_loggin.log
Normal file
@@ -0,0 +1,128 @@
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=0
|
||||
Motor: dir=1, speed=0%, pwm=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=0
|
||||
Motor: dir=-1, speed=0%, pwm=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
CURRENT: R=0.00A L=0.00A dir=-1 spd=0
|
||||
Motor: dir=-1, speed=100%, pwm=255
|
||||
CURRENT: R=0.00A L=0.88A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.07A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.49A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.56A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.25A dir=-1 spd=100
|
||||
Motor: dir=-1, speed=100%, pwm=255
|
||||
CURRENT: R=0.00A L=1.10A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.18A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.36A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.28A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.39A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.47A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.25A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.31A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.02A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.36A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.35A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.24A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.57A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.64A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.32A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.52A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=3.77A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=21.04A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=18.61A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=19.38A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.53A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.53A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.50A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.41A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.30A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.28A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.21A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.17A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=17.06A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=16.99A dir=-1 spd=100
|
||||
Motor: dir=1, speed=100%, pwm=255
|
||||
CURRENT: R=0.00A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.77A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.36A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.19A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.10A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.18A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.36A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.24A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.50A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.45A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.01A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.64A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.10A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.31A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.34A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.90A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.46A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.60A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=2.27A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=1.96A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=3.08A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=15.12A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=18.53A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=20.75A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=19.59A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=18.54A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=17.88A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=17.35A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=17.06A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=16.99A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=16.85A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=16.90A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=16.84A L=0.00A dir=1 spd=100
|
||||
CURRENT: R=16.81A L=0.00A dir=1 spd=100
|
||||
Motor: dir=-1, speed=100%, pwm=255
|
||||
CURRENT: R=0.00A L=5.35A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=0.46A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.29A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=0.86A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.22A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.90A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.00A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.33A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.42A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.27A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=1.25A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=2.21A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=0.67A dir=-1 spd=100
|
||||
CURRENT: R=0.00A L=0.85A dir=-1 spd=100
|
||||
Motor: STOPPED (manual)
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
CURRENT: R=0.00A L=0.00A dir=0 spd=100
|
||||
24
src/main.cpp
24
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...");
|
||||
|
||||
@@ -33,30 +40,17 @@ void setupWiFi() {
|
||||
}
|
||||
}
|
||||
|
||||
// Stall protection callback - stops motor immediately when stall detected
|
||||
// (unless pingpong with stallReturn is active - it handles stall by switching direction)
|
||||
void onMotorStall(float current) {
|
||||
if (motor.isPingpongActive() && motor.getPingpongStallReturn()) {
|
||||
// Pingpong will handle stall by switching direction
|
||||
Serial.printf("STALL DETECTED: Pingpong will switch direction (current: %.2fA)\n", current);
|
||||
return;
|
||||
}
|
||||
Serial.printf("STALL PROTECTION: Stopping motor (current: %.2fA)\n", current);
|
||||
motor.stop();
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(1000);
|
||||
|
||||
Serial.println("\n=============================");
|
||||
Serial.println(" BTS7960 Motor Controller");
|
||||
Serial.println(" Simple Stall Detection");
|
||||
Serial.println("=============================\n");
|
||||
|
||||
// Initialize motor controller
|
||||
motor.begin();
|
||||
|
||||
// Register stall protection callback
|
||||
motor.setStallCallback(onMotorStall);
|
||||
|
||||
// Connect to WiFi
|
||||
@@ -72,6 +66,6 @@ void setup() {
|
||||
|
||||
void loop() {
|
||||
handleWebServer();
|
||||
motor.update(); // Update current sensing and stall detection
|
||||
motor.update(); // Update current sensing and logging
|
||||
delay(1);
|
||||
}
|
||||
|
||||
249
src/motor.cpp
249
src/motor.cpp
@@ -45,32 +45,26 @@ void MotorController::begin() {
|
||||
|
||||
void MotorController::setSpeed(int speed) {
|
||||
_speed = constrain(speed, 0, 100);
|
||||
resetStallDetection();
|
||||
applyMotorState();
|
||||
}
|
||||
|
||||
void MotorController::setDirection(int dir) {
|
||||
_direction = constrain(dir, -1, 1);
|
||||
resetStallDetection();
|
||||
int newDir = constrain(dir, -1, 1);
|
||||
if (newDir != _direction) {
|
||||
_lastDirectionChangeTime = millis();
|
||||
resetStallDetection();
|
||||
}
|
||||
_direction = newDir;
|
||||
applyMotorState();
|
||||
}
|
||||
|
||||
void MotorController::stop() {
|
||||
// Don't reset _speed - keep last speed setting
|
||||
_direction = 0;
|
||||
resetStallDetection();
|
||||
ledcWrite(PWM_CHANNEL_R, 0);
|
||||
ledcWrite(PWM_CHANNEL_L, 0);
|
||||
}
|
||||
|
||||
void MotorController::resetStallDetection() {
|
||||
_stalled = false;
|
||||
_stallStartTime = 0;
|
||||
_undercurrentStartTime = 0;
|
||||
_stallCandidateCount = 0;
|
||||
_stallCandidateWindowStart = 0;
|
||||
_emaInitialized = false; // Re-seed EMA on motor state change
|
||||
_motorStartTime = millis();
|
||||
resetStallDetection();
|
||||
Serial.println("Motor: STOPPED (manual)");
|
||||
}
|
||||
|
||||
void MotorController::update() {
|
||||
@@ -81,13 +75,12 @@ void MotorController::update() {
|
||||
_currentRight = readCurrentSense(R_IS_PIN);
|
||||
_currentLeft = readCurrentSense(L_IS_PIN);
|
||||
|
||||
// Log current readings every 500ms when motor is running
|
||||
if ((_direction != 0 || _speed != 0) && (millis() - lastPrintTime > 500)) {
|
||||
lastPrintTime = millis();
|
||||
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);
|
||||
// Log current readings frequently for data collection
|
||||
unsigned long now = millis();
|
||||
if (now - lastPrintTime >= CURRENT_LOG_INTERVAL_MS) {
|
||||
lastPrintTime = now;
|
||||
Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
|
||||
_currentRight, _currentLeft, _direction, _speed);
|
||||
}
|
||||
|
||||
// Check for stall condition
|
||||
@@ -123,14 +116,61 @@ float MotorController::getCurrentActive() {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// Stall detection
|
||||
bool MotorController::isStalled() {
|
||||
return _stalled;
|
||||
}
|
||||
|
||||
void MotorController::setStallCallback(void (*callback)(float current)) {
|
||||
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;
|
||||
@@ -206,32 +246,27 @@ void MotorController::calibrateCurrentOffset() {
|
||||
#endif
|
||||
}
|
||||
|
||||
// Pingpong mode implementation
|
||||
void MotorController::startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent, bool useStallReturn) {
|
||||
// Pingpong mode implementation (time-based only)
|
||||
void MotorController::startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent) {
|
||||
_pingpongBaseSpeed = constrain(speed, 0, 100);
|
||||
_pingpongBaseTime = constrain(timeMs, 100, 30000);
|
||||
_pingpongSpeedRandomPercent = constrain(speedRandomPercent, 0, 100);
|
||||
_pingpongTimeRandomPercent = constrain(timeRandomPercent, 0, 100);
|
||||
_pingpongUseStallReturn = useStallReturn;
|
||||
|
||||
_pingpongDirection = 1;
|
||||
_pingpongCurrentSpeed = applyRandomness(_pingpongBaseSpeed, _pingpongSpeedRandomPercent);
|
||||
// Time randomness disabled when using stall return
|
||||
_pingpongCurrentTime = _pingpongUseStallReturn ? _pingpongBaseTime : applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
|
||||
_pingpongCurrentTime = applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
|
||||
_pingpongLastSwitch = millis();
|
||||
_pingpongActive = true;
|
||||
_stalled = false; // Reset stall state when starting pingpong
|
||||
_stallStartTime = 0;
|
||||
|
||||
// Apply initial state
|
||||
_speed = _pingpongCurrentSpeed;
|
||||
_direction = _pingpongDirection;
|
||||
applyMotorState();
|
||||
|
||||
Serial.printf("Pingpong started: speed=%d%% (base=%d, rand=%d%%), time=%dms (base=%d, rand=%d%%), stallReturn=%s\n",
|
||||
Serial.printf("Pingpong started: speed=%d%% (base=%d, rand=%d%%), time=%dms (base=%d, rand=%d%%)\n",
|
||||
_pingpongCurrentSpeed, _pingpongBaseSpeed, _pingpongSpeedRandomPercent,
|
||||
_pingpongCurrentTime, _pingpongBaseTime, _pingpongTimeRandomPercent,
|
||||
_pingpongUseStallReturn ? "true" : "false");
|
||||
_pingpongCurrentTime, _pingpongBaseTime, _pingpongTimeRandomPercent);
|
||||
}
|
||||
|
||||
void MotorController::stopPingpong() {
|
||||
@@ -260,41 +295,19 @@ int MotorController::getPingpongTimeRandom() {
|
||||
return _pingpongTimeRandomPercent;
|
||||
}
|
||||
|
||||
bool MotorController::getPingpongStallReturn() {
|
||||
return _pingpongUseStallReturn;
|
||||
}
|
||||
|
||||
void MotorController::updatePingpong() {
|
||||
if (!_pingpongActive) return;
|
||||
|
||||
bool shouldSwitch = false;
|
||||
unsigned long now = millis();
|
||||
|
||||
if (_pingpongUseStallReturn) {
|
||||
// Switch direction only when stall is detected
|
||||
if (_stalled) {
|
||||
shouldSwitch = true;
|
||||
Serial.println("Pingpong: stall detected, switching direction");
|
||||
}
|
||||
} else {
|
||||
// Time-based switching
|
||||
if ((now - _pingpongLastSwitch) >= (unsigned long)_pingpongCurrentTime) {
|
||||
shouldSwitch = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (shouldSwitch) {
|
||||
// Time-based switching
|
||||
if ((now - _pingpongLastSwitch) >= (unsigned long)_pingpongCurrentTime) {
|
||||
// Switch direction
|
||||
_pingpongDirection = -_pingpongDirection;
|
||||
|
||||
// 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);
|
||||
// Time randomness disabled when using stall return
|
||||
_pingpongCurrentTime = _pingpongUseStallReturn ? _pingpongBaseTime : applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
|
||||
_pingpongCurrentTime = applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
|
||||
_pingpongLastSwitch = now;
|
||||
|
||||
// Apply new state
|
||||
@@ -302,9 +315,8 @@ void MotorController::updatePingpong() {
|
||||
_direction = _pingpongDirection;
|
||||
applyMotorState();
|
||||
|
||||
Serial.printf("Pingpong switch: dir=%d, speed=%d%%, next_time=%dms, stallReturn=%s\n",
|
||||
_pingpongDirection, _pingpongCurrentSpeed, _pingpongCurrentTime,
|
||||
_pingpongUseStallReturn ? "true" : "false");
|
||||
Serial.printf("Pingpong switch: dir=%d, speed=%d%%, next_time=%dms\n",
|
||||
_pingpongDirection, _pingpongCurrentSpeed, _pingpongCurrentTime);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -318,120 +330,3 @@ int MotorController::applyRandomness(int baseValue, int randomPercent) {
|
||||
// Ensure result stays positive and reasonable
|
||||
return max(1, result);
|
||||
}
|
||||
|
||||
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;
|
||||
_undercurrentStartTime = 0;
|
||||
_emaInitialized = false;
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned long now = millis();
|
||||
float activeCurrent = getCurrentActive();
|
||||
|
||||
// Initialize EMA at expected baseline (prevents inrush from looking like stall)
|
||||
if (!_emaInitialized) {
|
||||
_currentEMA = STALL_EMA_BASELINE;
|
||||
_emaInitialized = true;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate delta from average
|
||||
float delta = activeCurrent - _currentEMA;
|
||||
|
||||
// Check if current spike exceeds delta threshold
|
||||
bool spikeDetected = (delta > STALL_DELTA_THRESHOLD);
|
||||
|
||||
// Update EMA only when no spike is being investigated
|
||||
// This prevents EMA from "chasing" up to the spike during detection window
|
||||
if (!spikeDetected && !_stalled) {
|
||||
_currentEMA = (STALL_EMA_ALPHA * activeCurrent) + ((1.0f - STALL_EMA_ALPHA) * _currentEMA);
|
||||
}
|
||||
|
||||
if (spikeDetected) {
|
||||
if (_stallStartTime == 0) {
|
||||
// Start timing potential stall
|
||||
_stallStartTime = now;
|
||||
|
||||
// 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) {
|
||||
// Sustained spike - stall confirmed
|
||||
if (!_stalled) {
|
||||
_stalled = true;
|
||||
Serial.printf("STALL DETECTED! Current: %.2fA (avg: %.2fA, delta: %.2fA)\n",
|
||||
activeCurrent, _currentEMA, delta);
|
||||
|
||||
// Call callback if registered
|
||||
if (_stallCallback != nullptr) {
|
||||
_stallCallback(activeCurrent);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 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;
|
||||
// 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
|
||||
}
|
||||
|
||||
@@ -58,15 +58,6 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
font-size: 12px;
|
||||
color: #888;
|
||||
}
|
||||
.stall-warning {
|
||||
background: #ff5252;
|
||||
color: white;
|
||||
padding: 10px;
|
||||
border-radius: 5px;
|
||||
margin-bottom: 20px;
|
||||
display: none;
|
||||
}
|
||||
.stall-warning.active { display: block; }
|
||||
.slider-container {
|
||||
background: #16213e;
|
||||
padding: 20px;
|
||||
@@ -118,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;
|
||||
@@ -178,18 +182,16 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
<div class="container">
|
||||
<h1>Motor Control</h1>
|
||||
|
||||
<div class="stall-warning" id="stallWarning">
|
||||
⚠️ STALL DETECTED - Motor Stopped
|
||||
</div>
|
||||
<div class="stall-warning" id="stallWarning">⚠️ STALL DETECTED!</div>
|
||||
|
||||
<div class="current-display">
|
||||
<div>
|
||||
<div class="current-label">CURRENT (Active)</div>
|
||||
<div class="current-value"><span id="currentActive">0.00</span>A</div>
|
||||
<div class="current-label">CURRENT R</div>
|
||||
<div class="current-value"><span id="currentR">0.00</span>A</div>
|
||||
</div>
|
||||
<div>
|
||||
<div class="current-label">THRESHOLD</div>
|
||||
<div class="current-value" style="color:#ff9100;">4.0A</div>
|
||||
<div class="current-label">CURRENT L</div>
|
||||
<div class="current-value"><span id="currentL">0.00</span>A</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@@ -239,12 +241,6 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
<span class="value"><span id="ppTimeRandVal">0</span>%</span>
|
||||
</div>
|
||||
<input type="range" class="setting-slider" id="ppTimeRand" min="0" max="50" value="0">
|
||||
|
||||
<div class="setting-row" style="margin-top:15px;">
|
||||
<label for="ppStallReturn">Return on stall only</label>
|
||||
<input type="checkbox" id="ppStallReturn" style="width:25px;height:25px;">
|
||||
</div>
|
||||
<small style="color:#888;display:block;margin-top:5px;">When enabled, direction switches only after stall detection (time randomness disabled)</small>
|
||||
</div>
|
||||
|
||||
<div class="buttons">
|
||||
@@ -269,16 +265,6 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
const ppSpeedRandVal = document.getElementById('ppSpeedRandVal');
|
||||
const ppTimeRandVal = document.getElementById('ppTimeRandVal');
|
||||
const ppStatus = document.getElementById('pingpongStatus');
|
||||
const ppStallReturn = document.getElementById('ppStallReturn');
|
||||
|
||||
// Disable time randomness when stall return is checked
|
||||
ppStallReturn.onchange = function() {
|
||||
ppTimeRand.disabled = this.checked;
|
||||
if (this.checked) {
|
||||
ppTimeRand.value = 0;
|
||||
ppTimeRandVal.textContent = '0';
|
||||
}
|
||||
};
|
||||
|
||||
slider.oninput = function() {
|
||||
speedVal.textContent = this.value;
|
||||
@@ -316,8 +302,7 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
speed: ppSpeed.value,
|
||||
time: ppTime.value,
|
||||
speedRand: ppSpeedRand.value,
|
||||
timeRand: ppStallReturn.checked ? 0 : ppTimeRand.value,
|
||||
stallReturn: ppStallReturn.checked ? 1 : 0
|
||||
timeRand: ppTimeRand.value
|
||||
});
|
||||
fetch('/pingpong/start?' + params)
|
||||
.then(r => r.text())
|
||||
@@ -350,11 +335,10 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
updateStatus();
|
||||
|
||||
// Update current display
|
||||
const active = data.direction > 0 ? data.currentR :
|
||||
data.direction < 0 ? data.currentL : 0;
|
||||
document.getElementById('currentActive').textContent = active.toFixed(2);
|
||||
document.getElementById('currentR').textContent = data.currentR.toFixed(2);
|
||||
document.getElementById('currentL').textContent = data.currentL.toFixed(2);
|
||||
|
||||
// Show/hide stall warning
|
||||
// Update stall warning
|
||||
const stallWarning = document.getElementById('stallWarning');
|
||||
if (data.stalled) {
|
||||
stallWarning.classList.add('active');
|
||||
@@ -376,8 +360,6 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
ppSpeedRandVal.textContent = data.ppSpeedRand;
|
||||
ppTimeRand.value = data.ppTimeRand;
|
||||
ppTimeRandVal.textContent = data.ppTimeRand;
|
||||
ppStallReturn.checked = data.ppStallReturn;
|
||||
ppTimeRand.disabled = data.ppStallReturn;
|
||||
} else {
|
||||
ppStatus.textContent = 'INACTIVE';
|
||||
ppStatus.classList.remove('active');
|
||||
@@ -433,8 +415,7 @@ void handleStatus() {
|
||||
",\"ppSpeed\":" + String(motor.getPingpongSpeed()) +
|
||||
",\"ppTime\":" + String(motor.getPingpongTime()) +
|
||||
",\"ppSpeedRand\":" + String(motor.getPingpongSpeedRandom()) +
|
||||
",\"ppTimeRand\":" + String(motor.getPingpongTimeRandom()) +
|
||||
",\"ppStallReturn\":" + (motor.getPingpongStallReturn() ? "true" : "false") + "}";
|
||||
",\"ppTimeRand\":" + String(motor.getPingpongTimeRandom()) + "}";
|
||||
server.send(200, "application/json", json);
|
||||
}
|
||||
|
||||
@@ -443,7 +424,6 @@ void handlePingpongStart() {
|
||||
int time = 2000;
|
||||
int speedRand = 0;
|
||||
int timeRand = 0;
|
||||
bool stallReturn = false;
|
||||
|
||||
if (server.hasArg("speed")) {
|
||||
speed = server.arg("speed").toInt();
|
||||
@@ -457,11 +437,8 @@ void handlePingpongStart() {
|
||||
if (server.hasArg("timeRand")) {
|
||||
timeRand = server.arg("timeRand").toInt();
|
||||
}
|
||||
if (server.hasArg("stallReturn")) {
|
||||
stallReturn = server.arg("stallReturn").toInt() != 0;
|
||||
}
|
||||
|
||||
motor.startPingpong(speed, time, speedRand, timeRand, stallReturn);
|
||||
motor.startPingpong(speed, time, speedRand, timeRand);
|
||||
server.send(200, "text/plain", "OK");
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user