feat(flex-stacker): implement home motor sequence (#475)

Opentrons · Nov 19, 2024 · 89f15dd · 89f15dd
1 parent bc59475
commit 89f15dd
Show file tree

Hide file tree

Showing 9 changed files with 369 additions and 88 deletions.
diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp
@@ -23,6 +23,11 @@ const char* const TMC2160_INVALID_VALUE = "ERR904:TMC2160 invalid value\n";
 const char* const MOTOR_ENABLE_FAILED = "ERR401:motor enable error\n";
 const char* const MOTOR_DISABLE_FAILED = "ERR402:motor disable error\n";
 const char* const MOTOR_STALL_DETECTED = "ERR403:motor stall error\n";
+const char* const MOTOR_QUEUE_FULL = "ERR404:motor queue full error\n";
+
+const char* const X_MOTOR_BUSY = "ERR501:X motor busy error\n";
+const char* const Z_MOTOR_BUSY = "ERR502:Z motor busy error\n";
+const char* const L_MOTOR_BUSY = "ERR503:L motor busy error\n";
 
 const char* const UNKNOWN_ERROR = "ERR-1:unknown error code\n";
 
@@ -49,6 +54,10 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(MOTOR_ENABLE_FAILED);
         HANDLE_CASE(MOTOR_DISABLE_FAILED);
         HANDLE_CASE(MOTOR_STALL_DETECTED);
+        HANDLE_CASE(MOTOR_QUEUE_FULL);
+        HANDLE_CASE(X_MOTOR_BUSY);
+        HANDLE_CASE(Z_MOTOR_BUSY);
+        HANDLE_CASE(L_MOTOR_BUSY);
     }
     return UNKNOWN_ERROR;
 }
