func too complex

Opentrons · Sep 17, 2024 · 2e2e270 · 2e2e270
1 parent 6ad1b09
commit 2e2e270
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 22 deletions.
diff --git 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
@@ -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;