From b63fb4dc6f5fd5eb963236e2ec6ce251df4fd84c Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Tue, 2 Jul 2024 15:04:06 -0400 Subject: [PATCH 1/9] remove USE_SENSOR_BUFFER from fw source code --- CMakePresets.json | 22 ------ gripper/firmware/interfaces_z_motor.cpp | 6 -- .../can/core/message_handlers/move_group.hpp | 8 -- include/gripper/core/can_task.hpp | 10 --- .../core/stepper_motor/motion_controller.hpp | 76 +------------------ .../core/stepper_motor/motor.hpp | 4 - .../stepper_motor/motor_interrupt_handler.hpp | 65 ---------------- include/motor-control/core/tasks/messages.hpp | 21 ----- .../core/tasks/motion_controller_task.hpp | 10 --- .../core/tasks/move_group_task.hpp | 12 --- include/pipettes/core/dispatch_builder.hpp | 11 --- include/pipettes/core/interfaces.hpp | 5 -- include/pipettes/firmware/interfaces.hpp | 7 -- include/pipettes/firmware/interfaces_g4.hpp | 7 -- .../sensors/core/tasks/capacitive_driver.hpp | 24 +++--- pipettes/simulator/interfaces.cpp | 12 --- 16 files changed, 12 insertions(+), 288 deletions(-) diff --git a/CMakePresets.json b/CMakePresets.json index 44b3d417d..de644b8f5 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -35,17 +35,6 @@ "binaryDir": "${sourceDir}/build-cross", "inherits": "cross-no-directory-reqs" }, - { - "name": "cross-sensor-buffer", - "displayName": "STM32 G4 OT-3 cross-compilation with sensor data buffers", - "description": "Build application firmware for OT-3 systems that use STM32, for flashing onto boards", - "installDir": "${sourceDir}/dist-sensor", - "binaryDir": "${sourceDir}/build-cross-sensor", - "cacheVariables": { - "USE_SENSOR_MOVE": true - }, - "inherits": "cross" - }, { "name": "host", "displayName": "STM32 OT-3 host compilation for tests", @@ -99,17 +88,6 @@ "firmware-images" ] }, - { - "name": "firmware-g4-sensors", - "displayName": "All G4 Firmwares With Sensor Data Buffers", - "description": "Environment to build all g4 firmware - see firmware-l5", - "configurePreset": "cross-sensor-buffer", - "jobs": 4, - "targets": [ - "firmware-applications", - "firmware-images" - ] - }, { "name": "pipettes", "displayName": "pipettes binaries", diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp index 69405dfde..c9c43c9a9 100644 --- a/gripper/firmware/interfaces_z_motor.cpp +++ b/gripper/firmware/interfaces_z_motor.cpp @@ -150,14 +150,8 @@ static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{ /** * The pending move queue */ -#ifdef USE_SENSOR_MOVE -static freertos_message_queue::FreeRTOSMessageQueue< - motor_messages::SensorSyncMove> - motor_queue("Motor Queue"); -#else static freertos_message_queue::FreeRTOSMessageQueue motor_queue("Motor Queue"); -#endif static freertos_message_queue::FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest> diff --git a/include/can/core/message_handlers/move_group.hpp b/include/can/core/message_handlers/move_group.hpp index 55b5f2492..f4e597997 100644 --- a/include/can/core/message_handlers/move_group.hpp +++ b/include/can/core/message_handlers/move_group.hpp @@ -11,18 +11,10 @@ using namespace can::messages; template class MoveGroupHandler { public: -#ifdef USE_SENSOR_MOVE - using MessageType = - std::variant; -#else using MessageType = std::variant; -#endif MoveGroupHandler(Client &task_client) : task_client{task_client} {} MoveGroupHandler(const MoveGroupHandler &) = delete; diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index e3b3f294c..bffe2cbef 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -19,22 +19,12 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, can::messages::WriteMotorCurrentRequest>; -#ifdef USE_SENSOR_MOVE -using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< - can::message_handlers::move_group::MoveGroupHandler, - can::messages::AddLinearMoveRequest, - can::messages::ClearAllMoveGroupsRequest, - can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, - can::messages::HomeRequest, can::messages::StopRequest, - can::messages::AddSensorMoveRequest>; -#else using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler, can::messages::AddLinearMoveRequest, can::messages::ClearAllMoveGroupsRequest, can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; -#endif using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motion::MotionHandler, can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index 9cd1e9c4f..e911cdc9b 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -24,12 +24,7 @@ using namespace motor_hardware; template class MotionController { public: - using GenericQueue = -#ifdef USE_SENSOR_MOVE - freertos_message_queue::FreeRTOSMessageQueue; -#else - freertos_message_queue::FreeRTOSMessageQueue; -#endif + using GenericQueue = freertos_message_queue::FreeRTOSMessageQueue; using UpdatePositionQueue = freertos_message_queue::FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest>; MotionController(lms::LinearMotionSystemConfig lms_config, @@ -63,73 +58,6 @@ class MotionController { -> const lms::LinearMotionSystemConfig& { return linear_motion_sys_config; } -#ifdef USE_SENSOR_MOVE - void move(const can::messages::AddSensorMoveRequest& 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); - SensorSyncMove msg{ - can_msg.message_index, - can_msg.duration, - velocity_steps, - acceleration_steps, - can_msg.group_id, - can_msg.seq_id, - can_msg.request_stop_condition, - 0, - hardware.get_usage_eeprom_config().get_distance_key(), - can_msg.sensor_id, - can_msg.sensor_type}; - if (!enabled) { - enable_motor(); - } - queue.try_write(msg); - } - - void move(const can::messages::AddLinearMoveRequest& 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); - SensorSyncMove msg{ - .message_index = can_msg.message_index, - .duration = can_msg.duration, - .velocity = velocity_steps, - .acceleration = acceleration_steps, - .group_id = can_msg.group_id, - .seq_id = can_msg.seq_id, - .stop_condition = can_msg.request_stop_condition, - .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), - .sensor_id = can::ids::SensorId::UNUSED, - .sensor_type = can::ids::SensorType::UNUSED}; - if (!enabled) { - enable_motor(); - } - queue.try_write(msg); - } - - void move(const can::messages::HomeRequest& can_msg) { - steps_per_tick velocity_steps = - fixed_point_multiply(steps_per_mm, can_msg.velocity); - SensorSyncMove msg{ - .message_index = can_msg.message_index, - .duration = can_msg.duration, - .velocity = velocity_steps, - .acceleration = 0, - .group_id = can_msg.group_id, - .seq_id = can_msg.seq_id, - .stop_condition = - static_cast(MoveStopCondition::limit_switch), - .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), - .sensor_id = can::ids::SensorId::UNUSED, - .sensor_type = can::ids::SensorType::UNUSED}; - if (!enabled) { - enable_motor(); - } - queue.try_write(msg); - } -#else void move(const can::messages::AddLinearMoveRequest& can_msg) { steps_per_tick velocity_steps = @@ -170,8 +98,6 @@ class MotionController { queue.try_write(msg); } -#endif - [[nodiscard]] auto update_position( const can::messages::UpdateMotorPositionEstimationRequest& can_msg) -> bool { diff --git a/include/motor-control/core/stepper_motor/motor.hpp b/include/motor-control/core/stepper_motor/motor.hpp index b9ff17ee6..20b111fef 100644 --- a/include/motor-control/core/stepper_motor/motor.hpp +++ b/include/motor-control/core/stepper_motor/motor.hpp @@ -18,11 +18,7 @@ using namespace freertos_message_queue; template struct Motor { -#ifdef USE_SENSOR_MOVE - using GenericQueue = FreeRTOSMessageQueue; -#else using GenericQueue = FreeRTOSMessageQueue; -#endif using UpdatePositionQueue = FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest>; diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 5196a7f2d..7702692f7 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -10,13 +10,10 @@ #include "motor-control/core/stall_check.hpp" #include "motor-control/core/tasks/move_status_reporter_task.hpp" #include "motor-control/core/tasks/tmc_motor_driver_common.hpp" -#ifdef USE_SENSOR_MOVE #ifdef PIPETTE_TYPE_DEFINE -#include "pipettes/core/sensor_tasks.hpp" #else #include "gripper/core/tasks.hpp" #endif // PIPETTE_TYPE_DEFINE -#endif // USE_SENSOR_MOVE namespace motor_handler { using namespace motor_messages; @@ -376,52 +373,12 @@ class MotorInterruptHandler { */ return tick_count < buffered_move.duration; } -#ifdef USE_SENSOR_MOVE - auto send_bind_message(can::ids::SensorType sensor_type, - can::ids::SensorId sensor_id, uint8_t binding) - -> void { - auto msg = can::messages::BindSensorOutputRequest{ - .message_index = buffered_move.message_index, - .sensor = sensor_type, - .sensor_id = sensor_id, - .binding = binding}; - if (sensor_type == can::ids::SensorType::pressure) { - if (sensor_id == can::ids::SensorId::S0) { - send_to_pressure_sensor_queue_rear(msg); - } else { - send_to_pressure_sensor_queue_front(msg); - } - } else if (sensor_type == can::ids::SensorType::capacitive) { - if (sensor_id == can::ids::SensorId::S0) { - send_to_capacitive_sensor_queue_rear(msg); - } else { - send_to_capacitive_sensor_queue_front(msg); - } - } - } -#endif void update_move() { _has_active_move = move_queue.try_read_isr(&buffered_move); if (_has_active_move) { hardware.enable_encoder(); buffered_move.start_encoder_position = hardware.get_encoder_pulses(); -#ifdef USE_SENSOR_MOVE - if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { - if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { - send_bind_message(buffered_move.sensor_type, - can::ids::SensorId::S0, - buffered_move.binding_flags); - send_bind_message(buffered_move.sensor_type, - can::ids::SensorId::S1, - buffered_move.binding_flags); - } else { - send_bind_message(buffered_move.sensor_type, - buffered_move.sensor_id, - buffered_move.binding_flags); - } - } -#endif } if (set_direction_pin()) { hardware.positive_direction(); @@ -646,28 +603,6 @@ class MotorInterruptHandler { hardware.set_step_tracker( static_cast(position_tracker >> 31)); } -#ifdef USE_SENSOR_MOVE - void send_to_pressure_sensor_queue_rear( - can::messages::BindSensorOutputRequest& m) { - sensor_tasks::get_queues().send_pressure_sensor_queue_rear_isr(m); - } - void send_to_pressure_sensor_queue_front( - can::messages::BindSensorOutputRequest& m) { - // send to both queues, they will handle their own gating based on - // sensor id - sensor_tasks::get_queues().send_pressure_sensor_queue_front_isr(m); - } - void send_to_capacitive_sensor_queue_rear( - can::messages::BindSensorOutputRequest& m) { - sensor_tasks::get_queues().send_capacitive_sensor_queue_rear_isr(m); - } - void send_to_capacitive_sensor_queue_front( - can::messages::BindSensorOutputRequest& m) { - // send to both queues, they will handle their own gating based on - // sensor id - sensor_tasks::get_queues().send_pressure_sensor_queue_front_isr(m); - } -#endif uint64_t tick_count = 0x0; static constexpr const q31_31 tick_flag = 0x80000000; static constexpr const uint64_t overflow_flag = 0x8000000000000000; diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp index 759a28308..118e44011 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -6,26 +6,6 @@ namespace motor_control_task_messages { -#ifdef USE_SENSOR_MOVE -using MotionControlTaskMessage = std::variant< - std::monostate, can::messages::AddLinearMoveRequest, - can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, - can::messages::GetMotionConstraintsRequest, - can::messages::SetMotionConstraints, can::messages::StopRequest, - can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest, - can::messages::HomeRequest, - can::messages::UpdateMotorPositionEstimationRequest, - can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest, - can::messages::AddSensorMoveRequest>; - -using MoveGroupTaskMessage = - std::variant; -#else using MotionControlTaskMessage = std::variant< std::monostate, can::messages::AddLinearMoveRequest, can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, @@ -42,7 +22,6 @@ using MoveGroupTaskMessage = can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; -#endif using MotorDriverTaskMessage = std::variant; -#else using MoveGroupType = move_group::MoveGroupManager; -#endif using TaskMessage = motor_control_task_messages::MoveGroupTaskMessage; @@ -126,12 +120,6 @@ class MoveGroupMessageHandler { mc_client.send_motion_controller_queue(m); } -#ifdef USE_SENSOR_MOVE - void visit_move(const can::messages::AddSensorMoveRequest& m) { - mc_client.send_motion_controller_queue(m); - } -#endif - MoveGroupType& move_groups; MotionControllerClient& mc_client; CanClient& can_client; diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp index e5d0950c6..3ed5d6471 100644 --- a/include/pipettes/core/dispatch_builder.hpp +++ b/include/pipettes/core/dispatch_builder.hpp @@ -39,16 +39,6 @@ using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::GearWriteMotorDriverRegister, can::messages::GearWriteMotorCurrentRequest>; -#ifdef USE_SENSOR_MOVE -using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< - can::message_handlers::move_group::MoveGroupHandler< - linear_motor_tasks::QueueClient>, - can::messages::AddLinearMoveRequest, - can::messages::ClearAllMoveGroupsRequest, - can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, - can::messages::HomeRequest, can::messages::StopRequest, - can::messages::AddSensorMoveRequest>; -#else using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< linear_motor_tasks::QueueClient>, @@ -56,7 +46,6 @@ using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::ClearAllMoveGroupsRequest, can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; -#endif using GearMoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< gear_move_group_handler::GearMoveGroupHandler< diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp index 999e5f7ea..955062674 100644 --- a/include/pipettes/core/interfaces.hpp +++ b/include/pipettes/core/interfaces.hpp @@ -6,13 +6,8 @@ #include "motor-control/core/tasks/motor_hardware_task.hpp" namespace interfaces { -#ifdef USE_SENSOR_MOVE -using MoveQueue = freertos_message_queue::FreeRTOSMessageQueue< - motor_messages::SensorSyncMove>; -#else using MoveQueue = freertos_message_queue::FreeRTOSMessageQueue; -#endif using GearMoveQueue = freertos_message_queue::FreeRTOSMessageQueue; using MotionControlType = diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp index 2ff87fcc4..cdcfcdfa3 100644 --- a/include/pipettes/firmware/interfaces.hpp +++ b/include/pipettes/firmware/interfaces.hpp @@ -19,17 +19,10 @@ namespace interfaces { -#ifdef USE_SENSOR_MOVE -template -using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, - motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; -#else template using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, motor_hardware::MotorHardware>; -#endif template using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp index ecf9c0813..922f9f21a 100644 --- a/include/pipettes/firmware/interfaces_g4.hpp +++ b/include/pipettes/firmware/interfaces_g4.hpp @@ -28,17 +28,10 @@ namespace interfaces { -#ifdef USE_SENSOR_MOVE -template -using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, - motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; -#else template using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, motor_hardware::MotorHardware>; -#endif template using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 4fd5ed3ef..3dd3d5cf6 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -204,7 +204,6 @@ class FDC1004 { } void send_accumulated_sensor_data(uint32_t message_index) { -#ifdef USE_SENSOR_MOVE for (int i = 0; i < sensor_buffer_index; i++) { // send over buffer adn then clear buffer values can_client.send_can_message( @@ -224,9 +223,6 @@ class FDC1004 { can_client.send_can_message( can::ids::NodeId::host, can::messages::Acknowledgment{.message_index = message_index}); -#else - std::ignore = message_index; -#endif } void handle_fdc_response(i2c::messages::TransactionResponse &m) { @@ -296,14 +292,7 @@ class FDC1004 { } if (echoing) { -#ifdef USE_SENSOR_MOVE - // send a response with 9999 to make an overload of the buffer - // visible - if (sensor_buffer_index < SENSOR_BUFFER_SIZE) { - (*sensor_buffer).at(sensor_buffer_index) = capacitance; - sensor_buffer_index++; - } -#else + sensor_buffer_log(capacitance); can_client.send_can_message( can::ids::NodeId::host, can::messages::ReadFromSensorResponse{ @@ -312,7 +301,15 @@ class FDC1004 { .sensor_id = sensor_id, .sensor_data = convert_to_fixed_point(capacitance, S15Q16_RADIX)}); -#endif + } + } + + auto sensor_buffer_log(float data) -> void { + sensor_buffer->at(sensor_buffer_index) = data; + sensor_buffer_index++; + if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { + sensor_buffer_index = 0; + crossed_buffer_index = true; } } @@ -533,6 +530,7 @@ class FDC1004 { } std::array *sensor_buffer; uint16_t sensor_buffer_index = 0; + bool crossed_buffer_index = false; }; // end of FDC1004 class diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp index 8d0301c85..6e6948801 100644 --- a/pipettes/simulator/interfaces.cpp +++ b/pipettes/simulator/interfaces.cpp @@ -66,15 +66,9 @@ auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, MotorInterruptHandlerType& handler) -#ifdef USE_SENSOR_MOVE - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, - sim_motor_hardware_iface::SimMotorHardwareIface> { -#else -> motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { -#endif return motor_interrupt_driver::MotorInterruptDriver( queues.plunger_queue, handler, hw, queues.plunger_update_queue); } @@ -83,15 +77,9 @@ auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, MotorInterruptHandlerType& handler) -#ifdef USE_SENSOR_MOVE - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, - sim_motor_hardware_iface::SimMotorHardwareIface> { -#else -> motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { -#endif return motor_interrupt_driver::MotorInterruptDriver( queues.plunger_queue, handler, hw, queues.plunger_update_queue); } From b3dba447a79377d97933266581db735bac47af24 Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Tue, 2 Jul 2024 15:06:03 -0400 Subject: [PATCH 2/9] remove sensors arg from push --- push | 31 ++++++++----------------------- 1 file changed, 8 insertions(+), 23 deletions(-) diff --git a/push b/push index 9eb3939f3..55d028d7d 100755 --- a/push +++ b/push @@ -123,10 +123,8 @@ def _update_shortsha(scp, host, json_data_path, targets): # copy updated subsystem data to the robot _scp_to_robot(scp, host, json_data_path, _ROBOT_MANIFEST_FILE_PATH) -def _transfer_firmware(host, repo_path, scp, ssh, sensors, targets): +def _transfer_firmware(host, repo_path, scp, ssh, targets): dist_dir = "dist" - if sensors: - dist_dir = dist_dir+"-sensor" apps_path = os.path.join(repo_path, dist_dir, 'applications') with _controlled_tempdir() as td: local_zip_path = os.path.join(td, 'fw.zip') @@ -139,15 +137,10 @@ def _transfer_firmware(host, repo_path, scp, ssh, sensors, targets): _ssh(ssh, host, 'unzip -o {zip_path} -d /usr/lib/firmware/'.format(zip_path=robot_zip_path)) _ssh(ssh, host, 'rm {zip_path}'.format(zip_path=robot_zip_path)) -def _prep_firmware(repo_path, cmake, sensors, targets): +def _prep_firmware(repo_path, cmake, targets): working_dir = "./build-cross" full_build_preset = "firmware-g4" - if sensors: - working_dir = working_dir+"-sensor" - full_build_preset = full_build_preset+"-sensors" - # if sensors is true, disregard targets within the scope of this function - targets = None if targets: for target in targets: _cmd([cmake, '--build', 'build-cross', '--target', f'{target}-images'], cwd=repo_path) @@ -187,26 +180,23 @@ def _check_targets(targets): def _restart_robot(host, ssh): _ssh(ssh, host, 'nohup systemctl restart opentrons-robot-server &') -def _do_push(host, repo_path, build, restart, sensors, targets): +def _do_push(host, repo_path, build, restart, targets): ssh, scp, cmake = _find_utils() if targets: targets = _check_targets(targets) if build: - _prep_firmware(repo_path, cmake, sensors, targets) + _prep_firmware(repo_path, cmake, targets) with _prep_robot(host, ssh): - _transfer_firmware(host, repo_path, scp, ssh, sensors, targets) + _transfer_firmware(host, repo_path, scp, ssh, targets) if restart: _restart_robot(host, ssh) -def push(host, repo_path=None, build=True, restart=True, sensors=False, targets=[]): - # sensors is logically independent from targets here- if you specify both: - # - all hex files under firmware-g4-sensors will be built and installed, but - # - only the targets selected will actually be copied over to the robot +def push(host, repo_path=None, build=True, restart=True, targets=[]): repo = repo_path or os.dirname(__file__) try: - _do_push(host, repo, build, restart, sensors, targets) + _do_push(host, repo, build, restart, targets) return 0 except subprocess.CalledProcessError as e: print( @@ -223,7 +213,7 @@ 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, args.sensors, args.targets) + return push(args.host, os.path.abspath(args.repo_path), not args.no_build, not args.no_restart, args.targets) def _arg_parser(parent=None): parents = [] @@ -254,11 +244,6 @@ def _arg_parser(parent=None): type=str, help='Private SSH key to use' ) - parser.add_argument( - '--sensors', - action='store_true', - help='Private SSH key to use' - ) parser.add_argument( '--targets', nargs='*' From 3b15ae04f47bcef557eb6738ff5e9d741aa7b9db Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Tue, 2 Jul 2024 15:42:28 -0400 Subject: [PATCH 3/9] use constexpr for undefined sensor_buff_size --- include/common/core/sensor_buffer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/common/core/sensor_buffer.hpp b/include/common/core/sensor_buffer.hpp index ea4aabd85..b486ed119 100644 --- a/include/common/core/sensor_buffer.hpp +++ b/include/common/core/sensor_buffer.hpp @@ -1,5 +1,5 @@ #pragma once #ifndef SENSOR_BUFF_SIZE -#define SENSOR_BUFF_SIZE 1 +constexpr size_t SENSOR_BUFF_SIZE = 1; #endif constexpr size_t SENSOR_BUFFER_SIZE = SENSOR_BUFF_SIZE; From 4e36eed7f2d27a740c138216a0e754dbe247a1e0 Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Tue, 2 Jul 2024 15:44:34 -0400 Subject: [PATCH 4/9] get rid of rest of use_sensor_move references --- common/core/CMakeLists.txt | 4 ---- gripper/core/CMakeLists.txt | 3 --- pipettes/core/CMakeLists.txt | 3 --- 3 files changed, 10 deletions(-) diff --git a/common/core/CMakeLists.txt b/common/core/CMakeLists.txt index 1f6564e5a..292f4d36d 100644 --- a/common/core/CMakeLists.txt +++ b/common/core/CMakeLists.txt @@ -54,11 +54,7 @@ function(add_revision) message(STATUS "add_revision has target ${_ar_TARGET} and revision ${_ar_REVISION}") string(SUBSTRING ${_ar_REVISION} 0 1 PRIMARY_REVISION) string(SUBSTRING ${_ar_REVISION} 1 1 SECONDARY_REVISION) - if (${USE_SENSOR_MOVE}) - set(TERTIARY_FLAG "'1'") - else() set(TERTIARY_FLAG "0x00") - endif() configure_file(${CMAKE_SOURCE_DIR}/common/core/revision.c.in ${CMAKE_CURRENT_BINARY_DIR}/${_ar_TARGET}_revision.c) diff --git a/gripper/core/CMakeLists.txt b/gripper/core/CMakeLists.txt index c939e9121..50a9adab7 100644 --- a/gripper/core/CMakeLists.txt +++ b/gripper/core/CMakeLists.txt @@ -1,7 +1,4 @@ function(target_gripper_core TARGET) - if(${USE_SENSOR_MOVE}) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) - endif() target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true) target_sources(${TARGET} PUBLIC diff --git a/pipettes/core/CMakeLists.txt b/pipettes/core/CMakeLists.txt index 29daf4dd4..70bc6fd53 100644 --- a/pipettes/core/CMakeLists.txt +++ b/pipettes/core/CMakeLists.txt @@ -1,7 +1,4 @@ function(target_pipettes_core_common TARGET REVISION) - if(${USE_SENSOR_MOVE}) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) - endif() target_sources(${TARGET} PUBLIC ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/configs.cpp ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/peripheral_tasks.cpp From 27f226f7fe69690049616cfbf560656b8232ac9b Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Tue, 2 Jul 2024 16:15:42 -0400 Subject: [PATCH 5/9] remove sensor github workflows --- .github/workflows/build_sensor_fw.yaml | 44 ------- .../cross-compile-special-sensors.yaml | 118 ------------------ .../sensors/core/tasks/capacitive_driver.hpp | 18 +-- 3 files changed, 9 insertions(+), 171 deletions(-) delete mode 100644 .github/workflows/build_sensor_fw.yaml delete mode 100644 .github/workflows/cross-compile-special-sensors.yaml diff --git a/.github/workflows/build_sensor_fw.yaml b/.github/workflows/build_sensor_fw.yaml deleted file mode 100644 index d6e4e6d52..000000000 --- a/.github/workflows/build_sensor_fw.yaml +++ /dev/null @@ -1,44 +0,0 @@ -name: "Build firmware sensor variant bundles" -on: - push: - branches: - - '*' - tags: - - 'v*' - workflow_dispatch: - -env: - ci: 1 - -defaults: - run: - shell: bash - -jobs: - build: - name: "Build" - runs-on: "ubuntu-latest" - timeout-minutes: 20 - steps: - - uses: "actions/checkout@v2" - with: - fetch-depth: 0 - - uses: "actions/cache@v3" - with: - path: "./stm32-tools" - key: ${{ runner.os }}-${{ hashFiles('**/cmake/*') }}-${{ secrets.CACHE_VERSION }} - - name: 'CMake configure' - run: | - cmake --preset=cross-sensor-buffer . -DCMAKE_BUILD_TYPE=RelWithDebInfo - - name: 'Build images' - run: | - cmake --build --preset=firmware-g4-sensors --target firmware-applications firmware-images - - name: 'Prep images for upload' - run: | - cmake --install ./build-cross-sensor - - name: 'Upload application artifact' - uses: actions/upload-artifact@v3 - with: - name: 'firmware-applications-${{github.ref_name}}' - path: | - dist-sensor/applications/* diff --git a/.github/workflows/cross-compile-special-sensors.yaml b/.github/workflows/cross-compile-special-sensors.yaml deleted file mode 100644 index 355e067ac..000000000 --- a/.github/workflows/cross-compile-special-sensors.yaml +++ /dev/null @@ -1,118 +0,0 @@ -name: "cross-compile-sensors/format/lint all targets" -on: - push: - branches: - - "*" - workflow_dispatch: - -concurrency: - group: ${{ github.workflow }}-${{ github.ref_name }} - cancel-in-progress: true - -defaults: - run: - shell: bash - working-directory: ot3-firmware - -jobs: - build-cross: - runs-on: "ubuntu-20.04" - timeout-minutes: 20 - strategy: - matrix: - target: [ - pipettes-single, - pipettes-multi, - pipettes-96, - gripper, - ] - fail-fast: false - name: Build ${{ matrix.target }} - steps: - - name: Checkout ot3-firmware repo - uses: actions/checkout@v4 - with: - path: ot3-firmware - - - name: Checkout github actions directory - uses: actions/checkout@v4 - with: - sparse-checkout: | - .github/actions - sparse-checkout-cone-mode: false - path: actions - - - name: Setup main - uses: ./actions/.github/actions/main-setup - with: - cache-version: ${{ secrets.CACHE_VERSION }} - - - name: Configure - run: cmake --preset=cross-sensor-buffer . - - - name: Build all STM32G4 applications - run: cmake --build ./build-cross-sensor --target ${{ matrix.target }}-images ${{ matrix.target }}-applications - - format: - runs-on: "ubuntu-20.04" - timeout-minutes: 20 - name: Formatting - steps: - - name: Checkout ot3-firmware repo - uses: actions/checkout@v4 - with: - path: ot3-firmware - - - name: Checkout github actions directory - uses: actions/checkout@v4 - with: - sparse-checkout: | - .github/actions - sparse-checkout-cone-mode: false - path: actions - - - name: Setup main - uses: ./actions/.github/actions/main-setup - with: - cache-version: ${{ secrets.CACHE_VERSION }} - - - name: Configure - run: cmake --preset=cross-sensor-buffer . - - - name: Format - run: cmake --build ./build-cross-sensor --target format-ci - - lint: - runs-on: "ubuntu-20.04" - timeout-minutes: 20 - name: ${{ matrix.target }} lint - strategy: - matrix: - target: [ - pipettes, - gripper, - ] - steps: - - name: Checkout ot3-firmware repo - uses: actions/checkout@v4 - with: - path: ot3-firmware - - - name: Checkout github actions directory - uses: actions/checkout@v4 - with: - sparse-checkout: | - .github/actions - sparse-checkout-cone-mode: false - path: actions - - - name: Setup main - uses: ./actions/.github/actions/main-setup - with: - cache-version: ${{ secrets.CACHE_VERSION }} - - - name: Configure - run: cmake --preset=cross-sensor-buffer . - - - name: Format - run: cmake --build ./build-cross-sensor --target ${{ matrix.target }}-lint diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 3dd3d5cf6..002bc5696 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -225,6 +225,15 @@ class FDC1004 { can::messages::Acknowledgment{.message_index = message_index}); } + auto sensor_buffer_log(float data) -> void { + sensor_buffer->at(sensor_buffer_index) = data; + sensor_buffer_index++; + if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { + sensor_buffer_index = 0; + crossed_buffer_index = true; + } + } + void handle_fdc_response(i2c::messages::TransactionResponse &m) { uint16_t reg_int = 0; static_cast(bit_utils::bytes_to_int( @@ -304,15 +313,6 @@ class FDC1004 { } } - auto sensor_buffer_log(float data) -> void { - sensor_buffer->at(sensor_buffer_index) = data; - sensor_buffer_index++; - if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { - sensor_buffer_index = 0; - crossed_buffer_index = true; - } - } - void handle_baseline_response(i2c::messages::TransactionResponse &m) { static_cast(bit_utils::bytes_to_int( m.read_buffer.cbegin(), m.read_buffer.cend(), From dcdbfe722f91671270e458ffb0563fcea96e5a4f Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Wed, 3 Jul 2024 15:35:26 -0400 Subject: [PATCH 6/9] set use_sensor_move true for pipettes and gripper --- gripper/core/CMakeLists.txt | 1 + gripper/firmware/interfaces_z_motor.cpp | 6 ++ .../can/core/message_handlers/move_group.hpp | 8 ++ include/gripper/core/can_task.hpp | 10 +++ .../core/stepper_motor/motion_controller.hpp | 76 ++++++++++++++++++- .../core/stepper_motor/motor.hpp | 4 + .../stepper_motor/motor_interrupt_handler.hpp | 65 ++++++++++++++++ include/motor-control/core/tasks/messages.hpp | 21 +++++ .../core/tasks/motion_controller_task.hpp | 10 +++ .../core/tasks/move_group_task.hpp | 12 +++ include/pipettes/core/dispatch_builder.hpp | 11 +++ include/pipettes/core/interfaces.hpp | 5 ++ include/pipettes/firmware/interfaces.hpp | 7 ++ include/pipettes/firmware/interfaces_g4.hpp | 7 ++ pipettes/core/CMakeLists.txt | 5 ++ pipettes/simulator/interfaces.cpp | 12 +++ 16 files changed, 259 insertions(+), 1 deletion(-) diff --git a/gripper/core/CMakeLists.txt b/gripper/core/CMakeLists.txt index 50a9adab7..e3c012332 100644 --- a/gripper/core/CMakeLists.txt +++ b/gripper/core/CMakeLists.txt @@ -1,4 +1,5 @@ function(target_gripper_core TARGET) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true) target_sources(${TARGET} PUBLIC diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp index c9c43c9a9..69405dfde 100644 --- a/gripper/firmware/interfaces_z_motor.cpp +++ b/gripper/firmware/interfaces_z_motor.cpp @@ -150,8 +150,14 @@ static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{ /** * The pending move queue */ +#ifdef USE_SENSOR_MOVE +static freertos_message_queue::FreeRTOSMessageQueue< + motor_messages::SensorSyncMove> + motor_queue("Motor Queue"); +#else static freertos_message_queue::FreeRTOSMessageQueue motor_queue("Motor Queue"); +#endif static freertos_message_queue::FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest> diff --git a/include/can/core/message_handlers/move_group.hpp b/include/can/core/message_handlers/move_group.hpp index f4e597997..55b5f2492 100644 --- a/include/can/core/message_handlers/move_group.hpp +++ b/include/can/core/message_handlers/move_group.hpp @@ -11,10 +11,18 @@ using namespace can::messages; template class MoveGroupHandler { public: +#ifdef USE_SENSOR_MOVE + using MessageType = + std::variant; +#else using MessageType = std::variant; +#endif MoveGroupHandler(Client &task_client) : task_client{task_client} {} MoveGroupHandler(const MoveGroupHandler &) = delete; diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index bffe2cbef..e3b3f294c 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -19,12 +19,22 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, can::messages::WriteMotorCurrentRequest>; +#ifdef USE_SENSOR_MOVE +using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< + can::message_handlers::move_group::MoveGroupHandler, + can::messages::AddLinearMoveRequest, + can::messages::ClearAllMoveGroupsRequest, + can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, + can::messages::HomeRequest, can::messages::StopRequest, + can::messages::AddSensorMoveRequest>; +#else using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler, can::messages::AddLinearMoveRequest, can::messages::ClearAllMoveGroupsRequest, can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; +#endif using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motion::MotionHandler, can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index e911cdc9b..9cd1e9c4f 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -24,7 +24,12 @@ using namespace motor_hardware; template class MotionController { public: - using GenericQueue = freertos_message_queue::FreeRTOSMessageQueue; + using GenericQueue = +#ifdef USE_SENSOR_MOVE + freertos_message_queue::FreeRTOSMessageQueue; +#else + freertos_message_queue::FreeRTOSMessageQueue; +#endif using UpdatePositionQueue = freertos_message_queue::FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest>; MotionController(lms::LinearMotionSystemConfig lms_config, @@ -58,6 +63,73 @@ class MotionController { -> const lms::LinearMotionSystemConfig& { return linear_motion_sys_config; } +#ifdef USE_SENSOR_MOVE + void move(const can::messages::AddSensorMoveRequest& 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); + SensorSyncMove msg{ + can_msg.message_index, + can_msg.duration, + velocity_steps, + acceleration_steps, + can_msg.group_id, + can_msg.seq_id, + can_msg.request_stop_condition, + 0, + hardware.get_usage_eeprom_config().get_distance_key(), + can_msg.sensor_id, + can_msg.sensor_type}; + if (!enabled) { + enable_motor(); + } + queue.try_write(msg); + } + + void move(const can::messages::AddLinearMoveRequest& 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); + SensorSyncMove msg{ + .message_index = can_msg.message_index, + .duration = can_msg.duration, + .velocity = velocity_steps, + .acceleration = acceleration_steps, + .group_id = can_msg.group_id, + .seq_id = can_msg.seq_id, + .stop_condition = can_msg.request_stop_condition, + .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), + .sensor_id = can::ids::SensorId::UNUSED, + .sensor_type = can::ids::SensorType::UNUSED}; + if (!enabled) { + enable_motor(); + } + queue.try_write(msg); + } + + void move(const can::messages::HomeRequest& can_msg) { + steps_per_tick velocity_steps = + fixed_point_multiply(steps_per_mm, can_msg.velocity); + SensorSyncMove msg{ + .message_index = can_msg.message_index, + .duration = can_msg.duration, + .velocity = velocity_steps, + .acceleration = 0, + .group_id = can_msg.group_id, + .seq_id = can_msg.seq_id, + .stop_condition = + static_cast(MoveStopCondition::limit_switch), + .usage_key = hardware.get_usage_eeprom_config().get_distance_key(), + .sensor_id = can::ids::SensorId::UNUSED, + .sensor_type = can::ids::SensorType::UNUSED}; + if (!enabled) { + enable_motor(); + } + queue.try_write(msg); + } +#else void move(const can::messages::AddLinearMoveRequest& can_msg) { steps_per_tick velocity_steps = @@ -98,6 +170,8 @@ class MotionController { queue.try_write(msg); } +#endif + [[nodiscard]] auto update_position( const can::messages::UpdateMotorPositionEstimationRequest& can_msg) -> bool { diff --git a/include/motor-control/core/stepper_motor/motor.hpp b/include/motor-control/core/stepper_motor/motor.hpp index 20b111fef..b9ff17ee6 100644 --- a/include/motor-control/core/stepper_motor/motor.hpp +++ b/include/motor-control/core/stepper_motor/motor.hpp @@ -18,7 +18,11 @@ using namespace freertos_message_queue; template struct Motor { +#ifdef USE_SENSOR_MOVE + using GenericQueue = FreeRTOSMessageQueue; +#else using GenericQueue = FreeRTOSMessageQueue; +#endif using UpdatePositionQueue = FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest>; 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 7702692f7..5196a7f2d 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -10,10 +10,13 @@ #include "motor-control/core/stall_check.hpp" #include "motor-control/core/tasks/move_status_reporter_task.hpp" #include "motor-control/core/tasks/tmc_motor_driver_common.hpp" +#ifdef USE_SENSOR_MOVE #ifdef PIPETTE_TYPE_DEFINE +#include "pipettes/core/sensor_tasks.hpp" #else #include "gripper/core/tasks.hpp" #endif // PIPETTE_TYPE_DEFINE +#endif // USE_SENSOR_MOVE namespace motor_handler { using namespace motor_messages; @@ -373,12 +376,52 @@ class MotorInterruptHandler { */ return tick_count < buffered_move.duration; } +#ifdef USE_SENSOR_MOVE + auto send_bind_message(can::ids::SensorType sensor_type, + can::ids::SensorId sensor_id, uint8_t binding) + -> void { + auto msg = can::messages::BindSensorOutputRequest{ + .message_index = buffered_move.message_index, + .sensor = sensor_type, + .sensor_id = sensor_id, + .binding = binding}; + if (sensor_type == can::ids::SensorType::pressure) { + if (sensor_id == can::ids::SensorId::S0) { + send_to_pressure_sensor_queue_rear(msg); + } else { + send_to_pressure_sensor_queue_front(msg); + } + } else if (sensor_type == can::ids::SensorType::capacitive) { + if (sensor_id == can::ids::SensorId::S0) { + send_to_capacitive_sensor_queue_rear(msg); + } else { + send_to_capacitive_sensor_queue_front(msg); + } + } + } +#endif void update_move() { _has_active_move = move_queue.try_read_isr(&buffered_move); if (_has_active_move) { hardware.enable_encoder(); buffered_move.start_encoder_position = hardware.get_encoder_pulses(); +#ifdef USE_SENSOR_MOVE + if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { + if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { + send_bind_message(buffered_move.sensor_type, + can::ids::SensorId::S0, + buffered_move.binding_flags); + send_bind_message(buffered_move.sensor_type, + can::ids::SensorId::S1, + buffered_move.binding_flags); + } else { + send_bind_message(buffered_move.sensor_type, + buffered_move.sensor_id, + buffered_move.binding_flags); + } + } +#endif } if (set_direction_pin()) { hardware.positive_direction(); @@ -603,6 +646,28 @@ class MotorInterruptHandler { hardware.set_step_tracker( static_cast(position_tracker >> 31)); } +#ifdef USE_SENSOR_MOVE + void send_to_pressure_sensor_queue_rear( + can::messages::BindSensorOutputRequest& m) { + sensor_tasks::get_queues().send_pressure_sensor_queue_rear_isr(m); + } + void send_to_pressure_sensor_queue_front( + can::messages::BindSensorOutputRequest& m) { + // send to both queues, they will handle their own gating based on + // sensor id + sensor_tasks::get_queues().send_pressure_sensor_queue_front_isr(m); + } + void send_to_capacitive_sensor_queue_rear( + can::messages::BindSensorOutputRequest& m) { + sensor_tasks::get_queues().send_capacitive_sensor_queue_rear_isr(m); + } + void send_to_capacitive_sensor_queue_front( + can::messages::BindSensorOutputRequest& m) { + // send to both queues, they will handle their own gating based on + // sensor id + sensor_tasks::get_queues().send_pressure_sensor_queue_front_isr(m); + } +#endif uint64_t tick_count = 0x0; static constexpr const q31_31 tick_flag = 0x80000000; static constexpr const uint64_t overflow_flag = 0x8000000000000000; diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp index 118e44011..759a28308 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -6,6 +6,26 @@ namespace motor_control_task_messages { +#ifdef USE_SENSOR_MOVE +using MotionControlTaskMessage = std::variant< + std::monostate, can::messages::AddLinearMoveRequest, + can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, + can::messages::GetMotionConstraintsRequest, + can::messages::SetMotionConstraints, can::messages::StopRequest, + can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest, + can::messages::HomeRequest, + can::messages::UpdateMotorPositionEstimationRequest, + can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest, + can::messages::AddSensorMoveRequest>; + +using MoveGroupTaskMessage = + std::variant; +#else using MotionControlTaskMessage = std::variant< std::monostate, can::messages::AddLinearMoveRequest, can::messages::DisableMotorRequest, can::messages::EnableMotorRequest, @@ -22,6 +42,7 @@ using MoveGroupTaskMessage = can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; +#endif using MotorDriverTaskMessage = std::variant; +#else using MoveGroupType = move_group::MoveGroupManager; +#endif using TaskMessage = motor_control_task_messages::MoveGroupTaskMessage; @@ -120,6 +126,12 @@ class MoveGroupMessageHandler { mc_client.send_motion_controller_queue(m); } +#ifdef USE_SENSOR_MOVE + void visit_move(const can::messages::AddSensorMoveRequest& m) { + mc_client.send_motion_controller_queue(m); + } +#endif + MoveGroupType& move_groups; MotionControllerClient& mc_client; CanClient& can_client; diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp index 3ed5d6471..e5d0950c6 100644 --- a/include/pipettes/core/dispatch_builder.hpp +++ b/include/pipettes/core/dispatch_builder.hpp @@ -39,6 +39,16 @@ using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::GearWriteMotorDriverRegister, can::messages::GearWriteMotorCurrentRequest>; +#ifdef USE_SENSOR_MOVE +using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< + can::message_handlers::move_group::MoveGroupHandler< + linear_motor_tasks::QueueClient>, + can::messages::AddLinearMoveRequest, + can::messages::ClearAllMoveGroupsRequest, + can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, + can::messages::HomeRequest, can::messages::StopRequest, + can::messages::AddSensorMoveRequest>; +#else using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< linear_motor_tasks::QueueClient>, @@ -46,6 +56,7 @@ using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::messages::ClearAllMoveGroupsRequest, can::messages::ExecuteMoveGroupRequest, can::messages::GetMoveGroupRequest, can::messages::HomeRequest, can::messages::StopRequest>; +#endif using GearMoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< gear_move_group_handler::GearMoveGroupHandler< diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp index 955062674..999e5f7ea 100644 --- a/include/pipettes/core/interfaces.hpp +++ b/include/pipettes/core/interfaces.hpp @@ -6,8 +6,13 @@ #include "motor-control/core/tasks/motor_hardware_task.hpp" namespace interfaces { +#ifdef USE_SENSOR_MOVE +using MoveQueue = freertos_message_queue::FreeRTOSMessageQueue< + motor_messages::SensorSyncMove>; +#else using MoveQueue = freertos_message_queue::FreeRTOSMessageQueue; +#endif using GearMoveQueue = freertos_message_queue::FreeRTOSMessageQueue; using MotionControlType = diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp index cdcfcdfa3..2ff87fcc4 100644 --- a/include/pipettes/firmware/interfaces.hpp +++ b/include/pipettes/firmware/interfaces.hpp @@ -19,10 +19,17 @@ namespace interfaces { +#ifdef USE_SENSOR_MOVE +template +using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< + freertos_message_queue::FreeRTOSMessageQueue, Client, + motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; +#else template using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, motor_hardware::MotorHardware>; +#endif template using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp index 922f9f21a..ecf9c0813 100644 --- a/include/pipettes/firmware/interfaces_g4.hpp +++ b/include/pipettes/firmware/interfaces_g4.hpp @@ -28,10 +28,17 @@ namespace interfaces { +#ifdef USE_SENSOR_MOVE +template +using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< + freertos_message_queue::FreeRTOSMessageQueue, Client, + motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; +#else template using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, motor_hardware::MotorHardware>; +#endif template using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, Client, diff --git a/pipettes/core/CMakeLists.txt b/pipettes/core/CMakeLists.txt index 70bc6fd53..1dcd6c4bf 100644 --- a/pipettes/core/CMakeLists.txt +++ b/pipettes/core/CMakeLists.txt @@ -1,4 +1,5 @@ function(target_pipettes_core_common TARGET REVISION) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_sources(${TARGET} PUBLIC ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/configs.cpp ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/peripheral_tasks.cpp @@ -12,6 +13,7 @@ function(target_pipettes_core_common TARGET REVISION) endfunction() function(target_pipettes_core_single TARGET REVISION) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=SINGLE_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -20,6 +22,7 @@ function(target_pipettes_core_single TARGET REVISION) endfunction() function(target_pipettes_core_multi TARGET REVISION) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=EIGHT_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -29,6 +32,7 @@ function(target_pipettes_core_multi TARGET REVISION) endfunction() function(target_pipettes_core_96 TARGET REVISION) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=NINETY_SIX_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -38,6 +42,7 @@ function(target_pipettes_core_96 TARGET REVISION) endfunction() function(target_pipettes_core_384 TARGET REVISION) + target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=THREE_EIGHTY_FOUR_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp index 6e6948801..8d0301c85 100644 --- a/pipettes/simulator/interfaces.cpp +++ b/pipettes/simulator/interfaces.cpp @@ -66,9 +66,15 @@ auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, MotorInterruptHandlerType& handler) +#ifdef USE_SENSOR_MOVE + -> motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + sim_motor_hardware_iface::SimMotorHardwareIface> { +#else -> motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { +#endif return motor_interrupt_driver::MotorInterruptDriver( queues.plunger_queue, handler, hw, queues.plunger_update_queue); } @@ -77,9 +83,15 @@ auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, MotorInterruptHandlerType& handler) +#ifdef USE_SENSOR_MOVE + -> motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + sim_motor_hardware_iface::SimMotorHardwareIface> { +#else -> motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { +#endif return motor_interrupt_driver::MotorInterruptDriver( queues.plunger_queue, handler, hw, queues.plunger_update_queue); } From 681fb79a7582abc0821e54d18058b933678785ee Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Mon, 8 Jul 2024 10:58:47 -0400 Subject: [PATCH 7/9] replace vtaskdelay with hardware_delay function --- include/sensors/core/tasks/capacitive_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 002bc5696..505a52e06 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -216,7 +216,7 @@ class FDC1004 { (*sensor_buffer).at(i), S15Q16_RADIX)}); if (i % 10 == 0) { // slow it down so the can buffer doesn't choke - vTaskDelay(50); + vtask_hardware_delay(50); } (*sensor_buffer).at(i) = 0; } From 94e2953fce8cf1c74a23a20619a87d77595240b8 Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Mon, 8 Jul 2024 14:31:27 -0400 Subject: [PATCH 8/9] add use_sensor_move to sim interfaces --- gripper/simulator/interfaces.cpp | 7 +++++ include/pipettes/simulator/interfaces.hpp | 34 ++++++++++++++++++----- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp index f3dfd2a14..085c4ad77 100644 --- a/gripper/simulator/interfaces.cpp +++ b/gripper/simulator/interfaces.cpp @@ -27,8 +27,15 @@ static auto motor_interface = /** * The pending move queue */ + +#ifdef USE_SENSOR_MOVE +static freertos_message_queue::FreeRTOSMessageQueue< + motor_messages::SensorSyncMove> + motor_queue("Motor Queue"); +#else static freertos_message_queue::FreeRTOSMessageQueue motor_queue("Motor Queue"); +#endif static freertos_message_queue::FreeRTOSMessageQueue< can::messages::UpdateMotorPositionEstimationRequest> diff --git a/include/pipettes/simulator/interfaces.hpp b/include/pipettes/simulator/interfaces.hpp index 418f3417b..59ee3fbed 100644 --- a/include/pipettes/simulator/interfaces.hpp +++ b/include/pipettes/simulator/interfaces.hpp @@ -13,9 +13,17 @@ namespace interfaces { template -using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, - sim_motor_hardware_iface::SimMotorHardwareIface>; +using MotorInterruptHandlerType = +#ifdef USE_SENSOR_MOVE + motor_handler::MotorInterruptHandler< + freertos_message_queue::FreeRTOSMessageQueue, Client, + motor_messages::SensorSyncMove, + sim_motor_hardware_iface::SimMotorHardwareIface>; +#else + motor_handler::MotorInterruptHandler< + freertos_message_queue::FreeRTOSMessageQueue, Client, + motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; +#endif template using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< @@ -73,18 +81,30 @@ auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw, auto get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, - MotorInterruptHandlerType& handler) - -> motor_interrupt_driver::MotorInterruptDriver< + MotorInterruptHandlerType& handler) -> +#ifdef USE_SENSOR_MOVE + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + sim_motor_hardware_iface::SimMotorHardwareIface>; +#else + motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; +#endif auto get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, - MotorInterruptHandlerType& handler) - -> motor_interrupt_driver::MotorInterruptDriver< + MotorInterruptHandlerType& handler) -> +#ifdef USE_SENSOR_MOVE + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + sim_motor_hardware_iface::SimMotorHardwareIface>; +#else + motor_interrupt_driver::MotorInterruptDriver< linear_motor_tasks::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; +#endif auto get_motor_hardware() -> sim_motor_hardware_iface::SimMotorHardwareIface; From f356024c1418890f9c4dd15e1c1821e1970ae2b6 Mon Sep 17 00:00:00 2001 From: caila-marashaj Date: Mon, 8 Jul 2024 15:32:45 -0400 Subject: [PATCH 9/9] remove use_sensor_move defs from not common targets --- pipettes/core/CMakeLists.txt | 4 ---- 1 file changed, 4 deletions(-) diff --git a/pipettes/core/CMakeLists.txt b/pipettes/core/CMakeLists.txt index 1dcd6c4bf..f5725ec7c 100644 --- a/pipettes/core/CMakeLists.txt +++ b/pipettes/core/CMakeLists.txt @@ -13,7 +13,6 @@ function(target_pipettes_core_common TARGET REVISION) endfunction() function(target_pipettes_core_single TARGET REVISION) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=SINGLE_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -22,7 +21,6 @@ function(target_pipettes_core_single TARGET REVISION) endfunction() function(target_pipettes_core_multi TARGET REVISION) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=EIGHT_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -32,7 +30,6 @@ function(target_pipettes_core_multi TARGET REVISION) endfunction() function(target_pipettes_core_96 TARGET REVISION) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=NINETY_SIX_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500) @@ -42,7 +39,6 @@ function(target_pipettes_core_96 TARGET REVISION) endfunction() function(target_pipettes_core_384 TARGET REVISION) - target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=THREE_EIGHTY_FOUR_CHANNEL) target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)