move motor at frequency works

Opentrons · Aug 28, 2024 · 9e076b1 · 9e076b1
1 parent 452ef0d
commit 9e076b1
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 10 deletions.
diff --git a/stm32-modules/flex-stacker/src/motor_utils.cpp b/stm32-modules/flex-stacker/src/motor_utils.cpp
@@ -75,3 +75,8 @@ auto MovementProfile::tick() -> TickReturn {
 [[nodiscard]] auto MovementProfile::current_distance() const -> ticks {
     return _current_distance;
 }
+
+/** Returns the movement type.*/
+[[nodiscard]] auto MovementProfile::movement_type() const -> MovementType {
+    return _type;
+}
diff --git a/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp b/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp
@@ -1,4 +1,5 @@
 #pragma once
+#include <atomic>
 
 #include "firmware/motor_hardware.h"
 #include "firmware/motor_policy.hpp"
@@ -17,19 +18,24 @@ static constexpr double DEFAULT_ACCEL = 50000;     // steps per second^2
 
 class MotorInterruptController {
   public:
-    MotorInterruptController(MotorID id, MotorPolicy* policy)
+    explicit MotorInterruptController(MotorID id, MotorPolicy* policy)
         : _id(id),
           _policy(policy),
           _initialized(false),
           _profile(TIMER_FREQ, 0, 0, 0, motor_util::MovementType::OpenLoop, 0) {
     }
+    MotorInterruptController(MotorInterruptController const&) = delete;
+    void operator=(MotorInterruptController const&) = delete;
+    MotorInterruptController(MotorInterruptController const&&) = delete;
+    void operator=(MotorInterruptController const&&) = delete;
+    ~MotorInterruptController() = default;
 
     auto tick() -> bool {
         if (!_initialized) {
             return false;
         }
         auto ret = _profile.tick();
-        if (ret.step) {
+        if (ret.step && !stop_condition_met()) {
             _policy->step(_id);
         }
         if (ret.done) {
@@ -38,7 +44,10 @@ class MotorInterruptController {
         return ret.done;
     }
     auto set_freq(uint32_t freq) -> void { step_freq = freq; }
-    auto initialize(MotorPolicy* policy) -> void { _policy = policy; }
+    auto initialize(MotorPolicy* policy) -> void {
+        _policy = policy;
+        _initialized = true;
+    }
     auto start_movement(uint32_t id, long steps, uint32_t frequency) -> void {
         _profile = motor_util::MovementProfile(
             TIMER_FREQ, 0, frequency, 0,
@@ -48,20 +57,30 @@ class MotorInterruptController {
     }
     auto set_direction(bool direction) -> void {
         _policy->set_direction(_id, direction);
+        _direction = direction;
+    }
+    auto limit_switch_triggered() -> bool {
+        return _policy->check_limit_switch(_id, _direction);
     }
-
     [[nodiscard]] auto get_response_id() const -> uint32_t {
         return _response_id;
     }
+    auto stop_condition_met() -> bool {
+        if (_profile.movement_type() == motor_util::MovementType::OpenLoop) {
+            return limit_switch_triggered();
+        }
+        return false;
+    }
 
   private:
     MotorID _id;
     MotorPolicy* _policy;
-    bool _initialized;
+    std::atomic_bool _initialized;
     motor_util::MovementProfile _profile;
     uint32_t step_count = 0;
     uint32_t step_freq = DEFAULT_MOTOR_FREQ;
     uint32_t _response_id = 0;
+    bool _direction = false;
 };
 
 }  // namespace motor_interrupt_controller
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
@@ -144,9 +144,9 @@ class MotorTask {
     auto visit_message(const messages::MoveMotorAtFrequencyMessage& m,
                        Policy& policy) -> void {
         static_cast<void>(policy);
-        auto controller = controller_from_id(m.motor_id);
-        controller.set_direction(m.direction);
-        controller.start_movement(m.id, m.steps, m.frequency);
+//        auto controller = controller_from_id(m.motor_id);
+        controller_from_id(m.motor_id).set_direction(m.direction);
+        controller_from_id(m.motor_id).start_movement(m.id, m.steps, m.frequency);
     }
 
     template <MotorControlPolicy Policy>
@@ -160,9 +160,8 @@ class MotorTask {
     auto visit_message(const messages::MoveCompleteMessage& m, Policy& policy)
         -> void {
         static_cast<void>(policy);
-        auto controller = controller_from_id(m.motor_id);
         auto response = messages::AcknowledgePrevious{
-            .responding_to_id = controller.get_response_id()};
+            .responding_to_id = controller_from_id(m.motor_id).get_response_id()};
         static_cast<void>(_task_registry->send_to_address(
             response, Queues::HostCommsAddress));
     }

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp
@@ -91,6 +91,9 @@ class MovementProfile {
     /** Returns the number of ticks that have been taken.*/
     [[nodiscard]] auto current_distance() const -> ticks;
 
+    /** Returns the movement type.*/
+    [[nodiscard]] auto movement_type() const -> MovementType;
+
   private:
     uint32_t _ticks_per_second;           // Tick frequency
     steps_per_tick _velocity = 0;         // Current velocity