feat(flex-stacker): add Gcodes to enable motor movements (#455)

Add Gcodes for flex stacker * M921: write to register * M17/M18: enable/disable motor * G6: move motor at frequency * M0: stop
Opentrons · Aug 20, 2024 · 2bcd6bb · 2bcd6bb
1 parent 730bd08
commit 2bcd6bb
Show file tree

Hide file tree

Showing 15 changed files with 806 additions and 105 deletions.
diff --git a/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp b/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp
@@ -58,9 +58,6 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
     motor_hardware_init();
     initialize_callbacks(callback_glue);
     auto policy = motor_policy::MotorPolicy();
-    policy.enable_motor(MotorID::MOTOR_L);
-    policy.enable_motor(MotorID::MOTOR_X);
-    policy.enable_motor(MotorID::MOTOR_Z);
     while (true) {
         _top_task.run_once(policy);
     }

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c
@@ -3,6 +3,8 @@
 #include "stm32g4xx_it.h"
 #include "systemwide.h"
 
+#include <stdbool.h>
+
 /******************* Motor Z *******************/
 /** Motor hardware **/
 #define Z_STEP_PIN (GPIO_PIN_2)
@@ -276,7 +278,7 @@ void motor_hardware_init(void){
     motor_hardware_interrupt_init();
 }
 
-void hw_enable_motor(MotorID motor_id) {
+bool hw_enable_motor(MotorID motor_id) {
     void* port;
     uint16_t pin;
     HAL_StatusTypeDef     status = HAL_OK;
@@ -285,7 +287,7 @@ void hw_enable_motor(MotorID motor_id) {
             port = Z_EN_PORT;
             pin = Z_EN_PIN;
             status = HAL_TIM_Base_Start_IT(&htim20);
-            HAL_NVIC_SetPriority(TIM20_UP_IRQn, 5, 0);
+            HAL_NVIC_SetPriority(TIM20_UP_IRQn, 10, 0);
             HAL_NVIC_EnableIRQ(TIM20_UP_IRQn);
             if (status == HAL_OK)
             {
@@ -295,7 +297,7 @@ void hw_enable_motor(MotorID motor_id) {
             port = X_EN_PORT;
             pin = X_EN_PIN;
             status = HAL_TIM_Base_Start_IT(&htim17);
-            HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 6, 0);
+            HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 10, 0);
             HAL_NVIC_EnableIRQ(TIM1_TRG_COM_TIM17_IRQn);
             if (status == HAL_OK)
             {
@@ -305,19 +307,20 @@ void hw_enable_motor(MotorID motor_id) {
             port = L_EN_PORT;
             pin = L_EN_PIN;
             status = HAL_TIM_Base_Start_IT(&htim3);
-            HAL_NVIC_SetPriority(TIM3_IRQn, 6, 0);
+            HAL_NVIC_SetPriority(TIM3_IRQn, 10, 0);
             HAL_NVIC_EnableIRQ(TIM3_IRQn);
             if (status == HAL_OK)
             {
             }
             break;
         default:
-            return;
+            return false;
     }
     HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
+    return status == HAL_OK;
 }
 
-void hw_disable_motor(MotorID motor_id) {
+bool hw_disable_motor(MotorID motor_id) {
     void* port;
     uint16_t pin;
     HAL_StatusTypeDef     status = HAL_OK;
@@ -326,30 +329,22 @@ void hw_disable_motor(MotorID motor_id) {
             port = Z_EN_PORT;
             pin = Z_EN_PIN;
             status = HAL_TIM_Base_Stop_IT(&htim20);
-            if (!status == HAL_OK){
-                Error_Handler();
-            }
             break;
         case MOTOR_X:
             port = X_EN_PORT;
             pin = X_EN_PIN;
             status = HAL_TIM_Base_Stop_IT(&htim17);
-            if (!status == HAL_OK){
-                Error_Handler();
-            }
             break;
         case MOTOR_L:
             port = L_EN_PORT;
             pin = L_EN_PIN;
             status = HAL_TIM_Base_Stop_IT(&htim3);
-            if (!status == HAL_OK){
-                Error_Handler();
-            }
             break;
         default:
-            return;
+            return false;
     }
     HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);
+    return status == HAL_OK;
 }
 
 void step_motor(MotorID motor_id) {

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp
@@ -5,13 +5,13 @@
 using namespace motor_policy;
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
-auto MotorPolicy::enable_motor(MotorID motor_id) -> void {
-    hw_enable_motor(motor_id);
+auto MotorPolicy::enable_motor(MotorID motor_id) -> bool {
+    return hw_enable_motor(motor_id);
 }
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
-auto MotorPolicy::disable_motor(MotorID motor_id) -> void {
-    hw_disable_motor(motor_id);
+auto MotorPolicy::disable_motor(MotorID motor_id) -> bool {
+    return hw_disable_motor(motor_id);
 }
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)

diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp
@@ -16,6 +16,11 @@ const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR =
 const char* const SYSTEM_EEPROM_ERROR =
     "ERR303:system:EEPROM communication error\n";
 const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n";
+const char* const TMC2160_WRITE_ERROR = "ERR902:TMC2160 driver write error\n";
+const char* const TMC2160_INVALID_ADDRESS = "ERR903:TMC2160 invalid address\n";
+
+const char* const MOTOR_ENABLE_FAILED = "ERR401:motor enable error\n";
+const char* const MOTOR_DISABLE_FAILED = "ERR401:motor disable error\n";
 
 const char* const UNKNOWN_ERROR = "ERR-1:unknown error code\n";
 
@@ -36,6 +41,10 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR);
         HANDLE_CASE(SYSTEM_EEPROM_ERROR);
         HANDLE_CASE(TMC2160_READ_ERROR);
+        HANDLE_CASE(TMC2160_WRITE_ERROR);
+        HANDLE_CASE(TMC2160_INVALID_ADDRESS);
+        HANDLE_CASE(MOTOR_ENABLE_FAILED);
+        HANDLE_CASE(MOTOR_DISABLE_FAILED);
     }
     return UNKNOWN_ERROR;
 }
diff --git a/stm32-modules/flex-stacker/src/motor_utils.cpp b/stm32-modules/flex-stacker/src/motor_utils.cpp
@@ -0,0 +1,77 @@
+#include "flex-stacker/motor_utils.hpp"
+
+using namespace motor_util;
+
+MovementProfile::MovementProfile(uint32_t ticks_per_second,
+                                 double start_velocity, double peak_velocity,
+                                 double acceleration, MovementType type,
+                                 ticks distance)
+    : _ticks_per_second(ticks_per_second),
+      _type(type),
+      _target_distance(distance) {
+    // Clamp ticks_per_second to at least 1
+    _ticks_per_second = std::max(_ticks_per_second, static_cast<uint32_t>(1));
+
+    auto tick_freq = static_cast<double>(_ticks_per_second);
+
+    // Clamp the peak velocity so it is not under start velocity
+    start_velocity = std::max(start_velocity, static_cast<double>(0.0F));
+    acceleration = std::max(acceleration, static_cast<double>(0.0F));
+    peak_velocity = std::max(start_velocity, peak_velocity);
+
+    // Convert velocities by just dividing by the tick frequency
+    _start_velocity = convert_to_fixed_point(start_velocity / tick_freq, radix);
+    _peak_velocity = convert_to_fixed_point(peak_velocity / tick_freq, radix);
+    // Acceleration must be dividied by (tick/sec)^2 for unit conversion
+    _acceleration =
+        convert_to_fixed_point(acceleration / (tick_freq * tick_freq), radix);
+
+    if (_acceleration <= 0) {
+        _start_velocity = _peak_velocity;
+    }
+
+    // Ensures that all movement variables are initialized properly
+    reset();
+}
+
+auto MovementProfile::reset() -> void {
+    _velocity = _start_velocity;
+    _current_distance = 0;
+    _tick_tracker = 0;
+}
+
+auto MovementProfile::tick() -> TickReturn {
+    bool step = false;
+    // Acceleration gets clamped to _peak_velocity
+    if (_velocity < _peak_velocity) {
+        _velocity += _acceleration;
+        if (_velocity > _peak_velocity) {
+            _velocity = _peak_velocity;
+        }
+    }
+    auto old_tick_track = _tick_tracker;
+    _tick_tracker += _velocity;
+    // The bit _tick_flag represents a "whole" step. Once this flips,
+    // the code signals that a step should occur
+    if (((old_tick_track ^ _tick_tracker) & _tick_flag) != 0U) {
+        step = true;
+        ++_current_distance;
+    }
+    return TickReturn{.done = (_current_distance >= _target_distance &&
+                               _type == MovementType::FixedDistance),
+                      .step = step};
+}
+
+[[nodiscard]] auto MovementProfile::current_velocity() const -> steps_per_tick {
+    return _velocity;
+}
+
+/** Returns the number of ticks that have been taken.*/
+[[nodiscard]] auto MovementProfile::target_distance() const -> ticks {
+    return _target_distance;
+}
+
+/** Returns the number of ticks that have been taken.*/
+[[nodiscard]] auto MovementProfile::current_distance() const -> ticks {
+    return _current_distance;
+}
diff --git a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h
@@ -17,8 +17,8 @@ bool motor_spi_sendreceive(MotorID motor_id, uint8_t *tx_data, uint8_t *rx_data,
 
 void step_motor(MotorID motor_id);
 void unstep_motor(MotorID motor_id);
-void hw_enable_motor(MotorID motor_id);
-void hw_disable_motor(MotorID motor_id);
+bool hw_enable_motor(MotorID motor_id);
+bool hw_disable_motor(MotorID motor_id);
 
 #ifdef __cplusplus
 }  // extern "C"

diff --git a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp
@@ -8,8 +8,8 @@ namespace motor_policy {
 
 class MotorPolicy {
   public:
-    auto enable_motor(MotorID motor_id) -> void;
-    auto disable_motor(MotorID motor_id) -> void;
+    auto enable_motor(MotorID motor_id) -> bool;
+    auto disable_motor(MotorID motor_id) -> bool;
     auto step(MotorID motor_id) -> void;
 };
 

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
@@ -20,6 +20,11 @@ enum class ErrorCode {
     SYSTEM_EEPROM_ERROR = 303,
     // 9xx - TMC2160
     TMC2160_READ_ERROR = 901,
+    TMC2160_WRITE_ERROR = 902,
+    TMC2160_INVALID_ADDRESS = 903,
+    // 4xx - Motor Errors
+    MOTOR_ENABLE_FAILED = 401,
+    MOTOR_DISABLE_FAILED = 402,
 };
 
 auto errorstring(ErrorCode code) -> const char*;

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp
@@ -18,16 +18,11 @@
 #include "core/gcode_parser.hpp"
 #include "core/utility.hpp"
 #include "flex-stacker/errors.hpp"
+#include "flex-stacker/gcodes_motor.hpp"
 #include "systemwide.h"
 
 namespace gcode {
 
-auto inline motor_id_to_char(MotorID motor_id) -> const char* {
-    return static_cast<const char*>(motor_id == MotorID::MOTOR_X   ? "X"
-                                    : motor_id == MotorID::MOTOR_Z ? "Z"
-                                                                   : "L");
-}
-
 struct EnterBootloader {
     /**
      * EnterBootloader uses the command string "dfu" instead of a gcode to be
@@ -237,64 +232,4 @@ struct GetBoardRevision {
     }
 };
 
-struct GetTMCRegister {
-    MotorID motor_id;
-    uint8_t reg;
-
-    using ParseResult = std::optional<GetTMCRegister>;
-    static constexpr auto prefix = std::array{'M', '9', '2', '0', ' '};
-
-    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> {
-        MotorID motor_id_val = MotorID::MOTOR_X;
-        auto working = prefix_matches(input, limit, prefix);
-        if (working == input) {
-            return std::make_pair(ParseResult(), input);
-        }
-        switch (*working) {
-            case 'X':
-                motor_id_val = MotorID::MOTOR_X;
-                break;
-            case 'Z':
-                motor_id_val = MotorID::MOTOR_Z;
-                break;
-            case 'L':
-                motor_id_val = MotorID::MOTOR_L;
-                break;
-            default:
-                return std::make_pair(ParseResult(), input);
-        }
-        std::advance(working, 1);
-        if (working == limit) {
-            return std::make_pair(ParseResult(), input);
-        }
-
-        auto reg_res = gcode::parse_value<uint8_t>(working, limit);
-        if (!reg_res.first.has_value()) {
-            return std::make_pair(ParseResult(), input);
-        }
-        return std::make_pair(
-            ParseResult(GetTMCRegister{.motor_id = motor_id_val,
-                                       .reg = reg_res.first.value()}),
-            reg_res.second);
-    }
-
-    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,
-                                    MotorID motor_id, uint8_t reg,
-                                    uint32_t data) -> InputIt {
-        auto res = snprintf(&*buf, (limit - buf), "M920 %s%u %lu OK\n",
-                            motor_id_to_char(motor_id), reg, data);
-        if (res <= 0) {
-            return buf;
-        }
-        return buf + res;
-    }
-};
-
 }  // namespace gcode