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/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp index 584fd990e..348089c2f 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}, diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp index bf0de3359..b707492cb 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 = { diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c index f48bcb6d7..02d39d8ff 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_INPUT; + 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); } } diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp index 69405dfde..e3efda790 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 diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c index 551f5cc12..71804478f 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_INPUT; + 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/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/firmware/main_proto.cpp b/head/firmware/main_proto.cpp index d3ba07641..6e2240300 100644 --- a/head/firmware/main_proto.cpp +++ b/head/firmware/main_proto.cpp @@ -142,10 +142,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 +192,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 +209,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 +236,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}, diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp index ce5849129..2954337d4 100644 --- a/head/firmware/main_rev1.cpp +++ b/head/firmware/main_rev1.cpp @@ -154,6 +154,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 +204,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 +218,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 +244,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}, diff --git a/head/firmware/motor_hardware_common.c b/head/firmware/motor_hardware_common.c index d1313df4d..13ca4727c 100644 --- a/head/firmware/motor_hardware_common.c +++ b/head/firmware/motor_hardware_common.c @@ -55,6 +55,13 @@ 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_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } else if (hspi->Instance == SPI3) { @@ -91,6 +98,13 @@ 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_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } } diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index 636a70cdd..e0b8b3d73 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, @@ -105,6 +107,7 @@ enum class MessageId { gear_read_motor_driver_request = 0x507, max_sensor_value_request = 0x70, max_sensor_value_response = 0x71, + batch_read_sensor_response = 0x81, read_sensor_request = 0x82, write_sensor_request = 0x83, baseline_sensor_request = 0x84, @@ -169,11 +172,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/message_writer.hpp b/include/can/core/message_writer.hpp index 92649457f..14dfe81dd 100644 --- a/include/can/core/message_writer.hpp +++ b/include/can/core/message_writer.hpp @@ -29,7 +29,8 @@ class MessageWriter { * @param message The message to send */ template - void send_can_message(can::ids::NodeId node, ResponseMessage&& message) { + auto send_can_message(can::ids::NodeId node, ResponseMessage&& message) + -> bool { auto arbitration_id = can::arbitration_id::ArbitrationId{}; auto task_message = can::message_writer_task::TaskMessage{}; @@ -41,7 +42,7 @@ class MessageWriter { arbitration_id.originating_node_id(node_id); task_message.arbitration_id = arbitration_id; task_message.message = message; - queue->try_write(task_message); + return queue->try_write(task_message); } void set_queue(QueueType* q) { queue = q; } diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 3ad1da7c5..a2185c04a 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -646,6 +647,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; @@ -947,6 +983,41 @@ struct ReadFromSensorResponse : BaseMessage { -> bool = default; }; +// Max len = (max size - uint32(message_index) - 3x uint8(sensor_type, sensor_id +// and data_length))/uint32(data_element_size) +constexpr size_t BATCH_SENSOR_MAX_LEN = + size_t((can::message_core::MaxMessageSize - 4 - 1 - 1 - 1) / 4); +struct BatchReadFromSensorResponse + : BaseMessage { + uint32_t message_index = 0; + can::ids::SensorType sensor{}; + can::ids::SensorId sensor_id{}; + uint8_t data_length = 0; + std::array sensor_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(static_cast(sensor), iter, limit); + iter = bit_utils::int_to_bytes(static_cast(sensor_id), iter, + limit); + iter = bit_utils::int_to_bytes(static_cast(data_length), iter, + limit); + for (auto i = 0; i < data_length; i++) { + iter = bit_utils::int_to_bytes(sensor_data.at(i), iter, limit); + } + // Since python expects statically sized messages to unpack, pad the end + // with 0's + for (auto i = data_length; i < BATCH_SENSOR_MAX_LEN; i++) { + iter = bit_utils::int_to_bytes(int32_t(0), iter, limit); + } + return iter - body; + } + auto operator==(const BatchReadFromSensorResponse& other) const + -> bool = default; +}; + struct SetSensorThresholdRequest : BaseMessage { uint32_t message_index; @@ -1844,13 +1915,14 @@ using ResponseMessageType = std::variant< ReadMotorDriverRegisterResponse, ReadFromEEPromResponse, MoveCompleted, ReadPresenceSensingVoltageResponse, PushToolsDetectedNotification, ReadLimitSwitchResponse, MotorPositionResponse, ReadFromSensorResponse, - FirmwareUpdateStatusResponse, SensorThresholdResponse, - SensorDiagnosticResponse, TaskInfoResponse, PipetteInfoResponse, - BindSensorOutputResponse, GripperInfoResponse, TipActionResponse, - PeripheralStatusResponse, BrushedMotorConfResponse, + BatchReadFromSensorResponse, FirmwareUpdateStatusResponse, + SensorThresholdResponse, SensorDiagnosticResponse, TaskInfoResponse, + PipetteInfoResponse, BindSensorOutputResponse, GripperInfoResponse, + TipActionResponse, PeripheralStatusResponse, BrushedMotorConfResponse, UpdateMotorPositionEstimationResponse, BaselineSensorResponse, PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse, GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse, - GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse>; + GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse, + ReadMotorDriverErrorStatusResponse>; } // namespace can::messages diff --git a/include/common/tests/mock_message_writer.hpp b/include/common/tests/mock_message_writer.hpp index d2eddce8e..8eb0db35f 100644 --- a/include/common/tests/mock_message_writer.hpp +++ b/include/common/tests/mock_message_writer.hpp @@ -22,10 +22,10 @@ class MockMessageWriter { * @param message The message to send */ template - void send_can_message(can::ids::NodeId, ResponseMessage&& message) { + auto send_can_message(can::ids::NodeId, ResponseMessage&& message) -> bool { can::message_writer_task::TaskMessage task_msg{.arbitration_id = 0x1, .message = message}; - queue->try_write(task_msg); + return queue->try_write(task_msg); } void set_queue(QueueType* q) { queue = q; } 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/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/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/hepa-uv/core/uv_task.hpp b/include/hepa-uv/core/uv_task.hpp index 3d5fcb32a..4bc348d5f 100644 --- a/include/hepa-uv/core/uv_task.hpp +++ b/include/hepa-uv/core/uv_task.hpp @@ -72,9 +72,13 @@ class UVMessageHandler { // Helper to update safety relay state void update_safety_relay_state() { - if (drive_pins.safety_relay_active.has_value()) + if (drive_pins.safety_relay_active.has_value()) { safety_relay_active = gpio::is_set(drive_pins.safety_relay_active.value()); + } else { + // rev b1 units have no safety relay so always return true. + safety_relay_active = true; + } } void visit(const std::monostate &) {} @@ -149,9 +153,10 @@ class UVMessageHandler { return; } - // The safety relay needs to be active (Door Closed, Reed Switch set, and - // Push Button pressed) in order to turn on the UV Light. So Send an - // error if the safety relay is not active when trying to turn on the light. + // The safety relay needs to be active (Door Closed, Reed Switch set, + // and Push Button pressed) in order to turn on the UV Light. So Send an + // error if the safety relay is not active when trying to turn on the + // light. update_safety_relay_state(); if (light_on && !safety_relay_active) { if (_timer.is_running()) _timer.stop(); 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..bf49e447d 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -113,6 +113,8 @@ class MotionController { auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } + auto check_tmc_diag0() -> bool { return hardware.check_tmc_diag0(); } + auto read_encoder_pulses() { return fixed_point_multiply(um_per_encoder_pulse, hardware.get_encoder_pulses(), 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..e78538826 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 @@ -151,6 +151,7 @@ class BrushedMotorInterruptHandler { can::ids::ErrorCode::collision_detected); report_position(pulses); error_handled = true; + hardware.set_motor_state(BrushedMotorState::UNHOMED); } } else if (motor_state != BrushedMotorState::UNHOMED) { auto pulses = hardware.get_encoder_pulses(); @@ -166,6 +167,7 @@ class BrushedMotorInterruptHandler { motor_state == BrushedMotorState::FORCE_CONTROLLING ? can::ids::ErrorCode::labware_dropped : can::ids::ErrorCode::collision_detected; + hardware.set_motor_state(BrushedMotorState::UNHOMED); cancel_and_clear_moves(err); report_position(pulses); error_handled = true; @@ -198,8 +200,9 @@ class BrushedMotorInterruptHandler { in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); } else if (hardware.has_cancel_request()) { - if (!hardware.get_stay_enabled()) { - hardware.set_motor_state(BrushedMotorState::UNHOMED); + if (!hardware.get_stay_enabled() && + hardware.get_motor_state() != BrushedMotorState::UNHOMED) { + hardware.set_motor_state(BrushedMotorState::STOPPED); } cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning); diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp index 9a979143d..b115e4832 100644 --- a/include/motor-control/core/motor_hardware_interface.hpp +++ b/include/motor-control/core/motor_hardware_interface.hpp @@ -87,9 +87,11 @@ class MotorHardwareIface { virtual auto check_limit_switch() -> bool = 0; virtual auto check_estop_in() -> bool = 0; virtual auto check_sync_in() -> bool = 0; + virtual auto check_tmc_diag0() -> bool = 0; virtual void read_limit_switch() = 0; virtual void read_estop_in() = 0; virtual void read_sync_in() = 0; + virtual void read_tmc_diag0() = 0; virtual auto get_encoder_pulses() -> int32_t = 0; virtual void reset_encoder_pulses() = 0; virtual void start_timer_interrupt() = 0; diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index cc22efc48..0cadbe550 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -189,6 +189,8 @@ class MotionController { if (hardware.is_timer_interrupt_running()) { hardware.request_cancel(); } + hardware.deactivate_motor(); + hardware.activate_motor(); } auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } @@ -205,6 +207,8 @@ class MotionController { auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto check_tmc_diag0() -> bool { return hardware.check_tmc_diag0(); } + void enable_motor() { hardware.activate_motor(); hardware.start_timer_interrupt(); @@ -353,6 +357,8 @@ class PipetteMotionController { auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto check_tmc_diag0() -> bool { return hardware.check_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..f0d3e60be 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -105,12 +105,23 @@ class MotorInterruptHandler { if (!_has_active_move or hardware.position_flags.check_flag( MotorPositionStatus::Flags::stepper_position_ok) or - buffered_move.check_stop_condition( - MoveStopCondition::limit_switch) or buffered_move.check_stop_condition( MoveStopCondition::limit_switch_backoff)) { return; } + if (buffered_move.check_stop_condition( + MoveStopCondition::limit_switch)) { + // Since the encoders are always setup that negative is towards the + // limit switch if the encoder has increased past the start position + // we know that we've stalled which means on the z axis that + // something is falling so we want to trigger a collision so we can + // catch it before it hits something + if ((buffered_move.start_encoder_position - + hardware.get_encoder_pulses()) > 10) { + return; + } + cancel_and_clear_moves(can::ids::ErrorCode::collision_detected); + } if (buffered_move.check_stop_condition(MoveStopCondition::stall)) { // if expected, finish move and clear queue to prepare for position diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp index fefe1f049..7c3a74e7d 100644 --- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp @@ -52,10 +52,11 @@ class TMC2130 { auto operator=(const TMC2130&& c) = delete; ~TMC2130() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast(addr); - _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf, + uint32_t token = spi::utils::build_token(converted_addr, tags); + _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index); } diff --git a/include/motor-control/core/stepper_motor/tmc2160.hpp b/include/motor-control/core/stepper_motor/tmc2160.hpp index 5f656e3f5..1590535fb 100644 --- a/include/motor-control/core/stepper_motor/tmc2160.hpp +++ b/include/motor-control/core/stepper_motor/tmc2160.hpp @@ -505,6 +505,21 @@ struct __attribute__((packed, __may_alias__)) DriveStatus { uint32_t stst : 1 = 0; }; +struct __attribute__((packed, __may_alias__)) DriveConf { + static constexpr Registers address = Registers::DRV_CONF; + static constexpr bool writable = true; + static constexpr uint32_t value_mask = (1 << 22) - 1; + + uint32_t bbm_time : 5 = 0; + uint32_t bit_padding_1 : 3 = 0; + uint32_t bbm_clks : 4 = 0; + uint32_t bit_padding_2 : 4 = 0; + // 00 = 150*C, 01 = 143*C, 10 = 136*C, 11 = 120*C + uint32_t ot_select : 2 = 0; + uint32_t drv_strength : 2 = 0; + uint32_t filt_isense : 2 = 0; +}; + /** * This register sets the control current for voltage PWM mode stealth chop. */ @@ -544,6 +559,7 @@ struct TMC2160RegisterMap { StealthChop pwmconf = {}; DriveStatus drvstatus = {}; GStatus gstat = {}; + DriveConf drvconf = {}; GlobalScaler glob_scale = {}; }; diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp index bc8574cee..3958b917c 100644 --- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp @@ -53,10 +53,11 @@ class TMC2160 { auto operator=(const TMC2160&& c) = delete; ~TMC2160() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast(addr); - _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf, + uint32_t token = spi::utils::build_token(converted_addr, tags); + _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index); } @@ -105,6 +106,9 @@ class TMC2160 { if (!set_glob_scaler(_registers.glob_scale)) { return false; } + if (!set_drv_conf(_registers.drvconf)) { + return false; + } _initialized = true; return true; } @@ -272,6 +276,22 @@ class TMC2160 { return false; } + /** + * @brief Update DRV_CONF register + * @param reg New configuration register to set + * @param policy Instance of abstraction policy to use + * @return True if new register was set succesfully, false otherwise + */ + auto set_drv_conf(DriveConf reg) -> bool { + reg.bit_padding_1 = 0; + reg.bit_padding_2 = 0; + if (set_register(reg)) { + _registers.drvconf = reg; + return true; + } + return false; + } + /** * @brief Update GlobalScaler register * @param reg New configuration register to set diff --git a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp index cc817135d..b7578b4e0 100644 --- a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp +++ b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp @@ -74,15 +74,45 @@ class MotionControllerMessageHandler { } void handle(const can::messages::AddBrushedLinearMoveRequest& m) { - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } void handle(const can::messages::GripperGripRequest& m) { - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } void handle(const can::messages::GripperHomeRequest& m) { - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { 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 ce55d848c..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); + } } } @@ -87,6 +99,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + uint32_t data = 0; + std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; + uint8_t tag_byte = spi::utils::byte_from_tags(tags); + driver.read(tmc2160::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); + } + void handle(const can::messages::GearWriteMotorCurrentRequest& m) { LOG("Received gear write motor current request: hold_current=%d, " "run_current=%d", diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp index 759a28308..966ba8bd8 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -47,7 +47,8 @@ using MoveGroupTaskMessage = using MotorDriverTaskMessage = std::variant; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveStatusReporterTaskMessage = std::variant< std::monostate, motor_messages::Ack, motor_messages::UpdatePositionResponse, diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index f8838703b..5cf4e448f 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -52,9 +52,19 @@ class MotionControllerMessageHandler { void handle(const can::messages::EnableMotorRequest& m) { LOG("Received enable motor request"); - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); + } } void handle(const can::messages::DisableMotorRequest& m) { @@ -92,7 +102,17 @@ class MotionControllerMessageHandler { "groupid=%d, seqid=%d, duration=%d, stopcondition=%d", m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration, m.request_stop_condition); - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } #ifdef USE_SENSOR_MOVE @@ -101,7 +121,17 @@ class MotionControllerMessageHandler { "groupid=%d, seqid=%d, duration=%d, stopcondition=%d", m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration, m.request_stop_condition); - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } #endif @@ -109,7 +139,17 @@ class MotionControllerMessageHandler { LOG("Motion Controller Received home request: velocity=%d, " "groupid=%d, seqid=%d\n", m.velocity, m.group_id, m.seq_id); - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { diff --git a/include/motor-control/core/tasks/motor_hardware_task.hpp b/include/motor-control/core/tasks/motor_hardware_task.hpp index 49398db83..1a61f3eb8 100644 --- a/include/motor-control/core/tasks/motor_hardware_task.hpp +++ b/include/motor-control/core/tasks/motor_hardware_task.hpp @@ -34,6 +34,7 @@ class MotorHardwareTask { hardware->read_estop_in(); hardware->read_limit_switch(); hardware->read_sync_in(); + hardware->read_tmc_diag0(); } } diff --git a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp index 82cff0e01..b5b49bf08 100644 --- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp @@ -58,12 +58,24 @@ class MotorDriverMessageHandler { auto data = driver.handle_spi_read( tmc2130::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); + } } } @@ -86,6 +98,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + uint32_t data = 0; + std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; + uint8_t tag_byte = spi::utils::byte_from_tags(tags); + driver.read(tmc2130::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); + } + void handle(const can::messages::WriteMotorCurrentRequest& m) { LOG("Received write motor current request: hold_current=%d, " "run_current=%d", diff --git a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp index 9f4e69179..50f66030a 100644 --- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp @@ -10,6 +10,7 @@ #include "motor-control/core/tasks/messages.hpp" #include "motor-control/core/tasks/tmc_motor_driver_common.hpp" #include "spi/core/messages.hpp" +#include "spi/core/utils.hpp" namespace tmc2160 { @@ -57,12 +58,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); + } } } @@ -85,6 +98,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + uint32_t data = 0; + std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; + uint8_t tag_byte = spi::utils::byte_from_tags(tags); + driver.read(tmc2160::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); + } + void handle(const can::messages::WriteMotorCurrentRequest& m) { LOG("Received write motor current request: hold_current=%d, " "run_current=%d", diff --git a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp index 191373466..60349086b 100644 --- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp +++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp @@ -8,11 +8,14 @@ namespace tmc { namespace tasks { using SpiResponseMessage = std::tuple; -using CanMessageTuple = std::tuple; +using CanMessageTuple = + std::tuple; using GearCanMessageTuple = std::tuple; using CanMessage = diff --git a/include/motor-control/core/types.hpp b/include/motor-control/core/types.hpp index ebc761340..b0e35e00f 100644 --- a/include/motor-control/core/types.hpp +++ b/include/motor-control/core/types.hpp @@ -44,5 +44,6 @@ enum class BrushedMotorState : uint8_t { UNHOMED = 0x0, FORCE_CONTROLLING_HOME = 0x1, FORCE_CONTROLLING = 0x2, - POSITION_CONTROLLING = 0x3 -}; \ No newline at end of file + POSITION_CONTROLLING = 0x3, + STOPPED = 0x4 +}; diff --git a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp index 986c9fe1d..6763953a7 100644 --- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp +++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp @@ -22,6 +22,7 @@ struct BrushedHardwareConfig { gpio::PinConfig limit_switch; gpio::PinConfig sync_in; gpio::PinConfig estop_in; + gpio::PinConfig diag0; double encoder_interrupt_freq; double pid_kp; double pid_ki; @@ -59,9 +60,11 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { auto check_limit_switch() -> bool final { return limit.debounce_state(); } auto check_estop_in() -> bool final { return estop.debounce_state(); } auto check_sync_in() -> bool final { return sync.debounce_state(); } + auto check_tmc_diag0() -> bool final { return false; } void read_limit_switch() final; void read_estop_in() final; void read_sync_in() final; + void read_tmc_diag0() final; void grip() final; void ungrip() final; void stop_pwm() final; diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp index e750a4f02..b39cc8fa5 100644 --- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp +++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp @@ -18,6 +18,7 @@ struct HardwareConfig { gpio::PinConfig led; gpio::PinConfig sync_in; gpio::PinConfig estop_in; + gpio::PinConfig diag0; std::optional ebrake = std::nullopt; }; @@ -46,10 +47,12 @@ class MotorHardware : public StepperMotorHardwareIface { auto is_timer_interrupt_running() -> bool final; auto check_limit_switch() -> bool final { return limit.debounce_state(); } auto check_estop_in() -> bool final { return estop.debounce_state(); } + auto check_tmc_diag0() -> bool final { return diag.debounce_state(); } auto check_sync_in() -> bool final { return sync; } void read_limit_switch() final; void read_estop_in() final; void read_sync_in() final; + void read_tmc_diag0() final; void set_LED(bool status) final; auto get_encoder_pulses() -> int32_t final; void reset_encoder_pulses() final; @@ -69,6 +72,7 @@ class MotorHardware : public StepperMotorHardwareIface { private: debouncer::Debouncer estop = debouncer::Debouncer{}; + debouncer::Debouncer diag = debouncer::Debouncer{.holdoff_cnt = 90}; debouncer::Debouncer limit = debouncer::Debouncer{}; std::atomic_bool sync = false; static constexpr uint16_t encoder_reset_offset = 20; diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp index 2a8dd67cc..ddf74e69e 100644 --- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp +++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp @@ -51,6 +51,8 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface { void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + void read_tmc_diag0() final {} + bool check_tmc_diag0() final { return false; } void set_LED(bool) final {} void trigger_limit_switch() { limit_switch_status = true; } bool check_sync_in() final { @@ -137,6 +139,8 @@ class SimBrushedMotorHardwareIface void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + void read_tmc_diag0() final {} + bool check_tmc_diag0() final { return false; } void trigger_limit_switch() { limit_switch_status = true; } void grip() final {} void ungrip() final {} @@ -235,6 +239,8 @@ class SimGearMotorHardwareIface void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + void read_tmc_diag0() final {} + bool check_tmc_diag0() final { return false; } void set_LED(bool) final {} void trigger_limit_switch() { limit_switch_status = true; } bool check_sync_in() final { diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp index 410cc8632..59bb2c0bc 100644 --- a/include/motor-control/tests/mock_brushed_motor_components.hpp +++ b/include/motor-control/tests/mock_brushed_motor_components.hpp @@ -42,6 +42,8 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { void stop_pwm() final { move_dir = PWM_DIRECTION::unset; } auto check_sync_in() -> bool final { return sync_val; } void read_sync_in() final {} + void read_tmc_diag0() final {} + bool check_tmc_diag0() final { return diag0_val; } auto get_encoder_pulses() -> int32_t final { return (motor_encoder_overflow_count << 16) + enc_val; @@ -97,6 +99,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { int32_t motor_encoder_overflow_count = 0; bool ls_val = false; bool sync_val = false; + bool diag0_val = false; bool estop_in_val = false; bool is_gripping = false; bool motor_enabled = false; diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp index ff6e0bd69..d7fb199c8 100644 --- a/include/motor-control/tests/mock_motor_hardware.hpp +++ b/include/motor-control/tests/mock_motor_hardware.hpp @@ -27,9 +27,11 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { bool check_limit_switch() final { return mock_lim_sw_value; } bool check_estop_in() final { return mock_estop_in_value; } bool check_sync_in() final { return mock_sync_value; } + bool check_tmc_diag0() final { return mock_diag0_value; } void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + void read_tmc_diag0() final {} void set_LED(bool) final {} void set_mock_lim_sw(bool value) { mock_lim_sw_value = value; } void set_mock_estop_in(bool value) { mock_estop_in_value = value; } @@ -61,6 +63,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { bool mock_lim_sw_value = false; bool mock_estop_in_value = false; bool mock_sync_value = false; + bool mock_diag0_value = false; bool mock_sr_value = false; bool mock_dir_value = false; uint8_t finished_move_id = 0x0; diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp index 7bb94358b..c02cacd46 100644 --- a/include/pipettes/core/dispatch_builder.hpp +++ b/include/pipettes/core/dispatch_builder.hpp @@ -23,19 +23,22 @@ using TMC2130MotorDispatchTarget = can::dispatch::DispatchParseTarget< linear_motor_tasks::tmc2130_driver::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using TMC2160MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler< linear_motor_tasks::tmc2160_driver::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::GearMotorHandler< gear_motor_tasks::QueueClient>, can::messages::GearReadMotorDriverRegister, + can::messages::ReadMotorDriverErrorStatusRequest, can::messages::GearWriteMotorDriverRegister, can::messages::GearWriteMotorCurrentRequest>; diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index 71c3b3539..32f0f59d4 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -7,6 +7,7 @@ #include "common/core/logging.h" #include "motor-control/core/linear_motion_system.hpp" #include "motor-control/core/stepper_motor/motion_controller.hpp" +#include "motor-control/core/tasks/tmc_motor_driver_common.hpp" #include "motor-control/core/tasks/usage_storage_task.hpp" #include "pipettes/core/tasks/messages.hpp" @@ -61,9 +62,19 @@ class MotionControllerMessageHandler { LOG("Received enable motor request"); // TODO only toggle the enable pin once since all motors share // a single enable pin line. - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); + } } void handle(const can::messages::GearDisableMotorRequest& m) { @@ -102,7 +113,17 @@ class MotionControllerMessageHandler { LOG("Motion Controller Received a tip action request: velocity=%d, " "acceleration=%d, groupid=%d, seqid=%d\n", m.velocity, m.acceleration, m.group_id, m.seq_id); - controller.move(m); + if (controller.check_tmc_diag0()) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index ba3a6e508..1e55cfef3 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -90,7 +90,8 @@ class FDC1004 { void set_echoing(bool should_echo) { echoing = should_echo; if (should_echo) { - sensor_buffer_index = 0; // reset buffer index + sensor_buffer_index_start = 0; // reset buffer index + sensor_buffer_index_end = 0; // reset buffer index } } @@ -209,21 +210,8 @@ class FDC1004 { } void send_accumulated_sensor_data(uint32_t message_index) { - for (int i = 0; i < sensor_buffer_index; i++) { - // send over buffer adn then clear buffer values - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = message_index, - .sensor = can::ids::SensorType::capacitive, - .sensor_id = sensor_id, - .sensor_data = convert_to_fixed_point( - (*sensor_buffer).at(i), S15Q16_RADIX)}); - if (i % 10 == 0) { - // slow it down so the can buffer doesn't choke - vtask_hardware_delay(50); - } - (*sensor_buffer).at(i) = 0; + while (get_buffer_count() > 0) { + try_send_next_chunk(message_index); } can_client.send_can_message( can::ids::NodeId::host, @@ -231,12 +219,58 @@ class FDC1004 { } auto sensor_buffer_log(float data) -> void { - sensor_buffer->at(sensor_buffer_index) = data; - sensor_buffer_index++; - if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { - sensor_buffer_index = 0; + sensor_buffer->at(sensor_buffer_index_end) = data; + sensor_buffer_index_end++; + if (sensor_buffer_index_end == SENSOR_BUFFER_SIZE) { + sensor_buffer_index_end = 0; crossed_buffer_index = true; } + if (sensor_buffer_index_end == sensor_buffer_index_start) { + sensor_buffer_index_start = + (sensor_buffer_index_end + 1) % SENSOR_BUFFER_SIZE; + } + } + + auto get_buffer_count() -> uint16_t { + auto count = sensor_buffer_index_end; + if (sensor_buffer_index_end < sensor_buffer_index_start) { + count += (SENSOR_BUFFER_SIZE - sensor_buffer_index_start); + } else { + count -= sensor_buffer_index_start; + } + return count; + } + + auto try_send_next_chunk(uint32_t message_index) -> void { + std::array data{}; + auto count = get_buffer_count(); + auto data_len = std::min(uint8_t(count), + uint8_t(can::messages::BATCH_SENSOR_MAX_LEN)); + if (data_len == 0) { + return; + } + for (uint8_t i = 0; i < data_len; i++) { + data.at(i) = convert_to_fixed_point( + (*sensor_buffer) + .at((sensor_buffer_index_start + i) % SENSOR_BUFFER_SIZE), + S15Q16_RADIX); + } + auto response = can::messages::BatchReadFromSensorResponse{ + .message_index = message_index, + .sensor = can::ids::SensorType::capacitive, + .sensor_id = sensor_id, + .data_length = data_len, + .sensor_data = data, + }; + if (can_client.send_can_message(can::ids::NodeId::host, response)) { + // if we succesfully queue the can message, mark that data as sent + // by incrementing the buffer start pointer + sensor_buffer_index_start = + (sensor_buffer_index_start + data_len) % SENSOR_BUFFER_SIZE; + } else { + // if the queue is full release the task for bit + vtask_hardware_delay(20); + } } void handle_fdc_response(i2c::messages::TransactionResponse &m) { @@ -307,14 +341,9 @@ class FDC1004 { if (echoing) { sensor_buffer_log(capacitance); - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::capacitive, - .sensor_id = sensor_id, - .sensor_data = - convert_to_fixed_point(capacitance, S15Q16_RADIX)}); + if (get_buffer_count() >= can::messages::BATCH_SENSOR_MAX_LEN) { + try_send_next_chunk(0); + } } } @@ -534,7 +563,8 @@ class FDC1004 { return RG(*reinterpret_cast(&ret.value())); } std::array *sensor_buffer; - uint16_t sensor_buffer_index = 0; + uint16_t sensor_buffer_index_start = 0; + uint16_t sensor_buffer_index_end = 0; bool crossed_buffer_index = false; }; // end of FDC1004 class diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 142cf821f..c7c15126b 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -71,7 +71,8 @@ class MMR920 { void set_echoing(bool should_echo) { echoing = should_echo; if (should_echo) { - sensor_buffer_index = 0; // reset buffer index + sensor_buffer_index_start = 0; // reset buffer index + sensor_buffer_index_end = 0; // reset buffer index crossed_buffer_index = false; sensor_buffer->fill(0.0); } @@ -244,13 +245,27 @@ class MMR920 { return true; } + auto get_buffer_count() -> uint16_t { + auto count = sensor_buffer_index_end; + if (sensor_buffer_index_end < sensor_buffer_index_start) { + count += (SENSOR_BUFFER_SIZE - sensor_buffer_index_start); + } else { + count -= sensor_buffer_index_start; + } + return count; + } + auto sensor_buffer_log(float data) -> void { - sensor_buffer->at(sensor_buffer_index) = data; - sensor_buffer_index++; - if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { - sensor_buffer_index = 0; + sensor_buffer->at(sensor_buffer_index_end) = data; + sensor_buffer_index_end++; + if (sensor_buffer_index_end == SENSOR_BUFFER_SIZE) { + sensor_buffer_index_end = 0; crossed_buffer_index = true; } + if (sensor_buffer_index_end == sensor_buffer_index_start) { + sensor_buffer_index_start = + (sensor_buffer_index_end + 1) % SENSOR_BUFFER_SIZE; + } } auto save_temperature(int32_t data) -> bool { @@ -304,35 +319,40 @@ class MMR920 { mmr920::ADDRESS, reg_id, 3, STOP_DELAY, own_queue, transaction_id); } - void send_accumulated_sensor_data(uint32_t message_index) { - auto start = 0; - auto count = sensor_buffer_index; - if (crossed_buffer_index) { - start = sensor_buffer_index; - count = SENSOR_BUFFER_SIZE; + auto try_send_next_chunk(uint32_t message_index) -> void { + std::array data{}; + auto count = get_buffer_count(); + auto data_len = std::min(uint8_t(count), + uint8_t(can::messages::BATCH_SENSOR_MAX_LEN)); + if (data_len == 0) { + return; } + for (uint8_t i = 0; i < data_len; i++) { + data.at(i) = mmr920::reading_to_fixed_point( + (*sensor_buffer) + .at((sensor_buffer_index_start + i) % SENSOR_BUFFER_SIZE)); + } + auto response = can::messages::BatchReadFromSensorResponse{ + .message_index = message_index, + .sensor = can::ids::SensorType::pressure, + .sensor_id = sensor_id, + .data_length = data_len, + .sensor_data = data, + }; + if (can_client.send_can_message(can::ids::NodeId::host, response)) { + // if we succesfully queue the can message, mark that data as sent + // by incrementing the buffer start pointer + sensor_buffer_index_start = + (sensor_buffer_index_start + data_len) % SENSOR_BUFFER_SIZE; + } else { + // if the queue is full release the task for bit + vtask_hardware_delay(20); + } + } - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::Acknowledgment{.message_index = count}); - for (int i = 0; i < count; i++) { - // send over buffer and then clear buffer values - // NOLINTNEXTLINE(div-by-zero) - int current_index = - (i + start) % static_cast(SENSOR_BUFFER_SIZE); - - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = message_index, - .sensor = can::ids::SensorType::pressure, - .sensor_id = sensor_id, - .sensor_data = mmr920::reading_to_fixed_point( - (*sensor_buffer).at(current_index))}); - if (i % 10 == 0) { - // slow it down so the can buffer doesn't choke - vtask_hardware_delay(20); - } + void send_accumulated_sensor_data(uint32_t message_index) { + while (get_buffer_count() > 0) { + try_send_next_chunk(message_index); } can_client.send_can_message( can::ids::NodeId::host, @@ -351,8 +371,8 @@ class MMR920 { std::accumulate(sensor_buffer->begin() + AUTO_BASELINE_START, sensor_buffer->begin() + AUTO_BASELINE_END, 0.0) / float(AUTO_BASELINE_END - AUTO_BASELINE_START); - for (auto i = sensor_buffer_index - AUTO_BASELINE_END; - i < sensor_buffer_index; i++) { + for (auto i = sensor_buffer_index_end - AUTO_BASELINE_END; + i < sensor_buffer_index_end; i++) { // apply the moving baseline to older samples to so that // data is in the same format as later samples, don't apply // the current_pressure_baseline_pa since it has already @@ -364,7 +384,7 @@ class MMR920 { auto handle_sync_threshold(float pressure) -> void { if (enable_auto_baseline) { - if ((sensor_buffer_index > AUTO_BASELINE_END || + if ((sensor_buffer_index_end > AUTO_BASELINE_END || crossed_buffer_index) && (std::fabs(pressure - current_pressure_baseline_pa - current_moving_pressure_baseline_pa) > @@ -447,20 +467,12 @@ class MMR920 { response_pressure -= current_moving_pressure_baseline_pa; } sensor_buffer_log(response_pressure); - if (!enable_auto_baseline) { - // This preserves the old way of echoing continuous polls - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = 0, - .sensor = can::ids::SensorType::pressure, - .sensor_id = sensor_id, - .sensor_data = - mmr920::reading_to_fixed_point(response_pressure)}); + if (get_buffer_count() >= can::messages::BATCH_SENSOR_MAX_LEN) { + try_send_next_chunk(0); } if (enable_auto_baseline && - sensor_buffer_index == AUTO_BASELINE_END && + sensor_buffer_index_end == AUTO_BASELINE_END && !crossed_buffer_index) { compute_auto_baseline(); } @@ -655,7 +667,8 @@ class MMR920 { return write(Reg::address, value); } std::array *sensor_buffer; - uint16_t sensor_buffer_index = 0; + uint16_t sensor_buffer_index_start = 0; + uint16_t sensor_buffer_index_end = 0; bool crossed_buffer_index = false; }; diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp index e41b4468f..0f28e89fa 100644 --- a/include/spi/core/utils.hpp +++ b/include/spi/core/utils.hpp @@ -16,6 +16,39 @@ struct ChipSelectInterface { void* GPIO_handle; }; +// Bit positions to pack in an 8 bit response tag +enum class ResponseTag : size_t { + IS_ERROR_RESPONSE = 0, +}; + +[[nodiscard]] constexpr auto byte_from_tag(ResponseTag tag) -> uint8_t { + return (1 << static_cast(tag)); +} + +template +[[nodiscard]] auto byte_from_tags(const Tags& tags) -> uint8_t { + uint8_t result = 0; + for (auto tag : tags) { + result |= byte_from_tag(tag); + } + return result; +} + +[[nodiscard]] inline constexpr auto tag_in_token(uint32_t token, + ResponseTag tag) -> bool { + return bool(token & + (1 << (static_cast(tag) + static_cast(8)))); +} + +[[nodiscard]] inline constexpr auto build_token(uint8_t reg, uint8_t tags = 0) + -> uint32_t { + return (static_cast(tags) << 8) | (reg); +} + +template +[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id) -> RegType { + return static_cast(static_cast(id & 0xff)); +} } // namespace utils -} // namespace spi \ No newline at end of file +} // namespace spi diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp index a3e2e1710..cf887f4b6 100644 --- a/include/spi/core/writer.hpp +++ b/include/spi/core/writer.hpp @@ -46,13 +46,13 @@ class Writer { * @return A success boolean */ template - auto read(uint8_t register_addr, uint32_t command_data, - RQType& response_queue, utils::ChipSelectInterface cs_intf, - uint32_t message_index) -> bool { - auto txBuffer = build_message(register_addr, spi::hardware::Mode::READ, - command_data); + auto read(uint32_t token, uint32_t command_data, RQType& response_queue, + utils::ChipSelectInterface cs_intf, uint32_t message_index) + -> bool { + auto txBuffer = build_message(utils::reg_from_token(token), + spi::hardware::Mode::READ, command_data); TransactionIdentifier _transaction_id{ - .token = register_addr, + .token = token, .command_type = static_cast(spi::hardware::Mode::READ), .requires_response = false, .message_index = message_index}; @@ -83,7 +83,7 @@ class Writer { auto txBuffer = build_message(register_addr, spi::hardware::Mode::WRITE, command_data); TransactionIdentifier _transaction_id{ - .token = register_addr, + .token = utils::build_token(register_addr), .command_type = static_cast(spi::hardware::Mode::WRITE), .requires_response = true}; Transact message{ diff --git a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp index 2b5d1314d..2459e8f26 100644 --- a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp +++ b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp @@ -58,6 +58,8 @@ void BrushedMotorHardware::read_sync_in() { sync.debounce_update(gpio::is_set(pins.sync_in)); } +void BrushedMotorHardware::read_tmc_diag0() { return; } + int32_t BrushedMotorHardware::get_encoder_pulses() { if (!enc_handle) { return 0; diff --git a/motor-control/firmware/stepper_motor/motor_hardware.cpp b/motor-control/firmware/stepper_motor/motor_hardware.cpp index 76c64faa1..43a1aad5c 100644 --- a/motor-control/firmware/stepper_motor/motor_hardware.cpp +++ b/motor-control/firmware/stepper_motor/motor_hardware.cpp @@ -17,6 +17,9 @@ void MotorHardware::negative_direction() { gpio::reset(pins.direction); } void MotorHardware::activate_motor() { gpio::set(pins.enable); if (pins.ebrake.has_value()) { + // allow time for the motor current to stablize before releasing the + // brake + motor_hardware_delay(20); gpio::reset(pins.ebrake.value()); motor_hardware_delay(20); } @@ -50,6 +53,10 @@ void MotorHardware::read_estop_in() { void MotorHardware::read_sync_in() { sync = gpio::is_set(pins.sync_in); } +void MotorHardware::read_tmc_diag0() { + diag.debounce_update(gpio::is_set(pins.diag0)); +} + void MotorHardware::set_LED(bool status) { if (status) { gpio::set(pins.led); diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 578754c46..c4f9a1fba 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -526,10 +526,10 @@ SCENARIO("handler recovers from error state") { test_objs.hw.request_cancel(); test_objs.handler.run_interrupt(); THEN( - "motor state should become un-homed only if stay engaged is " - "falsy") { + "motor state should become stopped only if stay engaged is " + "false") { REQUIRE(test_objs.hw.get_motor_state() == - (!stay_engaged ? BrushedMotorState::UNHOMED + (!stay_engaged ? BrushedMotorState::STOPPED : og_motor_state)); } THEN("a stop requested warning is issued") { @@ -560,4 +560,4 @@ SCENARIO("handler recovers from error state") { } } } -} \ No newline at end of file +} diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp index 07ddb5006..5d58f9816 100644 --- a/motor-control/tests/test_motor_stall_handling.cpp +++ b/motor-control/tests/test_motor_stall_handling.cpp @@ -306,11 +306,17 @@ SCENARIO("motor handler stall detection") { REQUIRE(!test_objs.hw.position_flags.check_flag( Flags::stepper_position_ok)); THEN("move completed and no error was raised") { - REQUIRE(test_objs.reporter.messages.size() == 1); - Ack ack_msg = - std::get(test_objs.reporter.messages.front()); - REQUIRE(ack_msg.ack_id == - AckMessageId::complete_without_condition); + REQUIRE(test_objs.reporter.messages.size() == 6); + for (auto& element : test_objs.reporter.messages) { + printf("%ld\n", element.index()); + } + can::messages::ErrorMessage err = + std::get( + test_objs.reporter.messages.front()); + REQUIRE(err.error_code == + can::ids::ErrorCode::collision_detected); + REQUIRE(err.severity == + can::ids::ErrorSeverity::unrecoverable); } } } @@ -347,4 +353,4 @@ SCENARIO("motor handler stall detection") { } } } -} \ No newline at end of file +} diff --git a/pipettes/firmware/hardware_config.c b/pipettes/firmware/hardware_config.c index 336feaad2..8291eccee 100644 --- a/pipettes/firmware/hardware_config.c +++ b/pipettes/firmware/hardware_config.c @@ -219,3 +219,15 @@ uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO 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 GPIO_PIN_6; + case SINGLE_CHANNEL: + case EIGHT_CHANNEL: + default: + return GPIO_PIN_11; + } +} diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h index d203ddb49..733065a65 100644 --- a/pipettes/firmware/hardware_config.h +++ b/pipettes/firmware/hardware_config.h @@ -25,5 +25,6 @@ typedef enum { 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); +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_configurations.cpp b/pipettes/firmware/motor_configurations.cpp index 425cd544b..70c96da5e 100644 --- a/pipettes/firmware/motor_configurations.cpp +++ b/pipettes/firmware/motor_configurations.cpp @@ -11,7 +11,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) switch (which) { case TMC2160PipetteAxis::left_gear_motor: return tmc2160::configs::TMC2160DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -24,6 +24,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .tbl = 0x2, .mres = 0x4}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xa7}}, .current_config = { @@ -37,7 +38,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) }}; case TMC2160PipetteAxis::right_gear_motor: return tmc2160::configs::TMC2160DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -50,6 +51,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .tbl = 0x2, .mres = 0x4}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xa7}}, .current_config = { @@ -64,7 +66,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) case TMC2160PipetteAxis::linear_motor: default: return tmc2160::configs::TMC2160DriverConfig{ - .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}, @@ -79,6 +81,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .mres = 0x4, .intpol = 0x1}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xff}}, .current_config = { @@ -99,7 +102,7 @@ auto motor_configs::driver_config_by_axis(TMC2130PipetteAxis which) case TMC2130PipetteAxis::linear_motor: default: return tmc2130::configs::TMC2130DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x10, .hold_current_delay = 0x7}, @@ -163,6 +166,11 @@ auto motor_configs::hardware_config_by_axis(TMC2130PipetteAxis which) .port = GPIOC, .pin = GPIO_PIN_12, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_11, + .active_setting = GPIO_PIN_RESET}, }; } } @@ -199,6 +207,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; case TMC2160PipetteAxis::left_gear_motor: return motor_hardware::HardwareConfig{ @@ -229,6 +242,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; case TMC2160PipetteAxis::linear_motor: default: @@ -265,6 +283,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; } } diff --git a/pipettes/firmware/motor_hardware.c b/pipettes/firmware/motor_hardware.c index 0792f25d5..a25547850 100644 --- a/pipettes/firmware/motor_hardware.c +++ b/pipettes/firmware/motor_hardware.c @@ -94,6 +94,11 @@ void motor_driver_gpio_init() { // Enable Dir/Step pin 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_diag0_pin(pipette_type); + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } else { // Enable Dir/Step pin GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA); @@ -101,7 +106,11 @@ void motor_driver_gpio_init() { // Enable/Dir/Step pin 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_diag0_pin(pipette_type); + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } } diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt index 55f9d034b..e150331bb 100644 --- a/sensors/tests/CMakeLists.txt +++ b/sensors/tests/CMakeLists.txt @@ -21,7 +21,7 @@ set_target_properties(sensors PROPERTIES CXX_STANDARD 20 CXX_STANDARD_REQUIRED TRUE) - +target_compile_definitions(sensors PUBLIC SENSOR_BUFF_SIZE=300) target_compile_options(sensors PUBLIC -Wall diff --git a/sensors/tests/test_capacitive_sensor.cpp b/sensors/tests/test_capacitive_sensor.cpp index 8e0999ac4..c326ff3f0 100644 --- a/sensors/tests/test_capacitive_sensor.cpp +++ b/sensors/tests/test_capacitive_sensor.cpp @@ -541,8 +541,7 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { WHEN("A response for S1 is received") { auto buffer_a = i2c::messages::MaxMessageBuffer{0, 0, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{0, 0, 0, 0, 0}; - std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, - sensors::utils::ResponseTag::POLL_IS_CONTINUOUS}; + std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL}; i2c::messages::TransactionResponse first{ .id = i2c::messages::TransactionIdentifier{ @@ -558,6 +557,7 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { auto second = first; second.id.transaction_index = 1; second.read_buffer = buffer_b; + second.id.is_completed_poll = 1; auto first_task_msg = sensors::utils::TaskMessage(first); auto second_task_msg = sensors::utils::TaskMessage(second); sensor_shared.handle_message(first_task_msg); @@ -621,8 +621,10 @@ SCENARIO("capacitance driver tests no shared CINs") { auto second = first; second.id.transaction_index = 1; second.read_buffer = buffer_b; - callback_host.handle_ongoing_response(first); - callback_host.handle_ongoing_response(second); + for (auto i = 0; i < 14; i++) { + callback_host.handle_ongoing_response(first); + callback_host.handle_ongoing_response(second); + } THEN("it should forward the converted data via can") { REQUIRE(can_queue.has_message()); @@ -654,17 +656,20 @@ SCENARIO("capacitance driver tests no shared CINs") { second.id.transaction_index = 1; second.read_buffer = buffer_b; - callback_host.handle_ongoing_response(first); - callback_host.handle_ongoing_response(second); + for (auto i = 0; i < 14; i++) { + callback_host.handle_ongoing_response(first); + callback_host.handle_ongoing_response(second); + } THEN("it should forward the converted data via can") { can_queue.try_read(&empty_msg); - auto sent = std::get( - empty_msg.message); + auto sent = + std::get( + empty_msg.message); REQUIRE(sent.sensor == can::ids::SensorType::capacitive); // we're just checking that the data is faithfully represented, // don't really care what it is - REQUIRE(sent.sensor_data == + REQUIRE(sent.sensor_data[0] == convert_to_fixed_point(15, S15Q16_RADIX)); } THEN("it should not touch the sync line") { diff --git a/sensors/tests/test_pressure_driver.cpp b/sensors/tests/test_pressure_driver.cpp index 18793480f..fba0cf794 100644 --- a/sensors/tests/test_pressure_driver.cpp +++ b/sensors/tests/test_pressure_driver.cpp @@ -272,6 +272,50 @@ SCENARIO("Testing the pressure sensor driver") { } } + GIVEN("An unlimited poll with sensor binding set to report") { + driver.set_echoing(true); + driver.set_max_bind_sync(false); + std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, + sensors::utils::ResponseTag::POLL_IS_CONTINUOUS}; + auto tags_as_int = sensors::utils::byte_from_tags(tags); + WHEN("the continuous poll function is called") { + driver.poll_continuous_pressure(tags_as_int); + THEN("a READ_PRESSURE command only") { + REQUIRE(i2c_queue.get_size() == 0); + REQUIRE(i2c_poll_queue.get_size() == 1); + auto read_command = get_message< + i2c::messages::ConfigureSingleRegisterContinuousPolling>( + i2c_poll_queue); + REQUIRE( + read_command.first.write_buffer[0] == + static_cast( + sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ)); + } + } + WHEN("pressure driver receives the requested sensor readings") { + auto id = i2c::messages::TransactionIdentifier{ + .token = sensors::utils::build_id( + sensors::mmr920::ADDRESS, + static_cast( + sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ), + tags_as_int), + .is_completed_poll = false, + .transaction_index = static_cast(0)}; + auto sensor_response = i2c::messages::TransactionResponse{ + .id = id, .bytes_read = 3, .read_buffer = {0x00, 0xC0, 0xDE}}; + + for (size_t i = 0; i <= SENSOR_BUFFER_SIZE + 1; i++) { + driver.handle_ongoing_pressure_response(sensor_response); + if (i > 0 && i % 14 == 0) { + // Can queue here can't handle that many messages so just + // pop them off one at a time. + REQUIRE(can_queue.get_size() == 1); + can_queue.reset(); + } + } + } + } + GIVEN("output binding = report") { driver.set_bind_sync(true); std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, diff --git a/sensors/tests/test_pressure_sensor.cpp b/sensors/tests/test_pressure_sensor.cpp index 618485aaf..30fed1b42 100644 --- a/sensors/tests/test_pressure_sensor.cpp +++ b/sensors/tests/test_pressure_sensor.cpp @@ -98,13 +98,17 @@ SCENARIO("Receiving messages through the pressure sensor message handler") { sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ), sensors::utils::byte_from_tags(tags_for_continuous)); auto response_read = sensors::utils::TaskMessage(response_details); - sensor.handle_message(response_read); - THEN("there should be a ReadFromSensorResponse") { + THEN( + "there should be a Batch read sensor response after 14 " + "messages") { + for (auto i = 0; i < 14; i++) { + sensor.handle_message(response_read); + } REQUIRE(can_queue.has_message()); REQUIRE(can_queue.get_size() == 1); can_queue.try_read(&empty_can_msg); REQUIRE(std::holds_alternative< - can::messages::ReadFromSensorResponse>( + can::messages::BatchReadFromSensorResponse>( empty_can_msg.message)); } }