feat(flex-stacker): add estop (#482)

ahiuchingau · web-flow · commit 393bae85bc2b · 2024-11-21T15:06:33.000-05:00
diff --git a/stm32-modules/flex-stacker/firmware/main.cpp b/stm32-modules/flex-stacker/firmware/main.cpp
@@ -62,6 +62,7 @@ static auto system_task =
 extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
     switch (GPIO_Pin) {
         case MOTOR_DIAG0_PIN:
+        case N_ESTOP_PIN:
             static_cast<void>(aggregator.send_from_isr(
                 messages::GPIOInterruptMessage{.pin = GPIO_Pin}));
             break;
diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c
@@ -53,6 +53,7 @@ typedef struct motor_hardware_struct {
     stepper_hardware_t motor_z;
     stepper_hardware_t motor_l;
     platform_sensor_t platform_sensors;
+    PinConfig estop;
 } motor_hardware_t;
 
 
@@ -92,6 +93,7 @@ static motor_hardware_t _motor_hardware = {
         .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},
     },
+    .estop = {N_ESTOP_PORT, N_ESTOP_PIN, GPIO_PIN_RESET},
 };
 
 void motor_hardware_gpio_init(void){
@@ -179,9 +181,6 @@ void motor_hardware_gpio_init(void){
     init.Pin = L_N_RELEASED_PIN;
     HAL_GPIO_Init(L_N_RELEASED_PORT, &init);
 
-    init.Pin = ESTOP_PIN;
-    HAL_GPIO_Init(ESTOP_PORT, &init);
-
     /*Configure GPIO pins : INPUTs IRQ */
     init.Mode = GPIO_MODE_IT_FALLING;
     init.Pull = GPIO_PULLUP;
@@ -190,6 +189,11 @@ void motor_hardware_gpio_init(void){
     init.Pin = MOTOR_DIAG0_PIN;
     HAL_GPIO_Init(MOTOR_DIAG0_PORT, &init);
     HAL_NVIC_SetPriority(EXTI15_10_IRQn, 6, 0);
+
+    init.Pin = N_ESTOP_PIN;
+    HAL_GPIO_Init(N_ESTOP_PORT, &init);
+    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
 }
 
 // X motor timer
@@ -394,12 +398,25 @@ bool hw_read_platform_sensor(bool direction) {
        _motor_hardware.platform_sensors.x_minus.active_setting;
 }
 
+bool hw_read_estop(void) {
+    return HAL_GPIO_ReadPin(_motor_hardware.estop.port, _motor_hardware.estop.pin) ==
+           _motor_hardware.estop.active_setting;
+}
+
 void hw_set_diag0_irq(bool enable) {
     enable ?
         HAL_NVIC_EnableIRQ(EXTI15_10_IRQn) :
         HAL_NVIC_DisableIRQ(EXTI15_10_IRQn);
 }
 
+bool hw_is_diag0_pin(uint16_t pin) {
+    return pin == MOTOR_DIAG0_PIN;
+}
+
+bool hw_is_estop_pin(uint16_t pin) {
+    return pin == N_ESTOP_PIN;
+}
+
 void TIM3_IRQHandler(void)
 {
     HAL_TIM_IRQHandler(&_motor_hardware.motor_l.timer);
diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp
@@ -41,3 +41,16 @@ auto MotorPolicy::check_platform_sensor(bool direction) -> bool {
 auto MotorPolicy::set_diag0_irq(bool enable) -> void {
     hw_set_diag0_irq(enable);
 }
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::check_estop() -> bool { return hw_read_estop(); }
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::is_diag0_pin(uint16_t pin) -> bool {
+    return hw_is_diag0_pin(pin);
+}
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::is_estop_pin(uint16_t pin) -> bool {
+    return hw_is_estop_pin(pin);
+}
diff --git a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c
@@ -129,3 +129,10 @@ void EXTI15_10_IRQHandler(void) {
     }
 }
 
+// Estop interrupt
+void EXTI9_5_IRQHandler(void)
+{
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_6)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
+    }
+}
diff --git a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h
@@ -35,6 +35,7 @@ void DebugMon_Handler(void);
 // void SysTick_Handler(void);
 void RCC_IRQHandler(void);
 void EXTI15_10_IRQHandler(void);
+void EXTI9_5_IRQHandler(void);
 // void DMA1_Channel1_IRQHandler(void);
 // void DMA1_Channel2_IRQHandler(void);
 
diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp
@@ -9,6 +9,7 @@ const char* const UNHANDLED_GCODE = "ERR003:unhandled gcode\n";
 const char* const GCODE_CACHE_FULL = "ERR004:gcode cache full\n";
 const char* const BAD_MESSAGE_ACKNOWLEDGEMENT =
     "ERR005:bad message acknowledgement\n";
+const char* const ESTOP_TRIGGERED = "ERR006:estop triggered\n";
 const char* const SYSTEM_SERIAL_NUMBER_INVALID =
     "ERR301:system:serial number invalid format\n";
 const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR =
@@ -44,6 +45,7 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(UNHANDLED_GCODE);
         HANDLE_CASE(GCODE_CACHE_FULL);
         HANDLE_CASE(BAD_MESSAGE_ACKNOWLEDGEMENT);
+        HANDLE_CASE(ESTOP_TRIGGERED);
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_INVALID);
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR);
         HANDLE_CASE(SYSTEM_EEPROM_ERROR);
