feat(flex-stacker): implement platform sensors and door switch gpios (#…

…481)

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

@@ -42,11 +42,17 @@ typedef struct stepper_hardware_struct {
     PinConfig diag0;
 } stepper_hardware_t;
 
+typedef struct platform_sensor_struct {
+    PinConfig x_minus;
+    PinConfig x_plus;
+} platform_sensor_t;
+
 typedef struct motor_hardware_struct {
     bool initialized;
     stepper_hardware_t motor_x;
     stepper_hardware_t motor_z;
     stepper_hardware_t motor_l;
+    platform_sensor_t platform_sensors;
 } motor_hardware_t;
 
 
@@ -81,7 +87,11 @@ static motor_hardware_t _motor_hardware = {
         .limit_switch_plus = {L_N_RELEASED_PORT, L_N_RELEASED_PIN, GPIO_PIN_RESET},
         .diag0 = {MOTOR_DIAG0_PORT, MOTOR_DIAG0_PIN, GPIO_PIN_SET},
         .ebrake = {0},
-    }
+    },
+    .platform_sensors = {
+        .x_minus = {PLAT_SENSE_MINUS_PORT, PLAT_SENSE_MINUS_PIN, GPIO_PIN_SET},
+        .x_plus = {PLAT_SENSE_PLUS_PORT, PLAT_SENSE_PLUS_PIN, GPIO_PIN_SET},
+    },
 };
 
 void motor_hardware_gpio_init(void){
@@ -93,6 +103,7 @@ void motor_hardware_gpio_init(void){
     __HAL_RCC_GPIOF_CLK_ENABLE();
     __HAL_RCC_GPIOA_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
+    __HAL_RCC_GPIOD_CLK_ENABLE();
 
     /*Configure GPIO pins : OUTPUTS */
     init.Mode = GPIO_MODE_OUTPUT_PP;
@@ -154,6 +165,13 @@ void motor_hardware_gpio_init(void){
     init.Pin = X_PLUS_LIMIT_PIN;
     HAL_GPIO_Init(X_PLUS_LIMIT_PORT, &init);
 
+    // Platform sensors
+    init.Pin = PLAT_SENSE_PLUS_PIN;
+    HAL_GPIO_Init(PLAT_SENSE_PLUS_PORT, &init);
+
+    init.Pin = PLAT_SENSE_MINUS_PIN;
+    HAL_GPIO_Init(PLAT_SENSE_MINUS_PORT, &init);
+
     // L MOTOR
     init.Pin = L_N_HELD_PIN;
     HAL_GPIO_Init(L_N_HELD_PORT, &init);
@@ -365,6 +383,17 @@ bool hw_read_limit_switch(MotorID motor_id, bool direction) {
            motor.limit_switch_minus.active_setting;
 }
 
+bool hw_read_platform_sensor(bool direction) {
+    if (direction) {
+        return HAL_GPIO_ReadPin(_motor_hardware.platform_sensors.x_plus.port,
+                            _motor_hardware.platform_sensors.x_plus.pin) ==
+           _motor_hardware.platform_sensors.x_plus.active_setting;
+    }
+    return HAL_GPIO_ReadPin(_motor_hardware.platform_sensors.x_minus.port,
+                            _motor_hardware.platform_sensors.x_minus.pin) ==
+       _motor_hardware.platform_sensors.x_minus.active_setting;
+}
+
 void hw_set_diag0_irq(bool enable) {
     enable ?
         HAL_NVIC_EnableIRQ(EXTI15_10_IRQn) :

stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp

@@ -32,6 +32,11 @@ auto MotorPolicy::check_limit_switch(MotorID motor_id, bool direction) -> bool {
     return hw_read_limit_switch(motor_id, direction);
 }
 
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::check_platform_sensor(bool direction) -> bool {
+    return hw_read_platform_sensor(direction);
+}
+
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto MotorPolicy::set_diag0_irq(bool enable) -> void {
     hw_set_diag0_irq(enable);

stm32-modules/flex-stacker/firmware/system/main.h

@@ -33,8 +33,6 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
 void Error_Handler(void);
 
 /* Private defines -----------------------------------------------------------*/
-#define HOPPER_DOR_CLOSED_Pin GPIO_PIN_13
-#define HOPPER_DOR_CLOSED_GPIO_Port GPIOC
 
 #define MOTOR_DIR_Z_Pin GPIO_PIN_1
 #define MOTOR_DIR_Z_GPIO_Port GPIOC

stm32-modules/flex-stacker/firmware/system/system_hardware.c

@@ -58,3 +58,21 @@ void system_hardware_enter_bootloader(void) {
         : "r" (*sysmem_boot_loc)
         : "memory"  );
 }
+
+void system_hardware_gpio_init(void) {
+    GPIO_InitTypeDef init = {0};
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+
+    init.Mode = GPIO_MODE_INPUT;
+    init.Pull = GPIO_NOPULL;
+    init.Speed = GPIO_SPEED_FREQ_LOW;
+    init.Pin = HOPPER_DOR_CLOSED_PIN;
+    HAL_GPIO_Init(HOPPER_DOR_CLOSED_GPIO_PORT, &init);
+}
+
+
+bool system_hardware_read_door_closed(void) {
+    return HAL_GPIO_ReadPin(HOPPER_DOR_CLOSED_GPIO_PORT, HOPPER_DOR_CLOSED_PIN) == GPIO_PIN_SET;
+}

stm32-modules/flex-stacker/firmware/system/system_policy.cpp

@@ -57,3 +57,11 @@ auto SystemPolicy::get_serial_number()
     }
     return serial_number_array;
 }
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto SystemPolicy::initialize() -> void { system_hardware_gpio_init(); }
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto SystemPolicy::get_door_closed() -> bool {
+    return system_hardware_read_door_closed();
+}

stm32-modules/include/flex-stacker/firmware/motor_hardware.h

@@ -21,6 +21,7 @@ bool hw_stop_motor(MotorID motor_id);
 void hw_set_direction(MotorID, bool direction);
 bool hw_read_limit_switch(MotorID motor_id, bool direction);
 void hw_set_diag0_irq(bool enable);
+bool hw_read_platform_sensor(bool direction);
 
 #ifdef __cplusplus
 }  // extern "C"
@@ -69,6 +70,13 @@ void hw_set_diag0_irq(bool enable);
 #define X_PLUS_LIMIT_PIN (GPIO_PIN_2)
 #define X_PLUS_LIMIT_PORT (GPIOA)
 
+/** Platform Sensor **/
+/* + (PC6) and - (PD2) */
+#define PLAT_SENSE_PLUS_PIN (GPIO_PIN_6)
+#define PLAT_SENSE_PLUS_PORT (GPIOC)
+#define PLAT_SENSE_MINUS_PIN (GPIO_PIN_2)
+#define PLAT_SENSE_MINUS_PORT (GPIOD)
+
 /******************* Motor L *******************/
 
 /** Motor hardware **/

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

@@ -15,6 +15,7 @@ class MotorPolicy {
     auto set_direction(MotorID motor_id, bool direction) -> void;
     auto check_limit_switch(MotorID motor_id, bool direction) -> bool;
     auto set_diag0_irq(bool enable) -> void;
+    auto check_platform_sensor(bool direction) -> bool;
 };
 
 }  // namespace motor_policy

