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

implemented warm up time so assist doesn't trigger on startup

main
Jake 2026-01-31 17:23:38 +08:00
parent 9aa8dff1a7
commit e042d2ef62
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
motoraid.cpp
View File

@ -13,7 +13,11 @@ void MotorAid::addMotor(uint8_t motorID) {
AidedMotor motor; AidedMotor motor;
motor.motorID = motorID; motor.motorID = motorID;
motor.lastPosition = config.getMotorPosition(motorID);
// 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.lastUpdateTime = millis(); motor.lastUpdateTime = millis();
// Initialize velocity samples to zero // Initialize velocity samples to zero
@ -27,7 +31,9 @@ void MotorAid::addMotor(uint8_t motorID) {
servoManager[0]->disableTorque(motorID); servoManager[0]->disableTorque(motorID);
Serial.print("[MotorAid] Added motor "); Serial.print("[MotorAid] Added motor ");
Serial.println(motorID); Serial.print(motorID);
Serial.print(" at pos ");
Serial.println(motor.lastPosition);
} }
void MotorAid::removeMotor(uint8_t motorID) { void MotorAid::removeMotor(uint8_t motorID) {
@ -87,6 +93,17 @@ void MotorAid::update() {
motor.velocitySamples[motor.sampleIndex] = instantVelocity; motor.velocitySamples[motor.sampleIndex] = instantVelocity;
motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES; motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES;
// Track samples collected for warmup
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
motor.samplesCollected++;
}
// Don't calculate or trigger until we have enough samples (2x buffer = ~300ms warmup)
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
continue; // Still warming up
}
motor.warmedUp = true;
// Calculate smoothed velocity // Calculate smoothed velocity
motor.smoothedVelocity = calculateSmoothedVelocity(motor); motor.smoothedVelocity = calculateSmoothedVelocity(motor);

6
motoraid.h
View File

@ -16,6 +16,7 @@ struct AidedMotor {
// Velocity smoothing // Velocity smoothing
int16_t velocitySamples[VELOCITY_SAMPLES] = {0}; int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
uint8_t sampleIndex = 0; uint8_t sampleIndex = 0;
uint8_t samplesCollected = 0; // Track how many samples we've collected
int16_t smoothedVelocity = 0; int16_t smoothedVelocity = 0;
// Assist state // Assist state
@ -25,6 +26,9 @@ struct AidedMotor {
// Hysteresis - prevents re-triggering until velocity drops // Hysteresis - prevents re-triggering until velocity drops
bool wasTriggered = false; bool wasTriggered = false;
// Warmup - don't trigger until we have enough samples
bool warmedUp = false;
}; };
class MotorAid { class MotorAid {
@ -51,7 +55,7 @@ private:
std::vector<AidedMotor> aidedMotors; std::vector<AidedMotor> aidedMotors;
// Thresholds and settings // Thresholds and settings
int16_t velocityThreshold = 200; // Position units/sec to trigger assist int16_t velocityThreshold = 300; // Position units/sec to trigger assist
int16_t resetThreshold = 60; // Velocity must drop below this to re-trigger (hysteresis) int16_t resetThreshold = 60; // Velocity must drop below this to re-trigger (hysteresis)
unsigned long assistDuration = 250; // ms to assist for unsigned long assistDuration = 250; // ms to assist for
uint16_t assistSpeed = 600; // Speed to use when assisting uint16_t assistSpeed = 600; // Speed to use when assisting