From 96b28f440d42a9945510ee92d08f04cb73c3553c Mon Sep 17 00:00:00 2001
From: Zybkin Pavel <xruss08@gmail.com>
Date: Tue, 5 Dec 2017 22:34:52 +0300
Subject: [PATCH] added move and reset to multistepper

---
 src/MultiStepper.cpp | 38 ++++++++++++++++++++++++++++++++++++++
 src/MultiStepper.h   | 12 ++++++++++++
 2 files changed, 50 insertions(+)

diff --git a/src/MultiStepper.cpp b/src/MultiStepper.cpp
index de0ec94..29e2d97 100644
--- a/src/MultiStepper.cpp
+++ b/src/MultiStepper.cpp
@@ -48,6 +48,44 @@ void MultiStepper::moveTo(long absolute[])
     }
 }
 
+void MultiStepper::reset(){
+    uint8_t i;
+    for (i = 0; i < _num_steppers; i++)
+    {
+        _steppers[i]->setCurrentPosition(0);
+    }
+}
+
+void MultiStepper::move(long relative[])
+{
+    // First find the stepper that will take the longest time to move
+    float longestTime = 0.0;
+
+    uint8_t i;
+    for (i = 0; i < _num_steppers; i++)
+    {
+        float thisTime = abs(relative[i]) / _steppers[i]->maxSpeed();
+
+        if (thisTime > longestTime)
+            longestTime = thisTime;
+    }
+
+    if (longestTime > 0.0)
+    {
+        // Now work out a new max speed for each stepper so they will all
+        // arrived at the same time of longestTime
+        for (i = 0; i < _num_steppers; i++)
+        {
+            float thisSpeed = relative[i] / longestTime;
+            _steppers[i]->move(relative[i]); // New target position (resets speed)
+            _steppers[i]->setSpeed(thisSpeed); // New speed
+        }
+    }
+}
+
+
+
+
 // Returns true if any motor is still running to the target position.
 boolean MultiStepper::run()
 {
diff --git a/src/MultiStepper.h b/src/MultiStepper.h
index d801bb0..51357b6 100644
--- a/src/MultiStepper.h
+++ b/src/MultiStepper.h
@@ -50,6 +50,18 @@ class MultiStepper
     /// the absolute position of the first stepper added by addStepper() etc. The array must be at least as long as 
     /// the number of steppers that have been added by addStepper, else results are undefined.
     void moveTo(long absolute[]);
+   
+    /// Set the target positions of all managed steppers 
+    /// according to a coordinate array.
+    /// New speeds will be computed for each stepper so they will all arrive at their 
+    /// respective targets at very close to the same time.
+    /// \param[in] relative An array of desired relative stepper positions. relative[0] will be used to set
+    /// the relative position of the first stepper added by addStepper() etc. The array must be at least as long as 
+    /// the number of steppers that have been added by addStepper, else results are undefined.
+    void move(long relative[]);
+    
+    /// Sets current position to zero for all steppers and stops them. 
+    void reset();
     
     /// Calls runSpeed() on all the managed steppers
     /// that have not acheived their target position.