diff --git a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp index 517f1e577..d9593d395 100644 --- a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp @@ -59,12 +59,24 @@ class MotorDriverMessageHandler { auto data = driver.handle_spi_read( tmc2160::registers::Registers(static_cast(m.id.token)), m.rxBuffer); - can::messages::ReadMotorDriverRegisterResponse response_msg{ - .message_index = m.id.message_index, - .reg_address = static_cast(m.id.token), - .data = data, - }; - can_client.send_can_message(can::ids::NodeId::host, response_msg); + if (spi::utils::tag_in_token( + m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) { + can::messages::ReadMotorDriverErrorStatusResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } else { + can::messages::ReadMotorDriverRegisterResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } } } diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index e5bbe7f61..cc53aba6c 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -10,6 +10,9 @@ #include "motor-control/core/tasks/messages.hpp" #include "motor-control/core/tasks/tmc_motor_driver_common.hpp" +constexpr uint32_t DIAG0_DEBOUNCE_REPS = 9; +constexpr uint32_t DIAG0_DEBOUNCE_DELAY = 100; + namespace motion_controller_task { using TaskMessage = motor_control_task_messages::MotionControlTaskMessage; @@ -190,7 +193,7 @@ class MotionControllerMessageHandler { void handle( const motor_control_task_messages::RouteMotorDriverInterrupt& m) { - if (m.debounce_count > 9) { + if (m.debounce_count > DIAG0_DEBOUNCE_REPS) { if (controller.read_tmc_diag0()) { controller.stop( can::ids::ErrorSeverity::unrecoverable, @@ -207,12 +210,10 @@ class MotionControllerMessageHandler { can::messages::ReadMotorDriverErrorStatusRequest{ .message_index = m.message_index}); } - } else { - controller.clear_cancel_request(); } diag0_debounced = false; } else { - vTaskDelay(pdMS_TO_TICKS(100)); + vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY)); motion_client.send_motion_controller_queue( increment_message_debounce_count(m)); } diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index 081d8d5e8..5022263f6 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -173,8 +173,6 @@ class MotionControllerMessageHandler { can::messages::ReadMotorDriverErrorStatusRequest{ .message_index = m.message_index}); } - } else { - controller.clear_cancel_request(); } diag0_debounced = false; } else { diff --git a/pipettes/firmware/hardware_config.c b/pipettes/firmware/hardware_config.c index 22b790d2d..b4ada4b98 100644 --- a/pipettes/firmware/hardware_config.c +++ b/pipettes/firmware/hardware_config.c @@ -146,7 +146,7 @@ static uint16_t get_spi_pins_lt(GPIO_TypeDef* for_handle) { } } -static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle, bool for_diag0) { +static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle) { /* * Dir Pins * plunger -> PA6 @@ -162,23 +162,16 @@ static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle, bool for_diag * Motor Enable * pickup enable -> PA10 * plunger enable -> PB4 - * - * Diag0 Pin - * PB6 */ - if (!for_diag0) { - switch((uint32_t)for_handle) { - case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10; - case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4; - case (uint32_t)GPIOC: return GPIO_PIN_2; - default: return 0; - } - } else { - return GPIO_PIN_6; + switch((uint32_t)for_handle) { + case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10; + case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4; + case (uint32_t)GPIOC: return GPIO_PIN_2; + default: return 0; } } -static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle, bool for_diag0) { +static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle) { /* * Dir Pin -> PC6 * @@ -186,17 +179,11 @@ static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle, bool for_diag * Enable Pin -> PA10 * * VREF (TMC2130) -> PA5 - * - * DIAG0 Pin -> PC11 */ - if (!for_diag0) { - switch((uint32_t)for_handle) { - case (uint32_t)GPIOA: return GPIO_PIN_10; - case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7; - default: return 0; - } - } else { - return GPIO_PIN_11; + switch((uint32_t)for_handle) { + case (uint32_t)GPIOA: return GPIO_PIN_10; + case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7; + default: return 0; } } @@ -226,14 +213,26 @@ uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef* } } -uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle, bool for_diag0) { +uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle) { + switch (pipette_type) { + case NINETY_SIX_CHANNEL: + case THREE_EIGHTY_FOUR_CHANNEL: + return get_motor_driver_pins_ht(for_handle); + case SINGLE_CHANNEL: + case EIGHT_CHANNEL: + default: + return get_motor_driver_pins_lt(for_handle); + } +} + +uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type) { switch (pipette_type) { case NINETY_SIX_CHANNEL: case THREE_EIGHTY_FOUR_CHANNEL: - return get_motor_driver_pins_ht(for_handle, for_diag0); + return GPIO_PIN_6; case SINGLE_CHANNEL: case EIGHT_CHANNEL: default: - return get_motor_driver_pins_lt(for_handle, for_diag0); + return GPIO_PIN_11; } } diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h index 4a0ec859c..730cd48e2 100644 --- a/pipettes/firmware/hardware_config.h +++ b/pipettes/firmware/hardware_config.h @@ -25,6 +25,7 @@ typedef enum { }GPIOInterruptBlock; uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef* which_handle); -uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle, bool for_diag0); +uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle); +uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type); PipetteHardwarePin pipette_hardware_get_gpio(const PipetteType pipette_type, PipetteHardwareDevice device); IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type); diff --git a/pipettes/firmware/motor_hardware.c b/pipettes/firmware/motor_hardware.c index 9a3815b2e..867a7d4fd 100644 --- a/pipettes/firmware/motor_hardware.c +++ b/pipettes/firmware/motor_hardware.c @@ -83,7 +83,7 @@ void motor_driver_gpio_init() { GPIO_InitTypeDef GPIO_InitStruct = {0}; PipetteType pipette_type = get_pipette_type(); - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC, false); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC); GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; @@ -92,21 +92,21 @@ void motor_driver_gpio_init() { if (pipette_type != NINETY_SIX_CHANNEL) { // Driver Clock Pin. // Enable Dir/Step pin - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA, false); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA); HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); // Diag0 pin - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC, true); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type); GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } else { // Enable Dir/Step pin - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA, false); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA); HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); // Enable/Dir/Step pin - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB, false); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB); HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); // Diag0 pin - GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB, true); + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type); GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); }