From 2e2e270281c23c3193419a4d1a4d218076c918c2 Mon Sep 17 00:00:00 2001
From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com>
Date: Tue, 17 Sep 2024 16:22:04 -0400
Subject: [PATCH] func too complex

---
 .../flex-stacker/src/motor_utils.cpp          | 54 +++++++++++--------
 .../flex-stacker/flex-stacker/motor_utils.hpp |  2 +
 2 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/stm32-modules/flex-stacker/src/motor_utils.cpp b/stm32-modules/flex-stacker/src/motor_utils.cpp
index a123df90..faa13a37 100644
--- a/stm32-modules/flex-stacker/src/motor_utils.cpp
+++ b/stm32-modules/flex-stacker/src/motor_utils.cpp
@@ -45,28 +45,7 @@ auto MovementProfile::reset() -> void {
 auto MovementProfile::tick() -> TickReturn {
     bool step = false;
     if (_type == MovementType::FixedDistance) {
-        // Acceleration phase (until half of the profile is reached)
-        if (_velocity < _peak_velocity &&
-            _current_distance < _target_distance / 2) {
-            _velocity += _acceleration;
-            if (_velocity > _peak_velocity) {
-                _velocity = _peak_velocity;
-                // we're done accelerating
-                _accel_distance = _current_distance;
-            }
-        } else {
-            if (_accel_distance == 0) {
-                _accel_distance = _current_distance;
-            }
-            if (remaining_distance() <= _accel_distance) {
-                // Deceleration phase (after more than half of the target
-                // distance is covered)
-                _velocity -= _acceleration;
-                if (_velocity < _start_velocity) {
-                    _velocity = _start_velocity;
-                }
-            }
-        }
+        fixed_distance_tick();
     } else {
         // TODO: re-evaluate if we need to accelerate during open loop movement
         // Acceleration gets clamped to _peak_velocity
@@ -91,6 +70,37 @@ auto MovementProfile::tick() -> TickReturn {
                       .step = step};
 }
 
+auto MovementProfile::fixed_distance_tick() -> void {
+    // Acceleration phase
+    // 1. when the velocity hasn't reached peak value
+    // 2. traveled distance is less than half of the target
+    if (_velocity < _peak_velocity &&
+        _current_distance < _target_distance / 2) {
+        _velocity += _acceleration;
+        if (_velocity > _peak_velocity) {
+            _velocity = _peak_velocity;
+            // we're done accelerating, record the distance traveled,
+            // this will be used to determine when to start decelerating
+            _accel_distance = _current_distance;
+        }
+    } else {
+        // no longer accelerating
+        if (_accel_distance == 0) {
+            // if we haven't set the acceleration distance, set it now
+            _accel_distance = _current_distance;
+        }
+        // Deceleration phase
+        // when the remaining distance is equal/less than
+        // the accelerating distance
+        if (remaining_distance() <= _accel_distance) {
+            _velocity -= _acceleration;
+            if (_velocity < _start_velocity) {
+                _velocity = _start_velocity;
+            }
+        }
+    }
+}
+
 [[nodiscard]] auto MovementProfile::current_velocity() const -> steps_per_tick {
     return _velocity;
 }
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp
index dd828902..e48ce24c 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp
@@ -82,6 +82,8 @@ class MovementProfile {
      */
     auto tick() -> TickReturn __attribute__((optimize(3)));
 
+    auto fixed_distance_tick() -> void;
+
     /** Returns the current motor velocity in steps_per_tick.*/
     [[nodiscard]] auto current_velocity() const -> steps_per_tick;