diff --git a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h
@@ -22,6 +22,9 @@ 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);
+bool hw_read_estop(void);
+bool hw_is_diag0_pin(uint16_t pin);
+bool hw_is_estop_pin(uint16_t pin);
 
 #ifdef __cplusplus
 }  // extern "C"
@@ -95,7 +98,7 @@ bool hw_read_platform_sensor(bool direction);
 #define L_N_RELEASED_PORT (GPIOC)
 
 /**************** COMMON ********************/
-#define ESTOP_PIN (GPIO_PIN_6)
-#define ESTOP_PORT (GPIOB)
+#define N_ESTOP_PIN (GPIO_PIN_6)
+#define N_ESTOP_PORT (GPIOB)
 #define MOTOR_DIAG0_PIN (GPIO_PIN_12)
 #define MOTOR_DIAG0_PORT (GPIOB)
diff --git a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp
@@ -16,6 +16,9 @@ class MotorPolicy {
     auto check_limit_switch(MotorID motor_id, bool direction) -> bool;
     auto set_diag0_irq(bool enable) -> void;
     auto check_platform_sensor(bool direction) -> bool;
+    auto check_estop() -> bool;
+    auto is_diag0_pin(uint16_t pin) -> bool;
+    auto is_estop_pin(uint16_t pin) -> bool;
 };
 
 }  // namespace motor_policy
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
@@ -14,6 +14,7 @@ enum class ErrorCode {
     UNHANDLED_GCODE = 3,
     GCODE_CACHE_FULL = 4,
     BAD_MESSAGE_ACKNOWLEDGEMENT = 5,
+    ESTOP_TRIGGERED = 6,
     // 3xx - System General
     SYSTEM_SERIAL_NUMBER_INVALID = 301,
     SYSTEM_SERIAL_NUMBER_HAL_ERROR = 302,
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp
@@ -976,4 +976,34 @@ struct GetPlatformSensors {
     }
 };
 