stm32-modules/include/flex-stacker/firmware/system_hardware.h

@@ -6,10 +6,15 @@ extern "C" {
 
 #include <stdbool.h>
 
+#define HOPPER_DOR_CLOSED_PIN GPIO_PIN_13
+#define HOPPER_DOR_CLOSED_GPIO_PORT GPIOC
+
 /**
  * @brief Enter the bootloader. This function never returns.
  */
 void system_hardware_enter_bootloader(void);
+void system_hardware_gpio_init(void);
+bool system_hardware_read_door_closed(void);
 
 #ifdef __cplusplus
 }  // extern "C"

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

@@ -16,9 +16,11 @@ class SystemPolicy {
         SYSTEM_SERIAL_NUMBER_LENGTH / ADDRESS_LENGTH;
 
   public:
+    auto initialize() -> void;
     auto enter_bootloader() -> void;
     auto set_serial_number(
         std::array<char, SYSTEM_SERIAL_NUMBER_LENGTH> system_serial_number)
         -> errors::ErrorCode;
     auto get_serial_number() -> std::array<char, SYSTEM_SERIAL_NUMBER_LENGTH>;
+    auto get_door_closed() -> bool;
 };

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

@@ -181,4 +181,34 @@ struct SetSerialNumber {
     }
 };
 
+struct GetDoorClosed {
+    using ParseResult = std::optional<GetDoorClosed>;
+    static constexpr auto prefix = std::array{'M', '1', '2', '2'};
+
+    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(GetDoorClosed()), 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 door_closed)
+        -> InputIt {
+        int res = 0;
+        res = snprintf(&*buf, (limit - buf), "M122 %i OK\n", door_closed);
+        if (res <= 0) {
+            return buf;
+        }
+        return buf + res;
+    }
+};
+
 }  // namespace gcode

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

@@ -944,4 +944,36 @@ struct GetMotorStallGuard {
     }
 };
 
