feat(sensors): enable capacitive sensor logging (#768)

* USE_SENSOR_MOVE * intermediate updates * finished implementing capacitive_probe firmware * format lint test * build fixes * formatted * updates to match pressure work * fixes RAM space build issue * empty last line * removed comment * build update * pr review fix and format * faux commit * faux commit * fixed build issues stemming from removal of motor driver error surfacing branch * simulator fix * formatted * removed comments * formatted * get rid of pesky error messages like with pressure driver * rip out the unneeded templating of sensor hardware since that stuff is behind ifdefs * fix simulators * build issue fix * merge issue fix --------- Co-authored-by: Ryan howard <[email protected]>
Opentrons · Jun 5, 2024 · 0f4faff · 0f4faff
1 parent 83f8f09
commit 0f4faff
Show file tree

Hide file tree

Showing 45 changed files with 425 additions and 209 deletions.
diff --git a/.github/workflows/cross-compile-special-sensors.yaml b/.github/workflows/cross-compile-special-sensors.yaml
@@ -24,6 +24,7 @@ jobs:
           pipettes-single,
           pipettes-multi,
           pipettes-96,
+          gripper,
         ]
       fail-fast: false
     name: Build ${{ matrix.target }}
@@ -89,6 +90,7 @@ jobs:
       matrix:
         target: [
           pipettes,
+          gripper,
         ]
     steps:
       - name: Checkout ot3-firmware repo

diff --git a/CMakePresets.json b/CMakePresets.json
@@ -42,7 +42,7 @@
       "installDir": "${sourceDir}/dist-sensor",
       "binaryDir": "${sourceDir}/build-cross-sensor",
       "cacheVariables": {
-        "USE_PRESSURE_MOVE": true
+        "USE_SENSOR_MOVE": true
       },
       "inherits": "cross"
     },

diff --git a/common/core/CMakeLists.txt b/common/core/CMakeLists.txt
@@ -54,7 +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_PRESSURE_MOVE})
+  if (${USE_SENSOR_MOVE})
   set(TERTIARY_FLAG "'1'")
   else()
   set(TERTIARY_FLAG "0x00")

diff --git a/gripper/core/CMakeLists.txt b/gripper/core/CMakeLists.txt
@@ -1,4 +1,9 @@
 function(target_gripper_core TARGET)
+    if(${USE_SENSOR_MOVE})
+    target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE)
+    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1700)
+    target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
+    endif()
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_tasks.cpp
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/tasks.cpp

diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp
@@ -16,6 +16,10 @@
 
 static auto tasks = gripper_tasks::AllTask{};
 static auto queues = gripper_tasks::QueueClient{can::ids::NodeId::gripper};
+static std::array<float, SENSOR_BUFFER_SIZE> p_buff;
+#ifdef USE_TWO_BUFFERS
+static std::array<float, SENSOR_BUFFER_SIZE> p_buff_front;
+#endif
 
 static auto eeprom_task_builder =
     freertos_task::TaskStarter<512, eeprom::task::EEPromTask>{};
@@ -95,11 +99,16 @@ void gripper_tasks::start_tasks(
     auto& capacitive_sensor_task_front =
         capacitive_sensor_task_builder_front.start(
             5, "cap sensor S1", i2c2_task_client, i2c2_poll_client,
-            sensor_hardware, queues);
+            sensor_hardware, queues,
+#ifdef USE_TWO_BUFFERS
+            p_buff_front);
+#else
+            p_buff);
+#endif
     auto& capacitive_sensor_task_rear =
         capacitive_sensor_task_builder_rear.start(
             5, "cap sensor S0", i2c3_task_client, i2c3_poll_client,
-            sensor_hardware, queues);
+            sensor_hardware, queues, p_buff);
 
     tasks.i2c2_task = &i2c2_task;
     tasks.i2c3_task = &i2c3_task;
@@ -152,13 +161,27 @@ void gripper_tasks::QueueClient::send_capacitive_sensor_queue_rear(
     capacitive_sensor_queue_rear->try_write(m);
 }
 
