test working reasonably well on motor 25(right arm)

feetech.cpp

@@ -377,11 +377,16 @@ uint8_t Feetech::getMoving(uint8_t id) {
   return waitOnData1Byte(10);
 }
 
-// Multiplier could be wrong
+// STS/SMS: actual current at 0x45, SCS: use load at 0x3C as proxy
 uint16_t Feetech::getCurrent(uint8_t id) {
+  if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
     sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
-  //return waitOnData2Bytes(10) * 0.01; FLOAT
     return waitOnData2Bytes(10);
+  } else {
+    // SCS doesn't have current register, use load as proxy
+    sendRequest(id, REQUEST_CURRENT_LOAD, 2);
+    return flipBytes(waitOnData2Bytes(10));
+  }
 }
 
 void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {

ls_firmware.ino

@@ -19,6 +19,7 @@
 #include "animation.h"
 #include "commands.h"
 #include "motorcontrol.h"
+#include "motoraid.h"
 #include "nodegraph.h"
 #include "protocol.h"
 #include "sensors.h"
@@ -459,15 +460,46 @@ void updateMotorPositions() {
     servoManager[0]->setFeetechMode(motor.servoModel.major);
     uint16_t position = servoManager[0]->getPosition(motor.motorID);
     config.setMotorPosition(motor.motorID, position);
+    uint16_t current = servoManager[0]->getCurrent(motor.motorID);
+    config.setMotorCurrent(motor.motorID, current);
   }
 }
 
+void printMotorStats() {
+  Serial.println("--- Motor stats ---");
+  for (const Motor& motor : config.motors) {
+    Serial.print("ID:");
+    Serial.print(motor.motorID);
+    Serial.print(" pos:");
+    Serial.print(motor.position);
+    Serial.print(" cur:");
+    Serial.println(motor.current);
+  }
+  Serial.println("-------------------");
+}
+
 void handleMotorStreaming() {
   if (motorStream.shouldStream()) {
     sendMotorPositions();
   }
 }
 
+void printLoopRate() {
+  static unsigned long lastPrint = 0;
+  static uint32_t loopCount = 0;
+  
+  loopCount++;
+  
+  unsigned long now = millis();
+  if (now - lastPrint >= 1000) {
+    Serial.print("[Loop] ");
+    Serial.print(loopCount);
+    Serial.println(" Hz");
+    loopCount = 0;
+    lastPrint = now;
+  }
+}
+
 // ============================================================================
 // Heartbeat
 // ============================================================================
@@ -613,6 +645,11 @@ void setup() {
   }
   idleBehavior.initMotors(allMotorIDs);
 
+  motorStream.start();
+  
+  // Initialize motor-aided movement on motor 25
+  motorAid.addMotor(25);
+
   Serial.println("[HansonServo] Ready");
   Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
 
