From 4169f60b4b3fb6ac65c939df9170b09ae8e56efd Mon Sep 17 00:00:00 2001 From: pmoegenburg Date: Wed, 21 Aug 2024 13:39:27 -0400 Subject: [PATCH] Revert "fix(motor-control): Revert motor driver chip errors (#777)" (#796) * Revert "fix(motor-control): Revert motor driver chip errors (#777)" This reverts commit 30ff7703627e1654053cffc9835310ef200d23b7. * formatted * added CAN msg for debugging * fix: use namespacing * eliminated interrupt line checks * proposed change * eliminated debounce for testing * update * cleanup * refactor --- .github/workflows/build-tests.yaml | 2 +- gantry/core/tasks_proto.cpp | 24 +++- gantry/core/tasks_rev1.cpp | 24 +++- gantry/firmware/interfaces_proto.cpp | 37 +++-- gantry/firmware/interfaces_rev1.cpp | 39 +++-- gantry/firmware/main_proto.cpp | 6 +- gantry/firmware/main_rev1.cpp | 6 +- gantry/firmware/motor_hardware.c | 27 +++- gantry/firmware/motor_hardware.h | 3 +- gantry/firmware/stm32g4xx_it.c | 1 + gantry/simulator/interfaces.cpp | 8 +- gantry/simulator/main.cpp | 6 +- gripper/core/tasks.cpp | 11 +- gripper/core/tasks_z.cpp | 22 ++- gripper/firmware/interfaces_z_motor.cpp | 19 ++- gripper/firmware/main_proto.cpp | 6 +- gripper/firmware/main_rev1.cpp | 6 +- gripper/firmware/motor_hardware.h | 2 + gripper/firmware/motor_hardware_shared.c | 18 ++- gripper/firmware/stm32g4xx_it.c | 5 + gripper/firmware/utility_gpio.c | 7 + gripper/simulator/interfaces.cpp | 8 +- gripper/simulator/main.cpp | 6 +- head/core/can_task.cpp | 3 +- head/core/tasks_proto.cpp | 34 ++++- head/core/tasks_rev1.cpp | 34 ++++- head/firmware/main_proto.cpp | 57 +++++--- head/firmware/main_rev1.cpp | 44 ++++-- head/firmware/motor_hardware.h | 5 +- head/firmware/motor_hardware_common.c | 39 ++++- head/firmware/stm32g4xx_it.c | 8 ++ head/simulator/main.cpp | 10 +- include/can/core/ids.hpp | 4 + include/can/core/messages.hpp | 38 ++++- include/gantry/core/can_task.hpp | 3 +- include/gantry/core/interfaces_proto.hpp | 4 +- include/gantry/core/interfaces_rev1.hpp | 4 +- include/gantry/core/queues.hpp | 2 + include/gantry/core/tasks_proto.hpp | 8 +- include/gantry/core/tasks_rev1.hpp | 8 +- include/gripper/core/can_task.hpp | 3 +- include/gripper/core/interfaces.hpp | 4 +- include/gripper/core/tasks.hpp | 14 +- include/gripper/firmware/utility_gpio.h | 1 + include/gripper/simulation/sim_interfaces.hpp | 2 + include/head/core/queues.hpp | 2 + include/head/core/tasks_proto.hpp | 8 +- include/head/core/tasks_rev1.hpp | 8 +- .../brushed_motion_controller.hpp | 7 +- .../brushed_motor_interrupt_handler.hpp | 6 +- .../core/motor_hardware_interface.hpp | 12 +- .../core/stepper_motor/motion_controller.hpp | 33 ++++- .../stepper_motor/motor_interrupt_handler.hpp | 20 ++- .../core/stepper_motor/tmc2130_driver.hpp | 7 +- .../core/stepper_motor/tmc2160.hpp | 16 +++ .../core/stepper_motor/tmc2160_driver.hpp | 26 +++- .../tasks/gear_tmc2160_motor_driver_task.hpp | 33 ++++- include/motor-control/core/tasks/messages.hpp | 13 +- .../core/tasks/motion_controller_task.hpp | 133 +++++++++++++++--- .../core/tasks/tmc2130_motor_driver_task.hpp | 34 ++++- .../core/tasks/tmc2160_motor_driver_task.hpp | 44 +++++- .../core/tasks/tmc_motor_driver_common.hpp | 11 +- .../brushed_motor/brushed_motor_hardware.hpp | 20 ++- .../firmware/stepper_motor/motor_hardware.hpp | 25 +++- .../simulation/motor_interrupt_driver.hpp | 5 +- .../simulation/sim_motor_hardware_iface.hpp | 61 ++++++-- .../tests/mock_brushed_motor_components.hpp | 23 ++- .../tests/mock_motor_driver_client.hpp | 21 +++ .../tests/mock_motor_hardware.hpp | 23 ++- include/pipettes/core/dispatch_builder.hpp | 7 +- include/pipettes/core/gear_motor_tasks.hpp | 10 +- include/pipettes/core/interfaces.hpp | 2 + include/pipettes/core/linear_motor_tasks.hpp | 21 ++- include/pipettes/core/tasks.hpp | 2 + include/pipettes/core/tasks/messages.hpp | 7 +- .../core/tasks/motion_controller_task.hpp | 123 +++++++++++++--- include/pipettes/firmware/interfaces.hpp | 20 +-- include/pipettes/firmware/interfaces_g4.hpp | 20 +-- include/pipettes/simulator/interfaces.hpp | 44 ++++-- include/spi/core/utils.hpp | 35 ++++- include/spi/core/writer.hpp | 14 +- .../brushed_motor/brushed_motor_hardware.cpp | 2 + .../firmware/stepper_motor/motor_hardware.cpp | 2 + .../test_brushed_motor_interrupt_handler.cpp | 4 +- motor-control/tests/test_limit_switch.cpp | 10 +- .../tests/test_limit_switch_backoff.cpp | 10 +- .../tests/test_motor_interrupt_handler.cpp | 9 +- .../tests/test_motor_interrupt_queue.cpp | 6 +- motor-control/tests/test_motor_pulse.cpp | 10 +- .../tests/test_motor_stall_handling.cpp | 10 +- motor-control/tests/test_sync_handling.cpp | 10 +- pipettes/core/gear_motor_tasks.cpp | 35 ++++- pipettes/core/linear_motor_tasks.cpp | 29 ++-- pipettes/firmware/hardware_config.c | 21 ++- pipettes/firmware/hardware_config.h | 2 + pipettes/firmware/interfaces.cpp | 20 ++- pipettes/firmware/main.cpp | 32 ++++- pipettes/firmware/motor_configurations.cpp | 31 +++- pipettes/firmware/motor_hardware.c | 9 +- pipettes/firmware/stm32g4xx_it.c | 4 + pipettes/firmware/utility_gpio.c | 10 ++ pipettes/simulator/interfaces.cpp | 50 ++++--- sensors/tests/CMakeLists.txt | 2 +- 103 files changed, 1461 insertions(+), 375 deletions(-) create mode 100644 include/motor-control/tests/mock_motor_driver_client.hpp diff --git a/.github/workflows/build-tests.yaml b/.github/workflows/build-tests.yaml index b6fbfd09d..375c02434 100644 --- a/.github/workflows/build-tests.yaml +++ b/.github/workflows/build-tests.yaml @@ -18,7 +18,7 @@ jobs: build-and-test: name: Run all tests runs-on: "ubuntu-20.04" - timeout-minutes: 10 + timeout-minutes: 20 steps: - name: Checkout ot3-firmware repo uses: actions/checkout@v4 diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp index de020ffc3..965adbd0f 100644 --- a/gantry/core/tasks_proto.cpp +++ b/gantry/core/tasks_proto.cpp @@ -55,18 +55,20 @@ static auto eeprom_data_rev_update_builder = /** * Start gantry tasks. */ -void gantry::tasks::start_tasks( +auto gantry::tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, ::queues, ::queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, + ::queues, ::queues, ::queues, ::queues); auto& tmc2130_driver = motor_driver_task_builder.start( 5, "tmc2130 driver", driver_configs, ::queues, spi_task_client); auto& move_group = @@ -116,6 +118,15 @@ void gantry::tasks::start_tasks( ::queues.usage_storage_queue = &usage_storage_task.get_queue(); mh_tsk.start_task(); + + return gantry::tasks::call_run_diag0_interrupt; +} + +void gantry::tasks::call_run_diag0_interrupt() { + if (gantry::tasks::get_tasks().motion_controller) { + return gantry::tasks::get_tasks() + .motion_controller->run_diag0_interrupt(); + } } gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw) @@ -131,6 +142,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue( motor_driver_queue->try_write(m); } +void gantry::queues::QueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast(motor_driver_queue->try_write_isr(m)); +} + void gantry::queues::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp index 95c81ba29..4ba6f156c 100644 --- a/gantry/core/tasks_rev1.cpp +++ b/gantry/core/tasks_rev1.cpp @@ -54,18 +54,20 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues}; /** * Start gantry ::tasks. */ -void gantry::tasks::start_tasks( +auto gantry::tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2160::configs::TMC2160DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, ::queues, ::queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, + ::queues, ::queues, ::queues, ::queues); auto& tmc2160_driver = motor_driver_task_builder.start( 5, "tmc2160 driver", driver_configs, ::queues, spi_task_client); auto& move_group = @@ -115,6 +117,15 @@ void gantry::tasks::start_tasks( ::queues.usage_storage_queue = &usage_storage_task.get_queue(); mh_tsk.start_task(); + + return gantry::tasks::call_run_diag0_interrupt; +} + +void gantry::tasks::call_run_diag0_interrupt() { + if (gantry::tasks::get_tasks().motion_controller) { + return gantry::tasks::get_tasks() + .motion_controller->run_diag0_interrupt(); + } } gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw) @@ -130,6 +141,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue( motor_driver_queue->try_write(m); } +void gantry::queues::QueueClient::send_motor_driver_queue_isr( + const tmc2160::tasks::TaskMessage& m) { + static_cast(motor_driver_queue->try_write_isr(m)); +} + void gantry::queues::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp index 584fd990e..0c142d197 100644 --- a/gantry/firmware/interfaces_proto.cpp +++ b/gantry/firmware/interfaces_proto.cpp @@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; @@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y { .port = GPIOB, .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x18, .hold_current_delay = 0x7}, @@ -156,7 +168,7 @@ static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{ }}; static tmc2130::configs::TMC2130DriverConfig gantry_y_driver_configs{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x18, .hold_current_delay = 0x7}, @@ -232,8 +244,8 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_hardware_iface, stallcheck, update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -269,13 +281,14 @@ static constexpr auto can_bit_timings = can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100, 500 * can::bit_timings::KHZ, 800>{}; -void interfaces::initialize() { +void interfaces::initialize(diag0_handler* call_diag0_handler) { // Initialize SPI if (initialize_spi(get_axis_type()) != HAL_OK) { Error_Handler(); } - initialize_timer(call_motor_handler, enc_overflow_callback); + initialize_timer(call_motor_handler, call_diag0_handler, + enc_overflow_callback); // Start the can bus canbus.start(can_bit_timings); diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp index bf0de3359..d34b82c10 100644 --- a/gantry/firmware/interfaces_rev1.cpp +++ b/gantry/firmware/interfaces_rev1.cpp @@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; @@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -159,6 +171,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ .freewheel = 0, .pwm_reg = 0x7, .pwm_lim = 0xC}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xA7}}, .current_config = { @@ -172,7 +185,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ }}; static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -198,6 +211,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{ .freewheel = 0, .pwm_reg = 0x7, .pwm_lim = 0xC}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xA7}}, .current_config = { @@ -257,8 +271,8 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_hardware_iface, stallcheck, update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -295,13 +309,14 @@ static constexpr auto can_bit_timings = can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100, 500 * can::bit_timings::KHZ, 800>{}; -void interfaces::initialize() { +void interfaces::initialize(diag0_handler* call_diag0_handler) { // Initialize SPI if (initialize_spi(get_axis_type()) != HAL_OK) { Error_Handler(); } - initialize_timer(call_motor_handler, enc_overflow_callback); + initialize_timer(call_motor_handler, call_diag0_handler, + enc_overflow_callback); // Start the can bus canbus.start(can_bit_timings); diff --git a/gantry/firmware/main_proto.cpp b/gantry/firmware/main_proto.cpp index cf7c26e03..55d10cc16 100644 --- a/gantry/firmware/main_proto.cpp +++ b/gantry/firmware/main_proto.cpp @@ -16,6 +16,8 @@ #include "gantry/core/tasks_proto.hpp" #include "i2c/firmware/i2c_comms.hpp" +static interfaces::diag0_handler call_diag0_handler = nullptr; + static auto i2c_comms2 = i2c::hardware::I2C(); static auto i2c_handles = I2CHandlerStruct{}; @@ -44,11 +46,11 @@ auto main() -> int { app_update_clear_flags(); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); i2c_setup(&i2c_handles); i2c_comms2.set_handle(i2c_handles.i2c2); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface); diff --git a/gantry/firmware/main_rev1.cpp b/gantry/firmware/main_rev1.cpp index 39fed3b73..77b198539 100644 --- a/gantry/firmware/main_rev1.cpp +++ b/gantry/firmware/main_rev1.cpp @@ -16,6 +16,8 @@ #include "gantry/core/tasks_rev1.hpp" #include "i2c/firmware/i2c_comms.hpp" +static interfaces::diag0_handler call_diag0_handler = nullptr; + static auto i2c_comms2 = i2c::hardware::I2C(); static auto i2c_handles = I2CHandlerStruct{}; @@ -44,11 +46,11 @@ auto main() -> int { app_update_clear_flags(); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); i2c_setup(&i2c_handles); i2c_comms2.set_handle(i2c_handles.i2c2); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface); diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c index f48bcb6d7..637087c76 100644 --- a/gantry/firmware/motor_hardware.c +++ b/gantry/firmware/motor_hardware.c @@ -23,6 +23,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) { PC8 ------> Motor Step Pin Enable PA9 ------> Motor Enable Pin + DIAG0 + PC5 ------> Motor Diag0 Pin */ GPIO_InitStruct.Pin = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -54,6 +56,12 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(GPIOA, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // Diag0 + GPIO_InitStruct.Pin = GPIO_PIN_5; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } } SPI_HandleTypeDef hspi2 = { @@ -87,7 +95,7 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) { HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_1); HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9); - HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8); + HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8 | GPIO_PIN_5); } } @@ -119,6 +127,7 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) { static motor_interrupt_callback timer_callback = NULL; static encoder_overflow_callback enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_callback = NULL; /** @@ -140,6 +149,9 @@ void MX_GPIO_Init(void) { GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); } // motor timer: 200kHz from @@ -236,6 +248,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { } } +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + if (GPIO_Pin == GPIO_PIN_5) { + if (diag0_callback != NULL) { + if (*diag0_callback != NULL) { + (*diag0_callback)(); + } + } + } +} + void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) { if (htim == &htim2) { /* Peripheral clock enable */ @@ -255,9 +277,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) { } } -void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) { +void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) { timer_callback = callback; enc_overflow_callback = enc_callback; + diag0_callback = diag0_int_callback; MX_GPIO_Init(); MX_TIM7_Init(); Encoder_GPIO_Init(); diff --git a/gantry/firmware/motor_hardware.h b/gantry/firmware/motor_hardware.h index fa2e0ac71..3a3024ac6 100644 --- a/gantry/firmware/motor_hardware.h +++ b/gantry/firmware/motor_hardware.h @@ -14,11 +14,12 @@ extern TIM_HandleTypeDef htim2; typedef void (*motor_interrupt_callback)(); typedef void (*encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(enum GantryAxisType); void gantry_driver_CLK_init(enum GantryAxisType); -void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback); +void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback); #ifdef __cplusplus } // extern "C" diff --git a/gantry/firmware/stm32g4xx_it.c b/gantry/firmware/stm32g4xx_it.c index b4f589e8f..07463106c 100644 --- a/gantry/firmware/stm32g4xx_it.c +++ b/gantry/firmware/stm32g4xx_it.c @@ -147,6 +147,7 @@ void FDCAN1_IT0_IRQHandler(void) { */ void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); } void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); } +void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); } extern void xPortSysTickHandler(void); diff --git a/gantry/simulator/interfaces.cpp b/gantry/simulator/interfaces.cpp index f1a59f0bd..a22db5105 100644 --- a/gantry/simulator/interfaces.cpp +++ b/gantry/simulator/interfaces.cpp @@ -89,14 +89,16 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_interface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_interface, stallcheck, update_position_queue); static motor_interrupt_driver::MotorInterruptDriver A(motor_queue, motor_interrupt, motor_interface, update_position_queue); -void interfaces::initialize() {} +void interfaces::initialize(diag0_handler* call_diag0_handler) { + static_cast(call_diag0_handler); +} static po::variables_map options{}; diff --git a/gantry/simulator/main.cpp b/gantry/simulator/main.cpp index d78b931e2..77a2d985d 100644 --- a/gantry/simulator/main.cpp +++ b/gantry/simulator/main.cpp @@ -12,6 +12,8 @@ void signal_handler(int signum) { exit(signum); } +static interfaces::diag0_handler call_diag0_handler = NULL; + int main(int argc, char** argv) { signal(SIGINT, signal_handler); @@ -21,10 +23,10 @@ int main(int argc, char** argv) { return pcTaskGetName(xTaskGetCurrentTaskHandle()); }); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); interfaces::initialize_sim(argc, argv); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), *interfaces::get_sim_i2c2(), diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp index 9c9ea66ed..ae3dd7d2b 100644 --- a/gripper/core/tasks.cpp +++ b/gripper/core/tasks.cpp @@ -62,7 +62,7 @@ static auto eeprom_data_rev_update_builder = /** * Start gripper tasks. */ -void gripper_tasks::start_tasks( +auto gripper_tasks::start_tasks( can::bus::CanBus& can_bus, motor_class::Motor& z_motor, brushed_motor::BrushedMotor& grip_motor, @@ -72,7 +72,8 @@ void gripper_tasks::start_tasks( sensors::hardware::SensorHardwareBase& sensor_hardware, eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface, motor_hardware_task::MotorHardwareTask& zmh_tsk, - motor_hardware_task::MotorHardwareTask& gmh_tsk) { + motor_hardware_task::MotorHardwareTask& gmh_tsk) + -> z_motor_iface::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); tasks.can_writer = &can_writer; @@ -131,8 +132,8 @@ void gripper_tasks::start_tasks( queues.capacitive_sensor_queue_rear = &capacitive_sensor_task_rear.get_queue(); - z_tasks::start_task(z_motor, spi_device, driver_configs, tasks, queues, - tail_accessor); + auto diag0_handler = z_tasks::start_task( + z_motor, spi_device, driver_configs, tasks, queues, tail_accessor); g_tasks::start_task(grip_motor, tasks, queues, tail_accessor); @@ -141,6 +142,8 @@ void gripper_tasks::start_tasks( zmh_tsk.start_task(); gmh_tsk.start_task(); + + return diag0_handler; } gripper_tasks::QueueClient::QueueClient(can::ids::NodeId this_fw) diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp index 43432ea46..ce5f9b306 100644 --- a/gripper/core/tasks_z.cpp +++ b/gripper/core/tasks_z.cpp @@ -34,15 +34,16 @@ static auto z_usage_storage_task_builder = freertos_task::TaskStarter<512, usage_storage_task::UsageStorageTask>{}; #endif -void z_tasks::start_task( +auto z_tasks::start_task( motor_class::Motor& z_motor, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, AllTask& gripper_tasks, gripper_tasks::QueueClient& main_queues, eeprom::dev_data::DevDataTailAccessor& - tail_accessor) { - auto& motion = mc_task_builder.start(5, "z mc", z_motor.motion_controller, - z_queues, z_queues); + tail_accessor) -> z_motor_iface::diag0_handler { + auto& motion = + mc_task_builder.start(5, "z mc", z_motor.motion_controller, z_queues, + z_queues, z_queues, z_queues); auto& move_group = move_group_task_builder.start(5, "move group", z_queues, z_queues); auto& tmc2130_driver = motor_driver_task_builder.start( @@ -77,6 +78,14 @@ void z_tasks::start_task( z_queues.z_usage_storage_queue = &z_usage_storage_task.get_queue(); #endif spi_task_client.set_queue(&spi_task.get_queue()); + + return z_tasks::call_run_diag0_interrupt; +} + +void z_tasks::call_run_diag0_interrupt() { + if (get_all_tasks().motion_controller) { + return get_all_tasks().motion_controller->run_diag0_interrupt(); + } } z_tasks::QueueClient::QueueClient() @@ -92,6 +101,11 @@ void z_tasks::QueueClient::send_motor_driver_queue( tmc2130_driver_queue->try_write(m); } +void z_tasks::QueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast(tmc2130_driver_queue->try_write_isr(m)); +} + void z_tasks::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp index 69405dfde..a979ffca0 100644 --- a/gripper/firmware/interfaces_z_motor.cpp +++ b/gripper/firmware/interfaces_z_motor.cpp @@ -103,7 +103,13 @@ struct motion_controller::HardwareConfig motor_pins { .port = ESTOP_IN_PORT, .pin = ESTOP_IN_PIN, .active_setting = GPIO_PIN_RESET}, - .ebrake = ebrake, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_2, + .active_setting = GPIO_PIN_RESET}, + .ebrake = ebrake }; /** @@ -119,6 +125,7 @@ static motor_hardware::MotorHardware motor_hardware_iface(motor_pins, &htim7, */ static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{ .registers = {.gconfig = {.en_pwm_mode = 0x0, + .diag0_error = 1, .stop_enable = use_stop_enable}, .ihold_irun = {.hold_current = 0x2, // 0.177A .run_current = 0xA, // 0.648A @@ -196,8 +203,9 @@ static motor_class::Motor z_motor{ * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gripper_tasks::z_tasks::get_queues(), motor_hardware_iface, - stallcheck, update_position_queue); + motor_queue, gripper_tasks::z_tasks::get_queues(), + gripper_tasks::z_tasks::get_queues(), motor_hardware_iface, stallcheck, + update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -210,12 +218,13 @@ extern "C" void call_enc_handler(int32_t direction) { motor_hardware_iface.encoder_overflow(direction); } -void z_motor_iface::initialize() { +void z_motor_iface::initialize(diag0_handler* call_diag0_handler) { if (initialize_spi() != HAL_OK) { Error_Handler(); } initialize_hardware_z(); - set_z_motor_timer_callback(call_motor_handler, call_enc_handler); + set_z_motor_timer_callback(call_motor_handler, call_diag0_handler, + call_enc_handler); encoder_background_timer.start(); } diff --git a/gripper/firmware/main_proto.cpp b/gripper/firmware/main_proto.cpp index 96328dca9..c4a249b0c 100644 --- a/gripper/firmware/main_proto.cpp +++ b/gripper/firmware/main_proto.cpp @@ -25,6 +25,8 @@ #include "i2c/firmware/i2c_comms.hpp" #include "sensors/firmware/sensor_hardware.hpp" +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + static auto iWatchdog = iwdg::IndependentWatchDog{}; /** @@ -100,7 +102,7 @@ auto main() -> int { app_update_clear_flags(); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); i2c_setup(&i2c_handles); @@ -108,7 +110,7 @@ auto main() -> int { i2c_comms3.set_handle(i2c_handles.i2c3); canbus.start(can_bit_timings); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3, diff --git a/gripper/firmware/main_rev1.cpp b/gripper/firmware/main_rev1.cpp index b15eafec9..5e351ee53 100644 --- a/gripper/firmware/main_rev1.cpp +++ b/gripper/firmware/main_rev1.cpp @@ -26,6 +26,8 @@ #include "i2c/firmware/i2c_comms.hpp" #include "sensors/firmware/sensor_hardware.hpp" +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + static auto iWatchdog = iwdg::IndependentWatchDog{}; /** @@ -111,7 +113,7 @@ auto main() -> int { app_update_clear_flags(); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); i2c_setup(&i2c_handles); @@ -119,7 +121,7 @@ auto main() -> int { i2c_comms3.set_handle(i2c_handles.i2c3); canbus.start(can_bit_timings); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3, diff --git a/gripper/firmware/motor_hardware.h b/gripper/firmware/motor_hardware.h index 37bce25ca..4df6dbd0a 100644 --- a/gripper/firmware/motor_hardware.h +++ b/gripper/firmware/motor_hardware.h @@ -32,6 +32,7 @@ extern TIM_HandleTypeDef htim8; typedef void (*motor_interrupt_callback)(); typedef void (*z_encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(); @@ -39,6 +40,7 @@ void initialize_hardware_z(); void set_z_motor_timer_callback( motor_interrupt_callback callback, + diag0_interrupt_callback* diag0_int_callback, z_encoder_overflow_callback enc_callback); diff --git a/gripper/firmware/motor_hardware_shared.c b/gripper/firmware/motor_hardware_shared.c index 997322797..f6118af8b 100644 --- a/gripper/firmware/motor_hardware_shared.c +++ b/gripper/firmware/motor_hardware_shared.c @@ -3,6 +3,7 @@ static motor_interrupt_callback timer_callback = NULL; static z_encoder_overflow_callback z_enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_callback = NULL; static brushed_motor_interrupt_callback brushed_timer_callback = NULL; static encoder_overflow_callback gripper_enc_overflow_callback = NULL; static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback = @@ -10,11 +11,19 @@ static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback = static stopwatch_overflow_callback gripper_force_stopwatch_overflow_callback = NULL; +void MX_GPIO_Init(void) { + HAL_NVIC_SetPriority(EXTI2_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI2_IRQn); +} + void set_z_motor_timer_callback( motor_interrupt_callback callback, + diag0_interrupt_callback* diag0_int_callback, z_encoder_overflow_callback enc_callback) { timer_callback = callback; z_enc_overflow_callback = enc_callback; + diag0_callback = diag0_int_callback; + MX_GPIO_Init(); } void set_brushed_motor_timer_callback( @@ -212,9 +221,14 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) { } } - void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { - if (GPIO_Pin == ESTOP_IN_PIN) { + if (GPIO_Pin == GPIO_PIN_2) { + if (diag0_callback != NULL) { + if (*diag0_callback != NULL) { + (*diag0_callback)(); + } + } + } else 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 diff --git a/gripper/firmware/stm32g4xx_it.c b/gripper/firmware/stm32g4xx_it.c index 2051ebc6b..6a90023e6 100644 --- a/gripper/firmware/stm32g4xx_it.c +++ b/gripper/firmware/stm32g4xx_it.c @@ -198,6 +198,11 @@ void TIM7_IRQHandler(void) { call_motor_handler(); } +__attribute__((section(".ccmram"))) +void EXTI2_IRQHandler(void) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); +} + /** * @brief This function handles EXTI line[15:10] interrupts. */ diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c index 551f5cc12..9c25e0b46 100644 --- a/gripper/firmware/utility_gpio.c +++ b/gripper/firmware/utility_gpio.c @@ -108,6 +108,8 @@ static void z_motor_gpio_init(void) { PB1 ------> Motor Step Pin Enable PA9 ------> Motor Enable Pin + Diag0 + PB2 ------> Motor Diag0 Pin */ GPIO_InitStruct.Pin = Z_MOT_DIR_PIN | Z_MOT_STEP_PIN; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; @@ -119,6 +121,11 @@ static void z_motor_gpio_init(void) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(Z_MOT_ENABLE_PORT, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + GPIO_InitStruct.Pin = Z_MOT_DIAG0_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(Z_MOT_STEPDIR_PORT, &GPIO_InitStruct); } #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a' diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp index 085c4ad77..104d0575a 100644 --- a/gripper/simulator/interfaces.cpp +++ b/gripper/simulator/interfaces.cpp @@ -101,8 +101,9 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gripper_tasks::z_tasks::get_queues(), motor_interface, - stallcheck, update_position_queue); + motor_queue, gripper_tasks::z_tasks::get_queues(), + gripper_tasks::z_tasks::get_queues(), motor_interface, stallcheck, + update_position_queue); static motor_interrupt_driver::MotorInterruptDriver A(motor_queue, motor_interrupt, @@ -142,7 +143,8 @@ static brushed_motor_handler::BrushedMotorInterruptHandler static brushed_motor_interrupt_driver::BrushedMotorInterruptDriver G( brushed_motor_queue, brushed_motor_interrupt, brushed_motor_hardware_iface); -void z_motor_iface::initialize() { +void z_motor_iface::initialize(diag0_handler* call_diag0_handler) { + static_cast(call_diag0_handler); motor_interface.provide_mech_config(z_motor_sys_config); }; diff --git a/gripper/simulator/main.cpp b/gripper/simulator/main.cpp index 23ff9edff..6b2a93db5 100644 --- a/gripper/simulator/main.cpp +++ b/gripper/simulator/main.cpp @@ -21,6 +21,8 @@ namespace po = boost::program_options; +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + void signal_handler(int signum) { LOG("Interrupt signal (%d) received.", signum); exit(signum); @@ -114,9 +116,9 @@ int main(int argc, char** argv) { auto i2c3 = std::make_shared(i2c_device_map); static auto canbus = can::sim::bus::SimCANBus(can::sim::transport::create(options)); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c2, *i2c3, diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp index 6cc432b07..dcb1f3359 100644 --- a/head/core/can_task.cpp +++ b/head/core/can_task.cpp @@ -28,7 +28,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< head_tasks::MotorQueueClient>, diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp index 2690550ca..c3c09d968 100644 --- a/head/core/tasks_proto.cpp +++ b/head/core/tasks_proto.cpp @@ -78,10 +78,25 @@ static auto eeprom_data_rev_update_builder = freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{}; static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{head_queues}; + +void call_run_diag0_z_interrupt() { + if (head_tasks::get_left_tasks().motion_controller) { + return head_tasks::get_left_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +void call_run_diag0_a_interrupt() { + if (head_tasks::get_right_tasks().motion_controller) { + return head_tasks::get_right_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + /** * Start head tasks. */ -void head_tasks::start_tasks( +auto head_tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& left_motion_controller, @@ -95,7 +110,8 @@ void head_tasks::start_tasks( motor_hardware_task::MotorHardwareTask& rmh_tsk, motor_hardware_task::MotorHardwareTask& lmh_tsk, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple { // Start the head tasks auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); @@ -141,7 +157,8 @@ void head_tasks::start_tasks( // Start the left motor tasks auto& left_motion = left_mc_task_builder.start( - 5, "left mc", left_motion_controller, left_queues, left_queues); + 5, "left mc", left_motion_controller, left_queues, left_queues, + left_queues, left_queues); auto& left_tmc2130_driver = left_motor_driver_task_builder.start( 5, "left motor driver", left_driver_configs, left_queues, spi3_task_client); @@ -170,7 +187,8 @@ void head_tasks::start_tasks( // Start the right motor tasks auto& right_motion = right_mc_task_builder.start( - 5, "right mc", right_motion_controller, right_queues, right_queues); + 5, "right mc", right_motion_controller, right_queues, right_queues, + right_queues, right_queues); auto& right_tmc2130_driver = right_motor_driver_task_builder.start( 5, "right motor driver", right_driver_configs, right_queues, spi2_task_client); @@ -199,6 +217,9 @@ void head_tasks::start_tasks( right_queues.move_status_report_queue = &right_move_status_reporter.get_queue(); right_queues.usage_storage_queue = &right_usage_storage_task.get_queue(); + + return std::make_tuple(call_run_diag0_z_interrupt, + call_run_diag0_a_interrupt); } // Implementation of HeadQueueClient @@ -231,6 +252,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue( motor_queue->try_write(m); } +void head_tasks::MotorQueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast(motor_queue->try_write_isr(m)); +} + void head_tasks::MotorQueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp index 2cf2d803d..3e417f116 100644 --- a/head/core/tasks_rev1.cpp +++ b/head/core/tasks_rev1.cpp @@ -83,10 +83,25 @@ static auto right_usage_storage_task_builder = static auto eeprom_data_rev_update_builder = freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{}; #endif + +void call_run_diag0_z_interrupt() { + if (head_tasks::get_left_tasks().motion_controller) { + return head_tasks::get_left_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +void call_run_diag0_a_interrupt() { + if (head_tasks::get_right_tasks().motion_controller) { + return head_tasks::get_right_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + /** * Start head tasks. */ -void head_tasks::start_tasks( +auto head_tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& left_motion_controller, @@ -100,7 +115,8 @@ void head_tasks::start_tasks( motor_hardware_task::MotorHardwareTask& rmh_tsk, motor_hardware_task::MotorHardwareTask& lmh_tsk, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple { // Start the head tasks auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); @@ -146,7 +162,8 @@ void head_tasks::start_tasks( #endif // Start the left motor tasks auto& left_motion = left_mc_task_builder.start( - 5, "left mc", left_motion_controller, left_queues, left_queues); + 5, "left mc", left_motion_controller, left_queues, left_queues, + left_queues, left_queues); auto& left_tmc2160_driver = left_motor_driver_task_builder.start( 5, "left motor driver", left_driver_configs, left_queues, spi3_task_client); @@ -183,7 +200,8 @@ void head_tasks::start_tasks( // Start the right motor tasks auto& right_motion = right_mc_task_builder.start( - 5, "right mc", right_motion_controller, right_queues, right_queues); + 5, "right mc", right_motion_controller, right_queues, right_queues, + right_queues, right_queues); auto& right_tmc2160_driver = right_motor_driver_task_builder.start( 5, "right motor driver", right_driver_configs, right_queues, spi2_task_client); @@ -220,6 +238,9 @@ void head_tasks::start_tasks( #if PCBA_PRIMARY_REVISION != 'b' right_queues.usage_storage_queue = &right_usage_storage_task.get_queue(); #endif + + return std::make_tuple(call_run_diag0_z_interrupt, + call_run_diag0_a_interrupt); } // Implementation of HeadQueueClient @@ -256,6 +277,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue( motor_queue->try_write(m); } +void head_tasks::MotorQueueClient::send_motor_driver_queue_isr( + const tmc::tasks::TaskMessage& m) { + static_cast(motor_queue->try_write_isr(m)); +} + void head_tasks::MotorQueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp index d3ba07641..614af1142 100644 --- a/head/firmware/main_proto.cpp +++ b/head/firmware/main_proto.cpp @@ -1,6 +1,7 @@ #include #include #include +#include // clang-format off #include "FreeRTOS.h" @@ -40,6 +41,9 @@ static auto iWatchdog = iwdg::IndependentWatchDog{}; +static head_tasks::diag0_handler call_diag0_z_handler = nullptr; +static head_tasks::diag0_handler call_diag0_a_handler = nullptr; + static auto can_bus_1 = can::hal::bus::HalCanBus( can_get_device_handle(), gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -142,10 +146,16 @@ struct motor_hardware::HardwareConfig pin_configurations_left { .port = GPIOA, .pin = GPIO_PIN_8, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_4, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOB, - .pin = GPIO_PIN_4, + .port = GPIOC, + .pin = GPIO_PIN_13, .active_setting = GPIO_PIN_RESET} }; @@ -186,10 +196,16 @@ struct motor_hardware::HardwareConfig pin_configurations_right { .port = GPIOA, .pin = GPIO_PIN_8, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_4, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOB, - .pin = GPIO_PIN_4, + .port = GPIOC, + .pin = GPIO_PIN_15, .active_setting = GPIO_PIN_RESET} }; @@ -197,7 +213,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right { static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{ .registers = { - .gconfig = {.en_pwm_mode = 1}, + .gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -224,7 +240,7 @@ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_left{ .registers = { - .gconfig = {.en_pwm_mode = 1}, + .gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -268,8 +284,9 @@ static stall_check::StallCheck stallcheck_right( static motor_hardware::MotorHardware motor_hardware_right( pin_configurations_right, &htim7, &htim2, right_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right, + update_position_queue_right); static auto encoder_background_timer_right = motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right); @@ -289,8 +306,9 @@ static stall_check::StallCheck stallcheck_left( static motor_hardware::MotorHardware motor_hardware_left( pin_configurations_left, &htim7, &htim3, left_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left, + update_position_queue_left); static auto encoder_background_timer_left = motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left); @@ -393,7 +411,8 @@ auto main() -> int { app_update_clear_flags(); initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue, - right_enc_overflow_callback_glue); + right_enc_overflow_callback_glue, &call_diag0_z_handler, + &call_diag0_a_handler); if (initialize_spi(&hspi2) != HAL_OK) { Error_Handler(); @@ -407,12 +426,12 @@ auto main() -> int { i2c_setup(&i2c_handles); i2c_comms3.set_handle(i2c_handles.i2c3); - - head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, - motor_right.motion_controller, psd, spi_comms2, - spi_comms3, motor_driver_configs_left, - motor_driver_configs_right, rmh_tsk, lmh_tsk, - i2c_comms3, eeprom_hw_iface); + std::tie(call_diag0_z_handler, call_diag0_a_handler) = + head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, + motor_right.motion_controller, psd, spi_comms2, + spi_comms3, motor_driver_configs_left, + motor_driver_configs_right, rmh_tsk, lmh_tsk, + i2c_comms3, eeprom_hw_iface); timer_for_notifier.start(); diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp index ce5849129..e6d17776f 100644 --- a/head/firmware/main_rev1.cpp +++ b/head/firmware/main_rev1.cpp @@ -1,6 +1,7 @@ #include #include #include +#include // clang-format off #include "FreeRTOS.h" @@ -40,6 +41,9 @@ static auto iWatchdog = iwdg::IndependentWatchDog{}; +static head_tasks::diag0_handler call_diag0_z_handler = nullptr; +static head_tasks::diag0_handler call_diag0_a_handler = nullptr; + static auto can_bus_1 = can::hal::bus::HalCanBus( can_get_device_handle(), gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -154,6 +158,12 @@ struct motor_hardware::HardwareConfig pin_configurations_left { .port = GPIOB, .pin = GPIO_PIN_4, .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_13, + .active_setting = GPIO_PIN_RESET}, .ebrake = gpio::PinConfig { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET @@ -198,6 +208,12 @@ struct motor_hardware::HardwareConfig pin_configurations_right { .port = GPIOB, .pin = GPIO_PIN_4, .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_15, + .active_setting = GPIO_PIN_RESET}, .ebrake = gpio::PinConfig { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_0, .active_setting = GPIO_PIN_RESET @@ -206,7 +222,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right { // TODO clean up the head main file by using interfaces. static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -232,7 +248,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{ }}; static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_left{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -286,8 +302,9 @@ static stall_check::StallCheck stallcheck_right( static motor_hardware::MotorHardware motor_hardware_right( pin_configurations_right, &htim7, &htim2, right_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right, + update_position_queue_right); static auto encoder_background_timer_right = motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right); @@ -311,8 +328,9 @@ static stall_check::StallCheck stallcheck_left( static motor_hardware::MotorHardware motor_hardware_left( pin_configurations_left, &htim7, &htim3, left_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left, + update_position_queue_left); static auto encoder_background_timer_left = motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left); @@ -418,7 +436,8 @@ auto main() -> int { app_update_clear_flags(); initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue, - right_enc_overflow_callback_glue); + right_enc_overflow_callback_glue, &call_diag0_z_handler, + &call_diag0_a_handler); i2c_setup(&i2c_handles); i2c_comms3.set_handle(i2c_handles.i2c3); @@ -432,11 +451,12 @@ auto main() -> int { utility_gpio_init(); can_bus_1.start(can_bit_timings); - head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, - motor_right.motion_controller, psd, spi_comms2, - spi_comms3, motor_driver_configs_left, - motor_driver_configs_right, rmh_tsk, lmh_tsk, - i2c_comms3, eeprom_hw_iface); + std::tie(call_diag0_z_handler, call_diag0_a_handler) = + head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, + motor_right.motion_controller, psd, spi_comms2, + spi_comms3, motor_driver_configs_left, + motor_driver_configs_right, rmh_tsk, lmh_tsk, + i2c_comms3, eeprom_hw_iface); timer_for_notifier.start(); diff --git a/head/firmware/motor_hardware.h b/head/firmware/motor_hardware.h index e01e68d85..2cce7382f 100644 --- a/head/firmware/motor_hardware.h +++ b/head/firmware/motor_hardware.h @@ -14,11 +14,14 @@ extern TIM_HandleTypeDef htim3; typedef void (*motor_interrupt_callback)(); typedef void (*encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(SPI_HandleTypeDef* hspi); void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback left_enc_overflow_callback, - encoder_overflow_callback right_enc_overflow_callback); + encoder_overflow_callback right_enc_overflow_callback, + diag0_interrupt_callback* diag0_z_int_callback, + diag0_interrupt_callback* diag0_a_int_callback); void initialize_rev_specific_pins(); #ifdef __cplusplus diff --git a/head/firmware/motor_hardware_common.c b/head/firmware/motor_hardware_common.c index d1313df4d..e895252fe 100644 --- a/head/firmware/motor_hardware_common.c +++ b/head/firmware/motor_hardware_common.c @@ -21,6 +21,8 @@ TIM_HandleTypeDef htim3 = { motor_interrupt_callback motor_callback = NULL; encoder_overflow_callback left_enc_overflow_callback = NULL; encoder_overflow_callback right_enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_z_callback = NULL; +static diag0_interrupt_callback* diag0_a_callback = NULL; void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -55,6 +57,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // A motor diag0 + GPIO_InitStruct.Pin = GPIO_PIN_15; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } else if (hspi->Instance == SPI3) { @@ -91,6 +101,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // Z motor diag0 + GPIO_InitStruct.Pin = GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } } @@ -229,6 +247,9 @@ void MX_GPIO_Init(void) { __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); initialize_rev_specific_pins(); + + HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); } #if PCBA_PRIMARY_REVISION == 'b' || PCBA_PRIMARY_REVISION == 'a' @@ -345,15 +366,31 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { // 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); + } else if (GPIO_Pin == GPIO_PIN_13) { + if (diag0_z_callback != NULL) { + if (*diag0_z_callback != NULL) { + (*diag0_z_callback)(); + } + } + } else if (GPIO_Pin == GPIO_PIN_15) { + if (diag0_a_callback != NULL) { + if (*diag0_a_callback != NULL) { + (*diag0_a_callback)(); + } + } } } void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback l_f_callback, - encoder_overflow_callback r_f_callback) { + encoder_overflow_callback r_f_callback, + diag0_interrupt_callback* diag0_z_int_callback, + diag0_interrupt_callback* diag0_a_int_callback) { motor_callback = callback; left_enc_overflow_callback = l_f_callback; right_enc_overflow_callback = r_f_callback; + diag0_z_callback = diag0_z_int_callback; + diag0_a_callback = diag0_a_int_callback; MX_GPIO_Init(); Encoder_GPIO_Init(); encoder_init(&htim2); diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c index 6e031a2bf..929586b89 100644 --- a/head/firmware/stm32g4xx_it.c +++ b/head/firmware/stm32g4xx_it.c @@ -177,6 +177,14 @@ void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); } */ void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } +void EXTI15_10_IRQHandler(void) { + if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_13)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13); + } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_15)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); + } +} + extern void xPortSysTickHandler(void); void SysTick_Handler(void) { HAL_IncTick(); diff --git a/head/simulator/main.cpp b/head/simulator/main.cpp index 79db8f736..907f60bdb 100644 --- a/head/simulator/main.cpp +++ b/head/simulator/main.cpp @@ -94,8 +94,9 @@ static stall_check::StallCheck stallcheck_right( linear_config.get_usteps_per_mm() / 1000.0F, utils::STALL_THRESHOLD_UM); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_interface_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_interface_right, stallcheck_right, + update_position_queue_right); static stall_check::StallCheck stallcheck_left( linear_config.get_encoder_pulses_per_mm() / 1000.0F, @@ -117,8 +118,9 @@ static motor_class::Motor motor_right{ motor_queue_right, update_position_queue_right}; static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_interface_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_interface_left, stallcheck_left, + update_position_queue_left); static motor_class::Motor motor_left{ motor_sys_config, motor_interface_left, diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index 636a70cdd..d5b3a2d48 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -64,6 +64,8 @@ enum class MessageId { write_motor_current_request = 0x33, read_motor_current_request = 0x34, read_motor_current_response = 0x35, + read_motor_driver_error_status_request = 0x36, + read_motor_driver_error_status_response = 0x37, set_brushed_motor_vref_request = 0x40, set_brushed_motor_pwm_request = 0x41, gripper_grip_request = 0x42, @@ -169,11 +171,13 @@ enum class ErrorCode { over_pressure = 0xd, door_open = 0xe, reed_open = 0xf, + motor_driver_error_detected = 0x10, safety_relay_inactive = 0x11, }; /** Error Severity levels. */ enum class ErrorSeverity { + none = 0x0, warning = 0x1, recoverable = 0x2, unrecoverable = 0x3, diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 3ad1da7c5..71654f7de 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -646,6 +646,41 @@ struct ReadMotorDriverRegisterResponse -> bool = default; }; +struct ReadMotorDriverErrorStatusRequest + : BaseMessage { + uint32_t message_index; + + template + static auto parse(Input body, Limit limit) + -> ReadMotorDriverErrorStatusRequest { + uint32_t msg_ind = 0; + + body = bit_utils::bytes_to_int(body, limit, msg_ind); + return ReadMotorDriverErrorStatusRequest{.message_index = msg_ind}; + } + + auto operator==(const ReadMotorDriverErrorStatusRequest& other) const + -> bool = default; +}; + +struct ReadMotorDriverErrorStatusResponse + : BaseMessage { + uint32_t message_index; + uint8_t reg_address; + uint32_t data; + + template + auto serialize(Output body, Limit limit) const -> uint8_t { + auto iter = bit_utils::int_to_bytes(message_index, body, limit); + iter = bit_utils::int_to_bytes(reg_address, iter, limit); + iter = bit_utils::int_to_bytes(data, iter, limit); + return iter - body; + } + + auto operator==(const ReadMotorDriverErrorStatusResponse& other) const + -> bool = default; +}; + struct WriteMotorCurrentRequest : BaseMessage { uint32_t message_index; @@ -1851,6 +1886,7 @@ using ResponseMessageType = std::variant< UpdateMotorPositionEstimationResponse, BaselineSensorResponse, PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse, GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse, - GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse>; + GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse, + ReadMotorDriverErrorStatusResponse>; } // namespace can::messages diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp index 2c4918fff..53bf495f9 100644 --- a/include/gantry/core/can_task.hpp +++ b/include/gantry/core/can_task.hpp @@ -21,7 +21,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< gantry::queues::QueueClient>, diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp index d58ea63fd..ad72967b2 100644 --- a/include/gantry/core/interfaces_proto.hpp +++ b/include/gantry/core/interfaces_proto.hpp @@ -9,10 +9,12 @@ namespace interfaces { +extern "C" using diag0_handler = void(*)(); + /** * Initialize the hardware portability layer. */ -void initialize(); +void initialize(diag0_handler *call_diag0_handler); /** * Get the CAN bus interface. diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp index 129d2ac68..fa276cbff 100644 --- a/include/gantry/core/interfaces_rev1.hpp +++ b/include/gantry/core/interfaces_rev1.hpp @@ -9,10 +9,12 @@ namespace interfaces { +extern "C" using diag0_handler = void(*)(); + /** * Initialize the hardware portability layer. */ -void initialize(); +void initialize(diag0_handler *call_diag0_handler); /** * Get the CAN bus interface. diff --git a/include/gantry/core/queues.hpp b/include/gantry/core/queues.hpp index 3007e49cb..42884aa01 100644 --- a/include/gantry/core/queues.hpp +++ b/include/gantry/core/queues.hpp @@ -32,6 +32,8 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/gantry/core/tasks_proto.hpp b/include/gantry/core/tasks_proto.hpp index c9fe5be64..8fe6846d4 100644 --- a/include/gantry/core/tasks_proto.hpp +++ b/include/gantry/core/tasks_proto.hpp @@ -6,6 +6,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gantry/core/interfaces_proto.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -28,14 +29,17 @@ namespace tasks { /** * Start gantry tasks. */ -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler; + +void call_run_diag0_interrupt(); /** * Access to all tasks in the system. diff --git a/include/gantry/core/tasks_rev1.hpp b/include/gantry/core/tasks_rev1.hpp index 0a90e559d..ea086c412 100644 --- a/include/gantry/core/tasks_rev1.hpp +++ b/include/gantry/core/tasks_rev1.hpp @@ -6,6 +6,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gantry/core/interfaces_rev1.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -28,14 +29,17 @@ namespace tasks { /** * Start gantry tasks. */ -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2160::configs::TMC2160DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler; + +void call_run_diag0_interrupt(); /** * Access to all tasks in the system. diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index c31568ded..49a70ad44 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -18,7 +18,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; #ifdef USE_SENSOR_MOVE using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler, diff --git a/include/gripper/core/interfaces.hpp b/include/gripper/core/interfaces.hpp index f11dac6ef..5430d5801 100644 --- a/include/gripper/core/interfaces.hpp +++ b/include/gripper/core/interfaces.hpp @@ -11,7 +11,9 @@ namespace z_motor_iface { -void initialize(); +extern "C" using diag0_handler = void(*)(); + +void initialize(diag0_handler *call_diag0_handler); /** * Access to the z motor. diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp index c61ef164b..1de5f7c06 100644 --- a/include/gripper/core/tasks.hpp +++ b/include/gripper/core/tasks.hpp @@ -7,6 +7,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gripper/core/interfaces.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -36,7 +37,7 @@ namespace gripper_tasks { /** * Start gripper tasks. */ -void start_tasks(can::bus::CanBus& can_bus, +auto start_tasks(can::bus::CanBus& can_bus, motor_class::Motor& z_motor, brushed_motor::BrushedMotor& grip_motor, spi::hardware::SpiDeviceBase& spi_device, @@ -45,7 +46,8 @@ void start_tasks(can::bus::CanBus& can_bus, sensors::hardware::SensorHardwareBase& sensor_hardware, eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface, motor_hardware_task::MotorHardwareTask& zmh_tsk, - motor_hardware_task::MotorHardwareTask& gmh_tsk); + motor_hardware_task::MotorHardwareTask& gmh_tsk) + -> z_motor_iface::diag0_handler; /** * Access to all the message queues in the system. @@ -184,13 +186,15 @@ struct AllTask { namespace z_tasks { -void start_task( +auto start_task( motor_class::Motor& z_motor, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, AllTask& tasks, gripper_tasks::QueueClient& main_queues, eeprom::dev_data::DevDataTailAccessor& - tail_accessor); + tail_accessor) -> z_motor_iface::diag0_handler; + +void call_run_diag0_interrupt(); struct QueueClient : can::message_writer::MessageWriter { QueueClient(); @@ -200,6 +204,8 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc2130::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/gripper/firmware/utility_gpio.h b/include/gripper/firmware/utility_gpio.h index 4cb96d005..c0267033a 100644 --- a/include/gripper/firmware/utility_gpio.h +++ b/include/gripper/firmware/utility_gpio.h @@ -53,6 +53,7 @@ void utility_gpio_init(); // z motor pins #define Z_MOT_DIR_PIN GPIO_PIN_10 #define Z_MOT_STEP_PIN GPIO_PIN_1 +#define Z_MOT_DIAG0_PIN GPIO_PIN_2 #define Z_MOT_STEPDIR_PORT GPIOB #define Z_MOT_ENABLE_PIN GPIO_PIN_9 #define Z_MOT_ENABLE_PORT GPIOA diff --git a/include/gripper/simulation/sim_interfaces.hpp b/include/gripper/simulation/sim_interfaces.hpp index 4de28bad4..a3750ab63 100644 --- a/include/gripper/simulation/sim_interfaces.hpp +++ b/include/gripper/simulation/sim_interfaces.hpp @@ -4,6 +4,8 @@ namespace z_motor_iface { +extern "C" using diag0_handler = void(*)(); + auto get_z_motor_interface() -> sim_motor_hardware_iface::SimMotorHardwareIface&; diff --git a/include/head/core/queues.hpp b/include/head/core/queues.hpp index 2552275ff..6a07d9cd1 100644 --- a/include/head/core/queues.hpp +++ b/include/head/core/queues.hpp @@ -55,6 +55,8 @@ struct MotorQueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp index 6f87e5286..8be17643e 100644 --- a/include/head/core/tasks_proto.hpp +++ b/include/head/core/tasks_proto.hpp @@ -24,11 +24,12 @@ namespace head_tasks { +extern "C" using diag0_handler = void(*)(); + /** * Start head tasks. */ - -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& left_motion_controller, @@ -42,7 +43,8 @@ void start_tasks( motor_hardware_task::MotorHardwareTask& right_motor_hardware, motor_hardware_task::MotorHardwareTask& left_motor_hardware, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple; /** * Access to all tasks not associated with a motor. This will be a singleton. diff --git a/include/head/core/tasks_rev1.hpp b/include/head/core/tasks_rev1.hpp index bbb375586..430c89cf4 100644 --- a/include/head/core/tasks_rev1.hpp +++ b/include/head/core/tasks_rev1.hpp @@ -24,11 +24,12 @@ namespace head_tasks { +extern "C" using diag0_handler = void(*)(); + /** * Start head tasks. */ - -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController& left_motion_controller, @@ -42,7 +43,8 @@ void start_tasks( motor_hardware_task::MotorHardwareTask& right_motor_hardware, motor_hardware_task::MotorHardwareTask& left_motor_hardware, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple; /** * Access to all tasks not associated with a motor. This will be a singleton. diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp index 00ec0dddf..113f56214 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -101,12 +101,15 @@ class MotionController { enabled = false; } - void stop() { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); // if we're gripping something we need to flag this so we don't drop it if (!hardware.get_stay_enabled()) { if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(); + hardware.set_cancel_request(error_severity, error_code); } } } diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index 436311fad..634249d8c 100644 --- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp @@ -180,7 +180,7 @@ class BrushedMotorInterruptHandler { } else if (in_estop) { // if we've received a stop request during this time we can clear // that flag since there isn't anything running - std::ignore = hardware.has_cancel_request(); + std::ignore = hardware.get_cancel_request(); // return out of error state once the estop is disabled if (!estop_triggered()) { in_estop = false; @@ -197,7 +197,9 @@ class BrushedMotorInterruptHandler { } else if (estop_triggered()) { in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); - } else if (hardware.has_cancel_request()) { + } else if (static_cast( + hardware.get_cancel_request().code) != + can::ids::ErrorCode::ok) { if (!hardware.get_stay_enabled()) { hardware.set_motor_state(BrushedMotorState::UNHOMED); } diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp index 9a979143d..647603250 100644 --- a/include/motor-control/core/motor_hardware_interface.hpp +++ b/include/motor-control/core/motor_hardware_interface.hpp @@ -72,6 +72,11 @@ class UsageEEpromConfig { size_t num_keys = 0; }; +struct __attribute__((packed)) CancelRequest { + uint8_t severity; + uint8_t code; +}; + class MotorHardwareIface { public: MotorHardwareIface() = default; @@ -90,6 +95,7 @@ class MotorHardwareIface { virtual void read_limit_switch() = 0; virtual void read_estop_in() = 0; virtual void read_sync_in() = 0; + virtual auto read_tmc_diag0() -> bool = 0; virtual auto get_encoder_pulses() -> int32_t = 0; virtual void reset_encoder_pulses() = 0; virtual void start_timer_interrupt() = 0; @@ -98,8 +104,10 @@ class MotorHardwareIface { virtual void enable_encoder() = 0; virtual void disable_encoder() = 0; - virtual auto has_cancel_request() -> bool = 0; - virtual void request_cancel() = 0; + virtual auto get_cancel_request() -> CancelRequest = 0; + virtual void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) = 0; + virtual void clear_cancel_request() = 0; virtual auto get_usage_eeprom_config() -> const UsageEEpromConfig& = 0; // This variable can remain public because the only public methods diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index cc22efc48..6f3267fcc 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -12,6 +12,9 @@ #include "motor-control/core/types.hpp" #include "motor-control/core/utils.hpp" +constexpr uint32_t DIAG0_DEBOUNCE_REPS = 9; +constexpr uint32_t DIAG0_DEBOUNCE_DELAY = 100; + namespace motion_controller { using namespace motor_messages; @@ -184,13 +187,22 @@ class MotionController { return update_queue.try_write(can_msg); } - void stop() { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(); + hardware.set_cancel_request(error_severity, error_code); } } + void clear_cancel_request() { hardware.clear_cancel_request(); } + + auto is_timer_interrupt_running() -> bool { + return hardware.is_timer_interrupt_running(); + } + auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } [[nodiscard]] auto read_motor_position() const { @@ -205,6 +217,8 @@ class MotionController { auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } + void enable_motor() { hardware.activate_motor(); hardware.start_timer_interrupt(); @@ -339,20 +353,31 @@ class PipetteMotionController { return false; } - void stop() { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); // if the timer interrupt is running, cancel it. if it isn't running, // don't submit a cancel because then the cancel won't be read until // the timer starts the next time. if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(); + hardware.set_cancel_request(error_severity, error_code); } } + void clear_cancel_request() { hardware.clear_cancel_request(); } + + auto is_timer_interrupt_running() -> bool { + return hardware.is_timer_interrupt_running(); + } + auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } + void enable_motor() { hardware.start_timer_interrupt(); hardware.activate_motor(); diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 5196a7f2d..fab6d550c 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -49,7 +49,7 @@ using namespace motor_messages; */ template