RQA-2170 disengage Z motors whenever estop is activated

Opentrons · Jan 2, 2024 · 6899ced · 6899ced
1 parent c93f609
commit 6899ced
Show file tree

Hide file tree

Showing 8 changed files with 44 additions and 2 deletions.
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
@@ -196,6 +196,13 @@ static motor_handler::MotorInterruptHandler motor_interrupt(
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
 
+extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    // disengage motor whenever estop is engaged
+    if (GPIO_Pin == ESTOP_IN_PIN) {
+        z_motor.motion_controller.disable_motor();
+    }
+}
+
 /**
  * Timer callback.
  */

diff --git a/gripper/firmware/stm32g4xx_it.c b/gripper/firmware/stm32g4xx_it.c
@@ -198,6 +198,16 @@ void TIM7_IRQHandler(void) {
     call_motor_handler();
 }
 
+/**
+  * @brief This function handles EXTI line[15:10] interrupts.
+  */
+__attribute__((section(".ccmram")))
+void EXTI15_10_IRQHandler(void) {
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_10)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10);
+    }
+}
+
 extern void xPortSysTickHandler(void);
 void SysTick_Handler(void) {
     HAL_IncTick();

diff --git a/gripper/firmware/stm32g4xx_it.h b/gripper/firmware/stm32g4xx_it.h
@@ -48,6 +48,7 @@ void TIM3_IRQHandler(void);
 void TIM7_IRQHandler(void);
 void TIM8_CC_IRQHandler(void);
 void TIM8_UP_IRQHandler(void);
+void EXTI15_10_IRQHandler(void);
 
 #ifdef __cplusplus
 }

diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c
@@ -68,9 +68,12 @@ static void estop_input_gpio_init() {
     /*Configure GPIO pin EStopin : PA10 */
     GPIO_InitTypeDef GPIO_InitStruct = {0};
     GPIO_InitStruct.Pin = ESTOP_IN_PIN;
-    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     HAL_GPIO_Init(ESTOP_IN_PORT, &GPIO_InitStruct);
+
+    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 6, 0);
+    HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
 }
 
 static void tool_detect_gpio_init(void) {

diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
@@ -411,6 +411,14 @@ class EEPromHardwareInterface
 };
 static auto eeprom_hw_iface = EEPromHardwareInterface();
 
+extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    // disengage motor whenever estop is engaged
+    if (GPIO_Pin == pin_configurations_left.estop_in.pin) {
+        motor_left.motion_controller.disable_motor();
+        motor_right.motion_controller.disable_motor();
+    }
+}
+
 auto main() -> int {
     HardwareInit();
     RCC_Peripheral_Clock_Select();

diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c
@@ -144,6 +144,15 @@ void FDCAN1_IT0_IRQHandler(void) {
     HAL_FDCAN_IRQHandler(can_get_device_handle());
 }
 
+/**
+  * @brief This function handles EXTI line4 interrupt.
+  */
+void EXTI4_IRQHandler(void) {
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_4)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
+    }
+}
+
 /**
  * @brief This function handles TIM7 global interrupt.
  */

diff --git a/head/firmware/stm32g4xx_it.h b/head/firmware/stm32g4xx_it.h
@@ -42,6 +42,7 @@ void PendSV_Handler(void);
 void SysTick_Handler(void);
 void DMA1_Channel2_IRQHandler(void);
 void DMA1_Channel3_IRQHandler(void);
+void EXTI4_IRQHandler(void);
 
 #ifdef __cplusplus
 }

diff --git a/head/firmware/utility_hardware.c b/head/firmware/utility_hardware.c
@@ -62,9 +62,12 @@ void estop_input_gpio_init() {
     /*Configure GPIO pin EStopin : PB4 */
     GPIO_InitTypeDef GPIO_InitStruct = {0};
     GPIO_InitStruct.Pin = GPIO_PIN_4;
-    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    HAL_NVIC_SetPriority(EXTI4_IRQn, 6, 0);
+    HAL_NVIC_EnableIRQ(EXTI4_IRQn);
 }