feat(flex-stacker): add open loop movement gcode

stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c

@@ -125,7 +125,7 @@ static motor_hardware_t _motor_hardware = {
     .motor_z = {
         .timer = {0},
         .enable = {Z_EN_PORT, Z_EN_PIN, GPIO_PIN_SET},
-        .direction = {Z_DIR_PORT, Z_DIR_PIN, GPIO_PIN_SET},
+        .direction = {Z_DIR_PORT, Z_DIR_PIN, GPIO_PIN_RESET},
         .step = {Z_STEP_PORT, Z_STEP_PIN, GPIO_PIN_SET},
         .limit_switch_minus = {Z_MINUS_LIMIT_PORT, Z_MINUS_LIMIT_PIN, GPIO_PIN_SET},
         .limit_switch_plus = {Z_PLUS_LIMIT_PORT, Z_PLUS_LIMIT_PIN, GPIO_PIN_SET},
@@ -363,45 +363,52 @@ static uint8_t invert_gpio_value(uint8_t setting) {
     }
 }
 
+void set_pin(PinConfig config) {
+    HAL_GPIO_WritePin(config.port, config.pin, config.active_setting);
+}
+
+void reset_pin(PinConfig config) {
+    HAL_GPIO_WritePin(config.port, config.pin, invert_gpio_value(config.active_setting));
+}
+
 bool hw_enable_motor(MotorID motor_id) {
     stepper_hardware_t motor = get_motor(motor_id);
     hw_enable_ebrake(motor_id, false);
     HAL_StatusTypeDef status = HAL_OK;
     status = HAL_TIM_Base_Start_IT(&motor.timer);
-    HAL_GPIO_WritePin(motor.enable.port, motor.enable.pin, motor.enable.active_setting);
+    set_pin(motor.enable);
     return status == HAL_OK;
 }
 
 bool hw_disable_motor(MotorID motor_id) {
     stepper_hardware_t motor = get_motor(motor_id);
     HAL_StatusTypeDef status = HAL_OK;
-//    _HAL_TIM_DISABLE_IT(&motor.timer, TIM_IT_UPDATE);
     status = HAL_TIM_Base_Stop_IT(&motor.timer);
-    HAL_GPIO_WritePin(motor.enable.port, motor.enable.pin, invert_gpio_value(motor.enable.active_setting));
+    reset_pin(motor.enable);
     hw_enable_ebrake(motor_id, true);
     return status == HAL_OK;
 }
 
 void hw_step_motor(MotorID motor_id) {
     stepper_hardware_t motor = get_motor(motor_id);
-    HAL_GPIO_WritePin(motor.step.port, motor.step.pin, motor.step.active_setting);
-    HAL_GPIO_WritePin(motor.step.port, motor.step.pin, invert_gpio_value(motor.step.active_setting));
+    set_pin(motor.step);
+    reset_pin(motor.step);
 }
 
 void hw_set_direction(MotorID motor_id, bool direction) {
     stepper_hardware_t motor = get_motor(motor_id);
-    HAL_GPIO_WritePin(motor.direction.port, motor.direction.pin, direction ? GPIO_PIN_SET : GPIO_PIN_RESET);
+    direction ? set_pin(motor.direction) : reset_pin(motor.direction);
 }
 
 bool hw_read_limit_switch(MotorID motor_id, bool direction) {
     stepper_hardware_t motor = get_motor(motor_id);
     if (direction) {
         return HAL_GPIO_ReadPin(motor.limit_switch_plus.port,
-                                motor.limit_switch_plus.pin) ==
-               motor.limit_switch_plus.active_setting;
+                            motor.limit_switch_plus.pin) ==
+           motor.limit_switch_plus.active_setting;
     }
-    return HAL_GPIO_ReadPin(motor.limit_switch_minus.port,
-                            motor.limit_switch_minus.pin) ==
+        return HAL_GPIO_ReadPin(motor.limit_switch_minus.port,
+                                motor.limit_switch_minus.pin) ==
            motor.limit_switch_minus.active_setting;
 }
 

stm32-modules/include/common/core/debounce.hpp

@@ -0,0 +1,39 @@
+#pragma once
+#include <atomic>
+
+namespace debouncer {
+
+struct Debouncer {
+    int holdoff_cnt = 1;
+    std::atomic_bool state = false;
+    std::atomic_bool state_bounce = false;
+    int cnt = 0;
+
+    auto debounce_update(bool new_state) -> void {
+        // only set the state if the bounce matches the current gpio_is_set
+        // on the first state change it won't match but on the second tick it
+        // will and we can set it to the new state.
+
+        if (new_state == state_bounce) {
+            cnt++;
+            if (cnt == holdoff_cnt) {
+                // update state only when cnt reaches the holdoff count
+                state = new_state;
+                cnt = 0;
+            }
+        } else {
+            // reset count every time the mismatched
+            cnt = 0;
+        }
+        // state bounce is updated each time
+        state_bounce = new_state;
+    }
+    [[nodiscard]] auto debounce_state() const -> bool { return state.load(); }
+    auto reset() -> void {
+        state = false;
+        state_bounce = false;
+        cnt = 0;
+    }
+};
+
+}  // namespace debouncer

stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp

@@ -38,25 +38,35 @@ class MotorInterruptController {
         if (ret.step && !stop_condition_met()) {
             _policy->step(_id);
         }
-        if (ret.done) {
+        if (ret.done || stop_condition_met()) {
             _policy->disable_motor(_id);
+            return true;
         }
         return ret.done;
     }
+
     auto set_freq(uint32_t freq) -> void { step_freq = freq; }
     auto initialize(MotorPolicy* policy) -> void {
         _policy = policy;
         _initialized = true;
     }
-    auto start_movement(uint32_t move_id, bool direction, long steps,
-                        uint32_t frequency) -> void {
+    auto start_fixed_movement(uint32_t move_id, bool direction, long steps,
+                              uint32_t frequency) -> void {
         set_direction(direction);
         _profile = motor_util::MovementProfile(
             TIMER_FREQ, 0, frequency, 0,
             motor_util::MovementType::FixedDistance, steps);
         _policy->enable_motor(_id);
         _response_id = move_id;
     }
+    auto start_movement(uint32_t move_id, bool direction, uint32_t frequency)
+        -> void {
+        set_direction(direction);
+        _profile = motor_util::MovementProfile(
+            TIMER_FREQ, 0, frequency, 0, motor_util::MovementType::OpenLoop, 0);
+        _policy->enable_motor(_id);
+        _response_id = move_id;
+    }
     auto set_direction(bool direction) -> void {
         _policy->set_direction(_id, direction);
         _direction = direction;

stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp

@@ -347,6 +347,83 @@ struct MoveMotorAtFrequency {
     }
 };
 
+struct MoveMotor {
+    MotorID motor_id;
+    bool direction;
+    uint32_t frequency;
+
+    using ParseResult = std::optional<MoveMotor>;
+    static constexpr auto prefix = std::array{'G', '5', ' '};
+    static constexpr const char* response = "G5 OK\n";
+
+    struct XArg {
+        static constexpr auto prefix = std::array{'X'};
+        static constexpr bool required = false;
+        bool present = false;
+        int value = 0;
+    };
+    struct ZArg {
+        static constexpr auto prefix = std::array{'Z'};
+        static constexpr bool required = false;
+        bool present = false;
+        int value = 0;
+    };
+    struct LArg {
+        static constexpr auto prefix = std::array{'L'};
+        static constexpr bool required = false;
+        bool present = false;
+        int value = 0;
+    };
+    struct FreqArg {
+        static constexpr auto prefix = std::array{'F'};
+        static constexpr bool required = false;
+        bool present = false;
+        uint32_t value = 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> {
+        auto res = gcode::SingleParser<XArg, ZArg, LArg, FreqArg>::parse_gcode(
+            input, limit, prefix);
+        if (!res.first.has_value()) {
+            return std::make_pair(ParseResult(), input);
+        }
+        auto ret = MoveMotor{
+            .motor_id = MotorID::MOTOR_X,
+            .direction = false,
+            .frequency = 0,
+        };
+
+        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);
+        }
+
+        if (std::get<3>(arguments).present) {
+            ret.frequency = std::get<3>(arguments).value;
+        }
+        return std::make_pair(ret, 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) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+};
+
 struct StopMotor {
     using ParseResult = std::optional<StopMotor>;
     static constexpr auto prefix = std::array{'M', '0'};
@@ -372,4 +449,39 @@ struct StopMotor {
     }
 };
 
+struct GetLimitSwitches {
+    using ParseResult = std::optional<GetLimitSwitches>;
+    static constexpr auto prefix = std::array{'M', '1', '1', '9'};
+
+    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 working = prefix_matches(input, limit, prefix);
+        if (working == input) {
+            return std::make_pair(ParseResult(), input);
+        }
+        return std::make_pair(ParseResult(GetLimitSwitches()), working);
+    }
+
+    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, int x_extended,
+                                    int x_retracted, int z_extended,
+                                    int z_retracted, int l_released, int l_held)
+        -> InputIt {
+        int res = 0;
+        res = snprintf(&*buf, (limit - buf),
+                       "M119 XE:%i XR:%i ZE:%i ZR:%i LR:%i LH:%i OK\n",
+                       x_extended, x_retracted, z_extended, z_retracted,
+                       l_released, l_held);
+        if (res <= 0) {
+            return buf;
+        }
+        return buf + res;
+    }
+};
+
 }  // namespace gcode