+void gripper_tasks::QueueClient::send_capacitive_sensor_queue_front_isr(
+    const sensors::utils::TaskMessage& m) {
+    std::ignore = capacitive_sensor_queue_front->try_write_isr(m);
+}
+
+void gripper_tasks::QueueClient::send_capacitive_sensor_queue_rear_isr(
+    const sensors::utils::TaskMessage& m) {
+    std::ignore = capacitive_sensor_queue_rear->try_write_isr(m);
+}
+
 // gripper does not have environment nor pressure sensor
 void gripper_tasks::QueueClient::send_environment_sensor_queue(
     const sensors::utils::TaskMessage&) {}
 void gripper_tasks::QueueClient::send_pressure_sensor_queue_front(
     const sensors::utils::TaskMessage&) {}
 void gripper_tasks::QueueClient::send_pressure_sensor_queue_rear(
     const sensors::utils::TaskMessage&) {}
+void gripper_tasks::QueueClient::send_pressure_sensor_queue_front_isr(
+    const sensors::utils::TaskMessage&) {}
+void gripper_tasks::QueueClient::send_pressure_sensor_queue_rear_isr(
+    const sensors::utils::TaskMessage&) {}
 
 void gripper_tasks::QueueClient::send_tip_notification_queue_rear(
     const sensors::tip_presence::TaskMessage&) {}
@@ -176,3 +199,7 @@ auto gripper_tasks::get_all_tasks() -> AllTask& { return tasks; }
  * @return
  */
 auto gripper_tasks::get_main_queues() -> QueueClient& { return queues; }
+
+auto sensor_tasks::get_queues() -> gripper_tasks::QueueClient& {
+    return queues;
+}
diff --git 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_messages::Move>
     motor_queue("Motor Queue");
+#endif
 
 static freertos_message_queue::FreeRTOSMessageQueue<
     can::messages::UpdateMotorPositionEstimationRequest>

diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h
@@ -114,7 +114,7 @@ typedef enum {
     can_messageid_peripheral_status_request = 0x8c,
     can_messageid_peripheral_status_response = 0x8d,
     can_messageid_baseline_sensor_response = 0x8e,
-    can_messageid_send_accumulated_pressure_data = 0x8f,
+    can_messageid_send_accumulated_sensor_data = 0x8f,
     can_messageid_set_hepa_fan_state_request = 0x90,
     can_messageid_get_hepa_fan_state_request = 0x91,
     can_messageid_get_hepa_fan_state_response = 0x92,

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
@@ -116,7 +116,7 @@ enum class MessageId {
     peripheral_status_request = 0x8c,
     peripheral_status_response = 0x8d,
     baseline_sensor_response = 0x8e,
-    send_accumulated_pressure_data = 0x8f,
+    send_accumulated_sensor_data = 0x8f,
     set_hepa_fan_state_request = 0x90,
     get_hepa_fan_state_request = 0x91,
     get_hepa_fan_state_response = 0x92,
@@ -193,6 +193,7 @@ enum class SensorType {
     pressure_temperature = 0x4,
     humidity = 0x5,
     temperature = 0x6,
+    UNUSED = 0x7,
 };
 
 /** Sensor IDs available.

diff --git a/include/can/core/message_handlers/move_group.hpp b/include/can/core/message_handlers/move_group.hpp
@@ -11,7 +11,7 @@ using namespace can::messages;
 template <move_group_task::TaskClient Client>
 class MoveGroupHandler {
   public:
-#ifdef USE_PRESSURE_MOVE
+#ifdef USE_SENSOR_MOVE
     using MessageType =
         std::variant<std::monostate, AddLinearMoveRequest,
                      ClearAllMoveGroupsRequest, ExecuteMoveGroupRequest,

diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
@@ -751,24 +751,28 @@ struct FirmwareUpdateStatusResponse
         -> bool = default;
 };
 
-struct SendAccumulatedPressureDataRequest
-    : BaseMessage<MessageId::send_accumulated_pressure_data> {
+struct SendAccumulatedSensorDataRequest
+    : BaseMessage<MessageId::send_accumulated_sensor_data> {
     uint32_t message_index = 0;
     uint8_t sensor_id = 0;
+    uint8_t sensor_type = 0;
 
     template <bit_utils::ByteIterator Input, typename Limit>
     static auto parse(Input body, Limit limit)
-        -> SendAccumulatedPressureDataRequest {
+        -> SendAccumulatedSensorDataRequest {
         uint32_t msg_ind = 0;
         uint8_t sensor_id = 0;
+        uint8_t sensor_type = 0;
 
         body = bit_utils::bytes_to_int(body, limit, msg_ind);
         body = bit_utils::bytes_to_int(body, limit, sensor_id);
-        return SendAccumulatedPressureDataRequest{.message_index = msg_ind,
-                                                  .sensor_id = sensor_id};
+        body = bit_utils::bytes_to_int(body, limit, sensor_type);
+        return SendAccumulatedSensorDataRequest{.message_index = msg_ind,
+                                                .sensor_id = sensor_id,
+                                                .sensor_type = sensor_type};
     }
 
-    auto operator==(const SendAccumulatedPressureDataRequest& other) const
+    auto operator==(const SendAccumulatedSensorDataRequest& other) const
         -> bool = default;
 };
 
@@ -1737,6 +1741,7 @@ struct AddSensorMoveRequest : BaseMessage<MessageId::add_sensor_move_request> {
     mm_per_tick velocity;
     uint8_t request_stop_condition;
     can::ids::SensorId sensor_id{};
+    can::ids::SensorType sensor_type{};
 
     template <bit_utils::ByteIterator Input, typename Limit>
     static auto parse(Input body, Limit limit) -> AddSensorMoveRequest {
@@ -1748,6 +1753,7 @@ struct AddSensorMoveRequest : BaseMessage<MessageId::add_sensor_move_request> {
         uint8_t request_stop_condition = 0;
         uint32_t msg_ind = 0;
         uint8_t sensor_id = 0;
+        uint8_t sensor_type = 0;
 
         body = bit_utils::bytes_to_int(body, limit, msg_ind);
         body = bit_utils::bytes_to_int(body, limit, group_id);
@@ -1757,6 +1763,7 @@ struct AddSensorMoveRequest : BaseMessage<MessageId::add_sensor_move_request> {
         body = bit_utils::bytes_to_int(body, limit, velocity);
         body = bit_utils::bytes_to_int(body, limit, request_stop_condition);
         body = bit_utils::bytes_to_int(body, limit, sensor_id);
+        body = bit_utils::bytes_to_int(body, limit, sensor_type);
         return AddSensorMoveRequest{
             .message_index = msg_ind,
             .group_id = group_id,
@@ -1766,6 +1773,7 @@ struct AddSensorMoveRequest : BaseMessage<MessageId::add_sensor_move_request> {
             .velocity = velocity,
             .request_stop_condition = request_stop_condition,
             .sensor_id = static_cast<can::ids::SensorId>(sensor_id),
+            .sensor_type = static_cast<can::ids::SensorType>(sensor_type),
         };
     }
 

diff --git a/include/common/core/sensor_buffer.hpp b/include/common/core/sensor_buffer.hpp
@@ -0,0 +1,7 @@
+#pragma once
+
+#ifdef USE_SENSOR_MOVE
+constexpr size_t SENSOR_BUFFER_SIZE = P_BUFF_SIZE;
+#else
+constexpr size_t SENSOR_BUFFER_SIZE = 0;
+#endif
diff --git 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<z_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<z_tasks::QueueClient>,
     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<z_tasks::QueueClient>,
     can::messages::DisableMotorRequest, can::messages::EnableMotorRequest,
@@ -65,7 +75,7 @@ using GripperInfoDispatchTarget = can::dispatch::DispatchParseTarget<
 using SensorDispatchTarget = can::dispatch::DispatchParseTarget<
     sensors::handlers::SensorHandler<gripper_tasks::QueueClient>,
     can::messages::TipStatusQueryRequest, can::messages::ReadFromSensorRequest,
-    can::messages::SendAccumulatedPressureDataRequest,
+    can::messages::SendAccumulatedSensorDataRequest,
     can::messages::WriteToSensorRequest, can::messages::BaselineSensorRequest,
     can::messages::SetSensorThresholdRequest,
     can::messages::BindSensorOutputRequest,

diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp
@@ -67,11 +67,24 @@ struct QueueClient : can::message_writer::MessageWriter {
     void send_capacitive_sensor_queue_rear(
         const sensors::utils::TaskMessage& m);
 
+    void send_capacitive_sensor_queue_front_isr(
+        const sensors::utils::TaskMessage& m);
+
+    void send_capacitive_sensor_queue_rear_isr(
+        const sensors::utils::TaskMessage& m);
+
     void send_environment_sensor_queue(const sensors::utils::TaskMessage& m);
 
     void send_pressure_sensor_queue_front(const sensors::utils::TaskMessage& m);
+
     void send_pressure_sensor_queue_rear(const sensors::utils::TaskMessage& m);
 
+    void send_pressure_sensor_queue_front_isr(
+        const sensors::utils::TaskMessage& m);
+
+    void send_pressure_sensor_queue_rear_isr(
+        const sensors::utils::TaskMessage& m);
+
     void send_tip_notification_queue_rear(
         const sensors::tip_presence::TaskMessage& m);
     void send_tip_notification_queue_front(
@@ -258,3 +271,7 @@ struct QueueClient : can::message_writer::MessageWriter {
 }  // namespace g_tasks
 
 }  // namespace gripper_tasks
+
+namespace sensor_tasks {
+[[nodiscard]] auto get_queues() -> gripper_tasks::QueueClient&;
+}  // namespace sensor_tasks
diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp
@@ -90,6 +90,7 @@ struct SensorSyncMove {  // NOLINT(cppcoreguidelines-pro-type-member-init)
     int32_t start_encoder_position;
     uint16_t usage_key;
     can::ids::SensorId sensor_id;
+    can::ids::SensorType sensor_type;
 
     auto build_ack(uint32_t position, int32_t pulses, uint8_t flags,
                    AckMessageId _id) -> Ack {
@@ -119,6 +120,7 @@ struct GearMotorMove  // NOLINT(cppcoreguidelines-pro-type-member-init)
     can::ids::PipetteTipActionType action;
     can::ids::GearMotorId gear_motor_id;
     can::ids::SensorId sensor_id;
+    can::ids::SensorType sensor_type;
     auto build_ack(uint32_t position, int32_t pulses, uint8_t flags,
                    AckMessageId _id) -> GearMotorAck {
         return GearMotorAck{message_index, group_id,

diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -25,7 +25,7 @@ template <lms::MotorMechanicalConfig MEConfig>
 class MotionController {
   public:
     using GenericQueue =
-#ifdef USE_PRESSURE_MOVE
+#ifdef USE_SENSOR_MOVE
         freertos_message_queue::FreeRTOSMessageQueue<SensorSyncMove>;
 #else
         freertos_message_queue::FreeRTOSMessageQueue<Move>;
@@ -63,7 +63,7 @@ class MotionController {
         -> const lms::LinearMotionSystemConfig<MEConfig>& {
         return linear_motion_sys_config;
     }
-#ifdef USE_PRESSURE_MOVE
+#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);
@@ -79,7 +79,8 @@ class MotionController {
             can_msg.request_stop_condition,
             0,
             hardware.get_usage_eeprom_config().get_distance_key(),
-            can_msg.sensor_id};
+            can_msg.sensor_id,
+            can_msg.sensor_type};
         if (!enabled) {
             enable_motor();
         }
@@ -100,7 +101,8 @@ class MotionController {
             .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_id = can::ids::SensorId::UNUSED,
+            .sensor_type = can::ids::SensorType::UNUSED};
         if (!enabled) {
             enable_motor();
         }
@@ -120,7 +122,8 @@ class MotionController {
             .stop_condition =
                 static_cast<uint8_t>(MoveStopCondition::limit_switch),
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key(),
-            .sensor_id = can::ids::SensorId::UNUSED};
+            .sensor_id = can::ids::SensorId::UNUSED,
+            .sensor_type = can::ids::SensorType::UNUSED};
         if (!enabled) {
             enable_motor();
         }
@@ -318,7 +321,8 @@ class PipetteMotionController {
             hardware.get_step_tracker(),
             can_msg.action,
             gear_motor_id,
-            can::ids::SensorId::UNUSED};
+            can::ids::SensorId::UNUSED,
+            can::ids::SensorType::UNUSED};
 
         if (!enabled) {
             enable_motor();

diff --git 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 <lms::MotorMechanicalConfig MEConfig>
 struct Motor {
+#ifdef USE_SENSOR_MOVE
+    using GenericQueue = FreeRTOSMessageQueue<SensorSyncMove>;
+#else
     using GenericQueue = FreeRTOSMessageQueue<Move>;
+#endif
     using UpdatePositionQueue = FreeRTOSMessageQueue<
         can::messages::UpdateMotorPositionEstimationRequest>;