+struct GetPlatformSensors {
+    using ParseResult = std::optional<GetPlatformSensors>;
+    static constexpr auto prefix = std::array{'M', '1', '2', '1'};
+
+    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(GetPlatformSensors()), 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 extend_presence, int retract_presence)
+        -> InputIt {
+        int res = 0;
+        res = snprintf(&*buf, (limit - buf), "M121 E:%i R:%i OK\n",
+                       extend_presence, retract_presence);
+        if (res <= 0) {
+            return buf;
+        }
+        return buf + res;
+    }
+};
+
 }  // namespace gcode

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

@@ -46,7 +46,8 @@ 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::HomeMotor>;
+        gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor,
+        gcode::GetPlatformSensors, gcode::GetDoorClosed>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetTMCRegister, gcode::SetRunCurrent,
@@ -59,6 +60,8 @@ class HostCommsTask {
     using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>;
     using GetMoveParamsCache = AckCache<8, gcode::GetMoveParams>;
     using GetMotorStallGuardCache = AckCache<8, gcode::GetMotorStallGuard>;
+    using GetDoorClosedCache = AckCache<8, gcode::GetDoorClosed>;
+    using GetPlatformSensorsCache = AckCache<8, gcode::GetPlatformSensors>;
 
   public:
     static constexpr size_t TICKS_TO_WAIT_ON_SEND = 10;
@@ -77,7 +80,11 @@ class HostCommsTask {
           // NOLINTNEXTLINE(readability-redundant-member-init)
           get_move_params_cache(),
           // NOLINTNEXTLINE(readability-redundant-member-init)
-          get_motor_stall_guard_cache() {}
+          get_motor_stall_guard_cache(),
+          // NOLINTNEXTLINE(readability-redundant-member-init)
+          get_door_closed_cache(),
+          // NOLINTNEXTLINE(readability-redundant-member-init)
+          get_platform_sensors_cache() {}
     HostCommsTask(const HostCommsTask& other) = delete;
     auto operator=(const HostCommsTask& other) -> HostCommsTask& = delete;
     HostCommsTask(HostCommsTask&& other) noexcept = delete;
@@ -354,6 +361,51 @@ class HostCommsTask {
             cache_entry);
     }
 
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_message(const messages::GetDoorClosedResponse& response,
+                       InputIt tx_into, InputLimit tx_limit) -> InputIt {
+        auto cache_entry =
+            get_door_closed_cache.remove_if_present(response.responding_to_id);
+        return std::visit(
+            [tx_into, tx_limit, response](auto cache_element) {
+                using T = std::decay_t<decltype(cache_element)>;
+                if constexpr (std::is_same_v<std::monostate, T>) {
+                    return errors::write_into(
+                        tx_into, tx_limit,
+                        errors::ErrorCode::BAD_MESSAGE_ACKNOWLEDGEMENT);
+                } else {
+                    return cache_element.write_response_into(
+                        tx_into, tx_limit, response.door_closed);
+                }
+            },
+            cache_entry);
+    }
+
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_message(const messages::GetPlatformSensorsResponse& response,
+                       InputIt tx_into, InputLimit tx_limit) -> InputIt {
+        auto cache_entry = get_platform_sensors_cache.remove_if_present(
+            response.responding_to_id);
+        return std::visit(
+            [tx_into, tx_limit, response](auto cache_element) {
+                using T = std::decay_t<decltype(cache_element)>;
+                if constexpr (std::is_same_v<std::monostate, T>) {
+                    return errors::write_into(
+                        tx_into, tx_limit,
+                        errors::ErrorCode::BAD_MESSAGE_ACKNOWLEDGEMENT);
+                } else {
+                    return cache_element.write_response_into(
+                        tx_into, tx_limit, response.extend_presence,
+                        response.retract_presence);
+                }
+            },
+            cache_entry);
+    }
+
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
         std::sized_sentinel_for<InputLimit, InputIt>
@@ -856,6 +908,48 @@ class HostCommsTask {
             cache_entry);
     }
 
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::GetDoorClosed& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = get_door_closed_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::GetDoorClosedMessage{.id = id};
+        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);
+            get_door_closed_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>
+    auto visit_gcode(const gcode::GetPlatformSensors& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = get_platform_sensors_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::GetPlatformSensorsMessage{.id = id};
+        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);
+            get_platform_sensors_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+        return std::make_pair(true, tx_into);
+    }
+
     // Our error handler just writes an error and bails
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
@@ -876,6 +970,8 @@ class HostCommsTask {
     GetLimitSwitchesCache get_limit_switches_cache;
     GetMoveParamsCache get_move_params_cache;
     GetMotorStallGuardCache get_motor_stall_guard_cache;
+    GetDoorClosedCache get_door_closed_cache;
+    GetPlatformSensorsCache get_platform_sensors_cache;
     bool may_connect_latch = true;
 };