+struct GetEstopStatus {
+    using ParseResult = std::optional<GetEstopStatus>;
+    static constexpr auto prefix = std::array{'M', '1', '1', '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(GetEstopStatus()), 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 triggered)
+        -> InputIt {
+        int res = 0;
+        res = snprintf(&*buf, (limit - buf), "M112 %i OK", triggered);
+        if (res <= 0) {
+            return buf;
+        }
+        return buf + res;
+    }
+};
+
 }  // namespace gcode
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
@@ -47,7 +47,7 @@ class HostCommsTask {
         gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm,
         gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams,
         gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor,
-        gcode::GetPlatformSensors, gcode::GetDoorClosed>;
+        gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetTMCRegister, gcode::SetRunCurrent,
@@ -62,6 +62,7 @@ class HostCommsTask {
     using GetMotorStallGuardCache = AckCache<8, gcode::GetMotorStallGuard>;
     using GetDoorClosedCache = AckCache<8, gcode::GetDoorClosed>;
     using GetPlatformSensorsCache = AckCache<8, gcode::GetPlatformSensors>;
+    using GetEstopCache = AckCache<8, gcode::GetEstopStatus>;
 
   public:
     static constexpr size_t TICKS_TO_WAIT_ON_SEND = 10;
@@ -84,7 +85,9 @@ class HostCommsTask {
           // NOLINTNEXTLINE(readability-redundant-member-init)
           get_door_closed_cache(),
           // NOLINTNEXTLINE(readability-redundant-member-init)
-          get_platform_sensors_cache() {}
+          get_platform_sensors_cache(),
+          // NOLINTNEXTLINE(readability-redundant-member-init)
+          get_estop_cache() {}
     HostCommsTask(const HostCommsTask& other) = delete;
     auto operator=(const HostCommsTask& other) -> HostCommsTask& = delete;
     HostCommsTask(HostCommsTask&& other) noexcept = delete;
@@ -224,7 +227,8 @@ class HostCommsTask {
     auto visit_message(const messages::ErrorMessage& msg, InputIt tx_into,
                        InputLimit tx_limit) -> InputIt {
         // stall detected, clear the message cache.
-        if (msg.code == errors::ErrorCode::MOTOR_STALL_DETECTED) {
+        if ((msg.code == errors::ErrorCode::MOTOR_STALL_DETECTED) ||
+            (msg.code == errors::ErrorCode::ESTOP_TRIGGERED)) {
             ack_only_cache.clear();
         }
         return errors::write_into_async(tx_into, tx_limit, msg.code);
@@ -406,6 +410,28 @@ 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::GetEstopResponse& response,
+                       InputIt tx_into, InputLimit tx_limit) -> InputIt {
+        auto cache_entry =
+            get_estop_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.triggered);
+                }
+            },
+            cache_entry);
+    }
+
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
         std::sized_sentinel_for<InputLimit, InputIt>
@@ -950,6 +976,27 @@ 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::GetEstopStatus& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = get_estop_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::GetEstopMessage{.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_estop_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> &&
@@ -972,6 +1019,7 @@ class HostCommsTask {
     GetMotorStallGuardCache get_motor_stall_guard_cache;
     GetDoorClosedCache get_door_closed_cache;
     GetPlatformSensorsCache get_platform_sensors_cache;
+    GetEstopCache get_estop_cache;
     bool may_connect_latch = true;
 };
 
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
@@ -264,12 +264,22 @@ struct HomeMotorMessage {
     bool direction;
 };
 
+struct GetEstopMessage {
+    uint32_t id;
+};
+
+struct GetEstopResponse {
+    uint32_t responding_to_id;
+    bool triggered;
+};
+
 using HostCommsMessage =
     ::std::variant<std::monostate, IncomingMessageFromHost, ForceUSBDisconnect,
                    ErrorMessage, AcknowledgePrevious, GetSystemInfoResponse,
                    GetTMCRegisterResponse, GetLimitSwitchesResponses,
                    GetMoveParamsResponse, GetMotorStallGuardResponse,
-                   GetDoorClosedResponse, GetPlatformSensorsResponse>;
+                   GetDoorClosedResponse, GetPlatformSensorsResponse,
+                   GetEstopResponse>;
 
 using SystemMessage =
     ::std::variant<std::monostate, AcknowledgePrevious, GetSystemInfoMessage,
@@ -287,6 +297,6 @@ using MotorMessage = ::std::variant<
     MoveToLimitSwitchMessage, StopMotorMessage, MoveCompleteMessage,
     GetLimitSwitchesMessage, MoveMotorInMmMessage, SetMicrostepsMessage,
     GetMoveParamsMessage, SetDiag0IRQMessage, GPIOInterruptMessage,
-    HomeMotorMessage, GetPlatformSensorsMessage>;
+    HomeMotorMessage, GetPlatformSensorsMessage, GetEstopMessage>;
 
 };  // namespace messages
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
@@ -350,6 +350,15 @@ class MotorTask {
             response, Queues::HostCommsAddress));
     }
 
+    template <MotorControlPolicy Policy>
+    auto visit_message(const messages::GetEstopMessage& m, Policy& policy)
+        -> void {
+        auto response = messages::GetEstopResponse{
+            .responding_to_id = m.id, .triggered = policy.check_estop()};
+        static_cast<void>(_task_registry->send_to_address(
+            response, Queues::HostCommsAddress));
+    }
+
     template <MotorControlPolicy Policy>
     auto visit_message(const messages::MoveCompleteMessage& m, Policy& policy)
         -> void {
@@ -402,11 +411,17 @@ class MotorTask {
     auto visit_message(const messages::GPIOInterruptMessage& m, Policy& policy)
         -> void {
         static_cast<void>(m);
-        static_cast<void>(policy);
+        if (policy.is_diag0_pin(m.pin)) {
+            send_error_message(Error::MOTOR_STALL_DETECTED);
+        } else if (policy.is_estop_pin(m.pin)) {
+            send_error_message(Error::ESTOP_TRIGGERED);
+        } else {
+            // don't care about other interrupts
+            return;
+        }
         _z_controller.stop_movement(0, true);
         _x_controller.stop_movement(0, false);
         _l_controller.stop_movement(0, false);
-        send_error_message(Error::MOTOR_STALL_DETECTED);
     }
 
     /**

Original file line number	Diff line number	Diff line change
`@@ -129,3 +129,10 @@ void EXTI15_10_IRQHandler(void) {`
`129`	`129`	`}`
`130`	`130`	`}`
`131`	`131`
	`132`	`+// Estop interrupt`
	`133`	`+void EXTI9_5_IRQHandler(void)`
	`134`	`+{`
	`135`	`+ if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_6)) {`
	`136`	`+ HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);`
	`137`	`+ }`
	`138`	`+}`