diff --git a/motoraid.cpp b/motoraid.cpp
index 2db3444..da7e1bb 100644
--- a/motoraid.cpp
+++ b/motoraid.cpp
@@ -13,7 +13,11 @@ void MotorAid::addMotor(uint8_t motorID) {
   
   AidedMotor motor;
   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();
   
   // Initialize velocity samples to zero
@@ -27,7 +31,9 @@ void MotorAid::addMotor(uint8_t motorID) {
   servoManager[0]->disableTorque(motorID);
   
   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) {
@@ -87,6 +93,17 @@ void MotorAid::update() {
     motor.velocitySamples[motor.sampleIndex] = instantVelocity;
     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
     motor.smoothedVelocity = calculateSmoothedVelocity(motor);
     
diff --git a/motoraid.h b/motoraid.h
index 8813a4d..2cf0301 100644
--- a/motoraid.h
+++ b/motoraid.h
@@ -16,6 +16,7 @@ struct AidedMotor {
   // 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;
   
   // Assist state
@@ -25,6 +26,9 @@ struct AidedMotor {
   
   // Hysteresis - prevents re-triggering until velocity drops
   bool wasTriggered = false;
+  
+  // Warmup - don't trigger until we have enough samples
+  bool warmedUp = false;
 };
 
 class MotorAid {
@@ -51,7 +55,7 @@ private:
   std::vector<AidedMotor> aidedMotors;
   
   // 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)
   unsigned long assistDuration = 250;  // ms to assist for
   uint16_t assistSpeed = 600;          // Speed to use when assisting