From 3aed5426f66f85bb9d33ac6392596ea658161b12 Mon Sep 17 00:00:00 2001
From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com>
Date: Tue, 2 Jan 2024 16:12:35 -0500
Subject: [PATCH] disengage Z motors whenever estop is activated

---
 gripper/firmware/interfaces_z_motor.cpp |  7 +++++++
 gripper/firmware/stm32g4xx_it.c         | 10 ++++++++++
 gripper/firmware/stm32g4xx_it.h         |  1 +
 gripper/firmware/utility_gpio.c         |  5 ++++-
 head/firmware/main_rev1.cpp             |  8 ++++++++
 head/firmware/stm32g4xx_it.c            |  9 +++++++++
 head/firmware/stm32g4xx_it.h            |  1 +
 head/firmware/utility_hardware.c        |  5 ++++-
 8 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index c9c43c9a9..72f1a535d 100644
--- 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
index 7efd9fff0..2051ebc6b 100644
--- 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
index 3354a2cda..b5eb3bed8 100644
--- 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
index b206651a2..ed5b371da 100644
--- 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
index ce5849129..b48baa463 100644
--- 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
index 57ba8aef7..6e031a2bf 100644
--- 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
index 563c2a6ba..9bb73af46 100644
--- 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
index 3c13932e5..7d7c91f96 100644
--- 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);
 }