fix(motor): estop release causes z to drop (#737)

* duplicated code, brake gpio already defined in initialize_rev_specific_pins.c

* RQA-2170 disengage Z motors whenever estop is activated

* use callback

* use rising edge (when estop releases) and no need for delay

Author: ahiuchingau

gripper/firmware/motor_hardware_shared.c

@@ -1,7 +1,6 @@
 #include "motor_hardware.h"
 #include "system_stm32g4xx.h"
 
-
 static motor_interrupt_callback timer_callback = NULL;
 static z_encoder_overflow_callback z_enc_overflow_callback = NULL;
 static brushed_motor_interrupt_callback brushed_timer_callback = NULL;
@@ -214,3 +213,12 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) {
 }
 
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    if (GPIO_Pin == ESTOP_IN_PIN) {
+        #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a'
+            HAL_GPIO_WritePin(EBRAKE_PORT, EBRAKE_PIN, GPIO_PIN_RESET);
+        #endif
+        // this keeps the motor disengaged when estop is released
+        HAL_GPIO_WritePin(Z_MOT_ENABLE_PORT, Z_MOT_ENABLE_PIN, GPIO_PIN_RESET);
+    }
+}

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();

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
 }

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) {

head/firmware/motor_hardware.h

@@ -14,6 +14,7 @@ extern TIM_HandleTypeDef htim3;
 
 typedef void (*motor_interrupt_callback)();
 typedef void (*encoder_overflow_callback)(int32_t);
+
 HAL_StatusTypeDef initialize_spi(SPI_HandleTypeDef* hspi);
 void initialize_timer(motor_interrupt_callback callback,
                       encoder_overflow_callback left_enc_overflow_callback,

head/firmware/motor_hardware_common.c

@@ -334,6 +334,20 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
     }
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    // disengage motor whenever estop is engaged
+    if (GPIO_Pin == GPIO_PIN_4) {
+        #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a'
+            // right & left brake
+            HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0 | GPIO_PIN_5, GPIO_PIN_RESET);
+        #endif
+
+        // disable both left and right enable pins
+        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
+    }
+}
+
 void initialize_timer(motor_interrupt_callback callback,
                       encoder_overflow_callback l_f_callback,
                       encoder_overflow_callback r_f_callback) {

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.
  */

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
 }

head/firmware/utility_hardware.c

@@ -62,30 +62,15 @@ 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);
-}
 
-void ebrake_gpio_init() {
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-    // left brake PB0
-    // right brake PB5
-    GPIO_InitTypeDef GPIO_InitStruct = {0};
-    GPIO_InitStruct.Pin = GPIO_PIN_0;
-    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET);
-
-
-    GPIO_InitStruct.Pin = GPIO_PIN_5;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET);
+    HAL_NVIC_SetPriority(EXTI4_IRQn, 6, 0);
+    HAL_NVIC_EnableIRQ(EXTI4_IRQn);
 }
 
+
 void carrier_detect_gpio_init() {
     // Z/left: PC5
     // A/right: PB2
@@ -109,7 +94,6 @@ void carrier_detect_gpio_init() {
 }
 
 void utility_gpio_init() {
-    ebrake_gpio_init();
     carrier_detect_gpio_init();
     limit_switch_gpio_init();
     estop_input_gpio_init();