@@ -666,11 +703,19 @@ void loop() {
   updateMotorPositions();
   handleMotorStreaming();
   
+  // Motor-aided movement (when motorStream is active)
+  if (motorStream.active) {
+    motorAid.update();
+  }
+  //printMotorStats();
   // Sensor updates and streaming
   //sensors.update();
 
   // Heartbeat
-  sendHeartbeat();
+  //sendHeartbeat();
+  
+  // Debug: print loop rate
+  printLoopRate();
   
   // ============================================================================
   // TEST: Uncomment to enable motor 40 sweep test

motoraid.cpp

@@ -0,0 +1,173 @@
+#include "motoraid.h"
+#include "motorcontrol.h"
+#include "robotconfig.h"
+
+// Global instance
+MotorAid motorAid;
+
+extern RobotConfig config;
+
+void MotorAid::addMotor(uint8_t motorID) {
+  // Don't add duplicates
+  if (findMotor(motorID) != nullptr) return;
+  
+  AidedMotor motor;
+  motor.motorID = motorID;
+  motor.lastPosition = config.getMotorPosition(motorID);
+  motor.lastUpdateTime = millis();
+  
+  // Initialize velocity samples to zero
+  for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
+    motor.velocitySamples[i] = 0;
+  }
+  
+  aidedMotors.push_back(motor);
+  
+  // Disable torque on this motor so it can be moved freely
+  servoManager[0]->disableTorque(motorID);
+  
+  Serial.print("[MotorAid] Added motor ");
+  Serial.println(motorID);
+}
+
+void MotorAid::removeMotor(uint8_t motorID) {
+  for (auto it = aidedMotors.begin(); it != aidedMotors.end(); ++it) {
+    if (it->motorID == motorID) {
+      aidedMotors.erase(it);
+      Serial.print("[MotorAid] Removed motor ");
+      Serial.println(motorID);
+      return;
+    }
+  }
+}
+
+bool MotorAid::isMotorAided(uint8_t motorID) const {
+  return findMotor(motorID) != nullptr;
+}
+
+AidedMotor* MotorAid::findMotor(uint8_t motorID) {
+  for (auto& motor : aidedMotors) {
+    if (motor.motorID == motorID) return &motor;
+  }
+  return nullptr;
+}
+
+const AidedMotor* MotorAid::findMotor(uint8_t motorID) const {
+  for (const auto& motor : aidedMotors) {
+    if (motor.motorID == motorID) return &motor;
+  }
+  return nullptr;
+}
+
+int16_t MotorAid::calculateSmoothedVelocity(AidedMotor& motor) {
+  // Calculate average of velocity samples
+  int32_t sum = 0;
+  for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
+    sum += motor.velocitySamples[i];
+  }
+  return (int16_t)(sum / VELOCITY_SAMPLES);
+}
+
+void MotorAid::update() {
+  unsigned long now = millis();
+  
+  for (AidedMotor& motor : aidedMotors) {
+    // Calculate time delta
+    unsigned long deltaTime = now - motor.lastUpdateTime;
+    if (deltaTime < updateInterval) continue;
+    
+    // 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;
+    
+    // Add to velocity samples (circular buffer)
+    motor.velocitySamples[motor.sampleIndex] = instantVelocity;
+    motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES;
+    
+    // Calculate smoothed velocity
+    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;
+      motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
+      
+      // Send updated position command to lead the movement
+      uint8_t model = config.getMotorModel(motor.motorID);
+      servoManager[0]->setFeetechMode(model);
+      
+      uint8_t ids[1] = { motor.motorID };
+      uint16_t positions[1] = { motor.assistTargetPosition };
+      uint16_t speeds[1] = { 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) {
+        motor.assisting = false;
+        // Disable torque again
+        servoManager[0]->disableTorque(motor.motorID);
+        
+        Serial.print("[MotorAid] Assist ended, motor ");
+        Serial.print(motor.motorID);
+        Serial.print(" at pos ");
+        Serial.println(currentPosition);
+      }
+      continue;
+    }
+    
+    // Hysteresis: if we were triggered, wait for velocity to drop before re-triggering
+    if (motor.wasTriggered) {
+      if (abs(motor.smoothedVelocity) < resetThreshold) {
+        motor.wasTriggered = false;  // Reset, can trigger again
+      }
+      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!
+      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;
+      motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
+      
+      // Set servo mode based on motor model
+      uint8_t model = config.getMotorModel(motor.motorID);
+      servoManager[0]->setFeetechMode(model);
+      
+      // Enable torque and move AHEAD to assist
+      servoManager[0]->enableTorque(motor.motorID);
+      
+      uint8_t ids[1] = { motor.motorID };
+      uint16_t positions[1] = { motor.assistTargetPosition };
+      uint16_t speeds[1] = { assistSpeed };
+      servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
+      
+      // Debug output
+      Serial.print("[MotorAid] Assisting motor ");
+      Serial.print(motor.motorID);
+      Serial.print(" vel=");
+      Serial.print(motor.smoothedVelocity);
+      Serial.print(" pos=");
+      Serial.print(currentPosition);
+      Serial.print(" -> ");
+      Serial.println(motor.assistTargetPosition);
+    }
+  }
+}

motoraid.h

@@ -0,0 +1,67 @@
+#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;
+  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;
+};
+
+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 = 200;     // 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;

robotconfig.cpp

@@ -12,6 +12,15 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
   return 2047;
 }
 
+uint16_t RobotConfig::getMotorCurrent(uint8_t motorID) const {
+  for (const Motor& motor : motors) {
+    if (motor.motorID == motorID) {
+      return motor.current;
+    }
+  }
+  return 0;
+}
+
 uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
   for (const Motor& motor : motors) {
     if (motor.motorID == motorID) {
@@ -33,6 +42,16 @@ bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
   return false;
 }
 
+bool RobotConfig::setMotorCurrent(uint8_t motorID, uint16_t newCurrent) {
+  for (Motor& motor : motors) {
+    if (motor.motorID == motorID) {
+      motor.current = newCurrent;
+      return true;
+    }
+  }
+  return false;
+}
+
 bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
   for (Motor& motor : motors) {
     if (motor.motorID == motorID) {

robotconfig.h

@@ -61,6 +61,7 @@ struct Motor {
   uint8_t motorID;
   ServoModel servoModel;
   uint16_t position;
+  uint16_t current = 0;
   bool isEnabled = true;
 };
 
@@ -70,8 +71,10 @@ struct RobotConfig {
   std::vector<Motor> motors;
 
   uint16_t getMotorPosition(uint8_t motorID) const;
+  uint16_t getMotorCurrent(uint8_t motorID) const;
   uint8_t getMotorModel(uint8_t motorID);
   bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
+  bool setMotorCurrent(uint8_t motorID, uint16_t newCurrent);
   bool setMotorEnabled(uint8_t motorID, bool enable);
   bool isMotorEnabled(uint8_t motorID);
   void enableAllMotors();