diff --git a/ls_firmware.ino b/ls_firmware.ino index baf2d5e..b447801 100644 --- a/ls_firmware.ino +++ b/ls_firmware.ino @@ -647,8 +647,22 @@ void setup() { motorStream.start(); - // Initialize motor-aided movement on motor 25 + // Initialize motor-aided movement + // Motor 25 - default settings motorAid.addMotor(25); + + // Motors 30, 31, 35, 36 - high gear ratio, maximum sensitivity + AidedMotorSettings highGearSettings; + highGearSettings.velocityThreshold = 8; // Extremely sensitive trigger + highGearSettings.resetThreshold = 4; // Minimal hysteresis + highGearSettings.leadOffset = 100; // Smaller lead for fine control + highGearSettings.assistSpeed = 600; // Moderate speed + highGearSettings.assistDuration = 400; // Shorter burst + + motorAid.addMotor(30, highGearSettings); + motorAid.addMotor(31, highGearSettings); + motorAid.addMotor(35, highGearSettings); + motorAid.addMotor(36, highGearSettings); Serial.println("[HansonServo] Ready"); Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); diff --git a/motoraid.cpp b/motoraid.cpp index da7e1bb..a19c9c2 100644 --- a/motoraid.cpp +++ b/motoraid.cpp @@ -8,16 +8,24 @@ MotorAid motorAid; extern RobotConfig config; void MotorAid::addMotor(uint8_t motorID) { + // Use default settings + AidedMotorSettings defaultSettings; + addMotor(motorID, defaultSettings); +} + +void MotorAid::addMotor(uint8_t motorID, const AidedMotorSettings& settings) { // Don't add duplicates if (findMotor(motorID) != nullptr) return; AidedMotor motor; motor.motorID = motorID; + motor.settings = settings; // Read actual position directly from servo (not config which may be stale) uint8_t model = config.getMotorModel(motorID); servoManager[0]->setFeetechMode(model); motor.lastPosition = servoManager[0]->getPosition(motorID); + motor.stablePosition = motor.lastPosition; // Initialize stable position motor.lastUpdateTime = millis(); // Initialize velocity samples to zero @@ -33,7 +41,12 @@ void MotorAid::addMotor(uint8_t motorID) { Serial.print("[MotorAid] Added motor "); Serial.print(motorID); Serial.print(" at pos "); - Serial.println(motor.lastPosition); + Serial.print(motor.lastPosition); + Serial.print(" (thresh="); + Serial.print(settings.velocityThreshold); + Serial.print(", lead="); + Serial.print(settings.leadOffset); + Serial.println(")"); } void MotorAid::removeMotor(uint8_t motorID) { @@ -51,6 +64,12 @@ bool MotorAid::isMotorAided(uint8_t motorID) const { return findMotor(motorID) != nullptr; } +AidedMotorSettings* MotorAid::getMotorSettings(uint8_t motorID) { + AidedMotor* motor = findMotor(motorID); + if (motor) return &motor->settings; + return nullptr; +} + AidedMotor* MotorAid::findMotor(uint8_t motorID) { for (auto& motor : aidedMotors) { if (motor.motorID == motorID) return &motor; @@ -78,6 +97,8 @@ void MotorAid::update() { unsigned long now = millis(); for (AidedMotor& motor : aidedMotors) { + const AidedMotorSettings& s = motor.settings; // Shorthand + // Calculate time delta unsigned long deltaTime = now - motor.lastUpdateTime; if (deltaTime < updateInterval) continue; @@ -85,9 +106,17 @@ void MotorAid::update() { // Get current position from config (already updated by updateMotorPositions) uint16_t currentPosition = config.getMotorPosition(motor.motorID); - // Calculate instantaneous velocity (position units per second) - int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.lastPosition; - int16_t instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime; + // Calculate position delta from stable position (filters noise) + int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.stablePosition; + + // Only register movement if it exceeds minimum delta (noise filter) + int16_t instantVelocity = 0; + if (abs(positionDelta) >= MIN_POSITION_DELTA) { + // Significant movement - calculate velocity and update stable position + instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime; + motor.stablePosition = currentPosition; + } + // else: tiny movement, treat as zero velocity (noise) // Add to velocity samples (circular buffer) motor.velocitySamples[motor.sampleIndex] = instantVelocity; @@ -108,13 +137,12 @@ void MotorAid::update() { motor.smoothedVelocity = calculateSmoothedVelocity(motor); // Update tracking - motor.lastPosition = currentPosition; motor.lastUpdateTime = now; // Check if currently assisting if (motor.assisting) { // Calculate target position AHEAD of current position in direction of movement - int16_t leadOffset = (motor.smoothedVelocity > 0) ? 150 : -150; + int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset; motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095); // Send updated position command to lead the movement @@ -123,16 +151,11 @@ void MotorAid::update() { uint8_t ids[1] = { motor.motorID }; uint16_t positions[1] = { motor.assistTargetPosition }; - uint16_t speeds[1] = { assistSpeed }; + uint16_t speeds[1] = { s.assistSpeed }; servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0); - // Extend assist if still moving fast - if (abs(motor.smoothedVelocity) > resetThreshold) { - motor.assistStartTime = now; // Keep extending - } - - // Check if assist duration has elapsed (only ends when movement stops) - if (now - motor.assistStartTime >= assistDuration) { + // Check if assist duration has elapsed (fixed duration, always ends) + if (now - motor.assistStartTime >= s.assistDuration) { motor.assisting = false; // Disable torque again servoManager[0]->disableTorque(motor.motorID); @@ -147,21 +170,29 @@ void MotorAid::update() { // Hysteresis: if we were triggered, wait for velocity to drop before re-triggering if (motor.wasTriggered) { - if (abs(motor.smoothedVelocity) < resetThreshold) { + if (abs(motor.smoothedVelocity) < s.resetThreshold) { motor.wasTriggered = false; // Reset, can trigger again + motor.consecutiveHighCount = 0; // Reset consecutive counter too } continue; // Don't trigger while in hysteresis zone } // Check if smoothed velocity exceeds threshold (in either direction) - if (abs(motor.smoothedVelocity) > velocityThreshold) { - // Someone is moving the motor - assist! + if (abs(motor.smoothedVelocity) > s.velocityThreshold) { + motor.consecutiveHighCount++; + } else { + motor.consecutiveHighCount = 0; // Reset on low reading + } + + // Only trigger after CONSECUTIVE_REQUIRED high readings (filters noise spikes) + if (motor.consecutiveHighCount >= CONSECUTIVE_REQUIRED) { + // Sustained movement detected - assist! motor.assisting = true; motor.assistStartTime = now; motor.wasTriggered = true; // Calculate target position AHEAD of current position in direction of movement - int16_t leadOffset = (motor.smoothedVelocity > 0) ? 150 : -150; + int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset; motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095); // Set servo mode based on motor model @@ -173,7 +204,7 @@ void MotorAid::update() { uint8_t ids[1] = { motor.motorID }; uint16_t positions[1] = { motor.assistTargetPosition }; - uint16_t speeds[1] = { assistSpeed }; + uint16_t speeds[1] = { s.assistSpeed }; servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0); // Debug output diff --git a/motoraid.h b/motoraid.h index 2cf0301..7c2fd2f 100644 --- a/motoraid.h +++ b/motoraid.h @@ -6,18 +6,32 @@ // 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 +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; @@ -29,36 +43,33 @@ struct AidedMotor { // 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 to be aided + // 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(); - // 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 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);