diff --git a/.github/workflows/release_bundle.yaml b/.github/workflows/release_bundle.yaml index 693843247..08ae74737 100644 --- a/.github/workflows/release_bundle.yaml +++ b/.github/workflows/release_bundle.yaml @@ -103,10 +103,10 @@ jobs: replacesArtifacts: true allowUpdates: true - - name: 'Add firmware images artifact' + - name: 'Add firmware category zips' uses: 'ncipollo/release-action@v1.12.0' with: - artifacts: ./firmware-images-${{github.ref_name}}.zip + artifacts: ./firmware-*-${{github.ref_name}}.zip artifactContentType: application/zip allowUpdates: true omitBodyDuringUpdate: true @@ -114,44 +114,11 @@ jobs: omitNameDuringUpdate: true omitPrereleaseDuringUpdate: true - - name: 'Add firmware robot images artifact' + - name: 'Add latest firmware images' uses: 'ncipollo/release-action@v1.12.0' with: - artifacts: ./firmware-robot-images-${{github.ref_name}}.zip - artifactContentType: application/zip - allowUpdates: true - omitBodyDuringUpdate: true - omitDraftDuringUpdate: true - omitNameDuringUpdate: true - omitPrereleaseDuringUpdate: true - - - name: 'Add firmware pipette images artifact' - uses: 'ncipollo/release-action@v1.12.0' - with: - artifacts: ./firmware-pipette-images-${{github.ref_name}}.zip - artifactContentType: application/zip - allowUpdates: true - omitBodyDuringUpdate: true - omitDraftDuringUpdate: true - omitNameDuringUpdate: true - omitPrereleaseDuringUpdate: true - - - name: 'Add firmware gripper images artifact' - uses: 'ncipollo/release-action@v1.12.0' - with: - artifacts: ./firmware-gripper-images-${{github.ref_name}}.zip - artifactContentType: application/zip - allowUpdates: true - omitBodyDuringUpdate: true - omitDraftDuringUpdate: true - omitNameDuringUpdate: true - omitPrereleaseDuringUpdate: true - - - name: 'Add firmware applications artifact' - uses: 'ncipollo/release-action@v1.12.0' - with: - artifacts: ./firmware-applications-${{github.ref_name}}.zip - artifactContentType: application/zip + artifacts: './firmware-*-images-${{github.ref_name}}/*.hex' + artifactContentType: text/plain allowUpdates: true omitBodyDuringUpdate: true omitDraftDuringUpdate: true diff --git a/CMakeLists.txt b/CMakeLists.txt index d4c82e6c7..bef77ac12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,8 @@ if (${CMAKE_CROSSCOMPILING}) add_compile_definitions(ENABLE_CCMRAM) + add_compile_definitions(ENABLE_CROSS_ONLY_HEADERS) + configure_file(common/firmware/platform_specific_hal_conf.h.in ${CMAKE_BINARY_DIR}/generated/platform_specific_hal_conf.h) configure_file(common/firmware/platform_specific_hal.h.in ${CMAKE_BINARY_DIR}/generated/platform_specific_hal.h) diff --git a/bootloader/firmware/stm32G4/CMakeLists.txt b/bootloader/firmware/stm32G4/CMakeLists.txt index ac1082841..27520bfc5 100644 --- a/bootloader/firmware/stm32G4/CMakeLists.txt +++ b/bootloader/firmware/stm32G4/CMakeLists.txt @@ -64,8 +64,8 @@ endmacro() foreach_revision( PROJECT_NAME bootloader-gantry-x CALL_FOREACH_REV gantry_x_bootloader_loop - REVISIONS a1 b1 c1 - SOURCES _g4_sources _g4_sources _g4_sources + REVISIONS a1 b1 c1 c2 + SOURCES _g4_sources _g4_sources _g4_sources _g4_sources NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) @@ -73,8 +73,8 @@ foreach_revision( foreach_revision( PROJECT_NAME bootloader-gantry-y CALL_FOREACH_REV gantry_y_bootloader_loop - REVISIONS a1 b1 c1 - SOURCES _g4_sources _g4_sources _g4_sources + REVISIONS a1 b1 c1 c2 + SOURCES _g4_sources _g4_sources _g4_sources _g4_sources NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) @@ -93,8 +93,8 @@ endmacro() foreach_revision( PROJECT_NAME bootloader-gripper CALL_FOREACH_REV gripper_bootloader_loop - REVISIONS a1 b1 c1 - SOURCES gripper_sources gripper_sources gripper_sources + REVISIONS a1 b1 c1 c2 + SOURCES gripper_sources gripper_sources gripper_sources gripper_sources NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) @@ -132,8 +132,8 @@ set(_pipette_sources ${_g4_sources} ./pipette_handle_messages.c) foreach_revision( PROJECT_NAME bootloader-pipettes-single CALL_FOREACH_REV pipettes_single_bootloader_loop - REVISIONS b1 c2 - SOURCES _pipette_sources _pipette_sources + REVISIONS b1 c2 d1 + SOURCES _pipette_sources _pipette_sources _pipette_sources NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) @@ -141,8 +141,8 @@ foreach_revision( foreach_revision( PROJECT_NAME bootloader-pipettes-multi CALL_FOREACH_REV pipettes_multi_bootloader_loop - SOURCES _pipette_sources _pipette_sources - REVISIONS b1 c2 + SOURCES _pipette_sources _pipette_sources _pipette_sources + REVISIONS b1 c2 d1 NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) @@ -150,8 +150,8 @@ foreach_revision( foreach_revision( PROJECT_NAME bootloader-pipettes-96 CALL_FOREACH_REV pipettes_ninety_six_bootloader_loop - REVISIONS b1 c1 - SOURCES _pipette_sources _pipette_sources + REVISIONS b1 c1 d2 + SOURCES _pipette_sources _pipette_sources _pipette_sources NO_CREATE_IMAGE_HEX NO_CREATE_INSTALL_RULES ) diff --git a/common/tests/test_debounce.cpp b/common/tests/test_debounce.cpp index 9df0c25f1..bf4fde690 100644 --- a/common/tests/test_debounce.cpp +++ b/common/tests/test_debounce.cpp @@ -32,3 +32,71 @@ SCENARIO("debouncing gpio pins works") { } } } + +SCENARIO("debouncing with holdoff cnt works") { + GIVEN("a state value and bounce value") { + debouncer::Debouncer subject{.holdoff_cnt = 2}; + WHEN("switching from unset to set") { + subject.debounce_update(true); + CHECK(subject.cnt == 0); + subject.debounce_update(true); + CHECK(subject.cnt == 1); + THEN("state stays the same on the first two ticks") { + REQUIRE(subject.debounce_state() == false); + } + subject.debounce_update(true); + THEN("state updates on the third tick") { + REQUIRE(subject.debounce_state() == true); + THEN("cnt gets reset") { CHECK(subject.cnt == 0); } + } + WHEN("reset is called") { + subject.reset(); + THEN("values are reset") { + REQUIRE(subject.cnt == 0); + REQUIRE(subject.state_bounce == false); + REQUIRE(subject.state == false); + // holdoff cnt remains the same + REQUIRE(subject.holdoff_cnt == 2); + } + } + } + WHEN("switching from set to unset") { + // make sure the state is true + subject.debounce_update(true); + subject.debounce_update(true); + subject.debounce_update(true); + + subject.debounce_update(false); + subject.debounce_update(false); + THEN("state stays the same on the first two ticks") { + REQUIRE(subject.debounce_state() == true); + } + subject.debounce_update(false); + THEN("state updates on the third tick") { + REQUIRE(subject.debounce_state() == false); + } + } + WHEN("switching from set to unset before holdoff cnt is reached") { + // make sure the state is true + subject.debounce_update(true); + subject.debounce_update(true); + CHECK(subject.cnt == 1); + subject.debounce_update(false); + THEN("cnt gets reset back to 0") { + CHECK(subject.cnt == 0); + REQUIRE(subject.debounce_state() == false); + } + THEN("it requires another 3 updates to actually update the state") { + subject.debounce_update(true); + CHECK(subject.cnt == 0); + REQUIRE(subject.debounce_state() == false); + subject.debounce_update(true); + CHECK(subject.cnt == 1); + REQUIRE(subject.debounce_state() == false); + subject.debounce_update(true); + CHECK(subject.cnt == 0); + REQUIRE(subject.debounce_state() == true); + } + } + } +} diff --git a/gantry/firmware/CMakeLists.txt b/gantry/firmware/CMakeLists.txt index 1b350b027..53f95d8d1 100644 --- a/gantry/firmware/CMakeLists.txt +++ b/gantry/firmware/CMakeLists.txt @@ -53,6 +53,7 @@ set(GANTRY_X_B1_SRCS ${CMAKE_SOURCE_DIR}/gantry/firmware/axis_x_rev1_hardware_config.c ) set(GANTRY_X_C1_SRCS ${GANTRY_X_B1_SRCS}) +set(GANTRY_X_C2_SRCS ${GANTRY_X_C1_SRCS}) set(GANTRY_Y_A1_SRCS ${GANTRY_BASE_SOURCES} @@ -65,6 +66,7 @@ set(GANTRY_Y_B1_SRCS ${CMAKE_SOURCE_DIR}/gantry/firmware/axis_y_rev1_hardware_config.c ) set(GANTRY_Y_C1_SRCS ${GANTRY_Y_B1_SRCS}) +set(GANTRY_Y_C2_SRCS ${GANTRY_Y_C1_SRCS}) macro(gantry_loop_internal) set(_gli_options) @@ -122,14 +124,14 @@ endmacro() foreach_revision( PROJECT_NAME gantry-x CALL_FOREACH_REV gantry_loop_internal_x - REVISIONS a1 b1 c1 - SOURCES GANTRY_X_A1_SRCS GANTRY_X_B1_SRCS GANTRY_X_C1_SRCS + REVISIONS a1 b1 c1 c2 + SOURCES GANTRY_X_A1_SRCS GANTRY_X_B1_SRCS GANTRY_X_C1_SRCS GANTRY_X_C2_SRCS ) foreach_revision( PROJECT_NAME gantry-y CALL_FOREACH_REV gantry_loop_internal_y - REVISIONS a1 b1 c1 - SOURCES GANTRY_Y_A1_SRCS GANTRY_Y_B1_SRCS GANTRY_Y_C1_SRCS + REVISIONS a1 b1 c1 c2 + SOURCES GANTRY_Y_A1_SRCS GANTRY_Y_B1_SRCS GANTRY_Y_C1_SRCS GANTRY_Y_C2_SRCS ) diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp index 42bd5d5ad..cd16770c6 100644 --- a/gripper/core/tasks.cpp +++ b/gripper/core/tasks.cpp @@ -160,7 +160,9 @@ void gripper_tasks::QueueClient::send_pressure_sensor_queue_front( void gripper_tasks::QueueClient::send_pressure_sensor_queue_rear( const sensors::utils::TaskMessage&) {} -void gripper_tasks::QueueClient::send_tip_notification_queue( +void gripper_tasks::QueueClient::send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage&) {} +void gripper_tasks::QueueClient::send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage&) {} /** diff --git a/gripper/firmware/CMakeLists.txt b/gripper/firmware/CMakeLists.txt index 1ef2885cf..84979c452 100644 --- a/gripper/firmware/CMakeLists.txt +++ b/gripper/firmware/CMakeLists.txt @@ -49,6 +49,7 @@ set(GRIPPER_SRCS_B1 ${CMAKE_CURRENT_SOURCE_DIR}/main_rev1.cpp ) set(GRIPPER_SRCS_C1 ${GRIPPER_SRCS_B1}) +set(GRIPPER_SRCS_C2 ${GRIPPER_SRCS_C1}) macro(gripper_loop) set(_driver_suffix ${PROJECT_NAME}_${REVISION}) @@ -88,8 +89,8 @@ endmacro() foreach_revision( PROJECT_NAME gripper - REVISIONS a1 b1 c1 - SOURCES GRIPPER_SRCS_A1 GRIPPER_SRCS_B1 GRIPPER_SRCS_C1 + REVISIONS a1 b1 c1 c2 + SOURCES GRIPPER_SRCS_A1 GRIPPER_SRCS_B1 GRIPPER_SRCS_C1 GRIPPER_SRCS_C2 CALL_FOREACH_REV gripper_loop) alias_for_revision(PROJECT_NAME gripper REVISION a1 REVISION_ALIAS proto) diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h index 32b400abb..812d0134f 100644 --- a/include/bootloader/core/ids.h +++ b/include/bootloader/core/ids.h @@ -67,6 +67,8 @@ typedef enum { can_messageid_brushed_motor_conf_request = 0x45, can_messageid_brushed_motor_conf_response = 0x46, can_messageid_set_gripper_error_tolerance = 0x47, + can_messageid_gripper_jaw_state_request = 0x48, + can_messageid_gripper_jaw_state_response = 0x49, can_messageid_acknowledgement = 0x50, can_messageid_read_presence_sensing_voltage_request = 0x600, can_messageid_read_presence_sensing_voltage_response = 0x601, diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index ba6926300..e7be2ef8b 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -72,6 +72,8 @@ enum class MessageId { brushed_motor_conf_request = 0x45, brushed_motor_conf_response = 0x46, set_gripper_error_tolerance = 0x47, + gripper_jaw_state_request = 0x48, + gripper_jaw_state_response = 0x49, acknowledgement = 0x50, read_presence_sensing_voltage_request = 0x600, read_presence_sensing_voltage_response = 0x601, diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp index e59d5fd95..7434a5fab 100644 --- a/include/can/core/message_handlers/motion.hpp +++ b/include/can/core/message_handlers/motion.hpp @@ -48,7 +48,8 @@ class BrushedMotionHandler { using MessageType = std::variant; + SetGripperErrorToleranceRequest, GetMotorUsageRequest, + GripperJawStateRequest>; BrushedMotionHandler(BrushedMotionTaskClient &motion_client) : motion_client{motion_client} {} diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 52dcd7345..30668c1d7 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1041,6 +1041,8 @@ struct GripperGripRequest : BaseMessage { uint8_t seq_id; brushed_timer_ticks duration; uint32_t duty_cycle; + int32_t encoder_position_um; + uint8_t stay_engaged; template static auto parse(Input body, Limit limit) -> GripperGripRequest { @@ -1048,6 +1050,8 @@ struct GripperGripRequest : BaseMessage { uint8_t seq_id = 0; brushed_timer_ticks duration = 0; uint32_t duty_cycle = 0; + int32_t encoder_position_um = 0; + uint8_t stay_engaged = 0; uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind); @@ -1055,12 +1059,16 @@ struct GripperGripRequest : BaseMessage { body = bit_utils::bytes_to_int(body, limit, seq_id); body = bit_utils::bytes_to_int(body, limit, duration); body = bit_utils::bytes_to_int(body, limit, duty_cycle); + body = bit_utils::bytes_to_int(body, limit, encoder_position_um); + body = bit_utils::bytes_to_int(body, limit, stay_engaged); return GripperGripRequest{.message_index = msg_ind, .group_id = group_id, .seq_id = seq_id, .duration = duration, - .duty_cycle = duty_cycle}; + .duty_cycle = duty_cycle, + .encoder_position_um = encoder_position_um, + .stay_engaged = stay_engaged}; } auto operator==(const GripperGripRequest& other) const -> bool = default; @@ -1267,11 +1275,14 @@ struct PushTipPresenceNotification : BaseMessage { uint32_t message_index; uint8_t ejector_flag_status; + can::ids::SensorId sensor_id{}; 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(ejector_flag_status, iter, limit); + iter = bit_utils::int_to_bytes(static_cast(sensor_id), iter, + limit); return iter - body; } @@ -1288,6 +1299,7 @@ struct TipActionRequest mm_per_tick velocity; can::ids::PipetteTipActionType action; uint8_t request_stop_condition; + um_per_tick_sq acceleration; template static auto parse(Input body, Limit limit) -> TipActionRequest { @@ -1297,6 +1309,7 @@ struct TipActionRequest mm_per_tick velocity = 0; uint8_t _action = 0; uint8_t request_stop_condition = 0; + um_per_tick_sq acceleration = 0; uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind); @@ -1306,6 +1319,7 @@ struct TipActionRequest body = bit_utils::bytes_to_int(body, limit, velocity); body = bit_utils::bytes_to_int(body, limit, _action); body = bit_utils::bytes_to_int(body, limit, request_stop_condition); + body = bit_utils::bytes_to_int(body, limit, acceleration); return TipActionRequest{ .message_index = msg_ind, @@ -1314,7 +1328,8 @@ struct TipActionRequest .duration = duration, .velocity = velocity, .action = static_cast(_action), - .request_stop_condition = request_stop_condition}; + .request_stop_condition = request_stop_condition, + .acceleration = acceleration}; } auto operator==(const TipActionRequest& other) const -> bool = default; @@ -1522,6 +1537,26 @@ struct GetMotorUsageResponse auto operator==(const GetMotorUsageResponse& other) const -> bool = default; }; + +using GripperJawStateRequest = Empty; + +struct GripperJawStateResponse + : BaseMessage { + uint32_t message_index; + uint8_t jaw_state; + + 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(jaw_state), iter, + limit); + return iter - body; + } + + auto operator==(const GripperJawStateResponse& other) const + -> bool = default; +}; + /** * A variant of all message types we might send.. */ @@ -1538,6 +1573,6 @@ using ResponseMessageType = std::variant< PeripheralStatusResponse, BrushedMotorConfResponse, UpdateMotorPositionEstimationResponse, BaselineSensorResponse, PushTipPresenceNotification, GetMotorUsageResponse, - ReadMotorDriverErrorRegisterResponse>; + ReadMotorDriverErrorRegisterResponse, GripperJawStateResponse>; } // namespace can::messages diff --git a/include/common/core/debounce.hpp b/include/common/core/debounce.hpp index 1c2f5d11e..2efc9f2cf 100644 --- a/include/common/core/debounce.hpp +++ b/include/common/core/debounce.hpp @@ -4,18 +4,36 @@ namespace debouncer { struct Debouncer { + int holdoff_cnt = 1; std::atomic_bool state = false; std::atomic_bool state_bounce = false; + int cnt = 0; + auto debounce_update(bool new_state) -> void { // only set the state if the bounce matches the current gpio_is_set // on the first state change it won't match but on the second tick it // will and we can set it to the new state. + if (new_state == state_bounce) { - state = new_state; + cnt++; + if (cnt == holdoff_cnt) { + // update state only when cnt reaches the holdoff count + state = new_state; + cnt = 0; + } + } else { + // reset count every time the mismatched + cnt = 0; } + // state bounce is updated each time state_bounce = new_state; } [[nodiscard]] auto debounce_state() const -> bool { return state.load(); } + auto reset() -> void { + state = false; + state_bounce = false; + cnt = 0; + } }; } // namespace debouncer diff --git a/include/common/core/freertos_synchronization.hpp b/include/common/core/freertos_synchronization.hpp index 1781a090d..197716659 100644 --- a/include/common/core/freertos_synchronization.hpp +++ b/include/common/core/freertos_synchronization.hpp @@ -15,9 +15,11 @@ class FreeRTOSMutex { ~FreeRTOSMutex() { vSemaphoreDelete(handle); } - bool acquire() { return xSemaphoreTake(handle, portMAX_DELAY) == pdTRUE; } + auto acquire() -> bool { + return xSemaphoreTake(handle, portMAX_DELAY) == pdTRUE; + } - bool release() { return xSemaphoreGive(handle) == pdTRUE; } + auto release() -> bool { return xSemaphoreGive(handle) == pdTRUE; } auto get_count() -> int { return uxSemaphoreGetCount(handle); } @@ -69,10 +71,10 @@ class FreeRTOSCriticalSection { public: FreeRTOSCriticalSection() = default; FreeRTOSCriticalSection(const FreeRTOSCriticalSection &) = delete; - FreeRTOSCriticalSection(const FreeRTOSCriticalSection &&) = delete; + FreeRTOSCriticalSection(FreeRTOSCriticalSection &&) = delete; auto operator=(const FreeRTOSCriticalSection &) -> FreeRTOSCriticalSection & = delete; - auto operator=(const FreeRTOSCriticalSection &&) + auto operator=(FreeRTOSCriticalSection &&) -> FreeRTOSCriticalSection && = delete; ~FreeRTOSCriticalSection() = default; @@ -83,4 +85,16 @@ class FreeRTOSCriticalSection { void release() { taskEXIT_CRITICAL(); } }; +class FreeRTOSCriticalSectionRAII { + public: + FreeRTOSCriticalSectionRAII() { taskENTER_CRITICAL(); } + FreeRTOSCriticalSectionRAII(const FreeRTOSCriticalSectionRAII &) = delete; + FreeRTOSCriticalSectionRAII(FreeRTOSCriticalSectionRAII &&) = delete; + auto operator=(const FreeRTOSCriticalSectionRAII &) + -> FreeRTOSCriticalSectionRAII & = delete; + auto operator=(FreeRTOSCriticalSectionRAII &&) + -> FreeRTOSCriticalSectionRAII && = delete; + ~FreeRTOSCriticalSectionRAII() { taskEXIT_CRITICAL(); } +}; + } // namespace freertos_synchronization diff --git a/include/eeprom/core/update_data_rev_task.hpp b/include/eeprom/core/update_data_rev_task.hpp index ca5bced63..04792eb47 100644 --- a/include/eeprom/core/update_data_rev_task.hpp +++ b/include/eeprom/core/update_data_rev_task.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include "common/core/bit_utils.hpp" #include "eeprom/core/data_rev.hpp" #include "eeprom/core/dev_data.hpp" diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index a5afa5800..53d8ebddd 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -50,7 +50,7 @@ using BrushedMotionDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, can::messages::ReadLimitSwitchRequest, can::messages::MotorPositionRequest, can::messages::SetGripperErrorToleranceRequest, - can::messages::GetMotorUsageRequest>; + can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest>; using BrushedMoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::BrushedMoveGroupHandler< g_tasks::QueueClient>, diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp index 637994c65..74d58a7ea 100644 --- a/include/gripper/core/tasks.hpp +++ b/include/gripper/core/tasks.hpp @@ -72,7 +72,9 @@ struct QueueClient : can::message_writer::MessageWriter { void send_pressure_sensor_queue_front(const sensors::utils::TaskMessage& m); void send_pressure_sensor_queue_rear(const sensors::utils::TaskMessage& m); - void send_tip_notification_queue( + void send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage& m); + void send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage& m); freertos_message_queue::FreeRTOSMessageQueue< 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 eefa230b7..449965b92 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -48,6 +48,7 @@ class MotionController { .duty_cycle = can_msg.duty_cycle, .group_id = can_msg.group_id, .seq_id = can_msg.seq_id, + .stay_engaged = can_msg.stay_engaged, .stop_condition = MoveStopCondition::none, .usage_key = hardware.get_usage_eeprom_config().get_distance_key()}; if (!enabled) { @@ -104,10 +105,9 @@ class MotionController { queue.reset(); // if we're gripping something we need to flag this so we don't drop it if (!hardware.get_stay_enabled()) { - disable_motor(); - } - if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(static_cast(error_severity)); + if (hardware.is_timer_interrupt_running()) { + hardware.request_cancel(static_cast(error_severity)); + } } } @@ -139,6 +139,10 @@ class MotionController { usage_client.send_usage_storage_queue(req); } + auto get_jaw_state() -> BrushedMotorState { + return hardware.get_motor_state(); + } + private: lms::LinearMotionSystemConfig linear_motion_sys_config; BrushedMotorHardwareIface& hardware; 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 dbd3824df..5fabf2b0a 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 @@ -5,6 +5,7 @@ #include "can/core/ids.hpp" #include "can/core/messages.hpp" +#include "common/core/debounce.hpp" #include "common/core/logging.h" #include "common/core/message_queue.hpp" #include "motor-control/core/brushed_motor/driver_interface.hpp" @@ -24,16 +25,6 @@ using namespace motor_messages; * */ -enum class ControlState { - FORCE_CONTROLLING_HOME, - FORCE_CONTROLLING, - POSITION_CONTROLLING, - IDLE, - ACTIVE, - ERROR, - ESTOP -}; - static constexpr uint32_t HOLDOFF_TICKS = 32; // hold off for 1 ms (with a 32k Hz timer) // using the logic analyzer it takes about 0.2-0.3 ms for the output @@ -98,7 +89,7 @@ class BrushedMotorInterruptHandler { void execute_active_move() { if (timeout_ticks > buffered_move.duration) { - finish_current_move(AckMessageId::timeout); + finish_current_move(false, AckMessageId::timeout); return; } timeout_ticks++; @@ -121,7 +112,8 @@ class BrushedMotorInterruptHandler { controlled_move_to(move_delta); if (std::abs(move_delta) < error_conf.acceptable_position_error) { - finish_current_move(AckMessageId::stopped_by_condition); + finish_current_move(true, + AckMessageId::stopped_by_condition); } break; } @@ -131,7 +123,7 @@ class BrushedMotorInterruptHandler { case MoveStopCondition::stall: if (is_sensing() && is_idle) { finish_current_move( - AckMessageId::complete_without_condition); + true, AckMessageId::complete_without_condition); } break; case MoveStopCondition::ignore_stalls: @@ -145,29 +137,43 @@ class BrushedMotorInterruptHandler { void execute_idle_move() { if (has_messages()) { update_and_start_move(); - } else if (motor_state == ControlState::POSITION_CONTROLLING) { - int32_t move_delta = - hardware.get_encoder_pulses() - hold_encoder_position; - controlled_move_to(move_delta); - // we use a value higher than the acceptable position here to allow - // the pid loop the opportunity to maintain small movements that - // occur from motion and vibration - if (move_delta > error_conf.unwanted_movement_threshold) { - cancel_and_clear_moves(can::ids::ErrorCode::collision_detected); - motor_state = ControlState::ERROR; - } - } else if (motor_state == ControlState::FORCE_CONTROLLING || - motor_state == ControlState::FORCE_CONTROLLING_HOME) { + } else if (!error_handled) { + auto motor_state = hardware.get_motor_state(); auto pulses = hardware.get_encoder_pulses(); - if (!is_idle && pulses >= 0 && - std::abs(pulses - hold_encoder_position) > - error_conf.unwanted_movement_threshold) { - // we have likely dropped a labware or had a collision - auto err = motor_state == ControlState::FORCE_CONTROLLING - ? can::ids::ErrorCode::labware_dropped - : can::ids::ErrorCode::collision_detected; - cancel_and_clear_moves(err); - motor_state = ControlState::ERROR; + // has not reported an error yet + if (motor_state == BrushedMotorState::POSITION_CONTROLLING) { + int32_t move_delta = pulses - hold_encoder_position; + controlled_move_to(move_delta); + // we use a value higher than the acceptable position here to + // allow the pid loop the opportunity to maintain small + // movements that occur from motion and vibration + enc_errored.debounce_update( + move_delta > error_conf.unwanted_movement_threshold); + if (enc_errored.debounce_state()) { + // enc value errored for 3 consecutive ticks + cancel_and_clear_moves( + can::ids::ErrorCode::collision_detected); + report_position(pulses); + error_handled = true; + } + } else if (motor_state != BrushedMotorState::UNHOMED) { + auto pulses = hardware.get_encoder_pulses(); + enc_errored.debounce_update( + !is_idle && pulses >= 0 && + std::abs(pulses - hold_encoder_position) > + error_conf.unwanted_movement_threshold); + + if (enc_errored.debounce_state()) { + // enc value errored for 3 consecutive ticks + // we have likely dropped a labware or had a collision + auto err = + motor_state == BrushedMotorState::FORCE_CONTROLLING + ? can::ids::ErrorCode::labware_dropped + : can::ids::ErrorCode::collision_detected; + cancel_and_clear_moves(err); + report_position(pulses); + error_handled = true; + } } } } @@ -175,13 +181,13 @@ class BrushedMotorInterruptHandler { void run_interrupt() { if (clear_queue_until_empty) { clear_queue_until_empty = pop_and_discard_move(); - } else if (motor_state == ControlState::ESTOP) { + } else if (in_estop) { // if we've received a stop request during this time we can clear - // that flag since there is isn't anything running + // that flag since there isn't anything running std::ignore = hardware.has_cancel_request(); // return out of error state once the estop is disabled if (!estop_triggered()) { - motor_state = ControlState::IDLE; + in_estop = false; status_queue_client.send_brushed_move_status_reporter_queue( can::messages::ErrorMessage{ .message_index = 0, @@ -193,14 +199,17 @@ class BrushedMotorInterruptHandler { .get_error_count_key()}); } } else if (estop_triggered()) { + in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); - motor_state = ControlState::ESTOP; } else if (hardware.has_cancel_request()) { - cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning); - motor_state = ControlState::IDLE; + if (!hardware.get_stay_enabled()) { + hardware.set_motor_state(BrushedMotorState::UNHOMED); + } + cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, + can::ids::ErrorSeverity::warning); } else if (tick < HOLDOFF_TICKS) { tick++; - } else if (motor_state == ControlState::ACTIVE) { + } else if (_has_active_move) { execute_active_move(); } else { execute_idle_move(); @@ -227,8 +236,9 @@ class BrushedMotorInterruptHandler { // this function is called when the timer overflows, which happens 25 // seconds after movement stops. if we're in force mode, increase the // usage in eeprom otherwise do nothing. - if (motor_state == ControlState::FORCE_CONTROLLING || - motor_state == ControlState::FORCE_CONTROLLING_HOME) { + auto motor_state = hardware.get_motor_state(); + if (motor_state == BrushedMotorState::FORCE_CONTROLLING || + motor_state == BrushedMotorState::FORCE_CONTROLLING_HOME) { status_queue_client.send_brushed_move_status_reporter_queue( usage_messages::IncreaseForceTimeUsage{ .key = hardware.get_usage_eeprom_config() @@ -238,45 +248,56 @@ class BrushedMotorInterruptHandler { } void update_and_start_move() { - if (queue.try_read_isr(&buffered_move)) { - if (motor_state == ControlState::FORCE_CONTROLLING || - motor_state == ControlState::FORCE_CONTROLLING_HOME) { - // if we've been applying force before the new move is called - // add that force application time to the usage storage - status_queue_client.send_brushed_move_status_reporter_queue( - usage_messages::IncreaseForceTimeUsage{ - .key = hardware.get_usage_eeprom_config() - .get_force_application_time_key(), - .seconds = uint16_t( - hardware.get_stopwatch_pulses(true) / 2600)}); - } - motor_state = ControlState::ACTIVE; - buffered_move.start_encoder_position = - hardware.get_encoder_pulses(); + _has_active_move = queue.try_read_isr(&buffered_move); + if (!_has_active_move) { + return; } + auto motor_state = hardware.get_motor_state(); + if (motor_state == BrushedMotorState::FORCE_CONTROLLING || + motor_state == BrushedMotorState::FORCE_CONTROLLING_HOME) { + // if we've been applying force before the new move is called + // add that force application time to the usage storage + status_queue_client.send_brushed_move_status_reporter_queue( + usage_messages::IncreaseForceTimeUsage{ + .key = hardware.get_usage_eeprom_config() + .get_force_application_time_key(), + .seconds = + uint16_t(hardware.get_stopwatch_pulses(true) / 2600)}); + } + buffered_move.start_encoder_position = hardware.get_encoder_pulses(); + if (buffered_move.duty_cycle != 0U) { driver_hardware.update_pwm_settings(buffered_move.duty_cycle); } // clear the old states hardware.reset_control(); - hardware.set_stay_enabled(false); timeout_ticks = 0; + error_handled = false; + enc_errored.reset(); + hardware.set_stay_enabled( + static_cast(buffered_move.stay_engaged)); switch (buffered_move.stop_condition) { case MoveStopCondition::limit_switch: + hardware.set_motor_state( + BrushedMotorState::FORCE_CONTROLLING_HOME); tick = 0; hardware.ungrip(); break; case MoveStopCondition::encoder_position: + hardware.set_motor_state( + BrushedMotorState::POSITION_CONTROLLING); if (hardware.get_encoder_pulses() == buffered_move.encoder_position) { LOG("Attempting to add a gripper move to our current " "position"); - finish_current_move(AckMessageId::stopped_by_condition); + finish_current_move(true, + AckMessageId::stopped_by_condition); } break; case MoveStopCondition::none: case MoveStopCondition::gripper_force: case MoveStopCondition::stall: + hardware.set_motor_state(BrushedMotorState::FORCE_CONTROLLING); tick = 0; hardware.grip(); break; @@ -286,11 +307,27 @@ class BrushedMotorInterruptHandler { // this is an unused move stop condition for the brushed motor // just return with no condition // TODO creat can bus error messages and send that instead - finish_current_move(AckMessageId::complete_without_condition); + finish_current_move(false, + AckMessageId::complete_without_condition); break; } } + void report_position(int32_t pulses) { + // this message is used only to report encoder position on the can bus + // when the move fails, and will not be used nor handled by the host + uint32_t message_index = 0; + if (_has_active_move) { + message_index = buffered_move.message_index; + } + status_queue_client.send_brushed_move_status_reporter_queue( + motor_messages::UpdatePositionResponse{ + .message_index = message_index, + .stepper_position_counts = 0, + .encoder_pulses = pulses, + .position_flags = 0}); + } + void homing_stopped() { // since the buffered move start position isn't reliable here, // and then end position will always be 0, we set the buffered move @@ -301,7 +338,7 @@ class BrushedMotorInterruptHandler { buffered_move.start_encoder_position - hardware.get_encoder_pulses(); hardware.reset_encoder_pulses(); - finish_current_move(AckMessageId::stopped_by_condition); + finish_current_move(true, AckMessageId::stopped_by_condition); } auto limit_switch_triggered() -> bool { @@ -318,20 +355,10 @@ class BrushedMotorInterruptHandler { // to it so the hardware controller can know what move was running // when the cancel happened uint32_t message_index = 0; - if (motor_state == ControlState::ACTIVE) { + if (_has_active_move) { message_index = buffered_move.message_index; } - // if we think we dropped a labware we don't want the controller - // to stop the motor in case we only slipped or collided and still - // have the labware in the jaws - // likewise if the estop is hit and the motor is gripping something - if (err_code == can::ids::ErrorCode::labware_dropped || - (motor_state == ControlState::FORCE_CONTROLLING && - err_code == can::ids::ErrorCode::estop_detected)) { - hardware.set_stay_enabled(true); - } - status_queue_client.send_brushed_move_status_reporter_queue( can::messages::ErrorMessage{.message_index = message_index, .severity = severity, @@ -344,28 +371,19 @@ class BrushedMotorInterruptHandler { } void finish_current_move( + bool update_hold_position, AckMessageId ack_msg_id = AckMessageId::complete_without_condition) { - // if we were instructed to move to particular spot maintain that - // position otherwise we want to continue to apply pressure - if (buffered_move.stop_condition == - MoveStopCondition::encoder_position) { - hold_encoder_position = buffered_move.encoder_position; - motor_state = ControlState::POSITION_CONTROLLING; - // the cap sensor move isn't valid here, so make sure if we got that - // type of message that we just pass through and return the ack - } else if (buffered_move.stop_condition != - MoveStopCondition::sync_line) { - hold_encoder_position = hardware.get_encoder_pulses(); - motor_state = - buffered_move.stop_condition == MoveStopCondition::limit_switch - ? ControlState::FORCE_CONTROLLING_HOME - : ControlState::FORCE_CONTROLLING; - // If we're in the gripping state, then we want to signal the - // hardware not to disable the motor on a - // brushed_motor_controller::stop which would drop the labware. - if (motor_state == ControlState::FORCE_CONTROLLING) { - hardware.set_stay_enabled(true); - } + _has_active_move = false; + + // only update hold position when the move is valid + if (update_hold_position) { + // use the commanded move position as the hold position if posiiton + // controling otherwise, we can use the actual jaw encoder position + hold_encoder_position = + hardware.get_motor_state() == + BrushedMotorState::POSITION_CONTROLLING + ? buffered_move.encoder_position + : hardware.get_encoder_pulses(); } if (buffered_move.group_id != NO_GROUP) { @@ -397,10 +415,13 @@ class BrushedMotorInterruptHandler { return has_messages(); } + auto has_active_move() -> bool { return _has_active_move.load(); } + std::atomic is_idle = true; uint32_t tick = 0; uint32_t timeout_ticks = 0; - ControlState motor_state = ControlState::IDLE; + bool error_handled = false; + bool in_estop = false; private: GenericQueue& queue; @@ -412,5 +433,7 @@ class BrushedMotorInterruptHandler { int32_t hold_encoder_position = 0; uint32_t current_control_pwm = 0; bool clear_queue_until_empty = false; + std::atomic_bool _has_active_move = false; + debouncer::Debouncer enc_errored = debouncer::Debouncer{}; }; } // namespace brushed_motor_handler diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp index 60eace601..54eb1178f 100644 --- a/include/motor-control/core/motor_hardware_interface.hpp +++ b/include/motor-control/core/motor_hardware_interface.hpp @@ -145,5 +145,7 @@ class BrushedMotorHardwareIface : virtual public MotorHardwareIface { virtual void set_stay_enabled(bool state) = 0; virtual auto get_stay_enabled() -> bool = 0; virtual auto get_stopwatch_pulses(bool clear) -> uint16_t = 0; + virtual void set_motor_state(BrushedMotorState state) = 0; + virtual auto get_motor_state() -> BrushedMotorState = 0; }; }; // namespace motor_hardware diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp index e27436356..bfe573397 100644 --- a/include/motor-control/core/motor_messages.hpp +++ b/include/motor-control/core/motor_messages.hpp @@ -43,6 +43,7 @@ struct GearMotorAck : public Ack { uint32_t start_step_position; can::ids::PipetteTipActionType action; can::ids::GearMotorId gear_motor_id; + uint8_t position_flags; }; struct Move { // NOLINT(cppcoreguidelines-pro-type-member-init) @@ -106,6 +107,7 @@ struct BrushedMove { // NOLINT(cppcoreguidelines-pro-type-member-init) uint8_t group_id; uint8_t seq_id; int32_t encoder_position; + uint8_t stay_engaged = 0; MoveStopCondition stop_condition = MoveStopCondition::none; int32_t start_encoder_position; uint16_t usage_key; @@ -132,6 +134,11 @@ struct UpdatePositionResponse { uint8_t position_flags; }; +struct GripperJawStateResponse { + uint32_t message_index; + uint8_t jaw_state; +}; + const uint8_t NO_GROUP = 0xff; constexpr const int RADIX = 31; diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index 901c476a9..c49ff0353 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -109,7 +109,6 @@ class MotionController { void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) { queue.reset(); - disable_motor(); if (hardware.is_timer_interrupt_running()) { hardware.request_cancel(static_cast(error_severity)); } @@ -210,6 +209,8 @@ class PipetteMotionController { hardware(hardware_iface), motion_constraints(constraints), queue(queue), + steps_per_um(convert_to_fixed_point_64_bit( + linear_motion_sys_config.get_usteps_per_um(), 31)), steps_per_mm(convert_to_fixed_point_64_bit( linear_motion_sys_config.get_usteps_per_mm(), 31)), um_per_step(convert_to_fixed_point_64_bit( @@ -233,11 +234,13 @@ class PipetteMotionController { void move(const can::messages::TipActionRequest& can_msg) { steps_per_tick velocity_steps = fixed_point_multiply(steps_per_mm, can_msg.velocity); + steps_per_tick_sq acceleration_steps = + fixed_point_multiply(steps_per_um, can_msg.acceleration); GearMotorMove msg{ can_msg.message_index, can_msg.duration, velocity_steps, - 0, + acceleration_steps, can_msg.group_id, can_msg.seq_id, can_msg.request_stop_condition, @@ -261,7 +264,6 @@ class PipetteMotionController { void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) { queue.reset(); - disable_motor(); // 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. @@ -321,6 +323,8 @@ class PipetteMotionController { StepperMotorHardwareIface& hardware; MotionConstraints motion_constraints; GenericQueue& queue; + + sq31_31 steps_per_um{0}; sq31_31 steps_per_mm{0}; sq31_31 um_per_step{0}; bool enabled = false; diff --git a/include/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp b/include/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp index ea9085914..8d8ec76d5 100644 --- a/include/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp +++ b/include/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp @@ -4,6 +4,7 @@ #include #include +#include "common/core/freertos_synchronization.hpp" #include "motor-control/core/motor_hardware_interface.hpp" #include "ot_utils/freertos/freertos_timer.hpp" @@ -29,9 +30,12 @@ class BackgroundTimer { auto stop() -> void { _timer.stop(); } auto callback() -> void { - if (!_interrupt_handler.has_active_move()) { + auto critical_section = + freertos_synchronization::FreeRTOSCriticalSectionRAII(); + if (!_interrupt_handler.is_moving()) { // Refresh the overflow counter if nothing else is doing it - std::ignore = _motor_hardware.get_encoder_pulses(); + // and update position flag if needed + std::ignore = _interrupt_handler.check_for_stall(); } } @@ -43,4 +47,4 @@ class BackgroundTimer { ot_utils::freertos_timer::FreeRTOSTimer _timer; }; -} // namespace motor_encoder \ No newline at end of file +} // namespace motor_encoder 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 68f9f15af..0906849f2 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -51,7 +51,6 @@ class MotorInterruptHandler { using MoveQueue = QueueImpl; using UpdatePositionQueue = QueueImpl; - MotorInterruptHandler() = delete; MotorInterruptHandler(MoveQueue& incoming_move_queue, StatusClient& outgoing_queue, @@ -81,9 +80,7 @@ class MotorInterruptHandler { hardware.step(); update_hardware_step_tracker(); if (stall_checker.step_itr(set_direction_pin())) { - if (stall_detected()) { - hardware.position_flags.clear_flag( - MotorPositionStatus::Flags::stepper_position_ok); + if (check_for_stall()) { handle_stall_during_movement(); } } @@ -91,6 +88,15 @@ class MotorInterruptHandler { } } + auto check_for_stall() -> bool { + if (stall_detected()) { + hardware.position_flags.clear_flag( + MotorPositionStatus::Flags::stepper_position_ok); + return true; + } + return false; + } + auto handle_stall_during_movement() -> void { if (!_has_active_move or hardware.position_flags.check_flag( @@ -388,6 +394,8 @@ class MotorInterruptHandler { update_hardware_step_tracker(); hardware.position_flags.clear_flag( can::ids::MotorPositionFlags::stepper_position_ok); + hardware.position_flags.clear_flag( + can::ids::MotorPositionFlags::encoder_position_ok); } } @@ -461,6 +469,11 @@ class MotorInterruptHandler { stall_handled = false; build_and_send_ack(ack_msg_id); set_buffered_move(MotorMoveMessage{}); + // update the stall check ideal encoder counts based on + // last known location + if (!has_move_messages()) { + stall_checker.reset_itr_counts(hardware.get_step_tracker()); + } } void reset() { @@ -513,7 +526,7 @@ class MotorInterruptHandler { * message and update the motor position if such a message exists. */ auto handle_update_position_queue() -> void { - can::messages::UpdateMotorPositionEstimationRequest msg; + can::messages::UpdateMotorPositionEstimationRequest msg = {}; if (update_position_queue.try_read_isr(&msg)) { auto encoder_pulses = hardware.get_encoder_pulses(); @@ -547,8 +560,8 @@ class MotorInterruptHandler { /** * @brief While a move is active, this function should be called to - * check if there is a pending UpdateMotorPositionEstimationRequest and send - * an error message over CAN if such a message exists. + * check if there is a pending UpdateMotorPositionEstimationRequest and + * send an error message over CAN if such a message exists. * */ auto handle_update_position_queue_error() -> void { @@ -556,7 +569,7 @@ class MotorInterruptHandler { return; } - can::messages::UpdateMotorPositionEstimationRequest msg; + can::messages::UpdateMotorPositionEstimationRequest msg = {}; if (update_position_queue.try_read_isr(&msg)) { auto response = can::messages::ErrorMessage{ .message_index = msg.message_index, @@ -584,6 +597,14 @@ class MotorInterruptHandler { auto has_active_move() -> bool { return _has_active_move.load(); } + /** + * @brief Returns whether the motor has an active move *and* + * the velocity is nonzero. + */ + auto is_moving() -> bool { + return has_active_move() && buffered_move.velocity != 0; + } + private: void update_hardware_step_tracker() { hardware.set_step_tracker( 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 5ca9f5fbf..41067b574 100644 --- a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp +++ b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp @@ -116,6 +116,14 @@ class MotionControllerMessageHandler { controller.send_usage_data(m.message_index, usage_client); } + void handle(const can::messages::GripperJawStateRequest& m) { + auto jaw_state = controller.get_jaw_state(); + can::messages::GripperJawStateResponse msg{ + .message_index = m.message_index, + .jaw_state = static_cast(jaw_state)}; + can_client.send_can_message(can::ids::NodeId::host, msg); + } + brushed_motion_controller::MotionController& controller; CanClient& can_client; UsageClient& usage_client; diff --git a/include/motor-control/core/tasks/brushed_move_group_task.hpp b/include/motor-control/core/tasks/brushed_move_group_task.hpp index 0bb2bc1ce..4cd2f1573 100644 --- a/include/motor-control/core/tasks/brushed_move_group_task.hpp +++ b/include/motor-control/core/tasks/brushed_move_group_task.hpp @@ -12,8 +12,8 @@ namespace brushed_move_group_task { -constexpr std::size_t max_groups = 6; -constexpr std::size_t max_moves_per_group = 5; +constexpr std::size_t max_groups = 3; +constexpr std::size_t max_moves_per_group = 12; using MoveGroupType = move_group::MoveGroupManager; + can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest>; using BrushedMoveGroupTaskMessage = std::variant< std::monostate, can::messages::ClearAllMoveGroupsRequest, diff --git a/include/motor-control/core/tasks/move_group_task.hpp b/include/motor-control/core/tasks/move_group_task.hpp index 917cd3021..04abfce07 100644 --- a/include/motor-control/core/tasks/move_group_task.hpp +++ b/include/motor-control/core/tasks/move_group_task.hpp @@ -12,8 +12,8 @@ namespace move_group_task { -constexpr std::size_t max_groups = 6; -constexpr std::size_t max_moves_per_group = 5; +constexpr std::size_t max_groups = 3; +constexpr std::size_t max_moves_per_group = 12; using MoveGroupType = move_group::MoveGroupManager BrushedMotorState final { return motor_state; } + private: bool stay_enabled = false; debouncer::Debouncer estop = debouncer::Debouncer{}; @@ -101,6 +104,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { std::atomic cancel_request = 0; const UsageEEpromConfig& eeprom_config; void* stopwatch_handle; + BrushedMotorState motor_state = BrushedMotorState::UNHOMED; }; }; // namespace motor_hardware diff --git a/include/motor-control/simulation/brushed_motor_interrupt_driver.hpp b/include/motor-control/simulation/brushed_motor_interrupt_driver.hpp index 7f1621329..d8ac34942 100644 --- a/include/motor-control/simulation/brushed_motor_interrupt_driver.hpp +++ b/include/motor-control/simulation/brushed_motor_interrupt_driver.hpp @@ -60,8 +60,8 @@ class BrushedMotorInterruptDriver { } handler.run_interrupt(); - } while (handler.motor_state == - brushed_motor_handler::ControlState::ACTIVE); + } while (iface.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING); LOG("Move completed. Stopping interrupt simulation.."); } } diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp index 3174648fd..43fa1812a 100644 --- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp +++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp @@ -185,6 +185,9 @@ class SimBrushedMotorHardwareIface void disable_encoder() final {} void enable_encoder() final {} + void set_motor_state(BrushedMotorState state) final { motor_state = state; } + auto get_motor_state() -> BrushedMotorState final { return motor_state; } + private: bool stay_enabled = false; bool limit_switch_status = false; @@ -199,6 +202,7 @@ class SimBrushedMotorHardwareIface MoveMessageHardware _id; bool estop_detected = false; std::atomic cancel_request = 0; + BrushedMotorState motor_state = BrushedMotorState::UNHOMED; motor_hardware::UsageEEpromConfig eeprom_config{ std::array{UsageRequestSet{ .eeprom_key = 0, diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp index 6ddc8f97a..a5d6f7143 100644 --- a/include/motor-control/tests/mock_brushed_motor_components.hpp +++ b/include/motor-control/tests/mock_brushed_motor_components.hpp @@ -87,8 +87,12 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { void disable_encoder() final {} void enable_encoder() final {} + void set_motor_state(BrushedMotorState state) { motor_state = state; } + auto get_motor_state() -> BrushedMotorState { return motor_state; } + private: bool stay_enabled = false; + BrushedMotorState motor_state = BrushedMotorState::UNHOMED; PWM_DIRECTION move_dir = PWM_DIRECTION::unset; int32_t motor_encoder_overflow_count = 0; bool ls_val = false; @@ -107,12 +111,22 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { uint8_t cancel_request = 0; bool timer_interrupt_running = true; motor_hardware::UsageEEpromConfig eeprom_config = - motor_hardware::UsageEEpromConfig{ - std::array{UsageRequestSet{ - .eeprom_key = 0, + motor_hardware::UsageEEpromConfig{std::array{ + UsageRequestSet{ + .eeprom_key = 0x0, .type_key = uint16_t( can::ids::MotorUsageValueType::linear_motor_distance), - .length = usage_storage_task::distance_data_usage_len}}}; + .length = usage_storage_task::distance_data_usage_len}, + UsageRequestSet{ + .eeprom_key = 0x1, + .type_key = uint16_t( + can::ids::MotorUsageValueType::force_application_time), + .length = usage_storage_task::force_time_data_usage_len}, + UsageRequestSet{ + .eeprom_key = 0x2, + .type_key = + uint16_t(can::ids::MotorUsageValueType::total_error_count), + .length = usage_storage_task::error_count_usage_len}}}; }; class MockBrushedMotorDriverIface : public BrushedMotorDriverIface { diff --git a/include/pipettes/core/sensor_tasks.hpp b/include/pipettes/core/sensor_tasks.hpp index ff8660c66..f3c51e821 100644 --- a/include/pipettes/core/sensor_tasks.hpp +++ b/include/pipettes/core/sensor_tasks.hpp @@ -72,8 +72,11 @@ struct Tasks { freertos_message_queue::FreeRTOSMessageQueue>* pressure_sensor_task_front{nullptr}; sensors::tasks::TipPresenceNotificationTask< - freertos_message_queue::FreeRTOSMessageQueue>* tip_notification_task{ - nullptr}; + freertos_message_queue::FreeRTOSMessageQueue>* + tip_notification_task_rear{nullptr}; + sensors::tasks::TipPresenceNotificationTask< + freertos_message_queue::FreeRTOSMessageQueue>* + tip_notification_task_front{nullptr}; }; /** @@ -96,7 +99,10 @@ struct QueueClient : can::message_writer::MessageWriter { void send_pressure_sensor_queue_front(const sensors::utils::TaskMessage& m); - void send_tip_notification_queue( + void send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage& m); + + void send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage& m); freertos_message_queue::FreeRTOSMessageQueue* @@ -112,7 +118,11 @@ struct QueueClient : can::message_writer::MessageWriter { freertos_message_queue::FreeRTOSMessageQueue* pressure_sensor_queue_front{nullptr}; freertos_message_queue::FreeRTOSMessageQueue< - sensors::tip_presence::TaskMessage>* tip_notification_queue{nullptr}; + sensors::tip_presence::TaskMessage>* tip_notification_queue_rear{ + nullptr}; + freertos_message_queue::FreeRTOSMessageQueue< + sensors::tip_presence::TaskMessage>* tip_notification_queue_front{ + nullptr}; }; /** diff --git a/include/pipettes/core/tasks/gear_move_status_reporter_task.hpp b/include/pipettes/core/tasks/gear_move_status_reporter_task.hpp index 2875c9292..72d18c68d 100644 --- a/include/pipettes/core/tasks/gear_move_status_reporter_task.hpp +++ b/include/pipettes/core/tasks/gear_move_status_reporter_task.hpp @@ -62,7 +62,7 @@ class MoveStatusMessageHandler { .seq_id = message.seq_id, .current_position_um = end_position, .encoder_position_um = 0, - .position_flags = 0, + .position_flags = message.position_flags, .ack_id = static_cast(message.ack_id), .action = message.action, // TODO: In a follow-up PR, tip sense reporting will diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index eaece9949..662094711 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -100,8 +100,8 @@ class MotionControllerMessageHandler { void handle(const can::messages::TipActionRequest& m) { LOG("Motion Controller Received a tip action request: velocity=%d, " - "groupid=%d, seqid=%d\n", - m.velocity, m.group_id, m.seq_id); + "acceleration=%d, groupid=%d, seqid=%d\n", + m.velocity, m.acceleration, m.group_id, m.seq_id); controller.move(m); } diff --git a/include/pipettes/core/tasks/move_group_task.hpp b/include/pipettes/core/tasks/move_group_task.hpp index f78a8350f..1be52c74f 100644 --- a/include/pipettes/core/tasks/move_group_task.hpp +++ b/include/pipettes/core/tasks/move_group_task.hpp @@ -15,8 +15,8 @@ namespace tasks { namespace move_group_task { -constexpr std::size_t max_groups = 6; -constexpr std::size_t max_moves_per_group = 5; +constexpr std::size_t max_groups = 3; +constexpr std::size_t max_moves_per_group = 12; using MoveGroupType = move_group::MoveGroupManager // This is used to mask the value before writing it to the sensor. concept FDC1004Register = std::same_as, - std::remove_cvref_t> && + std::remove_cvref_t> && std::integral; template @@ -559,5 +559,21 @@ inline auto update_offset(float capacitance_pf, float current_offset_pf) return static_cast(capdac) * CAPDAC_PF_PER_LSB; } + +inline auto measurement_ready(fdc1004::FDCConf &fdc, + fdc1004::MeasureConfigMode mode) -> bool { + switch (mode) { + case fdc1004::MeasureConfigMode::ONE: + return fdc.measure_mode_1_status; + case fdc1004::MeasureConfigMode::TWO: + return fdc.measure_mode_2_status; + case fdc1004::MeasureConfigMode::THREE: + return fdc.measure_mode_3_status; + case fdc1004::MeasureConfigMode::FOUR: + return fdc.measure_mode_4_status; + } + return false; +} + } // namespace fdc1004_utils }; // namespace sensors diff --git a/include/sensors/core/message_handlers/sensors.hpp b/include/sensors/core/message_handlers/sensors.hpp index b41632604..8eac2f770 100644 --- a/include/sensors/core/message_handlers/sensors.hpp +++ b/include/sensors/core/message_handlers/sensors.hpp @@ -57,7 +57,8 @@ class SensorHandler { } void visit(const can::messages::TipStatusQueryRequest &m) { - client.send_tip_notification_queue(m); + client.send_tip_notification_queue_rear(m); + client.send_tip_notification_queue_front(m); } void send_to_queue(can::ids::SensorType type, can::ids::SensorId id, diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 15947159b..58ab83fbf 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -1,7 +1,21 @@ #pragma once #include -#include + +#ifdef ENABLE_CROSS_ONLY_HEADERS +// TODO(fps 7/12/2023): This is super hacky and I hate throwing #ifdefs +// in our nicely host-independent code but for now we really just need +// the vTaskDelay function and hopefully sometime in the near future I +// can refactor this file with a nice templated sleep function. +#include "FreeRTOS.h" +// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) +#define HACKY_TASK_SLEEP(___timeout___) vTaskDelay(___timeout___) + +#else + +// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) +#define HACKY_TASK_SLEEP(___timeout___) (void)(___timeout___) +#endif #include "can/core/can_writer_task.hpp" #include "can/core/ids.hpp" @@ -10,7 +24,6 @@ #include "common/core/logging.h" #include "common/core/message_queue.hpp" #include "i2c/core/messages.hpp" -#include "ot_utils/core/filters/sma.hpp" #include "sensors/core/fdc1004.hpp" #include "sensors/core/sensor_hardware_interface.hpp" #include "sensors/core/utils.hpp" @@ -38,19 +51,23 @@ class FDC1004 { [[nodiscard]] auto initialized() const -> bool { return _initialized; } auto initialize() -> void { - // FIXME we should grab device id and compare it to the static - // device id in code. - - // We should send a message that the sensor is in a ready state, - // not sure if we should have a separate can message to do that - // holding off for this PR. - if (shared_sensor) { - measure_mode = fdc1004::MeasureConfigMode::TWO; + if (!_initialized) { + // FIXME we should grab device id and compare it to the static + // device id in code. + + // We should send a message that the sensor is in a ready state, + // not sure if we should have a separate can message to do that + // holding off for this PR. + + // Initial delay to avoid I2C bus traffic. + HACKY_TASK_SLEEP(100); update_capacitance_configuration(); + // Second delay to ensure IC is ready to start + // readings (and also to avoid I2C bus traffic). + HACKY_TASK_SLEEP(100); + set_sample_rate(); + _initialized = true; } - measure_mode = fdc1004::MeasureConfigMode::ONE; - update_capacitance_configuration(); - _initialized = true; } auto register_map() -> fdc1004::FDC1004RegisterMap & { return _registers; } @@ -70,7 +87,6 @@ class FDC1004 { } sensor_id = _id; update_capacitance_configuration(); - filter.reset_filter(); } } @@ -100,7 +116,6 @@ class FDC1004 { // mode. current_offset_pf = -1; set_offset(0); - set_sample_rate(); } void reset_limited() { @@ -116,13 +131,10 @@ class FDC1004 { _registers.fdc_conf.measurement_rate = static_cast(measurement_rate); _registers.fdc_conf.repeating_measurements = 1; - if (measure_mode == fdc1004::MeasureConfigMode::TWO) { - _registers.fdc_conf.measure_mode_1 = 0; - _registers.fdc_conf.measure_mode_2 = 1; - } else { - _registers.fdc_conf.measure_mode_1 = 1; - _registers.fdc_conf.measure_mode_2 = 0; - } + // Enable both measurements no matter what. We check the Ready + // bits anyways and the data doesn't overwrite, so there's no danger. + _registers.fdc_conf.measure_mode_1 = 1; + _registers.fdc_conf.measure_mode_2 = 1; _registers.fdc_conf.padding_0 = 0; _registers.fdc_conf.padding_1 = 0; set_register(_registers.fdc_conf); @@ -164,66 +176,57 @@ class FDC1004 { auto poll_limited_capacitance(uint16_t number_reads, can::ids::SensorId _id, uint8_t tags) -> void { set_sensor_id(_id); - auto converted_msb_addr = - static_cast(fdc1004::Registers::MEAS1_MSB); - auto converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS1_LSB); - if (measure_mode == fdc1004::MeasureConfigMode::TWO) { - converted_msb_addr = - static_cast(fdc1004::Registers::MEAS2_MSB); - converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS2_LSB); - poller.multi_register_poll( - fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2, - number_reads, DELAY, own_queue, - utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags)); - } else { - poller.multi_register_poll( - fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2, - number_reads, DELAY, own_queue, - utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags)); - } + poller.single_register_poll( + fdc1004::ADDRESS, + static_cast(fdc1004::Registers::FDC_CONF), 2, number_reads, + DELAY, own_queue, + utils::build_id(fdc1004::ADDRESS, + static_cast(fdc1004::Registers::FDC_CONF), + tags)); } auto poll_continuous_capacitance(can::ids::SensorId _id, uint8_t tags, uint8_t binding) -> void { set_sensor_id(_id); auto delay = delay_or_disable(binding); - auto converted_msb_addr = - static_cast(fdc1004::Registers::MEAS1_MSB); - auto converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS1_LSB); - if (measure_mode == fdc1004::MeasureConfigMode::TWO) { - converted_msb_addr = - static_cast(fdc1004::Registers::MEAS2_MSB); - converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS2_LSB); - poller.continuous_multi_register_poll( - fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2, - delay, own_queue, - utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags)); - } else { - poller.continuous_multi_register_poll( - fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2, - delay, own_queue, - utils::build_id(fdc1004::ADDRESS, converted_msb_addr, tags)); - } + poller.continuous_single_register_poll( + fdc1004::ADDRESS, + static_cast(fdc1004::Registers::FDC_CONF), 2, delay, + own_queue, + utils::build_id(fdc1004::ADDRESS, + static_cast(fdc1004::Registers::FDC_CONF), + tags)); } auto stop_continuous_polling(uint32_t transaction_id) -> void { - auto converted_msb_addr = - static_cast(fdc1004::Registers::MEAS1_MSB); - auto converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS1_LSB); - if (measure_mode == fdc1004::MeasureConfigMode::TWO) { - converted_msb_addr = - static_cast(fdc1004::Registers::MEAS2_MSB); - converted_lsb_addr = - static_cast(fdc1004::Registers::MEAS2_LSB); + poller.continuous_single_register_poll( + fdc1004::ADDRESS, + static_cast(fdc1004::Registers::FDC_CONF), 2, STOP_DELAY, + own_queue, transaction_id); + } + + void handle_fdc_response(i2c::messages::TransactionResponse &m) { + uint16_t reg_int = 0; + static_cast(bit_utils::bytes_to_int( + m.read_buffer.cbegin(), m.read_buffer.cend(), reg_int)); + auto fdc = read_register(reg_int).value(); + auto measurement_done = + fdc1004_utils::measurement_ready(fdc, measure_mode); + if (measurement_done) { + // Start a single-shot, two-register transaction for the data. + send_followup(m); + } else { + // Double check that the configuration reg seems okay. If not, we + // should reconfigure it before re-requesting a reading. + if (!check_fdc_readback_ok(fdc)) { + update_capacitance_configuration(); + set_sample_rate(); + } + i2c::messages::MaxMessageBuffer buffer = { + static_cast(fdc1004::Registers::FDC_CONF)}; + // Retrigger the same exact reading + writer.transact(fdc1004::ADDRESS, 1, buffer, 2, m.id, own_queue); } - poller.continuous_multi_register_poll( - fdc1004::ADDRESS, converted_msb_addr, 2, converted_lsb_addr, 2, - STOP_DELAY, own_queue, transaction_id); } void handle_ongoing_response(i2c::messages::TransactionResponse &m) { @@ -233,6 +236,7 @@ class FDC1004 { // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index) polling_results[m.id.transaction_index])); if (m.id.transaction_index == 0) { + send_followup(m); return; } @@ -244,19 +248,12 @@ class FDC1004 { auto raw_capacitance = fdc1004_utils::convert_reads(polling_results[0], polling_results[1]); - if (!data_stable[int(sensor_id)]) { - raw_capacitance = filter.compute(raw_capacitance); - data_stable.set(int(sensor_id), filter.stop_filter()); - } - auto capacitance = fdc1004_utils::convert_capacitance( raw_capacitance, 1, current_offset_pf); - if (data_stable[int(sensor_id)]) { - auto new_offset = - fdc1004_utils::update_offset(capacitance, current_offset_pf); - set_offset(new_offset); - } + auto new_offset = + fdc1004_utils::update_offset(capacitance, current_offset_pf); + set_offset(new_offset); if (max_capacitance_sync) { if (capacitance > fdc1004::MAX_CAPACITANCE_READING) { @@ -291,6 +288,7 @@ class FDC1004 { // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index) baseline_results[m.id.transaction_index])); if (m.id.transaction_index == 0) { + send_followup(m); return; } measurement += fdc1004_utils::convert_reads(baseline_results[0], @@ -350,7 +348,6 @@ class FDC1004 { OwnQueue &own_queue; hardware::SensorHardwareBase &hardware; - ot_utils::filters::SimpleMovingAverage filter{}; uint8_t sensor_binding{2}; fdc1004::FDC1004RegisterMap _registers{}; bool _initialized = false; @@ -371,10 +368,49 @@ class FDC1004 { bool echoing = false; bool bind_sync = false; bool max_capacitance_sync = false; - std::bitset<2> data_stable{"00"}; std::array baseline_results{}; std::array polling_results{}; + auto send_followup(i2c::messages::TransactionResponse &m) -> void { + auto address = utils::reg_from_id(m.id.token); + uint8_t followup_address = 0; + auto tags = utils::tags_from_token(m.id.token); + i2c::messages::TransactionIdentifier id = m.id; + switch (address) { + case static_cast(fdc1004::Registers::FDC_CONF): + // After CONF, read MSB for transaction index 0 + followup_address = + (measure_mode == fdc1004::MeasureConfigMode::ONE) + ? static_cast(fdc1004::Registers::MEAS1_MSB) + : static_cast(fdc1004::Registers::MEAS2_MSB); + id.transaction_index = 0; + break; + case static_cast(fdc1004::Registers::MEAS1_MSB): + // After MSB, read LSB for index 1 + followup_address = + static_cast(fdc1004::Registers::MEAS1_LSB); + id.transaction_index = 1; + break; + case static_cast(fdc1004::Registers::MEAS2_MSB): + // After MSB, read LSB for index 1 + followup_address = + static_cast(fdc1004::Registers::MEAS2_LSB); + id.transaction_index = 1; + break; + default: + // If this isn't one of the hardcoded regs, do nothing + return; + } + + i2c::messages::MaxMessageBuffer buffer; + buffer[0] = followup_address; + + // Overwrite the token to get the updated address + id.token = utils::build_id(fdc1004::ADDRESS, followup_address, tags); + // This maintains the entire transaction ID of the original request. + writer.transact(fdc1004::ADDRESS, 1, buffer, 2, id, own_queue); + } + auto build_mode_1_configuration_register() -> fdc1004::ConfMeasure1 { _registers.config_measure_1.CHA = static_cast(fdc1004::CHA::CIN1); @@ -415,6 +451,17 @@ class FDC1004 { return DELAY; } + auto check_fdc_readback_ok(fdc1004::FDCConf fdc) -> bool { + auto &comp = _registers.fdc_conf; + // Check everything except for the readback status + return (comp.measure_mode_4 == fdc.measure_mode_4 && + comp.measure_mode_3 == fdc.measure_mode_3 && + comp.measure_mode_2 == fdc.measure_mode_2 && + comp.measure_mode_1 == fdc.measure_mode_1 && + comp.repeating_measurements == fdc.repeating_measurements && + comp.measurement_rate == fdc.measurement_rate); + } + template requires fdc1004::WritableRegister auto set_register(Reg ®) -> bool { diff --git a/include/sensors/core/tasks/capacitive_sensor_task.hpp b/include/sensors/core/tasks/capacitive_sensor_task.hpp index 68b524064..9eea28cf4 100644 --- a/include/sensors/core/tasks/capacitive_sensor_task.hpp +++ b/include/sensors/core/tasks/capacitive_sensor_task.hpp @@ -44,8 +44,14 @@ class CapacitiveMessageHandler { void visit(i2c::messages::TransactionResponse &m) { auto reg_id = utils::reg_from_id(m.id.token); + if (reg_id == static_cast(fdc1004::Registers::FDC_CONF)) { + driver.handle_fdc_response(m); + return; + } if ((reg_id != static_cast(fdc1004::Registers::MEAS1_MSB)) && - (reg_id != static_cast(fdc1004::Registers::MEAS2_MSB))) { + (reg_id != static_cast(fdc1004::Registers::MEAS2_MSB)) && + (reg_id != static_cast(fdc1004::Registers::MEAS1_LSB)) && + (reg_id != static_cast(fdc1004::Registers::MEAS2_LSB))) { return; } diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 412fdd155..5b3a62d4b 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -147,9 +147,13 @@ class MMR920C04 { } auto poll_continuous_pressure(uint8_t tags) -> void { - auto mode_delay_with_buffer = + // in milliseconds + uint16_t mode_delay_with_buffer = MeasurementTimings[static_cast(measurement_mode_rate)] + DEFAULT_DELAY_BUFFER; + max_pressure_required_readings = + MAX_PRESSURE_TIME_MS / mode_delay_with_buffer; + max_pressure_consecutive_readings = 0; auto command_data = build_register_command(_registers.low_pass_pressure_command); if (filter_setting == mmr920C04::FilterSetting::NO_FILTER) { @@ -275,6 +279,8 @@ class MMR920C04 { stop_continuous_polling(m.id.token, static_cast(reg_id)); } + bool echo_this_time = echoing; + // Pressure is always a three-byte value static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), m.read_buffer.cend(), @@ -287,8 +293,21 @@ class MMR920C04 { _registers.pressure_result.reading); if (max_pressure_sync) { - if (std::fabs(pressure) - std::fabs(current_pressure_baseline_pa) > - mmr920C04::MAX_PRESSURE_READING) { + bool this_tick_over_threshold = + std::fabs(pressure - current_pressure_baseline_pa) >= + mmr920C04::MAX_PRESSURE_READING; + bool over_threshold = false; + if (this_tick_over_threshold) { + max_pressure_consecutive_readings = + std::min(max_pressure_consecutive_readings + 1, + max_pressure_required_readings); + over_threshold = (max_pressure_consecutive_readings == + max_pressure_required_readings); + echo_this_time = true; + } else { + max_pressure_consecutive_readings = 0; + } + if (over_threshold) { hardware.set_sync(); can_client.send_can_message( can::ids::NodeId::host, @@ -309,7 +328,7 @@ class MMR920C04 { } } - if (echoing) { + if (echo_this_time) { can_client.send_can_message( can::ids::NodeId::host, can::messages::ReadFromSensorResponse{ @@ -381,7 +400,7 @@ class MMR920C04 { can::ids::NodeId::host, can::messages::BaselineSensorResponse{ .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure_temperature, + .sensor = can::ids::SensorType::pressure, .offset_average = pressure_fixed_point}); set_threshold(current_pressure_baseline_pa, can::ids::SensorThresholdMode::auto_baseline, @@ -447,6 +466,13 @@ class MMR920C04 { static constexpr float DEFAULT_DELAY_BUFFER = 1.0; // in msec (TODO might need to change to fit in uint16_t) static constexpr uint16_t STOP_DELAY = 0; + + /** + * Time required before raising a Max Pressure error. The pressure must + * exceed the threshold for the entirety of this period. + */ + static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; + mmr920C04::MeasurementRate measurement_mode_rate = mmr920C04::MeasurementRate::MEASURE_4; @@ -462,6 +488,9 @@ class MMR920C04 { float current_pressure_baseline_pa = 0; float current_temperature_baseline = 0; + size_t max_pressure_consecutive_readings = 0; + size_t max_pressure_required_readings = 0; + // TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty // sure this is an arbitrarily large number to enable continuous reads. float threshold_pascals = 100.0F; diff --git a/include/sensors/core/tasks/tip_presence_notification_task.hpp b/include/sensors/core/tasks/tip_presence_notification_task.hpp index 9e049bfab..78429412b 100644 --- a/include/sensors/core/tasks/tip_presence_notification_task.hpp +++ b/include/sensors/core/tasks/tip_presence_notification_task.hpp @@ -5,8 +5,9 @@ template class TipPresenceNotificationHandler { public: explicit TipPresenceNotificationHandler( - CanClient &can_client, sensors::hardware::SensorHardwareBase &hardware) - : can_client{can_client}, hardware{hardware} {} + CanClient &can_client, sensors::hardware::SensorHardwareBase &hardware, + const can::ids::SensorId &id) + : can_client{can_client}, hardware{hardware}, sensor_id{id} {} TipPresenceNotificationHandler(const TipPresenceNotificationHandler &) = delete; TipPresenceNotificationHandler(const TipPresenceNotificationHandler &&) = @@ -29,7 +30,9 @@ class TipPresenceNotificationHandler { can::messages::PushTipPresenceNotification{ .message_index = 0, .ejector_flag_status = - static_cast(hardware.check_tip_presence())}); + static_cast(hardware.check_tip_presence()), + .sensor_id = sensor_id, + }); } void visit(const can::messages::TipStatusQueryRequest &m) { @@ -38,12 +41,14 @@ class TipPresenceNotificationHandler { can::messages::PushTipPresenceNotification{ .message_index = m.message_index, .ejector_flag_status = - static_cast(hardware.check_tip_presence())}); + static_cast(hardware.check_tip_presence()), + .sensor_id = sensor_id}); } private: CanClient &can_client; sensors::hardware::SensorHardwareBase &hardware; + can::ids::SensorId sensor_id; }; /** @@ -56,7 +61,8 @@ class TipPresenceNotificationTask { public: using Messages = tip_presence::TaskMessage; using QueueType = QueueImpl; - TipPresenceNotificationTask(QueueType &queue) : queue{queue} {} + TipPresenceNotificationTask(QueueType &queue, can::ids::SensorId id) + : queue{queue}, sensor_id{id} {} TipPresenceNotificationTask(const TipPresenceNotificationTask &c) = delete; TipPresenceNotificationTask(const TipPresenceNotificationTask &&c) = delete; auto operator=(const TipPresenceNotificationTask &c) = delete; @@ -70,7 +76,8 @@ class TipPresenceNotificationTask { [[noreturn]] void operator()( CanClient *can_client, sensors::hardware::SensorHardwareBase *hardware) { - auto handler = TipPresenceNotificationHandler{*can_client, *hardware}; + auto handler = + TipPresenceNotificationHandler{*can_client, *hardware, sensor_id}; Messages message{}; for (;;) { if (queue.try_read(&message, queue.max_delay)) { @@ -83,6 +90,7 @@ class TipPresenceNotificationTask { private: QueueType &queue; + can::ids::SensorId sensor_id; }; } // namespace tasks diff --git a/include/sensors/core/utils.hpp b/include/sensors/core/utils.hpp index a12123573..547e18080 100644 --- a/include/sensors/core/utils.hpp +++ b/include/sensors/core/utils.hpp @@ -70,6 +70,10 @@ template return bool(token & (1 << static_cast(tag))); } +[[nodiscard]] inline constexpr auto tags_from_token(uint32_t token) -> uint8_t { + return static_cast(token & 0xff); +} + [[nodiscard]] inline constexpr auto build_id(uint16_t address, uint8_t reg, uint8_t tags = 0) -> uint32_t { return (static_cast(address) << 16) | diff --git a/include/sensors/simulation/fdc1004.hpp b/include/sensors/simulation/fdc1004.hpp index d1eda7116..5e81215c9 100644 --- a/include/sensors/simulation/fdc1004.hpp +++ b/include/sensors/simulation/fdc1004.hpp @@ -13,9 +13,12 @@ class FDC1004 : public I2CRegisterMap { : I2CRegisterMap( fdc1004::ADDRESS, {{static_cast(fdc1004::Registers::CONF_MEAS1), 0}, - {static_cast(fdc1004::Registers::FDC_CONF), 0}, + // Mock out the Read Ready bits as set + {static_cast(fdc1004::Registers::FDC_CONF), 0x000F}, {static_cast(fdc1004::Registers::MEAS1_MSB), 100}, {static_cast(fdc1004::Registers::MEAS1_LSB), 0}, + {static_cast(fdc1004::Registers::MEAS2_MSB), 100}, + {static_cast(fdc1004::Registers::MEAS2_LSB), 0}, {static_cast(fdc1004::Registers::DEVICE_ID), fdc1004_utils::DEVICE_ID}}) {} }; diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 69206e224..7b2cbbf3a 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -18,6 +18,30 @@ auto gear_config = lms::LinearMotionSystemConfig{ auto error_config = error_tolerance_config::BrushedMotorErrorTolerance{gear_config}; +auto constexpr home_msg = + BrushedMove{.duration = 5 * 32000, + .duty_cycle = 50, + .group_id = 0, + .seq_id = 0, + .stay_engaged = 0, + .stop_condition = MoveStopCondition::limit_switch}; + +auto constexpr grip_msg = + BrushedMove{.duration = 5 * 32000, + .duty_cycle = 50, + .group_id = 0, + .seq_id = 0, + .stay_engaged = 1, + .stop_condition = MoveStopCondition::none}; + +auto constexpr move_msg = + BrushedMove{.duration = 5 * 32000, + .duty_cycle = 0, + .group_id = 0, + .seq_id = 0, + .encoder_position = 61054, // ~1cm + .stop_condition = MoveStopCondition::encoder_position}; + struct BrushedMotorContainer { test_mocks::MockBrushedMotorHardware hw{}; test_mocks::MockMessageQueue queue{}; @@ -32,13 +56,7 @@ struct BrushedMotorContainer { SCENARIO("Brushed motor interrupt handler handle move messages") { BrushedMotorContainer test_objs{}; GIVEN("A message to home") { - auto msg = - BrushedMove{.duration = 5 * 32000, - .duty_cycle = 50, - .group_id = 0, - .seq_id = 0, - .stop_condition = MoveStopCondition::limit_switch}; - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(home_msg); WHEN("A brushed move message is received and loaded") { // Burn through the startup ticks @@ -49,6 +67,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { THEN("The motor hardware proceeds to home") { /* motor shouldn't be gripping */ REQUIRE(!test_objs.hw.get_is_gripping()); + REQUIRE(!test_objs.hw.get_stay_enabled()); test_objs.hw.set_encoder_value(1000); test_objs.hw.set_limit_switch(true); @@ -67,6 +86,9 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); REQUIRE(test_objs.handler.is_idle); + REQUIRE(!test_objs.hw.get_stay_enabled()); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING_HOME); } } } @@ -79,16 +101,14 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { REQUIRE(test_objs.reporter.messages.size() == 1); Ack read_ack = std::get(test_objs.reporter.messages.back()); REQUIRE(read_ack.ack_id == AckMessageId::timeout); + REQUIRE(!test_objs.hw.get_stay_enabled()); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING_HOME); } } GIVEN("A message to grip") { - auto msg = BrushedMove{.duration = 5 * 32000, - .duty_cycle = 50, - .group_id = 0, - .seq_id = 0, - .stop_condition = MoveStopCondition::none}; - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(grip_msg); WHEN("A brushed move message is received and loaded") { // Burn through the startup ticks @@ -99,6 +119,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { THEN("The motor hardware proceeds to grip") { /* motor should be gripping */ REQUIRE(test_objs.hw.get_is_gripping()); + REQUIRE(test_objs.hw.get_stay_enabled()); test_objs.hw.set_encoder_value(30000); AND_WHEN("The encoder speed timer overflows") { @@ -125,6 +146,9 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { REQUIRE(read_ack.encoder_position == 30000); REQUIRE(read_ack.ack_id == AckMessageId::complete_without_condition); + REQUIRE(test_objs.hw.get_stay_enabled()); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING); } } } @@ -140,20 +164,16 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { REQUIRE(test_objs.reporter.messages.size() == 1); Ack read_ack = std::get(test_objs.reporter.messages.back()); REQUIRE(read_ack.ack_id == AckMessageId::timeout); + REQUIRE(test_objs.hw.get_stay_enabled()); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING); } } GIVEN("A message to move") { - auto msg = - BrushedMove{.duration = 5 * 32000, - .duty_cycle = 0, - .group_id = 0, - .seq_id = 0, - .encoder_position = 61054, // ~1cm - .stop_condition = MoveStopCondition::encoder_position}; int32_t last_pid_output = test_objs.hw.get_pid_controller_output(); REQUIRE(last_pid_output == 0.0); test_objs.hw.set_encoder_value(0); - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(move_msg); WHEN("A brushed move message is received and loaded") { // Burn through the startup ticks for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) { @@ -199,10 +219,13 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { Ack read_ack = std::get(test_objs.reporter.messages.back()); // check if position is withen acceptable parameters - REQUIRE( - std::abs(read_ack.encoder_position - msg.encoder_position) < - int32_t(gear_config.get_encoder_pulses_per_mm() * 2)); + REQUIRE(std::abs(read_ack.encoder_position - + move_msg.encoder_position) < + int32_t(gear_config.get_encoder_pulses_per_mm() * 2)); REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::POSITION_CONTROLLING); + REQUIRE(!test_objs.hw.get_stay_enabled()); } } WHEN("move message times out") { @@ -213,6 +236,9 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { REQUIRE(test_objs.reporter.messages.size() == 1); Ack read_ack = std::get(test_objs.reporter.messages.back()); REQUIRE(read_ack.ack_id == AckMessageId::timeout); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::POSITION_CONTROLLING); + REQUIRE(!test_objs.hw.get_stay_enabled()); } } } @@ -220,19 +246,15 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { SCENARIO("estop pressed during Brushed motor interrupt handler") { BrushedMotorContainer test_objs{}; - GIVEN("A message to home") { - auto msg = - BrushedMove{.duration = 5 * 32000, - .duty_cycle = 50, - .group_id = 0, - .seq_id = 0, - .stop_condition = MoveStopCondition::limit_switch}; - test_objs.queue.try_write_isr(msg); + GIVEN("A message to grip") { + test_objs.queue.try_write_isr(grip_msg); WHEN("Estop is pressed") { // Burn through the startup ticks for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) { test_objs.handler.run_interrupt(); } + REQUIRE(!test_objs.handler.in_estop); + REQUIRE(test_objs.hw.get_stay_enabled()); test_objs.hw.set_estop_in(true); test_objs.handler.run_interrupt(); THEN("Errors are sent") { @@ -242,6 +264,14 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") { std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::estop_detected); + THEN( + "motor should stay engaged and motor state should " + "persist") { + REQUIRE(test_objs.handler.in_estop); + REQUIRE(test_objs.hw.get_stay_enabled()); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING); + } } } } @@ -251,18 +281,14 @@ SCENARIO("labware dropped during grip move") { BrushedMotorContainer test_objs{}; GIVEN("A message to grip") { - auto msg = BrushedMove{.duration = 5 * 32000, - .duty_cycle = 50, - .group_id = 0, - .seq_id = 0, - .stop_condition = MoveStopCondition::none}; - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(grip_msg); WHEN("grip is complete") { test_objs.hw.set_encoder_value(1200); // Burn through the startup ticks for (uint32_t i = 0; i <= 100; i++) { test_objs.handler.run_interrupt(); } + REQUIRE(test_objs.hw.get_stay_enabled()); test_objs.handler.set_enc_idle_state(true); test_objs.handler.run_interrupt(); THEN("Move complete message is sent") { @@ -272,6 +298,7 @@ SCENARIO("labware dropped during grip move") { REQUIRE(read_ack.ack_id == AckMessageId::complete_without_condition); + REQUIRE(!test_objs.handler.error_handled); } test_objs.reporter.messages.clear(); THEN("Movement starts again") { @@ -282,13 +309,18 @@ SCENARIO("labware dropped during grip move") { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::labware_dropped); - - REQUIRE(test_objs.hw.get_stay_enabled() == true); + REQUIRE(test_objs.handler.error_handled); + REQUIRE(test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 40000); } } } @@ -298,13 +330,7 @@ SCENARIO("collision while homed") { BrushedMotorContainer test_objs{}; GIVEN("A message to home") { - auto msg = - BrushedMove{.duration = 5 * 32000, - .duty_cycle = 50, - .group_id = 0, - .seq_id = 0, - .stop_condition = MoveStopCondition::limit_switch}; - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(home_msg); WHEN("home is complete") { test_objs.hw.set_encoder_value(1200); // Burn through the startup ticks @@ -322,6 +348,7 @@ SCENARIO("collision while homed") { REQUIRE(read_ack.encoder_position == 0); REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); REQUIRE(test_objs.handler.is_idle); + REQUIRE(!test_objs.handler.error_handled); } test_objs.reporter.messages.clear(); THEN("Movement starts again") { @@ -332,14 +359,19 @@ SCENARIO("collision while homed") { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::collision_detected); - - REQUIRE(test_objs.hw.get_stay_enabled() == false); + REQUIRE(test_objs.handler.error_handled); + REQUIRE(!test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 40000); } } } @@ -347,17 +379,10 @@ SCENARIO("collision while homed") { SCENARIO("A collision during position controlled move") { BrushedMotorContainer test_objs{}; - auto msg = - BrushedMove{.duration = 5 * 32000, - .duty_cycle = 0, - .group_id = 0, - .seq_id = 0, - .encoder_position = 61054, // ~1cm - .stop_condition = MoveStopCondition::encoder_position}; int32_t last_pid_output = test_objs.hw.get_pid_controller_output(); REQUIRE(last_pid_output == 0.0); test_objs.hw.set_encoder_value(0); - test_objs.queue.try_write_isr(msg); + test_objs.queue.try_write_isr(move_msg); WHEN("A brushed move message is received and loaded") { // Burn through the startup ticks for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) { @@ -402,29 +427,126 @@ SCENARIO("A collision during position controlled move") { Ack read_ack = std::get(test_objs.reporter.messages.back()); // check if position is withen acceptable parameters - REQUIRE( - std::abs(read_ack.encoder_position - msg.encoder_position) < - int32_t(gear_config.get_encoder_pulses_per_mm() * 2)); + REQUIRE(std::abs(read_ack.encoder_position - + move_msg.encoder_position) < + int32_t(gear_config.get_encoder_pulses_per_mm() * 2)); REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); + REQUIRE(!test_objs.handler.error_handled); } test_objs.reporter.messages.clear(); THEN("Movement starts again") { + REQUIRE(!test_objs.handler.has_active_move()); test_objs.hw.set_encoder_value(200000); test_objs.handler.set_enc_idle_state(false); - // Burn through the holdoff ticks - for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) { + // Burn through the 2 x holdoff ticks + for (uint32_t i = 0; i <= HOLDOFF_TICKS * 2; i++) { test_objs.handler.run_interrupt(); } // An error and increase error count is sent - REQUIRE(test_objs.reporter.messages.size() == 2); + REQUIRE(test_objs.reporter.messages.size() == 3); can::messages::ErrorMessage err = std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::collision_detected); + REQUIRE(test_objs.handler.error_handled); + REQUIRE(!test_objs.hw.get_stay_enabled()); + // position reported + motor_messages::UpdatePositionResponse pos_response = + std::get( + test_objs.reporter.messages.back()); + REQUIRE(pos_response.encoder_pulses == 200000); + } + } + } +} + +SCENARIO("handler encounter error during idle move") { + BrushedMotorContainer test_objs{}; + auto og_motor_state = GENERATE(BrushedMotorState::FORCE_CONTROLLING_HOME, + BrushedMotorState::FORCE_CONTROLLING, + BrushedMotorState::POSITION_CONTROLLING); + + GIVEN("the encoder value begins to error during an idle move") { + test_objs.hw.set_motor_state(og_motor_state); + test_objs.hw.set_encoder_value(200000); + test_objs.handler.set_enc_idle_state(false); - REQUIRE(test_objs.hw.get_stay_enabled() == false); + WHEN("the first idle move is executed and an error is detected") { + test_objs.handler.execute_idle_move(); + THEN("error is noted but not handled") { + REQUIRE(!test_objs.handler.error_handled); + } + WHEN( + "idle move is executed the second time and the error is still " + "detected") { + test_objs.handler.execute_idle_move(); + THEN("error is handled") { + REQUIRE(test_objs.handler.error_handled); + } + } + WHEN( + "idle move is executed the second time and but the error is no " + "longer detected") { + test_objs.handler.set_enc_idle_state(true); + test_objs.hw.set_encoder_value(0); + test_objs.handler.execute_idle_move(); + THEN("error is not handled") { + REQUIRE(!test_objs.handler.error_handled); + } } } } } + +SCENARIO("handler recovers from error state") { + BrushedMotorContainer test_objs{}; + auto og_motor_state = GENERATE(BrushedMotorState::FORCE_CONTROLLING_HOME, + BrushedMotorState::FORCE_CONTROLLING, + BrushedMotorState::POSITION_CONTROLLING); + auto stay_engaged = GENERATE(true, false); + + GIVEN("an error occurred during an idle move") { + test_objs.hw.set_motor_state(og_motor_state); + test_objs.hw.set_stay_enabled(stay_engaged); + test_objs.handler.error_handled = true; + + WHEN("a cancel request is received") { + test_objs.hw.request_cancel(); + test_objs.handler.run_interrupt(); + THEN( + "motor state should become un-homed only if stay engaged is " + "falsy") { + REQUIRE(test_objs.hw.get_motor_state() == + (!stay_engaged ? BrushedMotorState::UNHOMED + : og_motor_state)); + } + THEN("a stop requested warning is issued") { + // TODO: do we need to increase the error count if it's only + // warning about a stop request? + REQUIRE(test_objs.reporter.messages.size() == 2); + can::messages::ErrorMessage err = + std::get( + test_objs.reporter.messages.front()); + REQUIRE(err.error_code == can::ids::ErrorCode::stop_requested); + REQUIRE(err.severity == can::ids::ErrorSeverity::warning); + } + THEN("error handled is not cleared, yet") { + REQUIRE(test_objs.handler.error_handled); + } + AND_WHEN("a new message is loaded and executed") { + test_objs.queue.try_write_isr(grip_msg); + test_objs.handler.update_and_start_move(); + THEN( + "error state is cleared and the new move is being " + "executed") { + REQUIRE(!test_objs.handler.error_handled); + REQUIRE(test_objs.hw.get_motor_state() == + BrushedMotorState::FORCE_CONTROLLING); + REQUIRE(test_objs.handler.has_active_move()); + REQUIRE(test_objs.hw.get_stay_enabled()); + } + } + } + } +} \ No newline at end of file diff --git a/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp index 717b2229e..4800a25d1 100644 --- a/motor-control/tests/test_limit_switch.cpp +++ b/motor-control/tests/test_limit_switch.cpp @@ -58,9 +58,11 @@ SCENARIO("MoveStopCondition::limit_switch with the limit switch triggered") { 0x7FFFFFFFFFFFFFFF); } - THEN("stepper position flag is cleared") { + THEN("stepper and encoder position flags are cleared") { REQUIRE(!test_objs.hw.position_flags.check_flag( MotorPositionStatus::Flags::stepper_position_ok)); + REQUIRE(!test_objs.hw.position_flags.check_flag( + MotorPositionStatus::Flags::encoder_position_ok)); } AND_WHEN("the limit switch has been triggered") { @@ -73,6 +75,7 @@ SCENARIO("MoveStopCondition::limit_switch with the limit switch triggered") { } test_objs.handler.run_interrupt(); } + THEN( "the move should be stopped with ack id = stopped " "by " @@ -84,10 +87,13 @@ SCENARIO("MoveStopCondition::limit_switch with the limit switch triggered") { REQUIRE(read_ack.encoder_position == 50); REQUIRE(read_ack.current_position_steps == 0); } - - THEN("the stepper position flag is still cleared") { + THEN( + "the stepper position flag and encoder position flags are " + "still cleared") { REQUIRE(!test_objs.hw.position_flags.check_flag( MotorPositionStatus::Flags::stepper_position_ok)); + REQUIRE(!test_objs.hw.position_flags.check_flag( + MotorPositionStatus::Flags::encoder_position_ok)); } } } @@ -117,9 +123,11 @@ SCENARIO("MoveStopCondition::limit_switch and limit switch is not triggered") { REQUIRE(test_objs.handler.get_current_position() == 0x7FFFFFFFFFFFFFFF); } - THEN("stepper position flag is cleared") { + THEN("stepper and encoder position flags are cleared") { REQUIRE(!test_objs.hw.position_flags.check_flag( MotorPositionStatus::Flags::stepper_position_ok)); + REQUIRE(!test_objs.hw.position_flags.check_flag( + MotorPositionStatus::Flags::encoder_position_ok)); } AND_WHEN("the limit switch has not been triggered") { @@ -147,6 +155,12 @@ SCENARIO("MoveStopCondition::limit_switch and limit switch is not triggered") { THEN("position should not be reset") { REQUIRE(!test_objs.handler.get_current_position() == 0); } + THEN("stepper and encoder position flags should remained cleared") { + REQUIRE(!test_objs.hw.position_flags.check_flag( + MotorPositionStatus::Flags::stepper_position_ok)); + REQUIRE(!test_objs.hw.position_flags.check_flag( + MotorPositionStatus::Flags::encoder_position_ok)); + } } } } diff --git a/pipettes/core/sensor_tasks.cpp b/pipettes/core/sensor_tasks.cpp index 484117a0e..a32206b97 100644 --- a/pipettes/core/sensor_tasks.cpp +++ b/pipettes/core/sensor_tasks.cpp @@ -30,9 +30,13 @@ static auto pressure_sensor_task_builder_front = freertos_task::TaskStarter<512, sensors::tasks::PressureSensorTask, can::ids::SensorId>(can::ids::SensorId::S1); +static auto tip_notification_task_builder_rear = + freertos_task::TaskStarter<256, sensors::tasks::TipPresenceNotificationTask, + can::ids::SensorId>(can::ids::SensorId::S0); + static auto tip_notification_task_builder_front = - freertos_task::TaskStarter<256, - sensors::tasks::TipPresenceNotificationTask>{}; + freertos_task::TaskStarter<256, sensors::tasks::TipPresenceNotificationTask, + can::ids::SensorId>(can::ids::SensorId::S1); void sensor_tasks::start_tasks( sensor_tasks::CanWriterTask& can_writer, @@ -70,14 +74,14 @@ void sensor_tasks::start_tasks( capacitive_sensor_task_builder_rear.start( 5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client, sensor_hardware_primary, queues); - auto& tip_notification_task = tip_notification_task_builder_front.start( - 5, "tip notification", queues, sensor_hardware_primary); + auto& tip_notification_task_rear = tip_notification_task_builder_rear.start( + 5, "tip notification sensor s0", queues, sensor_hardware_primary); tasks.eeprom_task = &eeprom_task; tasks.environment_sensor_task = &environment_sensor_task; tasks.capacitive_sensor_task_rear = &capacitive_sensor_task_rear; tasks.pressure_sensor_task_rear = &pressure_sensor_task_rear; - tasks.tip_notification_task = &tip_notification_task; + tasks.tip_notification_task_rear = &tip_notification_task_rear; queues.set_queue(&can_writer.get_queue()); queues.eeprom_queue = &eeprom_task.get_queue(); @@ -85,7 +89,8 @@ void sensor_tasks::start_tasks( queues.capacitive_sensor_queue_rear = &capacitive_sensor_task_rear.get_queue(); queues.pressure_sensor_queue_rear = &pressure_sensor_task_rear.get_queue(); - queues.tip_notification_queue = &tip_notification_task.get_queue(); + queues.tip_notification_queue_rear = + &tip_notification_task_rear.get_queue(); } void sensor_tasks::start_tasks( @@ -119,6 +124,8 @@ void sensor_tasks::start_tasks( ? i2c3_task_client : i2c2_task_client; auto shared_cap_task = PIPETTE_TYPE == EIGHT_CHANNEL ? true : false; + auto front_tip_presence_sensor = + PIPETTE_TYPE == NINETY_SIX_CHANNEL ? true : false; auto& eeprom_task = eeprom_task_builder.start( 5, "eeprom", eeprom_i2c_client, eeprom_hardware); @@ -134,14 +141,14 @@ void sensor_tasks::start_tasks( capacitive_sensor_task_builder_rear.start( 5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client, sensor_hardware_primary, queues, shared_cap_task); - auto& tip_notification_task = tip_notification_task_builder_front.start( - 5, "tip notification", queues, sensor_hardware_primary); + auto& tip_notification_task_rear = tip_notification_task_builder_rear.start( + 5, "tip notification sensor s0", queues, sensor_hardware_primary); tasks.eeprom_task = &eeprom_task; tasks.environment_sensor_task = &environment_sensor_task; tasks.pressure_sensor_task_rear = &pressure_sensor_task_rear; tasks.pressure_sensor_task_front = &pressure_sensor_task_front; - tasks.tip_notification_task = &tip_notification_task; + tasks.tip_notification_task_rear = &tip_notification_task_rear; tasks.capacitive_sensor_task_rear = &capacitive_sensor_task_rear; queues.set_queue(&can_writer.get_queue()); @@ -152,7 +159,8 @@ void sensor_tasks::start_tasks( queues.pressure_sensor_queue_rear = &pressure_sensor_task_rear.get_queue(); queues.pressure_sensor_queue_front = &pressure_sensor_task_front.get_queue(); - queues.tip_notification_queue = &tip_notification_task.get_queue(); + queues.tip_notification_queue_rear = + &tip_notification_task_rear.get_queue(); if (shared_cap_task) { // There is only one cap sensor on the eight channel and so the "front" @@ -170,6 +178,17 @@ void sensor_tasks::start_tasks( queues.capacitive_sensor_queue_front = &capacitive_sensor_task_front.get_queue(); } + if (front_tip_presence_sensor) { + // the eight channel only has one tip presence sensor, so the front + // task should only be started if we have a 96 channel pipette + auto& tip_notification_task_front = + tip_notification_task_builder_front.start( + 5, "tip notification sensor s1", queues, + sensor_hardware_secondary); + tasks.tip_notification_task_front = &tip_notification_task_front; + queues.tip_notification_queue_front = + &tip_notification_task_front.get_queue(); + } } sensor_tasks::QueueClient::QueueClient() @@ -216,9 +235,18 @@ void sensor_tasks::QueueClient::send_pressure_sensor_queue_front( } } -void sensor_tasks::QueueClient::send_tip_notification_queue( +void sensor_tasks::QueueClient::send_tip_notification_queue_rear( const sensors::tip_presence::TaskMessage& m) { - tip_notification_queue->try_write(m); + if (tip_notification_queue_rear != nullptr) { + tip_notification_queue_rear->try_write(m); + } +} + +void sensor_tasks::QueueClient::send_tip_notification_queue_front( + const sensors::tip_presence::TaskMessage& m) { + if (tip_notification_queue_front != nullptr) { + tip_notification_queue_front->try_write(m); + } } auto sensor_tasks::get_tasks() -> Tasks& { return tasks; } diff --git a/pipettes/firmware/CMakeLists.txt b/pipettes/firmware/CMakeLists.txt index 07d8991c3..dc4b5b0e2 100644 --- a/pipettes/firmware/CMakeLists.txt +++ b/pipettes/firmware/CMakeLists.txt @@ -97,23 +97,23 @@ set(pipette_srcs ${PIPETTE_FW_LINTABLE_SRCS} foreach_revision( PROJECT_NAME pipettes-single - REVISIONS b1 c2 - SOURCES pipette_srcs pipette_srcs + REVISIONS b1 c2 d1 + SOURCES pipette_srcs pipette_srcs pipette_srcs CALL_FOREACH_REV pipettes_single_loop ) foreach_revision( PROJECT_NAME pipettes-multi - REVISIONS b1 c2 + REVISIONS b1 c2 d1 CALL_FOREACH_REV pipettes_multi_loop - SOURCES pipette_srcs pipette_srcs + SOURCES pipette_srcs pipette_srcs pipette_srcs ) foreach_revision( PROJECT_NAME pipettes-96 - REVISIONS b1 c1 + REVISIONS b1 c1 d2 CALL_FOREACH_REV pipettes_96_loop - SOURCES pipette_srcs pipette_srcs + SOURCES pipette_srcs pipette_srcs pipette_srcs ) alias_for_revision(PROJECT_NAME pipettes-single REVISION b1 REVISION_ALIAS rev1) diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp index 67e0303f5..7baaeb2e4 100644 --- a/pipettes/firmware/main.cpp +++ b/pipettes/firmware/main.cpp @@ -130,9 +130,6 @@ static auto pins_for_sensor = static auto sensor_hardware_container = utility_configs::get_sensor_hardware_container(pins_for_sensor); -static auto ok_for_secondary = - pins_for_sensor.secondary.has_value() && - pins_for_sensor.secondary.value().tip_sense.has_value(); static auto tip_sense_gpio_primary = pins_for_sensor.primary.tip_sense.value(); static auto& sensor_queue_client = sensor_tasks::get_queues(); @@ -142,15 +139,11 @@ static auto tail_accessor = extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (GPIO_Pin == tip_sense_gpio_primary.pin) { - static_cast( - sensor_queue_client.tip_notification_queue->try_write_isr( - sensors::tip_presence::TipStatusChangeDetected{})); - } else if (ok_for_secondary && - GPIO_Pin == - pins_for_sensor.secondary.value().tip_sense.value().pin) { - static_cast( - sensor_queue_client.tip_notification_queue->try_write_isr( - sensors::tip_presence::TipStatusChangeDetected{})); + if (sensor_queue_client.tip_notification_queue_rear != nullptr) { + static_cast( + sensor_queue_client.tip_notification_queue_rear->try_write_isr( + sensors::tip_presence::TipStatusChangeDetected{})); + } } } diff --git a/pipettes/firmware/utility_configurations.cpp b/pipettes/firmware/utility_configurations.cpp index 2bf5c7aba..8edb42cda 100644 --- a/pipettes/firmware/utility_configurations.cpp +++ b/pipettes/firmware/utility_configurations.cpp @@ -121,8 +121,8 @@ auto utility_configs::sensor_configurations() gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_12, - .active_setting = GPIO_PIN_RESET}}, + .pin = GPIO_PIN_7, + .active_setting = GPIO_PIN_SET}}, .secondary = sensors::hardware::SensorHardwareConfiguration{ .sync_out = {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -138,8 +138,8 @@ auto utility_configs::sensor_configurations() .tip_sense = gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_7, - .active_setting = GPIO_PIN_RESET}}}; + .pin = GPIO_PIN_12, + .active_setting = GPIO_PIN_SET}}}; return pins; } @@ -166,7 +166,7 @@ auto utility_configs::sensor_configurations< // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, .pin = GPIO_PIN_12, - .active_setting = GPIO_PIN_RESET}}, + .active_setting = GPIO_PIN_SET}}, .secondary = sensors::hardware::SensorHardwareConfiguration{ .sync_out = {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -183,6 +183,6 @@ auto utility_configs::sensor_configurations< // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, .pin = GPIO_PIN_7, - .active_setting = GPIO_PIN_RESET}}}; + .active_setting = GPIO_PIN_SET}}}; return pins; } diff --git a/pipettes/firmware/utility_gpio.c b/pipettes/firmware/utility_gpio.c index aea32ed6d..e5c76a8d6 100644 --- a/pipettes/firmware/utility_gpio.c +++ b/pipettes/firmware/utility_gpio.c @@ -12,6 +12,8 @@ static void enable_gpio_port(void* port) { __HAL_RCC_GPIOB_CLK_ENABLE(); } else if (port == GPIOC) { __HAL_RCC_GPIOC_CLK_ENABLE(); + } else if (port == GPIOD) { + __HAL_RCC_GPIOD_CLK_ENABLE(); } } @@ -24,7 +26,6 @@ static void tip_sense_gpio_init() { PipetteType pipette_type = get_pipette_type(); GPIO_InitTypeDef GPIO_InitStruct = {0}; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; GPIO_InitStruct.Pull = GPIO_NOPULL; enable_gpio_port(GPIOC); if (pipette_type == NINETY_SIX_CHANNEL) { @@ -32,10 +33,12 @@ static void tip_sense_gpio_init() { * PC12, front tip sense * PC7, rear tip sense * */ + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_7; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } else { /*Configure GPIO pin : C2 */ + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; GPIO_InitStruct.Pin = GPIO_PIN_2; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } @@ -51,24 +54,12 @@ static void tip_sense_gpio_init() { static void nvic_priority_enable_init() { PipetteType pipette_type = get_pipette_type(); - if (pipette_type == NINETY_SIX_CHANNEL) { - IRQn_Type block_9_5 = get_interrupt_line(gpio_block_9_5); - IRQn_Type block_15_10 = get_interrupt_line(gpio_block_15_10); - /* EXTI interrupt init tip sense rear*/ - HAL_NVIC_SetPriority(block_9_5, 10, 0); - HAL_NVIC_EnableIRQ(block_9_5); - - /* EXTI interrupt init tip sense front*/ - HAL_NVIC_SetPriority(block_15_10, 10, 0); - HAL_NVIC_EnableIRQ(block_15_10); - } else { + if (pipette_type != NINETY_SIX_CHANNEL) { IRQn_Type block_2 = get_interrupt_line(gpio_block_2); /* EXTI interrupt init block tip sense*/ HAL_NVIC_SetPriority(block_2, 10, 0); HAL_NVIC_EnableIRQ(block_2); - } - } /** @@ -156,7 +147,7 @@ static void sync_drive_gpio_init() { PipetteHardwarePin sync_in_hardware = pipette_hardware_get_gpio(pipette_type, pipette_hardware_device_sync_in); /* GPIO Ports Clock Enable */ - enable_gpio_port(GPIOB); + enable_gpio_port(sync_in_hardware.port); /*Configure GPIO pin*/ GPIO_InitTypeDef sync_in_init = {0}; @@ -168,7 +159,7 @@ static void sync_drive_gpio_init() { PipetteHardwarePin sync_out_hardware = pipette_hardware_get_gpio(pipette_type, pipette_hardware_device_sync_out); - enable_gpio_port(GPIOC); + enable_gpio_port(sync_out_hardware.port); GPIO_InitTypeDef sync_out_init = {0}; sync_out_init.Pin = sync_out_hardware.pin; diff --git a/push b/push index d911fb2c2..5a35197ca 100755 --- a/push +++ b/push @@ -122,6 +122,9 @@ def push(host, repo_path=None, build=True, restart=True): return -1 def _push_from_argparse(args): + if args.key: + _SSH_EXTRA_OPTS.append('-i') + _SSH_EXTRA_OPTS.append(args.key) return push(args.host, os.path.abspath(args.repo_path), not args.no_build, not args.no_restart) def _arg_parser(parent=None): @@ -148,6 +151,11 @@ def _arg_parser(parent=None): action='store_true', help='Skip restarting robot server' ) + parser.add_argument( + '--key', + type=str, + help='Private SSH key to use' + ) return parser def _main(): diff --git a/rear-panel/firmware/CMakeLists.txt b/rear-panel/firmware/CMakeLists.txt index 22331ea71..c195a5354 100644 --- a/rear-panel/firmware/CMakeLists.txt +++ b/rear-panel/firmware/CMakeLists.txt @@ -250,8 +250,8 @@ macro(rearpanel_loop) endmacro() foreach_revision(PROJECT_NAME rear-panel - REVISIONS b1 b2 - SOURCES rearpanel_b1_srcs rearpanel_b1_srcs + REVISIONS b1 b2 c1 + SOURCES rearpanel_b1_srcs rearpanel_b1_srcs rearpanel_b1_srcs CALL_FOREACH_REV rearpanel_loop NO_CREATE_IMAGE_HEX NO_CREATE_APPLICATION_HEX diff --git a/rear-panel/firmware/led_hardware.c b/rear-panel/firmware/led_hardware.c index 8a8e522b5..e771daea8 100644 --- a/rear-panel/firmware/led_hardware.c +++ b/rear-panel/firmware/led_hardware.c @@ -1,22 +1,27 @@ -/**PB11 DECK_LED DRIVE - AF1 = TIM2_CH4 -//PC6 BLUE DRIVE - AF2 = TIM3_CH1 - AF4 = TIM8_CH1 +/**PB11 DECK_LED DRIVE + AF1 = TIM2_CH4 (REVB) + AF6 = TIM1_CH3 (REVC) +//PC6 BLUE DRIVE + AF2 = TIM3_CH1 + AF4 = TIM8_CH1 //PC7 WHITE DRIVE - AF2 = TIM3_CH2 - AF4 = TIM8_CH2 + AF2 = TIM3_CH2 + AF4 = TIM8_CH2 //PB14 GREEN Drive - AF1 = TIM15_CH1 + AF1 = TIM15_CH1 (REVB) + AF1 = TIM2_CH4 (REVC) //PB15 RED Drive - AF1 = TIM15_CH2 + AF1 = TIM15_CH2 **/ #include "rear-panel/firmware/led_hardware.h" #include "common/firmware/errors.h" #include "platform_specific_hal_conf.h" #include "system_stm32g4xx.h" - +#if !(PCBA_PRIMARY_REVISION == 'b') +TIM_HandleTypeDef htim1; +TIM_OC_InitTypeDef htim1_sConfigOC = {0}; +#endif TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim15; @@ -53,6 +58,13 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_pwm) /* Peripheral clock enable */ __HAL_RCC_TIM15_CLK_ENABLE(); } +#if !(PCBA_PRIMARY_REVISION == 'b') + else if(htim_pwm->Instance==TIM1) + { + /* Peripheral clock enable */ + __HAL_RCC_TIM1_CLK_ENABLE(); + } +#endif } @@ -81,7 +93,7 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) { GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - + GPIO_InitStruct.Pin = GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; @@ -91,16 +103,17 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) { } else if (htim->Instance == TIM15) { __HAL_RCC_GPIOB_CLK_ENABLE(); /**TIM3 GPIO Configuration - PB14 ------> TIM15_CH1 + PB14 ------> TIM15_CH1 (Not on Rev C) PB15 ------> TIM15_CH2 */ + #if PCBA_PRIMARY_REVISION == 'b' GPIO_InitStruct.Pin = GPIO_PIN_14; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF1_TIM15; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - + #endif GPIO_InitStruct.Pin = GPIO_PIN_15; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; @@ -108,11 +121,97 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) { GPIO_InitStruct.Alternate = GPIO_AF1_TIM15; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } + #if !(PCBA_PRIMARY_REVISION == 'b') + else if (htim->Instance == TIM1) { + __HAL_RCC_GPIOA_CLK_ENABLE(); + /**TIM3 GPIO Configuration + PA10 ------> TIM1_CH3 + */ + GPIO_InitStruct.Pin = GPIO_PIN_10; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF6_TIM1; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + } + #endif +} + +#if !(PCBA_PRIMARY_REVISION == 'b') +/** + * @brief TIM1 Initialization Function for DECK LED (RevC) + * @param None + * @retval None + */ +static void MX_TIM1_Init(void) { + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + + htim1.State = HAL_TIM_STATE_RESET; + htim1.Instance = TIM1; + /* + * Setting counter clock frequency to 2 kHz + */ + htim1.Init.Prescaler = + calc_prescaler(SystemCoreClock, LED_TIMER_FREQ); + htim1.Init.CounterMode = TIM_COUNTERMODE_UP; + htim1.Init.Period = LED_PWM_WIDTH - 1; + htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim1.Init.RepetitionCounter = 0; + htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim1) != HAL_OK) { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK) { + Error_Handler(); + } + if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != + HAL_OK) { + Error_Handler(); + } + htim1_sConfigOC.OCMode = TIM_OCMODE_PWM1; + /* Set duty cycle at 0% */ + htim1_sConfigOC.Pulse = 0; + htim1_sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + htim1_sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + htim1_sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + htim1_sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + htim1_sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (HAL_TIM_PWM_ConfigChannel(&htim1, &htim1_sConfigOC, TIM_CHANNEL_3) != + HAL_OK) { + Error_Handler(); + } + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 0; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.BreakFilter = 0; + sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT; + sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; + sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; + sBreakDeadTimeConfig.Break2Filter = 0; + sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != + HAL_OK) { + Error_Handler(); + } + HAL_TIM_MspPostInit(&htim1); } - +#endif /** - * @brief TIM2 Initialization Function for DECK LED + * @brief TIM2 Initialization Function for DECK LED (RevB) or Green LED (RevC) * @param None * @retval None */ @@ -306,10 +405,12 @@ static void MX_TIM15_Init(void) { htim15_sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; htim15_sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; htim15_sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + #if PCBA_PRIMARY_REVISION == 'b' if (HAL_TIM_PWM_ConfigChannel(&htim15, &htim15_sConfigOC, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } + #endif if (HAL_TIM_PWM_ConfigChannel(&htim15, &htim15_sConfigOC, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); @@ -336,29 +437,40 @@ static void MX_TIM15_Init(void) { void led_hw_update_pwm(uint32_t duty_cycle, LED_DEVICE led) { - - switch(led) { - case DECK_LED: - htim2.Instance->CCR4 = duty_cycle; - break; - case BLUE_UI_LED: - htim3.Instance->CCR1 = duty_cycle; - break; - case WHITE_UI_LED: - htim3.Instance->CCR2 = duty_cycle; - break; - case GREEN_UI_LED: - htim15.Instance->CCR1=duty_cycle; - break; - case RED_UI_LED: - htim15.Instance->CCR2=duty_cycle; - break; + + switch(led) { + case DECK_LED: + #if PCBA_PRIMARY_REVISION == 'b' + htim2.Instance->CCR4 = duty_cycle; + #else + htim1.Instance->CCR3 = duty_cycle; + #endif + break; + case BLUE_UI_LED: + htim3.Instance->CCR1 = duty_cycle; + break; + case WHITE_UI_LED: + htim3.Instance->CCR2 = duty_cycle; + break; + case GREEN_UI_LED: + #if PCBA_PRIMARY_REVISION == 'b' + htim15.Instance->CCR1=duty_cycle; + #else + htim2.Instance->CCR4 = duty_cycle; + #endif + break; + case RED_UI_LED: + htim15.Instance->CCR2=duty_cycle; + break; default: break; - } + } } void led_hw_initialize_leds() { + #if !(PCBA_PRIMARY_REVISION == 'b') + MX_TIM1_Init(); + #endif MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM15_Init(); @@ -373,7 +485,11 @@ void led_hw_initialize_leds() { HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_2); - + #if PCBA_PRIMARY_REVISION == 'b' + HAL_TIM_PWM_Start(&htim15, TIM_CHANNEL_1); + #else + + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); + #endif } diff --git a/rear-panel/firmware/stm32g4xx_it.c b/rear-panel/firmware/stm32g4xx_it.c index 4f1acd5dd..bb87e4689 100644 --- a/rear-panel/firmware/stm32g4xx_it.c +++ b/rear-panel/firmware/stm32g4xx_it.c @@ -45,6 +45,9 @@ /* External variables --------------------------------------------------------*/ +#if !(PCBA_PRIMARY_REVISION == 'b') +extern TIM_HandleTypeDef htim1; +#endif extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim15; @@ -119,8 +122,11 @@ void DebugMon_Handler(void) {} /******************************************************************************/ /** - * @brief This function handles TIM2,3,15 global interrupt. + * @brief This function handles TIM1,2,3,15 global interrupt. */ +#if !(PCBA_PRIMARY_REVISION == 'b') +void TIM1_CC_IRQHandler(void) { HAL_TIM_IRQHandler(&htim1); } +#endif void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } diff --git a/sensors/tests/test_capacitive_sensor.cpp b/sensors/tests/test_capacitive_sensor.cpp index c32c1ac6f..97c139c7b 100644 --- a/sensors/tests/test_capacitive_sensor.cpp +++ b/sensors/tests/test_capacitive_sensor.cpp @@ -69,7 +69,7 @@ SCENARIO("read capacitance sensor values without shared CINs") { auto message_2 = get_message_i2c(i2c_queue); - uint16_t expected_fdc_CIN1 = 0x580; + uint16_t expected_fdc_CIN1 = 0x5C0; uint16_t actual_fdc_CIN1 = 0; const auto* iter_1 = @@ -100,7 +100,7 @@ SCENARIO("read capacitance sensor values without shared CINs") { } AND_WHEN("we read the messages from the queue") { auto read_message = - get_message( + get_message( poller_queue); THEN("The write and read command addresses are correct") { @@ -108,36 +108,44 @@ SCENARIO("read capacitance sensor values without shared CINs") { sensors::fdc1004::ADDRESS); REQUIRE(read_message.first.write_buffer[0] == static_cast( - sensors::fdc1004::Registers::MEAS1_MSB)); - REQUIRE(read_message.second.address == - sensors::fdc1004::ADDRESS); - REQUIRE(read_message.second.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::MEAS1_LSB)); + sensors::fdc1004::Registers::FDC_CONF)); } AND_WHEN("we send just one response") { sensors::utils::TaskMessage first = test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - true)); + test_mocks::dummy_single_response(read_message, + true)); sensor_not_shared.handle_message(first); THEN("no can response is sent") { REQUIRE(!can_queue.has_message()); } } - AND_WHEN("we send two responses") { - sensors::utils::TaskMessage first = + AND_WHEN("we send the full responses") { + i2c::messages::MaxMessageBuffer fdc_resp = {0x00, 0xFF}; + sensors::utils::TaskMessage fdc = test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - true)); + test_mocks::dummy_single_response(read_message, + true, fdc_resp)); + sensor_not_shared.handle_message(fdc); + REQUIRE(i2c_queue.get_size() == 1); + auto msb_message = + get_message_i2c(i2c_queue); + msb_message.id.is_completed_poll = true; + sensors::utils::TaskMessage first = + test_mocks::launder_response( + msb_message, response_queue, + test_mocks::dummy_response(msb_message)); sensor_not_shared.handle_message(first); + REQUIRE(i2c_queue.get_size() == 1); + auto lsb_message = + get_message_i2c(i2c_queue); + lsb_message.id.is_completed_poll = true; sensors::utils::TaskMessage second = test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - true)); + lsb_message, response_queue, + test_mocks::dummy_response(lsb_message)); sensor_not_shared.handle_message(second); THEN("after the second, a response is sent") { REQUIRE(can_queue.has_message()); @@ -159,7 +167,7 @@ SCENARIO("read capacitance sensor values without shared CINs") { } AND_WHEN("we read the messages from the queue") { auto read_message = - get_message( + get_message( poller_queue); THEN("The write and read command addresses are correct") { @@ -167,14 +175,9 @@ SCENARIO("read capacitance sensor values without shared CINs") { sensors::fdc1004::ADDRESS); REQUIRE(read_message.first.write_buffer[0] == static_cast( - sensors::fdc1004::Registers::MEAS1_MSB)); + sensors::fdc1004::Registers::FDC_CONF)); REQUIRE(read_message.first.bytes_to_write == 1); - REQUIRE(read_message.second.address == - sensors::fdc1004::ADDRESS); - REQUIRE(read_message.second.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::MEAS1_LSB)); - REQUIRE(read_message.second.bytes_to_write == 1); + REQUIRE(read_message.first.bytes_to_read == 2); REQUIRE(read_message.delay_ms == 20); REQUIRE(read_message.polling == NUM_READS); } @@ -185,25 +188,41 @@ SCENARIO("read capacitance sensor values without shared CINs") { i2c::messages::MaxMessageBuffer{0x7f, 0xff, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{0xff, 0, 0, 0, 0}; - std::array responses{ + + i2c::messages::MaxMessageBuffer fdc_resp = {0x00, 0xFF}; + sensors::utils::TaskMessage fdc = test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - false, buffer_a)), + test_mocks::dummy_single_response(read_message, + true, fdc_resp)); + sensor_not_shared.handle_message(fdc); + auto msb_message = + get_message_i2c(i2c_queue); + + std::array responses{ test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - false, buffer_b)), + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_a)), test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - true, buffer_a)), + test_mocks::dummy_response(msb_message, buffer_b)), + test_mocks::launder_response( + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_a)), test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - true, buffer_b))}; + test_mocks::dummy_response(msb_message, buffer_b))}; + + responses[0].id.is_completed_poll = false; + responses[1].id.is_completed_poll = false; + responses[1].id.transaction_index = 1; + responses[2].id.is_completed_poll = true; + responses[3].id.transaction_index = 1; + responses[3].id.is_completed_poll = true; + for (auto& response : responses) { - sensor_not_shared.handle_message(response); + sensors::utils::TaskMessage msg{response}; + sensor_not_shared.handle_message(msg); } can::message_writer_task::TaskMessage can_msg{}; @@ -220,29 +239,44 @@ SCENARIO("read capacitance sensor values without shared CINs") { THEN( "using the callback with -saturated data returns the " "expected value") { + i2c::messages::MaxMessageBuffer fdc_resp = {0x00, 0xFF}; + sensors::utils::TaskMessage fdc = + test_mocks::launder_response( + read_message, response_queue, + test_mocks::dummy_single_response(read_message, + true, fdc_resp)); + sensor_not_shared.handle_message(fdc); + auto msb_message = + get_message_i2c(i2c_queue); + auto buffer_a = i2c::messages::MaxMessageBuffer{0x80, 0x00, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{0x00, 0, 0, 0, 0}; - std::array responses{ + std::array responses{ test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - false, buffer_a)), + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_a)), test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - false, buffer_b)), + test_mocks::dummy_response(msb_message, buffer_b)), test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - true, buffer_a)), + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_a)), test_mocks::launder_response( read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - true, buffer_b))}; + test_mocks::dummy_response(msb_message, buffer_b))}; + + responses[0].id.is_completed_poll = false; + responses[1].id.is_completed_poll = false; + responses[1].id.transaction_index = 1; + responses[2].id.is_completed_poll = true; + responses[3].id.transaction_index = 1; + responses[3].id.is_completed_poll = true; + for (auto& response : responses) { - sensor_not_shared.handle_message(response); + sensors::utils::TaskMessage msg{response}; + sensor_not_shared.handle_message(msg); } REQUIRE(can_queue.get_size() == 1); @@ -272,20 +306,36 @@ SCENARIO("read capacitance sensor values without shared CINs") { auto buffer_a = i2c::messages::MaxMessageBuffer{200, 80, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{100, 10, 0, 0, 0}; auto read_message = - get_message(poller_queue); + get_message( + poller_queue); + + i2c::messages::MaxMessageBuffer fdc_resp = {0x00, 0xFF}; + sensors::utils::TaskMessage fdc = + test_mocks::launder_response(read_message, response_queue, + test_mocks::dummy_single_response( + read_message, true, fdc_resp)); + sensor_not_shared.handle_message(fdc); + REQUIRE(i2c_queue.get_size() == 1); + auto msb_message = + get_message_i2c(i2c_queue); + for (int i = 0; i < NUM_READS; i++) { - sensors::utils::TaskMessage first_resp = + i2c::messages::TransactionResponse first_resp = test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response( - read_message, 0, (i == (NUM_READS - 1)), buffer_a)); - sensors::utils::TaskMessage second_resp = + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_a)); + i2c::messages::TransactionResponse second_resp = test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response( - read_message, 1, (i == (NUM_READS - 1)), buffer_b)); - sensor_not_shared.handle_message(first_resp); - sensor_not_shared.handle_message(second_resp); + msb_message, response_queue, + test_mocks::dummy_response(msb_message, buffer_b)); + first_resp.id.transaction_index = 0; + second_resp.id.transaction_index = 1; + first_resp.id.is_completed_poll = (i == (NUM_READS - 1)); + second_resp.id.is_completed_poll = (i == (NUM_READS - 1)); + auto first_msg = sensors::utils::TaskMessage{first_resp}; + auto second_msg = sensors::utils::TaskMessage{second_resp}; + sensor_not_shared.handle_message(first_msg); + sensor_not_shared.handle_message(second_msg); } THEN("it should adjust the offset accordingly") { // check for the offset @@ -338,44 +388,28 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { sensor_shared.initialize(); WHEN("the driver receives the message") { - THEN( - "the i2c queue receives four register commands to initialize") { - REQUIRE(i2c_queue.get_size() == 4); + THEN("the i2c queue receives two register commands to initialize") { + REQUIRE(i2c_queue.get_size() == 2); } AND_WHEN("we inspect the configuration messages") { auto message_1 = get_message_i2c(i2c_queue); auto message_2 = get_message_i2c(i2c_queue); - auto message_3 = - get_message_i2c(i2c_queue); - auto message_4 = - get_message_i2c(i2c_queue); - uint16_t expected_fdc_CIN1 = 0x580; - uint16_t expected_fdc_CIN2 = 0x540; - uint16_t actual_fdc_CIN1 = 0; - uint16_t actual_fdc_CIN2 = 0; + uint16_t expected_fdc_CIN = 0x5C0; + uint16_t actual_fdc_CIN = 0; const auto* iter_1 = - message_4.transaction.write_buffer.cbegin() + 1; - const auto* iter_2 = message_2.transaction.write_buffer.cbegin() + 1; static_cast(bit_utils::bytes_to_int( - iter_1, message_4.transaction.write_buffer.cend(), - actual_fdc_CIN1)); - static_cast(bit_utils::bytes_to_int( - iter_2, message_2.transaction.write_buffer.cend(), - actual_fdc_CIN2)); - THEN("We have FDC configurations for two CINs") { - REQUIRE(actual_fdc_CIN1 == expected_fdc_CIN1); - REQUIRE(actual_fdc_CIN2 == expected_fdc_CIN2); + iter_1, message_2.transaction.write_buffer.cend(), + actual_fdc_CIN)); + THEN("We have FDC configurations for both CINs") { + REQUIRE(actual_fdc_CIN == expected_fdc_CIN); } - THEN("We set the configuration registers for both CINs") { + THEN("We set the configuration registers for the first CIN") { REQUIRE(message_1.transaction.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::CONF_MEAS2)); - REQUIRE(message_3.transaction.write_buffer[0] == static_cast( sensors::fdc1004::Registers::CONF_MEAS1)); } @@ -390,37 +424,44 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { sensor_shared.handle_message(single_read_s1); WHEN("the handler function receives the message") { THEN( - "the i2c poller queue is populated with a poll request and two " - "i2c writes") { + "the i2c poller queue is populated with a poll request and one " + "i2c write") { REQUIRE(poller_queue.get_size() == 1); - REQUIRE(i2c_queue.get_size() == 2); + REQUIRE(i2c_queue.get_size() == 1); } AND_WHEN("we read the messages from the queue") { auto read_message = - get_message( + get_message( poller_queue); auto message_1 = get_message_i2c(i2c_queue); - auto message_2 = - get_message_i2c(i2c_queue); THEN("The write and read command addresses are correct") { REQUIRE(read_message.first.address == sensors::fdc1004::ADDRESS); REQUIRE(read_message.first.write_buffer[0] == static_cast( - sensors::fdc1004::Registers::MEAS2_MSB)); - REQUIRE(read_message.second.address == - sensors::fdc1004::ADDRESS); - REQUIRE(read_message.second.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::MEAS2_LSB)); + sensors::fdc1004::Registers::FDC_CONF)); REQUIRE(message_1.transaction.write_buffer[0] == static_cast( sensors::fdc1004::Registers::CONF_MEAS2)); - REQUIRE(message_2.transaction.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::FDC_CONF)); + } + AND_WHEN("the FDC response is read") { + i2c::messages::MaxMessageBuffer fdc_resp = {0x00, 0xFF}; + sensors::utils::TaskMessage fdc = + test_mocks::launder_response( + read_message, response_queue, + test_mocks::dummy_single_response(read_message, + true, fdc_resp)); + sensor_shared.handle_message(fdc); + THEN("a request to read CH2 MSB is sent") { + REQUIRE(i2c_queue.get_size() == 1); + auto message_msb = + get_message_i2c(i2c_queue); + REQUIRE(message_msb.transaction.write_buffer[0] == + static_cast( + sensors::fdc1004::Registers::MEAS2_MSB)); + } } AND_WHEN( "the handler function receives a second request for sensor " @@ -441,45 +482,34 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { sensor_shared.handle_message(single_read_s0); // throw out previous S1 call - static_cast(get_message( - poller_queue)); static_cast( - get_message_i2c(i2c_queue)); + get_message( + poller_queue)); static_cast( get_message_i2c(i2c_queue)); THEN( "the i2c poller queue is populated with a poll request and the " - "fdc register is updated") { + "CONF_MEAS1 register is updated") { REQUIRE(poller_queue.get_size() == 1); - REQUIRE(i2c_queue.get_size() == 2); + REQUIRE(i2c_queue.get_size() == 1); } AND_WHEN("we read the messages from the queue") { auto read_message = - get_message( + get_message( poller_queue); auto message_1 = get_message_i2c(i2c_queue); - auto message_2 = - get_message_i2c(i2c_queue); THEN("The write and read command addresses are correct") { REQUIRE(read_message.first.address == sensors::fdc1004::ADDRESS); REQUIRE(read_message.first.write_buffer[0] == static_cast( - sensors::fdc1004::Registers::MEAS1_MSB)); - REQUIRE(read_message.second.address == - sensors::fdc1004::ADDRESS); - REQUIRE(read_message.second.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::MEAS1_LSB)); + sensors::fdc1004::Registers::FDC_CONF)); REQUIRE(message_1.transaction.write_buffer[0] == static_cast( sensors::fdc1004::Registers::CONF_MEAS1)); - REQUIRE(message_2.transaction.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::FDC_CONF)); } AND_WHEN( "the handler function receives a second request for sensor " @@ -755,7 +785,7 @@ SCENARIO("threshold configuration") { } AND_WHEN("we read the messages from the queue") { auto read_message = - get_message( + get_message( poller_queue); THEN("The write and read command addresses are correct") { @@ -763,14 +793,8 @@ SCENARIO("threshold configuration") { sensors::fdc1004::ADDRESS); REQUIRE(read_message.first.write_buffer[0] == static_cast( - sensors::fdc1004::Registers::MEAS1_MSB)); + sensors::fdc1004::Registers::FDC_CONF)); REQUIRE(read_message.first.bytes_to_write == 1); - REQUIRE(read_message.second.address == - sensors::fdc1004::ADDRESS); - REQUIRE(read_message.second.write_buffer[0] == - static_cast( - sensors::fdc1004::Registers::MEAS1_LSB)); - REQUIRE(read_message.second.bytes_to_write == 1); REQUIRE(read_message.delay_ms == 20); REQUIRE(read_message.polling == NUM_READS); } @@ -779,32 +803,33 @@ SCENARIO("threshold configuration") { i2c::messages::MaxMessageBuffer{0x7f, 0xff, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{0xff, 0, 0, 0, 0}; - for (int i = 0; i < NUM_READS - 1; i++) { - auto response_a = sensors::utils::TaskMessage( - test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response( - read_message, 0, false, buffer_a))); + for (int i = 0; i < NUM_READS; i++) { + std::array tags{ + sensors::utils::ResponseTag::IS_THRESHOLD_SENSE}; + i2c::messages::TransactionResponse first{ + .id = + i2c::messages::TransactionIdentifier{ + .token = sensors::utils::build_id( + sensors::fdc1004::ADDRESS, + static_cast( + sensors::fdc1004::Registers:: + MEAS1_MSB), + sensors::utils::byte_from_tags(tags)), + .is_completed_poll = 0, + .transaction_index = 0}, + .bytes_read = 2, + .read_buffer = buffer_a}; + if (i == (NUM_READS - 1)) { + first.id.is_completed_poll = true; + } + auto second = first; + second.id.transaction_index = 1; + second.read_buffer = buffer_b; + auto response_a = sensors::utils::TaskMessage(first); sensor.handle_message(response_a); - auto response_b = sensors::utils::TaskMessage( - test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response( - read_message, 1, false, buffer_b))); + auto response_b = sensors::utils::TaskMessage(second); sensor.handle_message(response_b); } - auto final_response_a = sensors::utils::TaskMessage( - test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 0, - true, buffer_a))); - auto final_response_b = sensors::utils::TaskMessage( - test_mocks::launder_response( - read_message, response_queue, - test_mocks::dummy_multi_response(read_message, 1, - true, buffer_b))); - sensor.handle_message(final_response_a); - sensor.handle_message(final_response_b); THEN("the threshold is set to the proper value") { REQUIRE(sensor.driver.get_threshold() == Approx(15.375).epsilon(1e-4)); diff --git a/state_manager/state_manager/hardware.py b/state_manager/state_manager/hardware.py index b63125d7d..6315bddb8 100644 --- a/state_manager/state_manager/hardware.py +++ b/state_manager/state_manager/hardware.py @@ -4,7 +4,7 @@ from dataclasses import dataclass, field -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from .measurable_states import Force, Position from .pipette_model import PipetteModel @@ -14,14 +14,14 @@ class GantryX: """Gantry X Hardware.""" - position: Position = field(default_factory=lambda: Position(axis=OT3Axis.X)) + position: Position = field(default_factory=lambda: Position(axis=Axis.X)) @dataclass class GantryY: """Gantry Y Hardware.""" - position: Position = field(default_factory=lambda: Position(axis=OT3Axis.Y)) + position: Position = field(default_factory=lambda: Position(axis=Axis.Y)) @dataclass @@ -30,10 +30,10 @@ class Gripper: # Gripper z is controlled by gripper board so it is being filed under gripper gripper_z_position: Position = field( - default_factory=lambda: Position(axis=OT3Axis.Z_G) + default_factory=lambda: Position(axis=Axis.Z_G) ) - position: Position = field(default_factory=lambda: Position(axis=OT3Axis.G)) - jaw_force: Force = field(default_factory=lambda: Force(axis=OT3Axis.G)) + position: Position = field(default_factory=lambda: Position(axis=Axis.G)) + jaw_force: Force = field(default_factory=lambda: Force(axis=Axis.G)) @dataclass @@ -41,7 +41,7 @@ class LeftPipette: """Left Pipette Hardware.""" model: PipetteModel - position: Position = field(default_factory=lambda: Position(axis=OT3Axis.P_L)) + position: Position = field(default_factory=lambda: Position(axis=Axis.P_L)) @dataclass @@ -49,7 +49,7 @@ class RightPipette: """Right Pipette Hardware.""" model: PipetteModel - position: Position = field(default_factory=lambda: Position(axis=OT3Axis.P_R)) + position: Position = field(default_factory=lambda: Position(axis=Axis.P_R)) @dataclass @@ -57,10 +57,10 @@ class Head: """OT3 Head Hardware.""" left_pipette_z_position: Position = field( - default_factory=lambda: Position(axis=OT3Axis.Z_L) + default_factory=lambda: Position(axis=Axis.Z_L) ) right_pipette_z_position: Position = field( - default_factory=lambda: Position(axis=OT3Axis.Z_R) + default_factory=lambda: Position(axis=Axis.Z_R) ) diff --git a/state_manager/state_manager/measurable_states.py b/state_manager/state_manager/measurable_states.py index 897153ef6..260b66b00 100644 --- a/state_manager/state_manager/measurable_states.py +++ b/state_manager/state_manager/measurable_states.py @@ -1,14 +1,14 @@ """Contains the different states for OT3 that can be monitored.""" from dataclasses import dataclass -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis @dataclass class Position: """Represents position state.""" - axis: OT3Axis + axis: Axis current_position: int = 0 commanded_position: int = 0 encoder_position: int = 0 @@ -18,7 +18,7 @@ class Position: class Force: """Represents force state.""" - axis: OT3Axis + axis: Axis current_force: int = 0 commanded_force: int = 0 encoder_force: int = 0 diff --git a/state_manager/state_manager/messages.py b/state_manager/state_manager/messages.py index 0e21f034b..3d587ec73 100644 --- a/state_manager/state_manager/messages.py +++ b/state_manager/state_manager/messages.py @@ -11,7 +11,7 @@ from enum import Enum, unique from typing import Any, Optional, Type -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from .ot3_state import OT3State from .util import Direction, MoveMessageHardware, SyncPinState @@ -69,7 +69,7 @@ def handle(self, data: bytes, ot3_state: OT3State) -> Optional[Response]: class MoveMessage(Message): """Message for moving OT3 axis in specified direction.""" - axis: OT3Axis + axis: Axis direction: Direction @staticmethod @@ -126,7 +126,7 @@ def handle(self, data: bytes, ot3_state: OT3State) -> Optional[Response]: class GetAxisLocationMessage(Message): """Message to get current location of axis.""" - axis: OT3Axis + axis: Axis @staticmethod def build_message(message_content: bytes) -> GetAxisLocationMessage: diff --git a/state_manager/state_manager/ot3_state.py b/state_manager/state_manager/ot3_state.py index e2c435f7a..f46bcf9a6 100644 --- a/state_manager/state_manager/ot3_state.py +++ b/state_manager/state_manager/ot3_state.py @@ -5,7 +5,7 @@ import logging from typing import Dict, List, Optional -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from .hardware import ( GantryX, @@ -105,7 +105,7 @@ def __init__( self.gripper = gripper self.sync_pin = SyncPin() - def _pos_dict(self) -> Dict[OT3Axis, Optional[Position]]: + def _pos_dict(self) -> Dict[Axis, Optional[Position]]: """Generates lookup dict for class's position properties.""" left_pipette_pos = ( self.left_pipette.position if self.left_pipette is not None else None @@ -119,18 +119,18 @@ def _pos_dict(self) -> Dict[OT3Axis, Optional[Position]]: ) return { - OT3Axis.Z_L: self.head.left_pipette_z_position, - OT3Axis.Z_R: self.head.right_pipette_z_position, - OT3Axis.X: self.gantry_x.position, - OT3Axis.Y: self.gantry_y.position, - OT3Axis.P_L: left_pipette_pos, - OT3Axis.P_R: right_pipette_pos, - OT3Axis.G: gripper_pos, - OT3Axis.Z_G: gripper_mount_pos, + Axis.Z_L: self.head.left_pipette_z_position, + Axis.Z_R: self.head.right_pipette_z_position, + Axis.X: self.gantry_x.position, + Axis.Y: self.gantry_y.position, + Axis.P_L: left_pipette_pos, + Axis.P_R: right_pipette_pos, + Axis.G: gripper_pos, + Axis.Z_G: gripper_mount_pos, } @property - def current_position(self) -> Dict[OT3Axis, Optional[int]]: + def current_position(self) -> Dict[Axis, Optional[int]]: """Returns dictionary with axis mapped to current position of said axis. If hardware is not attached for axis, then position value will be None. @@ -144,7 +144,7 @@ def current_position(self) -> Dict[OT3Axis, Optional[int]]: } @property - def encoder_position(self) -> Dict[OT3Axis, Optional[int]]: + def encoder_position(self) -> Dict[Axis, Optional[int]]: """Returns dictionary with axis mapped to encoder position of said axis. If hardware is not attached for axis, then position value will be None. @@ -158,7 +158,7 @@ def encoder_position(self) -> Dict[OT3Axis, Optional[int]]: } @property - def updatable_axes(self) -> List[OT3Axis]: + def updatable_axes(self) -> List[Axis]: """Returns a list of positions that are available to be updated.""" return [key for key, value in self._pos_dict().items() if value is not None] @@ -185,17 +185,17 @@ def is_high_throughput_pipette_attached(self) -> bool: and self.right_pipette.model == PipetteModel.MULTI_96_1000 ) - def axis_current_position(self, axis: OT3Axis) -> Optional[int]: + def axis_current_position(self, axis: Axis) -> Optional[int]: """Returns current position or None for specified axis.""" return self.current_position[axis] - def axis_encoder_position(self, axis: OT3Axis) -> Optional[int]: + def axis_encoder_position(self, axis: Axis) -> Optional[int]: """Returns encoder position or None for specified axis.""" return self.encoder_position[axis] def update_position( self, - axis_to_update: OT3Axis, + axis_to_update: Axis, current_position: int, encoder_position: int, ) -> None: @@ -210,7 +210,7 @@ def update_position( pos_to_update.current_position = current_position pos_to_update.encoder_position = encoder_position - def pulse(self, axis: OT3Axis, direction: Direction) -> None: + def pulse(self, axis: Axis, direction: Direction) -> None: """Increments or decrements current and encoder position by 1 for axis.""" log.info(f"SERVER: Pulsing {axis}") mod = 1 if direction == Direction.POSITIVE else -1 diff --git a/state_manager/state_manager/util.py b/state_manager/state_manager/util.py index 07f487a12..804df71f9 100644 --- a/state_manager/state_manager/util.py +++ b/state_manager/state_manager/util.py @@ -5,7 +5,7 @@ import enum from enum import Enum, unique -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis class Direction(int, enum.Enum): @@ -44,27 +44,27 @@ def from_int(cls, state_val: int) -> SyncPinState: @unique class MoveMessageHardware(Enum): - """Enum representing a mapping of an integer hardware id to an OT3Axis object.""" + """Enum representing a mapping of an integer hardware id to an Axis object.""" - def __init__(self, hw_id: int, axis: OT3Axis) -> None: + def __init__(self, hw_id: int, axis: Axis) -> None: """Create a MoveMessageHardware object.""" self.hw_id = hw_id self.axis = axis # hw_id, axis - X_AXIS = 0x0000, OT3Axis.X - Y_AXIS = 0x0001, OT3Axis.Y - Z_L_AXIS = 0x0002, OT3Axis.Z_L - Z_R_AXIS = 0x0003, OT3Axis.Z_R - Z_G_AXIS = 0x0004, OT3Axis.Z_G - P_L_AXIS = 0x0005, OT3Axis.P_L - P_R_AXIS = 0x0006, OT3Axis.P_R - G_AXIS = 0x0007, OT3Axis.G - Q_AXIS = 0x0008, OT3Axis.Q + X_AXIS = 0x0000, Axis.X + Y_AXIS = 0x0001, Axis.Y + Z_L_AXIS = 0x0002, Axis.Z_L + Z_R_AXIS = 0x0003, Axis.Z_R + Z_G_AXIS = 0x0004, Axis.Z_G + P_L_AXIS = 0x0005, Axis.P_L + P_R_AXIS = 0x0006, Axis.P_R + G_AXIS = 0x0007, Axis.G + Q_AXIS = 0x0008, Axis.Q @classmethod - def from_axis(cls, axis: OT3Axis) -> MoveMessageHardware: - """Get MoveMessageHardware connected to passed OT3Axis object.""" + def from_axis(cls, axis: Axis) -> MoveMessageHardware: + """Get MoveMessageHardware connected to passed Axis object.""" for val in cls: if val.axis == axis: return val diff --git a/state_manager/tests/test_messages.py b/state_manager/tests/test_messages.py index b59de0d46..f7fd448c9 100644 --- a/state_manager/tests/test_messages.py +++ b/state_manager/tests/test_messages.py @@ -1,7 +1,7 @@ """Test message functionality""" import pytest -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from state_manager.messages import ( GetAxisLocationMessage, @@ -63,92 +63,92 @@ def test_bad_messages(message: bytes, error: str, ot3_state: OT3State) -> None: ( pytest.param( b"\x00\x00\x00\x00", - MoveMessage(OT3Axis.X, Direction.NEGATIVE), + MoveMessage(Axis.X, Direction.NEGATIVE), id="MOVE_X_NEGATIVE", ), pytest.param( b"\x00\x00\x00\x01", - MoveMessage(OT3Axis.X, Direction.POSITIVE), + MoveMessage(Axis.X, Direction.POSITIVE), id="MOVE_X_POSITIVE", ), pytest.param( b"\x00\x00\x01\x00", - MoveMessage(OT3Axis.Y, Direction.NEGATIVE), + MoveMessage(Axis.Y, Direction.NEGATIVE), id="MOVE_Y_NEGATIVE", ), pytest.param( b"\x00\x00\x01\x01", - MoveMessage(OT3Axis.Y, Direction.POSITIVE), + MoveMessage(Axis.Y, Direction.POSITIVE), id="MOVE_Y_POSITIVE", ), pytest.param( b"\x00\x00\x02\x00", - MoveMessage(OT3Axis.Z_L, Direction.NEGATIVE), + MoveMessage(Axis.Z_L, Direction.NEGATIVE), id="MOVE_Z_L_NEGATIVE", ), pytest.param( b"\x00\x00\x02\x01", - MoveMessage(OT3Axis.Z_L, Direction.POSITIVE), + MoveMessage(Axis.Z_L, Direction.POSITIVE), id="MOVE_Z_L_POSITIVE", ), pytest.param( b"\x00\x00\x03\x00", - MoveMessage(OT3Axis.Z_R, Direction.NEGATIVE), + MoveMessage(Axis.Z_R, Direction.NEGATIVE), id="MOVE_Z_R_NEGATIVE", ), pytest.param( b"\x00\x00\x03\x01", - MoveMessage(OT3Axis.Z_R, Direction.POSITIVE), + MoveMessage(Axis.Z_R, Direction.POSITIVE), id="MOVE_Z_R_POSITIVE", ), pytest.param( b"\x00\x00\x04\x00", - MoveMessage(OT3Axis.Z_G, Direction.NEGATIVE), + MoveMessage(Axis.Z_G, Direction.NEGATIVE), id="MOVE_Z_G_NEGATIVE", ), pytest.param( b"\x00\x00\x04\x01", - MoveMessage(OT3Axis.Z_G, Direction.POSITIVE), + MoveMessage(Axis.Z_G, Direction.POSITIVE), id="MOVE_Z_G_POSITIVE", ), pytest.param( b"\x00\x00\x05\x00", - MoveMessage(OT3Axis.P_L, Direction.NEGATIVE), + MoveMessage(Axis.P_L, Direction.NEGATIVE), id="MOVE_P_L_NEGATIVE", ), pytest.param( b"\x00\x00\x05\x01", - MoveMessage(OT3Axis.P_L, Direction.POSITIVE), + MoveMessage(Axis.P_L, Direction.POSITIVE), id="MOVE_P_L_POSITIVE", ), pytest.param( b"\x00\x00\x06\x00", - MoveMessage(OT3Axis.P_R, Direction.NEGATIVE), + MoveMessage(Axis.P_R, Direction.NEGATIVE), id="MOVE_P_R_NEGATIVE", ), pytest.param( b"\x00\x00\x06\x01", - MoveMessage(OT3Axis.P_R, Direction.POSITIVE), + MoveMessage(Axis.P_R, Direction.POSITIVE), id="MOVE_P_R_POSITIVE", ), pytest.param( b"\x00\x00\x07\x00", - MoveMessage(OT3Axis.G, Direction.NEGATIVE), + MoveMessage(Axis.G, Direction.NEGATIVE), id="MOVE_G_NEGATIVE", ), pytest.param( b"\x00\x00\x07\x01", - MoveMessage(OT3Axis.G, Direction.POSITIVE), + MoveMessage(Axis.G, Direction.POSITIVE), id="MOVE_G_POSITIVE", ), pytest.param( b"\x00\x00\x08\x00", - MoveMessage(OT3Axis.Q, Direction.NEGATIVE), + MoveMessage(Axis.Q, Direction.NEGATIVE), id="MOVE_Q_NEGATIVE", ), pytest.param( b"\x00\x00\x08\x01", - MoveMessage(OT3Axis.Q, Direction.POSITIVE), + MoveMessage(Axis.Q, Direction.POSITIVE), id="MOVE_Q_POSITIVE", ), pytest.param( @@ -163,47 +163,47 @@ def test_bad_messages(message: bytes, error: str, ot3_state: OT3State) -> None: ), pytest.param( b"\x02\x00\x00\x00", - GetAxisLocationMessage(OT3Axis.X), + GetAxisLocationMessage(Axis.X), id="GET_LOCATION_X", ), pytest.param( b"\x02\x00\x01\x00", - GetAxisLocationMessage(OT3Axis.Y), + GetAxisLocationMessage(Axis.Y), id="GET_LOCATION_Y", ), pytest.param( b"\x02\x00\x02\x00", - GetAxisLocationMessage(OT3Axis.Z_L), + GetAxisLocationMessage(Axis.Z_L), id="GET_LOCATION_Z_L", ), pytest.param( b"\x02\x00\x03\x00", - GetAxisLocationMessage(OT3Axis.Z_R), + GetAxisLocationMessage(Axis.Z_R), id="GET_LOCATION_Z_R", ), pytest.param( b"\x02\x00\x04\x00", - GetAxisLocationMessage(OT3Axis.Z_G), + GetAxisLocationMessage(Axis.Z_G), id="GET_LOCATION_Z_G", ), pytest.param( b"\x02\x00\x05\x00", - GetAxisLocationMessage(OT3Axis.P_L), + GetAxisLocationMessage(Axis.P_L), id="GET_LOCATION_P_L", ), pytest.param( b"\x02\x00\x06\x00", - GetAxisLocationMessage(OT3Axis.P_R), + GetAxisLocationMessage(Axis.P_R), id="GET_LOCATION_P_R", ), pytest.param( b"\x02\x00\x07\x00", - GetAxisLocationMessage(OT3Axis.G), + GetAxisLocationMessage(Axis.G), id="GET_LOCATION_G", ), pytest.param( b"\x02\x00\x08\x00", - GetAxisLocationMessage(OT3Axis.Q), + GetAxisLocationMessage(Axis.Q), id="GET_LOCATION_Q", ), pytest.param( @@ -222,92 +222,92 @@ def test_message_parsing(message: bytes, expected_message: Message) -> None: "message, expected_bytes", ( pytest.param( - MoveMessage(OT3Axis.X, Direction.NEGATIVE), + MoveMessage(Axis.X, Direction.NEGATIVE), b"\x00\x00\x00\x00", id="MOVE_X_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.X, Direction.POSITIVE), + MoveMessage(Axis.X, Direction.POSITIVE), b"\x00\x00\x00\x01", id="MOVE_X_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.Y, Direction.NEGATIVE), + MoveMessage(Axis.Y, Direction.NEGATIVE), b"\x00\x00\x01\x00", id="MOVE_Y_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.Y, Direction.POSITIVE), + MoveMessage(Axis.Y, Direction.POSITIVE), b"\x00\x00\x01\x01", id="MOVE_Y_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_L, Direction.NEGATIVE), + MoveMessage(Axis.Z_L, Direction.NEGATIVE), b"\x00\x00\x02\x00", id="MOVE_Z_L_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_L, Direction.POSITIVE), + MoveMessage(Axis.Z_L, Direction.POSITIVE), b"\x00\x00\x02\x01", id="MOVE_Z_L_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_R, Direction.NEGATIVE), + MoveMessage(Axis.Z_R, Direction.NEGATIVE), b"\x00\x00\x03\x00", id="MOVE_Z_R_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_R, Direction.POSITIVE), + MoveMessage(Axis.Z_R, Direction.POSITIVE), b"\x00\x00\x03\x01", id="MOVE_Z_R_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_G, Direction.NEGATIVE), + MoveMessage(Axis.Z_G, Direction.NEGATIVE), b"\x00\x00\x04\x00", id="MOVE_Z_G_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.Z_G, Direction.POSITIVE), + MoveMessage(Axis.Z_G, Direction.POSITIVE), b"\x00\x00\x04\x01", id="MOVE_Z_G_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.P_L, Direction.NEGATIVE), + MoveMessage(Axis.P_L, Direction.NEGATIVE), b"\x00\x00\x05\x00", id="MOVE_P_L_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.P_L, Direction.POSITIVE), + MoveMessage(Axis.P_L, Direction.POSITIVE), b"\x00\x00\x05\x01", id="MOVE_P_L_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.P_R, Direction.NEGATIVE), + MoveMessage(Axis.P_R, Direction.NEGATIVE), b"\x00\x00\x06\x00", id="MOVE_P_R_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.P_R, Direction.POSITIVE), + MoveMessage(Axis.P_R, Direction.POSITIVE), b"\x00\x00\x06\x01", id="MOVE_P_R_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.G, Direction.NEGATIVE), + MoveMessage(Axis.G, Direction.NEGATIVE), b"\x00\x00\x07\x00", id="MOVE_G_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.G, Direction.POSITIVE), + MoveMessage(Axis.G, Direction.POSITIVE), b"\x00\x00\x07\x01", id="MOVE_G_POSITIVE", ), pytest.param( - MoveMessage(OT3Axis.Q, Direction.NEGATIVE), + MoveMessage(Axis.Q, Direction.NEGATIVE), b"\x00\x00\x08\x00", id="MOVE_Q_NEGATIVE", ), pytest.param( - MoveMessage(OT3Axis.Q, Direction.POSITIVE), + MoveMessage(Axis.Q, Direction.POSITIVE), b"\x00\x00\x08\x01", id="MOVE_Q_POSITIVE", ), @@ -322,47 +322,47 @@ def test_message_parsing(message: bytes, expected_message: Message) -> None: id="SYNC_HIGH", ), pytest.param( - GetAxisLocationMessage(OT3Axis.X), + GetAxisLocationMessage(Axis.X), b"\x02\x00\x00\x00", id="GET_LOCATION_X", ), pytest.param( - GetAxisLocationMessage(OT3Axis.Y), + GetAxisLocationMessage(Axis.Y), b"\x02\x00\x01\x00", id="GET_LOCATION_Y", ), pytest.param( - GetAxisLocationMessage(OT3Axis.Z_L), + GetAxisLocationMessage(Axis.Z_L), b"\x02\x00\x02\x00", id="GET_LOCATION_Z_L", ), pytest.param( - GetAxisLocationMessage(OT3Axis.Z_R), + GetAxisLocationMessage(Axis.Z_R), b"\x02\x00\x03\x00", id="GET_LOCATION_Z_R", ), pytest.param( - GetAxisLocationMessage(OT3Axis.Z_G), + GetAxisLocationMessage(Axis.Z_G), b"\x02\x00\x04\x00", id="GET_LOCATION_Z_G", ), pytest.param( - GetAxisLocationMessage(OT3Axis.P_L), + GetAxisLocationMessage(Axis.P_L), b"\x02\x00\x05\x00", id="GET_LOCATION_P_L", ), pytest.param( - GetAxisLocationMessage(OT3Axis.P_R), + GetAxisLocationMessage(Axis.P_R), b"\x02\x00\x06\x00", id="GET_LOCATION_P_R", ), pytest.param( - GetAxisLocationMessage(OT3Axis.G), + GetAxisLocationMessage(Axis.G), b"\x02\x00\x07\x00", id="GET_LOCATION_G", ), pytest.param( - GetAxisLocationMessage(OT3Axis.Q), + GetAxisLocationMessage(Axis.Q), b"\x02\x00\x08\x00", id="GET_LOCATION_Q", ), @@ -381,8 +381,8 @@ def test_convert_message_to_bytes(message: Message, expected_bytes: bytes) -> No @pytest.mark.parametrize( "message,expected_val", ( - pytest.param(MoveMessage(OT3Axis.X, Direction.POSITIVE), 1, id="pos_pulse"), - pytest.param(MoveMessage(OT3Axis.X, Direction.NEGATIVE), -1, id="neg_pulse"), + pytest.param(MoveMessage(Axis.X, Direction.POSITIVE), 1, id="pos_pulse"), + pytest.param(MoveMessage(Axis.X, Direction.NEGATIVE), -1, id="neg_pulse"), ), ) def test_valid_handle_move_message( @@ -392,18 +392,18 @@ def test_valid_handle_move_message( print(message.to_bytes()) ack = handle_message(message.to_bytes(), ot3_state) assert ack is None - assert ot3_state.axis_current_position(OT3Axis.X) == expected_val + assert ot3_state.axis_current_position(Axis.X) == expected_val def test_valid_handle_get_location_message(ot3_state: OT3State) -> None: """Confirm that get location messages work correctly.""" - pulse_x_pos = MoveMessage(OT3Axis.X, Direction.POSITIVE).to_bytes() + pulse_x_pos = MoveMessage(Axis.X, Direction.POSITIVE).to_bytes() for _ in range(5): ack = handle_message(pulse_x_pos, ot3_state) assert ack is None - assert ot3_state.axis_current_position(OT3Axis.X) == 5 + assert ot3_state.axis_current_position(Axis.X) == 5 expected = b"\x02\x00\x005" - ack = handle_message(GetAxisLocationMessage(OT3Axis.X).to_bytes(), ot3_state) + ack = handle_message(GetAxisLocationMessage(Axis.X).to_bytes(), ot3_state) assert ack is not None assert ack.to_bytes() == expected diff --git a/state_manager/tests/test_ot3_state.py b/state_manager/tests/test_ot3_state.py index 504525e11..c2ac43d34 100644 --- a/state_manager/tests/test_ot3_state.py +++ b/state_manager/tests/test_ot3_state.py @@ -1,6 +1,6 @@ """OT3State object tests.""" import pytest -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from state_manager.hardware import Gripper, LeftPipette, RightPipette from state_manager.ot3_state import OT3State @@ -134,14 +134,14 @@ def test_current_position(ot3_state: OT3State) -> None: """Confirms that .current_position returns correctly.""" current_position_dict = ot3_state.current_position assert set(current_position_dict.keys()) == { - OT3Axis.X, - OT3Axis.Y, - OT3Axis.Z_L, - OT3Axis.Z_R, - OT3Axis.Z_G, - OT3Axis.P_L, - OT3Axis.P_R, - OT3Axis.G, + Axis.X, + Axis.Y, + Axis.Z_L, + Axis.Z_R, + Axis.Z_G, + Axis.P_L, + Axis.P_R, + Axis.G, } assert all([val == 0 for val in current_position_dict.values()]) @@ -152,52 +152,49 @@ def test_current_position_no_gripper_or_pipettes( """Confirms that .current_position returns correctly when no pipettes or gripper is attached.""" current_position_dict = ot3_state_no_pipettes_or_gripper.current_position assert set(current_position_dict.keys()) == { - OT3Axis.X, - OT3Axis.Y, - OT3Axis.Z_L, - OT3Axis.Z_R, - OT3Axis.Z_G, - OT3Axis.P_L, - OT3Axis.P_R, - OT3Axis.G, + Axis.X, + Axis.Y, + Axis.Z_L, + Axis.Z_R, + Axis.Z_G, + Axis.P_L, + Axis.P_R, + Axis.G, } assert all( - [ - current_position_dict[key] is None - for key in [OT3Axis.G, OT3Axis.P_L, OT3Axis.P_R] - ] + [current_position_dict[key] is None for key in [Axis.G, Axis.P_L, Axis.P_R]] ) def test_update_current_position(ot3_state: OT3State) -> None: """Confirms that updating the current position works.""" ot3_state.update_position( - axis_to_update=OT3Axis.X, + axis_to_update=Axis.X, current_position=5, encoder_position=0, ) - assert ot3_state.current_position[OT3Axis.X] == 5 + assert ot3_state.current_position[Axis.X] == 5 def test_update_encoder_position(ot3_state: OT3State) -> None: """Confirms that updating the encoder position works.""" ot3_state.update_position( - axis_to_update=OT3Axis.X, + axis_to_update=Axis.X, current_position=0, encoder_position=7, ) - assert ot3_state.encoder_position[OT3Axis.X] == 7 + assert ot3_state.encoder_position[Axis.X] == 7 def test_update_multiple_positions(ot3_state: OT3State) -> None: """Confirms that updating the multiple positions at the same time works.""" ot3_state.update_position( - axis_to_update=OT3Axis.X, + axis_to_update=Axis.X, current_position=8, encoder_position=10, ) - assert ot3_state.current_position[OT3Axis.X] == 8 - assert ot3_state.encoder_position[OT3Axis.X] == 10 + assert ot3_state.current_position[Axis.X] == 8 + assert ot3_state.encoder_position[Axis.X] == 10 def test_update_position_hardware_not_attached( @@ -206,7 +203,7 @@ def test_update_position_hardware_not_attached( """Confirms that updating a position of a piece of equipment, that is not attached, throws an exception.""" with pytest.raises(ValueError) as err: ot3_state_no_pipettes_or_gripper.update_position( - axis_to_update=OT3Axis.G, + axis_to_update=Axis.G, current_position=6, encoder_position=0, ) @@ -227,17 +224,17 @@ def test_pulse() -> None: True, ) - state_1.pulse(axis=OT3Axis.X, direction=Direction.POSITIVE) - assert state_1.axis_current_position(OT3Axis.X) == 1 - assert state_2.axis_current_position(OT3Axis.X) == 0 + state_1.pulse(axis=Axis.X, direction=Direction.POSITIVE) + assert state_1.axis_current_position(Axis.X) == 1 + assert state_2.axis_current_position(Axis.X) == 0 - state_1.pulse(axis=OT3Axis.X, direction=Direction.POSITIVE) - assert state_1.axis_current_position(OT3Axis.X) == 2 - assert state_2.axis_current_position(OT3Axis.X) == 0 + state_1.pulse(axis=Axis.X, direction=Direction.POSITIVE) + assert state_1.axis_current_position(Axis.X) == 2 + assert state_2.axis_current_position(Axis.X) == 0 - state_1.pulse(axis=OT3Axis.X, direction=Direction.NEGATIVE) - assert state_1.axis_current_position(OT3Axis.X) == 1 - assert state_2.axis_current_position(OT3Axis.X) == 0 + state_1.pulse(axis=Axis.X, direction=Direction.NEGATIVE) + assert state_1.axis_current_position(Axis.X) == 1 + assert state_2.axis_current_position(Axis.X) == 0 def test_sync_pin(ot3_state: OT3State) -> None: diff --git a/state_manager/tests/test_ot3_state_manager.py b/state_manager/tests/test_ot3_state_manager.py index 968477faa..dafd28f00 100644 --- a/state_manager/tests/test_ot3_state_manager.py +++ b/state_manager/tests/test_ot3_state_manager.py @@ -5,7 +5,7 @@ from unittest.mock import Mock, patch import pytest -from opentrons.hardware_control.types import OT3Axis +from opentrons.hardware_control.types import Axis from state_manager.messages import MoveMessage from state_manager.ot3_state import OT3State @@ -65,7 +65,7 @@ async def test_message_received( ot3_state: OT3State, ) -> None: """Confirm that pulse messages work correctly.""" - move_message_bytes = MoveMessage(OT3Axis.X, Direction.POSITIVE).to_bytes() + move_message_bytes = MoveMessage(Axis.X, Direction.POSITIVE).to_bytes() client.send_message(move_message_bytes) await asyncio.wait_for(server.is_connected(), 15.0) datagram_received_data_arg_content = patched_object.call_args.args[0]