72 lines
2.3 KiB
C++
72 lines
2.3 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 = 5; // Number of samples for velocity smoothing
|
|
|
|
struct AidedMotor {
|
|
uint8_t motorID;
|
|
uint16_t lastPosition = 0;
|
|
unsigned long lastUpdateTime = 0;
|
|
|
|
// 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;
|
|
|
|
// 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;
|
|
};
|
|
|
|
class MotorAid {
|
|
public:
|
|
// Add a motor to be aided
|
|
void addMotor(uint8_t motorID);
|
|
|
|
// Remove a motor from aided list
|
|
void removeMotor(uint8_t motorID);
|
|
|
|
// Check if a motor is being aided
|
|
bool isMotorAided(uint8_t motorID) const;
|
|
|
|
// Main update function - call from loop() when motorStream is active
|
|
void update();
|
|
|
|
// Configuration
|
|
void setVelocityThreshold(int16_t threshold) { velocityThreshold = threshold; }
|
|
void setResetThreshold(int16_t threshold) { resetThreshold = threshold; }
|
|
void setAssistDuration(unsigned long duration) { assistDuration = duration; }
|
|
void setAssistSpeed(uint16_t speed) { assistSpeed = speed; }
|
|
|
|
private:
|
|
std::vector<AidedMotor> aidedMotors;
|
|
|
|
// Thresholds and settings
|
|
int16_t velocityThreshold = 300; // Position units/sec to trigger assist
|
|
int16_t resetThreshold = 60; // Velocity must drop below this to re-trigger (hysteresis)
|
|
unsigned long assistDuration = 250; // ms to assist for
|
|
uint16_t assistSpeed = 600; // Speed to use when assisting
|
|
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;
|