From f260e6f4a826d4ccc64fa162c1f74c6041a1d0e5 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Tue, 11 Jul 2023 10:09:49 -0400 Subject: [PATCH 01/28] chore: individual artifact files (#701) * chore: individual artifact files * typo --- .github/workflows/release_bundle.yaml | 43 ++++----------------------- 1 file changed, 5 insertions(+), 38 deletions(-) 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 From bb7ac57cb4f4e6f5d855afc964cd4009a37902c8 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 11 Jul 2023 16:09:56 -0400 Subject: [PATCH 02/28] fix(motor-control): clear encoder status flag when home fails (#702) * clear enc position when home fails * do not set encoder flag until after backed off --- .../stepper_motor/motor_interrupt_handler.hpp | 3 ++- motor-control/tests/test_limit_switch.cpp | 24 +++++++++++++++---- motor-control/tests/test_motor_pulse.cpp | 2 +- 3 files changed, 22 insertions(+), 7 deletions(-) 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 39122274c..8c8d71021 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -282,7 +282,6 @@ class MotorInterruptHandler { auto homing_stopped() -> bool { if (limit_switch_triggered()) { position_tracker = 0; - hardware.reset_step_tracker(); finish_current_move(AckMessageId::stopped_by_condition); return true; } @@ -381,6 +380,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); } } diff --git a/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp index 2413fa499..278e41840 100644 --- a/motor-control/tests/test_limit_switch.cpp +++ b/motor-control/tests/test_limit_switch.cpp @@ -55,9 +55,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") { @@ -70,6 +72,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 " @@ -79,12 +82,15 @@ SCENARIO("MoveStopCondition::limit_switch with the limit switch triggered") { std::get(test_objs.reporter.messages.back()); REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); REQUIRE(read_ack.encoder_position == 50); - REQUIRE(read_ack.current_position_steps == 0); + REQUIRE(read_ack.current_position_steps == 350); } - - 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)); } } } @@ -114,9 +120,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") { @@ -144,6 +152,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/motor-control/tests/test_motor_pulse.cpp b/motor-control/tests/test_motor_pulse.cpp index 856edaad1..c75838e84 100644 --- a/motor-control/tests/test_motor_pulse.cpp +++ b/motor-control/tests/test_motor_pulse.cpp @@ -340,7 +340,7 @@ TEST_CASE("Finishing a move") { auto msg = std::get(test_objs.reporter.messages[0]); REQUIRE(msg.group_id == move.group_id); REQUIRE(msg.seq_id == move.seq_id); - REQUIRE(msg.current_position_steps == 0); + REQUIRE(msg.current_position_steps == 100); REQUIRE(msg.encoder_position == 200); REQUIRE(msg.position_flags == 0x0); From 6d7b2a492d07770a801eba7b793abb3a66ad5aca Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Thu, 13 Jul 2023 12:13:38 -0400 Subject: [PATCH 03/28] fix(sensors): mitigate cap sensor configuration issues (#703) * Fixes for FDC1004 - Check the Ready flag for each channel before reading data - Only set the sample rate ONE time ever - Add some delays during capsense initialization to avoid all of the I2C bus traffic - Take out the filtering stuff * Cleaned up tests --- CMakeLists.txt | 2 + include/sensors/core/fdc1004.hpp | 18 +- .../sensors/core/tasks/capacitive_driver.hpp | 198 ++++++----- .../core/tasks/capacitive_sensor_task.hpp | 8 +- include/sensors/core/utils.hpp | 4 + include/sensors/simulation/fdc1004.hpp | 5 +- sensors/tests/test_capacitive_sensor.cpp | 319 ++++++++++-------- 7 files changed, 320 insertions(+), 234 deletions(-) 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/include/sensors/core/fdc1004.hpp b/include/sensors/core/fdc1004.hpp index 359443142..c6db607d6 100644 --- a/include/sensors/core/fdc1004.hpp +++ b/include/sensors/core/fdc1004.hpp @@ -129,7 +129,7 @@ template // 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/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 15947159b..e4c35371f 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,51 @@ 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 { + 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 +230,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 +242,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 +282,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 +342,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 +362,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); 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/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/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)); From db1618c4370fe8177c51549563b96cb49e350e8a Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Thu, 13 Jul 2023 15:05:48 -0400 Subject: [PATCH 04/28] fix(motor-control): update position flag inside encoder background timer (#704) --- .../motor_encoder_background_timer.hpp | 3 ++- .../core/stepper_motor/motor_interrupt_handler.hpp | 13 ++++++++++--- 2 files changed, 12 insertions(+), 4 deletions(-) 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..1a467212a 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 @@ -31,7 +31,8 @@ class BackgroundTimer { auto callback() -> void { if (!_interrupt_handler.has_active_move()) { // 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(); } } 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 8c8d71021..027b52b02 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -78,9 +78,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(); } } @@ -88,6 +86,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( From 1bf363e3ae328bbd58504b50e46966b1ee44133b Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Tue, 18 Jul 2023 12:49:48 -0400 Subject: [PATCH 05/28] fix(state-manager): use Axis instead of OT3Axis (#706) * fix(state-manager): use Axis instead of OT3Axis https://github.com/Opentrons/opentrons/pull/12834 merged OT3Axis and Axis (at long last) and we must adapt. * format --- state_manager/state_manager/hardware.py | 20 +-- .../state_manager/measurable_states.py | 6 +- state_manager/state_manager/messages.py | 6 +- state_manager/state_manager/ot3_state.py | 34 ++--- state_manager/state_manager/util.py | 28 ++-- state_manager/tests/test_messages.py | 122 +++++++++--------- state_manager/tests/test_ot3_state.py | 73 +++++------ state_manager/tests/test_ot3_state_manager.py | 4 +- 8 files changed, 145 insertions(+), 148 deletions(-) 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] From a995710ed49c813099c160cff24c3d2705c970eb Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 18 Jul 2023 14:09:31 -0400 Subject: [PATCH 06/28] refresh the stall check threshold values after finish the last move in the queue (#705) --- .../core/stepper_motor/motor_interrupt_handler.hpp | 3 +++ 1 file changed, 3 insertions(+) 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 027b52b02..4004192a3 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -276,6 +276,9 @@ class MotorInterruptHandler { return true; } } + // update the stall check ideal encoder counts based on + // last known location + stall_checker.reset_itr_counts(hardware.get_step_tracker()); return false; } } else { From fbb28abbafdb532945573e5931e25dcc833207ce Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Wed, 19 Jul 2023 09:47:41 -0400 Subject: [PATCH 07/28] chore: add revisions for PVT boards (#707) Add revision handling for the variants: - Gantry C2 - Rear-panel C1 (B2 did not actually exist, keeping it just in case) - LT pipette D1 Head got a bump from C2.0 to C2.1, which we don't track; SOM got a bump but we don't track that; 96 hasn't gotten its bump yet. These changes are to enter PVT. --- bootloader/firmware/stm32G4/CMakeLists.txt | 20 ++++++++++---------- gantry/firmware/CMakeLists.txt | 10 ++++++---- gripper/firmware/CMakeLists.txt | 5 +++-- pipettes/firmware/CMakeLists.txt | 8 ++++---- rear-panel/firmware/CMakeLists.txt | 4 ++-- 5 files changed, 25 insertions(+), 22 deletions(-) diff --git a/bootloader/firmware/stm32G4/CMakeLists.txt b/bootloader/firmware/stm32G4/CMakeLists.txt index ac1082841..eb2d07902 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 ) 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/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/pipettes/firmware/CMakeLists.txt b/pipettes/firmware/CMakeLists.txt index 07d8991c3..9554af8e2 100644 --- a/pipettes/firmware/CMakeLists.txt +++ b/pipettes/firmware/CMakeLists.txt @@ -97,16 +97,16 @@ 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( 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 From 16019d5364a6412b123c6f98312538d05a541daa Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Wed, 19 Jul 2023 15:38:34 -0400 Subject: [PATCH 08/28] fix(sensors): reconfigure cap sensor if necessary (#708) --- .../sensors/core/tasks/capacitive_driver.hpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index e4c35371f..58ab83fbf 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -216,6 +216,12 @@ class FDC1004 { // 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 @@ -445,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 { From 087c080d974c560b5e87178f4675cb7278aa7a33 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 26 Jul 2023 15:27:27 -0400 Subject: [PATCH 09/28] reset itr count (#709) --- .../core/stepper_motor/motor_interrupt_handler.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 4004192a3..29df1d0f4 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -276,9 +276,6 @@ class MotorInterruptHandler { return true; } } - // update the stall check ideal encoder counts based on - // last known location - stall_checker.reset_itr_counts(hardware.get_step_tracker()); return false; } } else { @@ -461,6 +458,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() { From 4693209a76a9c0174eb2b6d4210fdceb46f3281d Mon Sep 17 00:00:00 2001 From: Caila Marashaj <98041399+caila-marashaj@users.noreply.github.com> Date: Thu, 27 Jul 2023 12:34:53 -0400 Subject: [PATCH 10/28] feat(pipettes): add acceleration to TipActionRequest message (#653) --- include/can/core/messages.hpp | 6 +++++- include/eeprom/core/update_data_rev_task.hpp | 2 ++ include/motor-control/core/motor_messages.hpp | 1 + .../core/stepper_motor/motion_controller.hpp | 8 +++++++- .../core/stepper_motor/motor_interrupt_handler.hpp | 10 +++++----- .../core/tasks/gear_move_status_reporter_task.hpp | 2 +- include/pipettes/core/tasks/motion_controller_task.hpp | 4 ++-- motor-control/tests/test_limit_switch.cpp | 2 +- motor-control/tests/test_motor_pulse.cpp | 2 +- 9 files changed, 25 insertions(+), 12 deletions(-) diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index f3583de20..9a0c2f0dc 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1252,6 +1252,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 { @@ -1261,6 +1262,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); @@ -1270,6 +1272,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, @@ -1278,7 +1281,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; 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/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp index e27436356..a6acf1c2d 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) diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index af76fb65f..ff670d374 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -206,6 +206,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( @@ -229,11 +231,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, @@ -317,6 +321,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_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 29df1d0f4..9beb5a253 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -50,7 +50,6 @@ class MotorInterruptHandler { using MoveQueue = QueueImpl; using UpdatePositionQueue = QueueImpl; - MotorInterruptHandler() = delete; MotorInterruptHandler(MoveQueue& incoming_move_queue, StatusClient& outgoing_queue, @@ -289,6 +288,7 @@ class MotorInterruptHandler { auto homing_stopped() -> bool { if (limit_switch_triggered()) { position_tracker = 0; + hardware.reset_step_tracker(); finish_current_move(AckMessageId::stopped_by_condition); return true; } @@ -515,7 +515,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(); @@ -549,8 +549,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 { @@ -558,7 +558,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, 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/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp index 278e41840..8dbd107a2 100644 --- a/motor-control/tests/test_limit_switch.cpp +++ b/motor-control/tests/test_limit_switch.cpp @@ -82,7 +82,7 @@ SCENARIO("MoveStopCondition::limit_switch with the limit switch triggered") { std::get(test_objs.reporter.messages.back()); REQUIRE(read_ack.ack_id == AckMessageId::stopped_by_condition); REQUIRE(read_ack.encoder_position == 50); - REQUIRE(read_ack.current_position_steps == 350); + REQUIRE(read_ack.current_position_steps == 0); } THEN( "the stepper position flag and encoder position flags are " diff --git a/motor-control/tests/test_motor_pulse.cpp b/motor-control/tests/test_motor_pulse.cpp index c75838e84..856edaad1 100644 --- a/motor-control/tests/test_motor_pulse.cpp +++ b/motor-control/tests/test_motor_pulse.cpp @@ -340,7 +340,7 @@ TEST_CASE("Finishing a move") { auto msg = std::get(test_objs.reporter.messages[0]); REQUIRE(msg.group_id == move.group_id); REQUIRE(msg.seq_id == move.seq_id); - REQUIRE(msg.current_position_steps == 100); + REQUIRE(msg.current_position_steps == 0); REQUIRE(msg.encoder_position == 200); REQUIRE(msg.position_flags == 0x0); From 0ce76585ab6cd76c6bbc8a4c10c123e9d45cc297 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 1 Aug 2023 11:17:35 -0400 Subject: [PATCH 11/28] fix(motor-control): do not disengage motor when a StopRequest is received (#710) * do not disengage * fix for pipette controller --- include/motor-control/core/stepper_motor/motion_controller.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index ff670d374..3a4b71c57 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() { queue.reset(); - disable_motor(); if (hardware.is_timer_interrupt_running()) { hardware.request_cancel(); } @@ -261,7 +260,6 @@ class PipetteMotionController { void stop() { 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. From 9718788be01d1c3e4883804f4702eca8e785ee5d Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 2 Aug 2023 16:39:57 -0400 Subject: [PATCH 12/28] use stay_engaged to determine whether or not to stay enabled (#711) --- include/can/core/messages.hpp | 10 +++++++++- .../brushed_motor/brushed_motion_controller.hpp | 1 + .../brushed_motor_interrupt_handler.hpp | 13 ++----------- include/motor-control/core/motor_messages.hpp | 1 + .../tests/test_brushed_motor_interrupt_handler.cpp | 14 ++++++++++++++ 5 files changed, 27 insertions(+), 12 deletions(-) diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 9a0c2f0dc..d73bc66e7 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1005,6 +1005,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 { @@ -1012,6 +1014,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); @@ -1019,12 +1023,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; 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 18fbc315d..38a1ff55d 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) { 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 a4c8b4ff8..3481827a7 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 @@ -251,6 +251,8 @@ class BrushedMotorInterruptHandler { .seconds = uint16_t( hardware.get_stopwatch_pulses(true) / 2600)}); } + hardware.set_stay_enabled( + static_cast(buffered_move.stay_engaged)); motor_state = ControlState::ACTIVE; buffered_move.start_encoder_position = hardware.get_encoder_pulses(); @@ -260,7 +262,6 @@ class BrushedMotorInterruptHandler { } // clear the old states hardware.reset_control(); - hardware.set_stay_enabled(false); timeout_ticks = 0; switch (buffered_move.stop_condition) { case MoveStopCondition::limit_switch: @@ -323,16 +324,6 @@ class BrushedMotorInterruptHandler { 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, diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp index a6acf1c2d..3813702bc 100644 --- a/include/motor-control/core/motor_messages.hpp +++ b/include/motor-control/core/motor_messages.hpp @@ -107,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; diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 69206e224..47e5fe190 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -37,6 +37,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { .duty_cycle = 50, .group_id = 0, .seq_id = 0, + .stay_engaged = 0, .stop_condition = MoveStopCondition::limit_switch}; test_objs.queue.try_write_isr(msg); @@ -49,6 +50,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 +69,7 @@ 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()); } } } @@ -79,6 +82,7 @@ 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()); } } @@ -87,6 +91,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { .duty_cycle = 50, .group_id = 0, .seq_id = 0, + .stay_engaged = 1, .stop_condition = MoveStopCondition::none}; test_objs.queue.try_write_isr(msg); @@ -99,6 +104,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 +131,7 @@ 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()); } } } @@ -140,6 +147,7 @@ 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()); } } GIVEN("A message to move") { @@ -226,6 +234,7 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") { .duty_cycle = 50, .group_id = 0, .seq_id = 0, + .stay_engaged = 1, .stop_condition = MoveStopCondition::limit_switch}; test_objs.queue.try_write_isr(msg); WHEN("Estop is pressed") { @@ -233,6 +242,7 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") { for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) { test_objs.handler.run_interrupt(); } + 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 +252,7 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") { std::get( test_objs.reporter.messages.front()); REQUIRE(err.error_code == can::ids::ErrorCode::estop_detected); + REQUIRE(test_objs.hw.get_stay_enabled()); } } } @@ -255,6 +266,7 @@ SCENARIO("labware dropped during grip move") { .duty_cycle = 50, .group_id = 0, .seq_id = 0, + .stay_engaged = 1, .stop_condition = MoveStopCondition::none}; test_objs.queue.try_write_isr(msg); WHEN("grip is complete") { @@ -263,6 +275,7 @@ SCENARIO("labware dropped during grip move") { 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") { @@ -303,6 +316,7 @@ SCENARIO("collision while homed") { .duty_cycle = 50, .group_id = 0, .seq_id = 0, + .stay_engaged = 0, .stop_condition = MoveStopCondition::limit_switch}; test_objs.queue.try_write_isr(msg); WHEN("home is complete") { From 821d49a9688cbcbd388bfd867afb3cc3b2ccf531 Mon Sep 17 00:00:00 2001 From: Ryan Howard Date: Mon, 7 Aug 2023 14:23:17 -0400 Subject: [PATCH 13/28] fix(rear-panel): update the pinout and timers for the new C1 rev (#712) --- rear-panel/firmware/led_hardware.c | 186 +++++++++++++++++++++++------ rear-panel/firmware/stm32g4xx_it.c | 8 +- 2 files changed, 158 insertions(+), 36 deletions(-) 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); } From 1b280e9f69aa1bf3ae13b43242d150215c94c6a9 Mon Sep 17 00:00:00 2001 From: Caila Marashaj <98041399+caila-marashaj@users.noreply.github.com> Date: Mon, 14 Aug 2023 16:50:58 -0400 Subject: [PATCH 14/28] modify tip presence sensor task (#713) --- gripper/core/tasks.cpp | 4 +- include/can/core/messages.hpp | 3 ++ include/gripper/core/tasks.hpp | 4 +- include/pipettes/core/sensor_tasks.hpp | 18 +++++-- .../sensors/core/message_handlers/sensors.hpp | 3 +- .../tasks/tip_presence_notification_task.hpp | 20 ++++--- pipettes/core/sensor_tasks.cpp | 52 ++++++++++++++----- pipettes/firmware/main.cpp | 16 +++--- pipettes/firmware/utility_configurations.cpp | 4 +- 9 files changed, 91 insertions(+), 33 deletions(-) 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/include/can/core/messages.hpp b/include/can/core/messages.hpp index d73bc66e7..3d348a6e2 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1239,11 +1239,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; } 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/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/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/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/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/main.cpp b/pipettes/firmware/main.cpp index 67e0303f5..55741bba9 100644 --- a/pipettes/firmware/main.cpp +++ b/pipettes/firmware/main.cpp @@ -142,15 +142,19 @@ 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{})); + 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{})); + } } 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_front != nullptr) { + static_cast( + sensor_queue_client.tip_notification_queue_front->try_write_isr( + sensors::tip_presence::TipStatusChangeDetected{})); + } } } diff --git a/pipettes/firmware/utility_configurations.cpp b/pipettes/firmware/utility_configurations.cpp index 2bf5c7aba..1cbb11d67 100644 --- a/pipettes/firmware/utility_configurations.cpp +++ b/pipettes/firmware/utility_configurations.cpp @@ -121,7 +121,7 @@ auto utility_configs::sensor_configurations() gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_12, + .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}}, .secondary = sensors::hardware::SensorHardwareConfiguration{ .sync_out = @@ -138,7 +138,7 @@ auto utility_configs::sensor_configurations() .tip_sense = gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_7, + .pin = GPIO_PIN_12, .active_setting = GPIO_PIN_RESET}}}; return pins; } From 0a453a747d5dd0892e1a06a237e472b079bf2d4f Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 16 Aug 2023 08:28:59 +0800 Subject: [PATCH 15/28] feat(motor-control): report brushed motor state (jaw state) (#714) * update mock brushed motor component * simplify control state to unhomed, force-ctrl, force-ctrl-home, and pos-ctrl * use new motor state to update handler logic & set stay engaged when we begin to execute the move * handle error properly (this happens only during idle move) * udpate tests * test error state recovery * add gripper jaw state headers * fix tests * no need to use reporter task for this * oops * fix typo * gotta implement those pure funcs --- include/bootloader/core/ids.h | 2 + include/can/core/ids.hpp | 2 + include/can/core/messages.hpp | 23 ++- .../brushed_motion_controller.hpp | 4 + .../brushed_motor_interrupt_handler.hpp | 155 +++++++------- .../core/motor_hardware_interface.hpp | 2 + include/motor-control/core/motor_messages.hpp | 5 + .../tasks/brushed_motion_controller_task.hpp | 8 + include/motor-control/core/tasks/messages.hpp | 2 +- include/motor-control/core/types.hpp | 7 + .../brushed_motor/brushed_motor_hardware.hpp | 4 + .../tests/mock_brushed_motor_components.hpp | 22 +- .../test_brushed_motor_interrupt_handler.cpp | 190 +++++++++++------- 13 files changed, 276 insertions(+), 150 deletions(-) diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h index 36150808d..71ee3a3dc 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 23857604e..3633df880 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -69,6 +69,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/messages.hpp b/include/can/core/messages.hpp index 3d348a6e2..2932cd005 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1501,6 +1501,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.. */ @@ -1516,6 +1536,7 @@ using ResponseMessageType = std::variant< BindSensorOutputResponse, GripperInfoResponse, TipActionResponse, PeripheralStatusResponse, BrushedMotorConfResponse, UpdateMotorPositionEstimationResponse, BaselineSensorResponse, - PushTipPresenceNotification, GetMotorUsageResponse>; + PushTipPresenceNotification, GetMotorUsageResponse, + GripperJawStateResponse>; } // namespace can::messages 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 38a1ff55d..d099adb0f 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -140,6 +140,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 3481827a7..f482be28c 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 @@ -24,16 +24,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 +88,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 +111,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 +122,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 +136,34 @@ 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) { - 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; + } else if (!error_handled) { + auto motor_state = hardware.get_motor_state(); + // has not reported an error yet + if (motor_state == BrushedMotorState::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); + error_handled = true; + } + } else if (motor_state != BrushedMotorState::UNHOMED) { + 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 == BrushedMotorState::FORCE_CONTROLLING + ? can::ids::ErrorCode::labware_dropped + : can::ids::ErrorCode::collision_detected; + cancel_and_clear_moves(err); + error_handled = true; + } } } } @@ -175,13 +171,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,15 +189,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()) { + if (!hardware.get_stay_enabled()) { + hardware.set_motor_state(BrushedMotorState::UNHOMED); + } cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning); - motor_state = ControlState::IDLE; } else if (tick < HOLDOFF_TICKS) { tick++; - } else if (motor_state == ControlState::ACTIVE) { + } else if (_has_active_move) { execute_active_move(); } else { execute_idle_move(); @@ -228,8 +226,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() @@ -239,9 +238,11 @@ 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) { + _has_active_move = queue.try_read_isr(&buffered_move); + if (_has_active_move) { + 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( @@ -251,9 +252,6 @@ class BrushedMotorInterruptHandler { .seconds = uint16_t( hardware.get_stopwatch_pulses(true) / 2600)}); } - hardware.set_stay_enabled( - static_cast(buffered_move.stay_engaged)); - motor_state = ControlState::ACTIVE; buffered_move.start_encoder_position = hardware.get_encoder_pulses(); } @@ -263,22 +261,31 @@ class BrushedMotorInterruptHandler { // clear the old states hardware.reset_control(); timeout_ticks = 0; + error_handled = false; + 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; @@ -288,7 +295,8 @@ 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; } } @@ -303,7 +311,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 { @@ -320,7 +328,7 @@ 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; } @@ -336,28 +344,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) { @@ -389,10 +388,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; @@ -404,5 +406,6 @@ 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; }; } // 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 5d2173415..9a979143d 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 3813702bc..bfe573397 100644 --- a/include/motor-control/core/motor_messages.hpp +++ b/include/motor-control/core/motor_messages.hpp @@ -134,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/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/messages.hpp b/include/motor-control/core/tasks/messages.hpp index 653e7a547..a34b6874b 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -45,7 +45,7 @@ using BrushedMotionControllerTaskMessage = std::variant< can::messages::AddBrushedLinearMoveRequest, can::messages::StopRequest, can::messages::ReadLimitSwitchRequest, can::messages::MotorPositionRequest, can::messages::SetGripperErrorToleranceRequest, - can::messages::GetMotorUsageRequest>; + can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest>; using BrushedMoveGroupTaskMessage = std::variant< std::monostate, can::messages::ClearAllMoveGroupsRequest, diff --git a/include/motor-control/core/types.hpp b/include/motor-control/core/types.hpp index cd58e28a2..ebc761340 100644 --- a/include/motor-control/core/types.hpp +++ b/include/motor-control/core/types.hpp @@ -38,4 +38,11 @@ struct __attribute__((__packed__)) UsageRequestSet { uint16_t eeprom_key; uint16_t type_key; uint16_t length; +}; + +enum class BrushedMotorState : uint8_t { + UNHOMED = 0x0, + FORCE_CONTROLLING_HOME = 0x1, + FORCE_CONTROLLING = 0x2, + POSITION_CONTROLLING = 0x3 }; \ No newline at end of file diff --git a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp index a3c3987b0..986c9fe1d 100644 --- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp +++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp @@ -88,6 +88,9 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { 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; debouncer::Debouncer estop = debouncer::Debouncer{}; @@ -101,6 +104,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { std::atomic cancel_request = false; const UsageEEpromConfig& eeprom_config; void* stopwatch_handle; + BrushedMotorState motor_state = BrushedMotorState::UNHOMED; }; }; // namespace motor_hardware diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp index dcc47eb8b..310ac3099 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 { bool cancel_request = false; 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/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 47e5fe190..83e73442d 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,14 +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, - .stay_engaged = 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 @@ -70,6 +87,8 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { 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); } } } @@ -83,17 +102,13 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { 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, - .stay_engaged = 1, - .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 @@ -132,6 +147,8 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { 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); } } } @@ -148,20 +165,15 @@ SCENARIO("Brushed motor interrupt handler handle move messages") { 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++) { @@ -207,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") { @@ -221,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()); } } } @@ -228,20 +246,14 @@ 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, - .stay_engaged = 1, - .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(); @@ -252,7 +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); - REQUIRE(test_objs.hw.get_stay_enabled()); + 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); + } } } } @@ -262,13 +281,7 @@ 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, - .stay_engaged = 1, - .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 @@ -285,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") { @@ -300,8 +314,8 @@ SCENARIO("labware dropped during grip move") { 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()); } } } @@ -311,14 +325,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, - .stay_engaged = 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 @@ -336,6 +343,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") { @@ -352,8 +360,8 @@ SCENARIO("collision while homed") { 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()); } } } @@ -361,17 +369,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++) { @@ -416,10 +417,11 @@ 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") { @@ -436,9 +438,61 @@ SCENARIO("A collision during position controlled move") { 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()); } } } } + +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 From de7435e789ada1aefd36259f94040e2dd8142039 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Tue, 29 Aug 2023 15:17:55 -0400 Subject: [PATCH 16/28] fix(motor-control): critical section for encoders (#715) When we do the encoder polling timer we really want the motor interrupt to not be able to start in the middle of the timer code, so let's wrap it in a critical section. --- .../common/core/freertos_synchronization.hpp | 22 +++++++++++++++---- .../motor_encoder_background_timer.hpp | 5 ++++- 2 files changed, 22 insertions(+), 5 deletions(-) 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/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp b/include/motor-control/core/stepper_motor/motor_encoder_background_timer.hpp index 1a467212a..ce1ee5882 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,6 +30,8 @@ class BackgroundTimer { auto stop() -> void { _timer.stop(); } auto callback() -> void { + auto critical_section = + freertos_synchronization::FreeRTOSCriticalSectionRAII(); if (!_interrupt_handler.has_active_move()) { // Refresh the overflow counter if nothing else is doing it // and update position flag if needed @@ -44,4 +47,4 @@ class BackgroundTimer { ot_utils::freertos_timer::FreeRTOSTimer _timer; }; -} // namespace motor_encoder \ No newline at end of file +} // namespace motor_encoder From 16e738287fa4de839bc16e8c38a6810054ab1a47 Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Fri, 1 Sep 2023 15:36:55 -0400 Subject: [PATCH 17/28] feat(all): allow for up to 12 steps in each move group (#717) * Allow for up to 12 steps within each move group * Drop from 6 to 3 maximum move groups --- include/motor-control/core/tasks/brushed_move_group_task.hpp | 4 ++-- include/motor-control/core/tasks/move_group_task.hpp | 4 ++-- include/pipettes/core/tasks/move_group_task.hpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) 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 Date: Tue, 5 Sep 2023 14:55:49 -0400 Subject: [PATCH 18/28] chore: add pipette 96 revision d2 (#718) --- bootloader/firmware/stm32G4/CMakeLists.txt | 4 ++-- pipettes/firmware/CMakeLists.txt | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bootloader/firmware/stm32G4/CMakeLists.txt b/bootloader/firmware/stm32G4/CMakeLists.txt index eb2d07902..27520bfc5 100644 --- a/bootloader/firmware/stm32G4/CMakeLists.txt +++ b/bootloader/firmware/stm32G4/CMakeLists.txt @@ -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/pipettes/firmware/CMakeLists.txt b/pipettes/firmware/CMakeLists.txt index 9554af8e2..dc4b5b0e2 100644 --- a/pipettes/firmware/CMakeLists.txt +++ b/pipettes/firmware/CMakeLists.txt @@ -111,9 +111,9 @@ foreach_revision( 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) From ae4a7ff040eb3c7f6488f58f21b896c8b2df28e7 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 12 Sep 2023 05:22:26 +0800 Subject: [PATCH 19/28] fix(gripper): add GripperJawStateRequest to gripper can task & do not disable brushed motor if it was set to stay engaged (#719) * add GripperJawStateRequest to gripper can task * no disengaging motor in stop request --- include/can/core/message_handlers/motion.hpp | 3 ++- include/gripper/core/can_task.hpp | 2 +- .../core/brushed_motor/brushed_motion_controller.hpp | 7 +++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp index d4d367999..d8575df3c 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/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index f76f0c960..0b560acfe 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -48,7 +48,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/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp index d099adb0f..a7a40cacd 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -105,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(); + if (hardware.is_timer_interrupt_running()) { + hardware.request_cancel(); + } } } From 0f1df6a116ff88a7a1c4b3d3273bf45bec2fc899 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 20 Sep 2023 04:04:14 +0800 Subject: [PATCH 20/28] report encoder pulses when something goes wrong (#722) --- .../brushed_motor_interrupt_handler.hpp | 21 +++++++++++++++++-- .../test_brushed_motor_interrupt_handler.cpp | 21 ++++++++++++++++--- 2 files changed, 37 insertions(+), 5 deletions(-) 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 f482be28c..8cfce6c04 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 @@ -138,10 +138,10 @@ class BrushedMotorInterruptHandler { update_and_start_move(); } else if (!error_handled) { auto motor_state = hardware.get_motor_state(); + auto pulses = hardware.get_encoder_pulses(); // has not reported an error yet if (motor_state == BrushedMotorState::POSITION_CONTROLLING) { - int32_t move_delta = - hardware.get_encoder_pulses() - hold_encoder_position; + 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 @@ -149,6 +149,7 @@ class BrushedMotorInterruptHandler { if (move_delta > error_conf.unwanted_movement_threshold) { cancel_and_clear_moves( can::ids::ErrorCode::collision_detected); + report_position(pulses); error_handled = true; } } else if (motor_state != BrushedMotorState::UNHOMED) { @@ -162,6 +163,7 @@ class BrushedMotorInterruptHandler { ? can::ids::ErrorCode::labware_dropped : can::ids::ErrorCode::collision_detected; cancel_and_clear_moves(err); + report_position(pulses); error_handled = true; } } @@ -301,6 +303,21 @@ class BrushedMotorInterruptHandler { } } + 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 diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 83e73442d..793af429b 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -309,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.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); } } } @@ -354,7 +359,7 @@ 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()); @@ -362,6 +367,11 @@ SCENARIO("collision while homed") { 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 == 40000); } } } @@ -432,7 +442,7 @@ SCENARIO("A collision during position controlled 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()); @@ -440,6 +450,11 @@ SCENARIO("A collision during position controlled move") { 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); } } } From 7aac8b9b9895751d06cd9107607f632685a33ea3 Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Tue, 19 Sep 2023 17:48:57 -0400 Subject: [PATCH 21/28] feat(pipettes): require overpressure for 500ms before error (#723) * echo the pressure if an overpressure happens * Added a time requirement to pipette overpressure detection --- .../sensors/core/tasks/pressure_driver.hpp | 37 +++++++++++++++++-- 1 file changed, 33 insertions(+), 4 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 412fdd155..06f4be486 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{ @@ -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 = 500; + 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; From 50b3ee699ceb5ae2cb0c16d43c5a2377f33d957b Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Tue, 19 Sep 2023 17:49:22 -0400 Subject: [PATCH 22/28] feat(push): add option to specify ssh key when pushing firmware to ot3 (#724) --- push | 8 ++++++++ 1 file changed, 8 insertions(+) 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(): From fb655801e8883c5872dc38164c63cd1e00bcf8e8 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 20 Sep 2023 07:39:02 +0800 Subject: [PATCH 23/28] feat(gripper): allow the encoder value to debounce if deviated from expected val (#725) There's potential noise on the gripper jaw encoder line that causes the gripper to falsely report a move has failed. We're now confirming the encoder value has deviated from the expected val twice consecutively when executing an idle move before reporting it has failed. --- common/tests/test_debounce.cpp | 68 +++++++++++++++++++ include/common/core/debounce.hpp | 20 +++++- .../brushed_motor_interrupt_handler.hpp | 47 ++++++++----- .../test_brushed_motor_interrupt_handler.cpp | 43 +++++++++++- 4 files changed, 157 insertions(+), 21 deletions(-) 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/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/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index 8cfce6c04..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" @@ -146,7 +147,10 @@ class BrushedMotorInterruptHandler { // 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) { + 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); @@ -154,9 +158,13 @@ class BrushedMotorInterruptHandler { } } else if (motor_state != BrushedMotorState::UNHOMED) { auto pulses = hardware.get_encoder_pulses(); - if (!is_idle && pulses >= 0 && + enc_errored.debounce_update( + !is_idle && pulses >= 0 && std::abs(pulses - hold_encoder_position) > - error_conf.unwanted_movement_threshold) { + 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 @@ -241,22 +249,23 @@ class BrushedMotorInterruptHandler { void update_and_start_move() { _has_active_move = queue.try_read_isr(&buffered_move); - if (_has_active_move) { - 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 (!_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); } @@ -264,6 +273,7 @@ class BrushedMotorInterruptHandler { hardware.reset_control(); timeout_ticks = 0; error_handled = false; + enc_errored.reset(); hardware.set_stay_enabled( static_cast(buffered_move.stay_engaged)); switch (buffered_move.stop_condition) { @@ -424,5 +434,6 @@ class BrushedMotorInterruptHandler { 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/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 793af429b..7b2cbbf3a 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -435,10 +435,11 @@ SCENARIO("A collision during position controlled move") { } 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 @@ -460,6 +461,44 @@ SCENARIO("A collision during position controlled move") { } } +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); + + 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, From ad507bfb45151334bd3f11d3158d2e0255130205 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Thu, 21 Sep 2023 01:18:14 +0800 Subject: [PATCH 24/28] fix(gripper): gripper simulator (#726) --- .../simulation/brushed_motor_interrupt_driver.hpp | 4 ++-- include/motor-control/simulation/sim_motor_hardware_iface.hpp | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) 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 2a666a95f..2a8dd67cc 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 = false; + BrushedMotorState motor_state = BrushedMotorState::UNHOMED; motor_hardware::UsageEEpromConfig eeprom_config{ std::array{UsageRequestSet{ .eeprom_key = 0, From 982f0ab297fcfa0fd75daafdf9da99f5322e3a60 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Thu, 21 Sep 2023 15:47:46 -0400 Subject: [PATCH 25/28] fix(sensors): Reduce overpressure time to 200ms (#727) * fix(sensors): reduce overpressure time to 250ms * reduce further to 200 --- include/sensors/core/tasks/pressure_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 06f4be486..7e4540814 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -471,7 +471,7 @@ class MMR920C04 { * 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 = 500; + static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; mmr920C04::MeasurementRate measurement_mode_rate = mmr920C04::MeasurementRate::MEASURE_4; From 25a8e1dbb6188b8a76ddde7895337fcd9e713bcf Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 4 Oct 2023 13:31:20 -0400 Subject: [PATCH 26/28] fix(pipettes): changes to enable the correct sync line ports for the 96-channel (#728) --- include/sensors/core/tasks/pressure_driver.hpp | 2 +- pipettes/firmware/utility_gpio.c | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 7e4540814..5b3a62d4b 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -400,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, diff --git a/pipettes/firmware/utility_gpio.c b/pipettes/firmware/utility_gpio.c index aea32ed6d..2d10c5ec5 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(); } } @@ -156,7 +158,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 +170,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; From 01859add10bdeb811d0da5a120335d307199cd06 Mon Sep 17 00:00:00 2001 From: Caila Marashaj <98041399+caila-marashaj@users.noreply.github.com> Date: Mon, 16 Oct 2023 16:20:16 -0400 Subject: [PATCH 27/28] only autmomatically notify about low throughput tip changes (#730) --- pipettes/firmware/main.cpp | 11 ----------- pipettes/firmware/utility_configurations.cpp | 8 ++++---- pipettes/firmware/utility_gpio.c | 17 +++-------------- 3 files changed, 7 insertions(+), 29 deletions(-) diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp index 55741bba9..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(); @@ -147,14 +144,6 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { sensor_queue_client.tip_notification_queue_rear->try_write_isr( sensors::tip_presence::TipStatusChangeDetected{})); } - } else if (ok_for_secondary && - GPIO_Pin == - pins_for_sensor.secondary.value().tip_sense.value().pin) { - if (sensor_queue_client.tip_notification_queue_front != nullptr) { - static_cast( - sensor_queue_client.tip_notification_queue_front->try_write_isr( - sensors::tip_presence::TipStatusChangeDetected{})); - } } } diff --git a/pipettes/firmware/utility_configurations.cpp b/pipettes/firmware/utility_configurations.cpp index 1cbb11d67..8edb42cda 100644 --- a/pipettes/firmware/utility_configurations.cpp +++ b/pipettes/firmware/utility_configurations.cpp @@ -122,7 +122,7 @@ 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}}, .secondary = sensors::hardware::SensorHardwareConfiguration{ .sync_out = {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -139,7 +139,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}}}; 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 2d10c5ec5..e5c76a8d6 100644 --- a/pipettes/firmware/utility_gpio.c +++ b/pipettes/firmware/utility_gpio.c @@ -26,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) { @@ -34,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); } @@ -53,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); - } - } /** From cd2ae0206ffc397b1dce1802720434322b1987a0 Mon Sep 17 00:00:00 2001 From: Frank Sinapi Date: Thu, 19 Oct 2023 14:22:46 -0400 Subject: [PATCH 28/28] fix(motor-control): monitor encoder during empty moves (#731) --- .../core/stepper_motor/motor_encoder_background_timer.hpp | 2 +- .../core/stepper_motor/motor_interrupt_handler.hpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) 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 ce1ee5882..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 @@ -32,7 +32,7 @@ class BackgroundTimer { auto callback() -> void { auto critical_section = freertos_synchronization::FreeRTOSCriticalSectionRAII(); - if (!_interrupt_handler.has_active_move()) { + if (!_interrupt_handler.is_moving()) { // Refresh the overflow counter if nothing else is doing it // and update position flag if needed std::ignore = _interrupt_handler.check_for_stall(); 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 9beb5a253..ee57dbd3d 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -586,6 +586,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(