83 lines
2.8 KiB
C++
83 lines
2.8 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
#include <vector>
|
|
|
|
// Motor-aided movement: detects external manipulation and assists to reduce gear stress
|
|
// Works by monitoring position velocity - when someone moves the motor manually,
|
|
// it briefly enables torque and moves to the current position to assist.
|
|
|
|
constexpr uint8_t VELOCITY_SAMPLES = 4; // Number of samples for velocity smoothing (fewer = faster response)
|
|
constexpr int16_t MIN_POSITION_DELTA = 1; // Ignore position changes smaller than this (minimum for high gear ratio)
|
|
constexpr uint8_t CONSECUTIVE_REQUIRED = 3; // Require this many consecutive high readings to trigger
|
|
|
|
struct AidedMotorSettings {
|
|
int16_t velocityThreshold = 80; // Position units/sec to trigger assist (low for high gear ratio)
|
|
int16_t resetThreshold = 30; // Velocity must drop below this to re-trigger
|
|
int16_t leadOffset = 200; // How far ahead to command (position units)
|
|
uint16_t assistSpeed = 1000; // Speed to use when assisting
|
|
unsigned long assistDuration = 500; // ms to assist for
|
|
};
|
|
|
|
struct AidedMotor {
|
|
uint8_t motorID;
|
|
uint16_t lastPosition = 0;
|
|
unsigned long lastUpdateTime = 0;
|
|
|
|
// Per-motor settings
|
|
AidedMotorSettings settings;
|
|
|
|
// Velocity smoothing
|
|
int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
|
|
uint8_t sampleIndex = 0;
|
|
uint8_t samplesCollected = 0; // Track how many samples we've collected
|
|
int16_t smoothedVelocity = 0;
|
|
uint16_t stablePosition = 0; // Position used for delta calculation (filters noise)
|
|
|
|
// Assist state
|
|
bool assisting = false;
|
|
unsigned long assistStartTime = 0;
|
|
uint16_t assistTargetPosition = 0;
|
|
|
|
// Hysteresis - prevents re-triggering until velocity drops
|
|
bool wasTriggered = false;
|
|
|
|
// Warmup - don't trigger until we have enough samples
|
|
bool warmedUp = false;
|
|
|
|
// Consecutive high readings counter (filters noise spikes)
|
|
uint8_t consecutiveHighCount = 0;
|
|
};
|
|
|
|
class MotorAid {
|
|
public:
|
|
// Add a motor with default settings
|
|
void addMotor(uint8_t motorID);
|
|
|
|
// Add a motor with custom settings
|
|
void addMotor(uint8_t motorID, const AidedMotorSettings& settings);
|
|
|
|
// Remove a motor from aided list
|
|
void removeMotor(uint8_t motorID);
|
|
|
|
// Check if a motor is being aided
|
|
bool isMotorAided(uint8_t motorID) const;
|
|
|
|
// Get settings for a motor (to modify)
|
|
AidedMotorSettings* getMotorSettings(uint8_t motorID);
|
|
|
|
// Main update function - call from loop() when motorStream is active
|
|
void update();
|
|
|
|
private:
|
|
std::vector<AidedMotor> aidedMotors;
|
|
unsigned long updateInterval = 30; // ms between position checks
|
|
|
|
AidedMotor* findMotor(uint8_t motorID);
|
|
const AidedMotor* findMotor(uint8_t motorID) const;
|
|
|
|
// Calculate smoothed velocity from samples
|
|
int16_t calculateSmoothedVelocity(AidedMotor& motor);
|
|
};
|
|
|
|
extern MotorAid motorAid;
|