realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

motor assist works on feet now. a little over sensitive

main
Jake 2026-01-31 17:59:56 +08:00
parent e042d2ef62
commit b35a384974
3 changed files with 90 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ls_firmware.ino
View File

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

69
motoraid.cpp
View File

@ -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

39
motoraid.h
View File

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