87a05fc80a
- 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
77 lines
2.5 KiB
C++
77 lines
2.5 KiB
C++
#ifndef MOTOR_H
|
|
#define MOTOR_H
|
|
|
|
#include <Arduino.h>
|
|
#include "config.h"
|
|
|
|
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
|
|
|
|
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));
|
|
|
|
// Pingpong mode
|
|
void startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent, bool useStallReturn);
|
|
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
|
|
|
|
// Pingpong state
|
|
bool _pingpongActive = false;
|
|
int _pingpongBaseSpeed = 50;
|
|
int _pingpongBaseTime = 2000;
|
|
int _pingpongSpeedRandomPercent = 0;
|
|
int _pingpongTimeRandomPercent = 0;
|
|
int _pingpongCurrentSpeed = 50;
|
|
int _pingpongCurrentTime = 2000;
|
|
unsigned long _pingpongLastSwitch = 0;
|
|
int _pingpongDirection = 1;
|
|
bool _pingpongUseStallReturn = false; // Return only after stall detection
|
|
|
|
void applyMotorState();
|
|
float readCurrentSense(int pin);
|
|
void calibrateCurrentOffset();
|
|
void checkStall();
|
|
void resetStallDetection();
|
|
void updatePingpong();
|
|
int applyRandomness(int baseValue, int randomPercent);
|
|
};
|
|
|
|
extern MotorController motor;
|
|
|
|
#endif
|