Select stepper parameters vMax, acceleration, pullin and pullout more…

… optimal when running multiple stepper in sync

src/StepControlBase.h

@@ -62,6 +62,7 @@ namespace TeensyStep
      protected:
         void accTimerISR();
 
+        uint32_t adjustToSlaveLimit(float reduceFactor, uint32_t stepperMax, uint32_t current);
         void doMove(int N, float speedOverride = 1.0f);
 
         Accelerator accelerator;
@@ -79,6 +80,12 @@ namespace TeensyStep
         this->mode = MotorControlBase<t>::Mode::target;
     }
 
+    template <typename a, typename t>
+    uint32_t StepControlBase<a, t>::adjustToSlaveLimit(float reduceFactor, uint32_t stepperMax, uint32_t current) {
+        uint32_t real = reduceFactor * current;
+        return real > stepperMax ? (stepperMax / reduceFactor) : current;
+    }
+
     template <typename a, typename t>
     void StepControlBase<a, t>::doMove(int N, float speedOverride)
     {
@@ -92,30 +99,31 @@ namespace TeensyStep
         }
 
         // Calculate acceleration parameters --------------------------------
-        uint32_t pullInSpeed = this->leadMotor->vPullIn;
-        uint32_t pullOutSpeed = this->leadMotor->vPullOut;
-        uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move
-
-        uint32_t targetSpeed = std::abs((*std::min_element(this->motorList, this->motorList + N, Stepper::cmpVmin))->vMax) * speedOverride; // use the lowest max frequency for the move, scale by relSpeed
-        if (this->leadMotor->A == 0 || targetSpeed == 0) return;
+        uint32_t targetPullInSpeed = this->leadMotor->vPullIn;
+        uint32_t targetPullOutSpeed = this->leadMotor->vPullOut;
+        uint32_t targetAcceleration = this->leadMotor->a;
+        uint32_t targetSpeed = this->leadMotor->vMax * speedOverride;
 
         // target speed----
+        if (this->leadMotor->A == 0 || targetSpeed == 0) return;
 
-        float x = 0;
-        float leadSpeed = std::abs(this->leadMotor->vMax);
-        for (int i = 0; i < N; i++)
+        //Serial.printf("targetPullInSpeed: %u, targetPullOutSpeed: %u, targetAcceleration: %u, targetSpeed: %u\n", targetPullInSpeed, targetPullOutSpeed, targetAcceleration, targetSpeed);
+        for (int i = 1; i < N; i++)
         {
-            float relDist = this->motorList[i]->A / (float)this->leadMotor->A * leadSpeed / std::abs(this->motorList[i]->vMax);
-            if (relDist > x) x = relDist;
-           // Serial.printf("%d %f\n", i, relDist);
+            if(this->motorList[i]->A == 0) continue;
+
+            float reduceFactor = this->motorList[i]->A / (float)this->leadMotor->A;
+            targetPullInSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vPullIn, targetPullInSpeed);
+            targetPullOutSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vPullOut, targetPullOutSpeed);
+            targetAcceleration = adjustToSlaveLimit(reduceFactor, this->motorList[i]->a, targetAcceleration);
+            targetSpeed = adjustToSlaveLimit(reduceFactor, this->motorList[i]->vMax, targetSpeed);
         }
-        targetSpeed = leadSpeed / x;
-        //Serial.printf("\n%d\n",targetSpeed);
+        //Serial.printf("targetPullInSpeed: %u, targetPullOutSpeed: %u, targetAcceleration: %u, targetSpeed: %u\n", targetPullInSpeed, targetPullOutSpeed, targetAcceleration, targetSpeed);
 
         // Start move--------------------------
         this->timerField.begin();
 
-        this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, pullInSpeed, pullOutSpeed, acceleration));
+        this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, targetPullInSpeed, targetPullOutSpeed, targetAcceleration));
         this->timerField.stepTimerStart();
         this->timerField.accTimerStart();
     }