diff --git a/stm32-modules/include/common/core/circular_buffer.hpp b/stm32-modules/include/common/core/circular_buffer.hpp
@@ -0,0 +1,69 @@
+#pragma once
+
+namespace circular_buffer {
+
+template <typename T, std::size_t MaxSize>
+class CircularBuffer {
+  public:
+    using BackingStore = std::array<T, MaxSize>;
+
+    explicit CircularBuffer(bool allow_overwrite = false)
+        : _buffer(BackingStore()), _overwrite(allow_overwrite) {}
+
+    CircularBuffer(CircularBuffer& other) = delete;
+    auto operator=(CircularBuffer& other) -> CircularBuffer& = delete;
+    CircularBuffer(CircularBuffer&& other) noexcept = delete;
+    auto operator=(CircularBuffer&& other) noexcept -> CircularBuffer& = delete;
+    ~CircularBuffer() = default;
+
+    [[nodiscard]] auto empty() const -> bool { return _count == 0; }
+
+    [[nodiscard]] auto full() const -> bool { return _count == MaxSize; }
+
+    [[nodiscard]] auto capacity() const -> std::size_t { return MaxSize; }
+
+    [[nodiscard]] auto size() const -> std::size_t { return _count; }
+
+    auto enqueue(T item) -> bool {
+        if (!_overwrite && full()) {
+            return false;
+        }
+
+        _buffer.at(_tail) = item;
+        _tail = (_tail + 1) % MaxSize;
+
+        if (full() && _overwrite) {
+            // overwrite the oldest item
+            _head = (_head + 1) % MaxSize;
+        } else if (!full()) {
+            _count++;
+        }
+        return true;
+    }
+
+    auto dequeue(T& item) -> bool {
+        if (empty()) {
+            return false;
+        }
+        item = _buffer.at(_head);
+        _head = (_head + 1) % MaxSize;
+        _count--;
+
+        return true;
+    }
+
+    auto reset() -> void {
+        _head = 0;
+        _tail = 0;
+        _count = 0;
+    }
+
+  private:
+    BackingStore _buffer;
+    bool _overwrite;
+    std::size_t _head = 0;
+    std::size_t _tail = 0;
+    std::size_t _count = 0;
+};
+
+}  // namespace circular_buffer
diff --git a/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp b/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp
@@ -12,10 +12,16 @@ namespace motor_interrupt_controller {
 using MotorPolicy = motor_policy::MotorPolicy;
 
 static constexpr int TIMER_FREQ = 100000;
-static constexpr int DEFAULT_MOTOR_FREQ = 50;
 
-static constexpr double DEFAULT_VELOCITY = 64000;  // steps per second
-static constexpr double DEFAULT_ACCEL = 50000;     // steps per second^2
+struct Move {
+    MotorID motor_id;
+    motor_util::MotorState* motor_state;
+    uint32_t move_id;
+    bool direction;
+    float distance;
+    bool limit_switch;
+    bool has_next_move;
+};
 
 class MotorInterruptController {
   public:
@@ -44,12 +50,11 @@ class MotorInterruptController {
             if (_id == MotorID::MOTOR_Z) {
                 _policy->disable_motor(_id);
             }
+            _stop = true;
             return true;
         }
         return ret.done;
     }
-
-    auto set_freq(uint32_t freq) -> void { step_freq = freq; }
     auto initialize(MotorPolicy* policy) -> void {
         _policy = policy;
         _initialized = true;
@@ -66,16 +71,20 @@ class MotorInterruptController {
         _policy->enable_motor(_id);
         _response_id = move_id;
     }
-    auto start_movement(uint32_t move_id, bool direction,
-                        uint32_t steps_per_sec_discont, uint32_t steps_per_sec,
-                        uint32_t step_per_sec_sq) -> void {
+    auto start_move(Move move) -> void {
+        motor_util::MotorState* state = move.motor_state;
         _stop = false;
-        set_direction(direction);
+        set_direction(move.direction);
         _profile = motor_util::MovementProfile(
-            TIMER_FREQ, steps_per_sec_discont, steps_per_sec, step_per_sec_sq,
-            motor_util::MovementType::OpenLoop, 0);
+            TIMER_FREQ, state->get_speed_discont(),
+            // if moving to limit switch, use max speed discont
+            move.limit_switch ? state->get_speed_discont() : state->get_speed(),
+            state->get_accel(),
+            move.limit_switch ? motor_util::MovementType::OpenLoop
+                              : motor_util::MovementType::FixedDistance,
+            state->get_distance(move.distance));
         _policy->enable_motor(_id);
-        _response_id = move_id;
+        _response_id = move.move_id;
     }
     auto stop_movement(uint32_t move_id, bool disable_motor) -> void {
         _stop = true;
@@ -105,16 +114,16 @@ class MotorInterruptController {
 
     auto set_diag0_irq(bool enable) -> void { _policy->set_diag0_irq(enable); }
 
+    [[nodiscard]] auto is_moving() const -> bool { return !_stop; }
+
   private:
     MotorID _id;
     MotorPolicy* _policy;
     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;
-    bool _stop = false;
+    bool _stop = true;
 };
 
 }  // namespace motor_interrupt_controller
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
@@ -26,7 +26,11 @@ enum class ErrorCode {
     // 4xx - Motor Errors
     MOTOR_ENABLE_FAILED = 401,
     MOTOR_DISABLE_FAILED = 402,
-    MOTOR_STALL_DETECTED = 403
+    MOTOR_STALL_DETECTED = 403,
+    MOTOR_QUEUE_FULL = 404,
+    X_MOTOR_BUSY = 501,
+    Z_MOTOR_BUSY = 502,
+    L_MOTOR_BUSY = 503,
 };
 
 auto errorstring(ErrorCode code) -> const char*;

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp
@@ -697,6 +697,52 @@ struct StopMotor {
     }
 };
 
+struct HomeMotor {
+    MotorID motor_id;
+    bool direction;
+
+    using ParseResult = std::optional<HomeMotor>;
+    static constexpr auto prefix = std::array{'G', '2', '8', ' '};
+    static constexpr const char* response = "G28 OK\n";
+
+    using XArg = Arg<int, 'X'>;
+    using ZArg = Arg<int, 'Z'>;
+    using LArg = Arg<int, 'L'>;
+
+    template <typename InputIt, typename InLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputIt, InLimit>
+    static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+
+    template <typename InputIt, typename Limit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<Limit, InputIt>
+    static auto parse(const InputIt& input, Limit limit)
+        -> std::pair<ParseResult, InputIt> {
+        auto res = gcode::SingleParser<XArg, ZArg, LArg>::parse_gcode(
+            input, limit, prefix);
+        if (!res.first.has_value()) {
+            return std::make_pair(ParseResult(), input);
+        }
+        auto ret = HomeMotor{.motor_id = MotorID::MOTOR_X, .direction = false};
+        auto arguments = res.first.value();
+        if (std::get<0>(arguments).present) {
+            ret.direction = static_cast<bool>(std::get<0>(arguments).value);
+        } else if (std::get<1>(arguments).present) {
+            ret.motor_id = MotorID::MOTOR_Z;
+            ret.direction = static_cast<bool>(std::get<1>(arguments).value);
+        } else if (std::get<2>(arguments).present) {
+            ret.motor_id = MotorID::MOTOR_L;
+            ret.direction = static_cast<bool>(std::get<2>(arguments).value);
+        } else {
+            return std::make_pair(ParseResult(), input);
+        }
+        return std::make_pair(ret, res.second);
+    }
+};
+
 struct GetLimitSwitches {
     using ParseResult = std::optional<GetLimitSwitches>;
     static constexpr auto prefix = std::array{'M', '1', '1', '9'};

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
@@ -46,14 +46,14 @@ class HostCommsTask {
         gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor,
         gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm,
         gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams,
-        gcode::SetMotorStallGuard, gcode::GetMotorStallGuard>;
+        gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetTMCRegister, gcode::SetRunCurrent,
                  gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor,
                  gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch,
                  gcode::MoveMotorInMm, gcode::SetMicrosteps,
-                 gcode::SetMotorStallGuard>;
+                 gcode::SetMotorStallGuard, gcode::HomeMotor>;
     using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
     using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>;
     using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>;
@@ -700,6 +700,28 @@ class HostCommsTask {
         return std::make_pair(true, tx_into);
     }
 
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::HomeMotor& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = ack_only_cache.add(gcode);
+        if (id == 0) {
+            return std::make_pair(
+                false, errors::write_into(tx_into, tx_limit,
+                                          errors::ErrorCode::GCODE_CACHE_FULL));
+        }
+        auto message = messages::HomeMotorMessage{
+            .id = id, .motor_id = gcode.motor_id, .direction = gcode.direction};
+        if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
+            auto wrote_to = errors::write_into(
+                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+            ack_only_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+        return std::make_pair(true, tx_into);
+    }
+
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
         std::sized_sentinel_for<InputLimit, InputIt>

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
@@ -239,6 +239,12 @@ struct GetMotorStallGuardResponse {
     int sgt;
 };
 
+struct HomeMotorMessage {
+    uint32_t id;
+    MotorID motor_id;
+    bool direction;
+};
+
 using HostCommsMessage =
     ::std::variant<std::monostate, IncomingMessageFromHost, ForceUSBDisconnect,
                    ErrorMessage, AcknowledgePrevious, GetSystemInfoResponse,
@@ -255,10 +261,12 @@ using MotorDriverMessage =
                    SetMotorCurrentMessage, SetMicrostepsMessage,
                    SetMotorStallGuardMessage, GetMotorStallGuardMessage>;
 
-using MotorMessage = ::std::variant<
-    std::monostate, MotorEnableMessage, MoveMotorInStepsMessage,
-    MoveToLimitSwitchMessage, StopMotorMessage, MoveCompleteMessage,
-    GetLimitSwitchesMessage, MoveMotorInMmMessage, SetMicrostepsMessage,
-    GetMoveParamsMessage, SetDiag0IRQMessage, GPIOInterruptMessage>;
+using MotorMessage =
+    ::std::variant<std::monostate, MotorEnableMessage, MoveMotorInStepsMessage,
+                   MoveToLimitSwitchMessage, StopMotorMessage,
+                   MoveCompleteMessage, GetLimitSwitchesMessage,
+                   MoveMotorInMmMessage, SetMicrostepsMessage,
+                   GetMoveParamsMessage, SetDiag0IRQMessage,
+                   GPIOInterruptMessage, HomeMotorMessage>;
 
 };  // namespace messages