241d1ae457
When pingpong detected a stall and switched direction, only _stalled and _stallStartTime were reset, leaving _stallCandidateCount and _motorStartTime unchanged. This caused motor inrush current after direction change to immediately trigger another stall, creating an infinite oscillation loop. Now calls resetStallDetection() which properly resets all stall state including triggering the STALL_STABILIZE_MS grace period to ignore inrush current.
84 lines
2.8 KiB
C++
84 lines
2.8 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
|
|
|
|
// 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;
|
|
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
|