motor assist works on feet now. a little over sensitive

ls_firmware.ino

@@ -647,9 +647,23 @@ 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");
 

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)
+    // Calculate position delta from stable position (filters noise)
-    int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.lastPosition;
+    int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.stablePosition;
-    int16_t instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime;
+    
+    // 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
+      // Check if assist duration has elapsed (fixed duration, always ends)
-      if (abs(motor.smoothedVelocity) > resetThreshold) {
+      if (now - motor.assistStartTime >= s.assistDuration) {
-        motor.assistStartTime = now;  // Keep extending
-      }
-      
-      // Check if assist duration has elapsed (only ends when movement stops)
-      if (now - motor.assistStartTime >= 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) {
+    if (abs(motor.smoothedVelocity) > s.velocityThreshold) {
-      // Someone is moving the motor - assist!
+      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

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<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);