From ac09f4e2d2762adda60fa3a9c0b9b9704fe238dd Mon Sep 17 00:00:00 2001
From: caila-marashaj <caila.marashaj@opentrons.com>
Date: Mon, 29 Jan 2024 10:12:18 -0500
Subject: [PATCH] send report message on start sensor move

---
 include/motor-control/core/motor_messages.hpp |  20 ++++
 .../core/stepper_motor/motion_controller.hpp  |  20 ++++
 .../stepper_motor/motor_interrupt_handler.hpp |  43 +++----
 .../pipettes/core/motor_interrupt_handler.hpp | 108 +++++++++++++-----
 4 files changed, 143 insertions(+), 48 deletions(-)

diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp
index bfe573397..48545cfce 100644
--- a/include/motor-control/core/motor_messages.hpp
+++ b/include/motor-control/core/motor_messages.hpp
@@ -79,6 +79,26 @@ struct Move {  // NOLINT(cppcoreguidelines-pro-type-member-init)
     }
 };
 
+struct SensorSyncMove  // NOLINT(cppcoreguidelines-pro-type-member-init)
+        : public Move {
+    can::ids::SensorId sensor_id;
+
+    auto build_ack(int32_t pulses, uint8_t flags, AckMessageId _id) -> Ack {
+        // not sure if we need to include sensor id in the ack
+        return Ack{
+                .message_index = message_index,
+                .group_id = group_id,
+                .seq_id = seq_id,
+                .current_position_steps = 0,
+                .encoder_position = pulses,
+                .position_flags = flags,
+                .ack_id = _id,
+                .start_encoder_position = start_encoder_position,
+                .usage_key = usage_key,
+        };
+    }
+};
+
 struct GearMotorMove  // NOLINT(cppcoreguidelines-pro-type-member-init)
     : public Move {
     uint32_t start_step_position;
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 3a4b71c57..3cdd3ffdc 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -59,6 +59,26 @@ class MotionController {
         return linear_motion_sys_config;
     }
 
+    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{
+                .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()};
+        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);
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 ee57dbd3d..cc8307ac1 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -594,26 +594,27 @@ class MotorInterruptHandler {
         return has_active_move() && buffered_move.velocity != 0;
     }
 
-  private:
-    void update_hardware_step_tracker() {
-        hardware.set_step_tracker(
-            static_cast<uint32_t>(position_tracker >> 31));
-    }
-
-    uint64_t tick_count = 0x0;
-    static constexpr const q31_31 tick_flag = 0x80000000;
-    static constexpr const uint64_t overflow_flag = 0x8000000000000000;
-    // Tracks position with sub-microstep accuracy
-    q31_31 position_tracker{0};
-    MoveQueue& move_queue;
-    StatusClient& status_queue_client;
-    MotorHardware& hardware;
-    stall_check::StallCheck& stall_checker;
-    UpdatePositionQueue& update_position_queue;
-    MotorMoveMessage buffered_move = MotorMoveMessage{};
-    bool clear_queue_until_empty = false;
-    bool stall_handled = false;
-    bool in_estop = false;
-    std::atomic_bool _has_active_move = false;
+//  private:
+protected:
+void update_hardware_step_tracker() {
+    hardware.set_step_tracker(
+        static_cast<uint32_t>(position_tracker >> 31));
+}
+
+uint64_t tick_count = 0x0;
+static constexpr const q31_31 tick_flag = 0x80000000;
+static constexpr const uint64_t overflow_flag = 0x8000000000000000;
+// Tracks position with sub-microstep accuracy
+q31_31 position_tracker{0};
+MoveQueue& move_queue;
+StatusClient& status_queue_client;
+MotorHardware& hardware;
+stall_check::StallCheck& stall_checker;
+UpdatePositionQueue& update_position_queue;
+MotorMoveMessage buffered_move = MotorMoveMessage{};
+bool clear_queue_until_empty = false;
+bool stall_handled = false;
+bool in_estop = false;
+std::atomic_bool _has_active_move = false;
 };
 }  // namespace motor_handler
diff --git a/include/pipettes/core/motor_interrupt_handler.hpp b/include/pipettes/core/motor_interrupt_handler.hpp
index 6dec18f00..989589703 100644
--- a/include/pipettes/core/motor_interrupt_handler.hpp
+++ b/include/pipettes/core/motor_interrupt_handler.hpp
@@ -5,19 +5,20 @@
 
 namespace pipettes {
 using namespace motor_messages;
+//using InterruptHandler = motor_handler::MotorInterruptHandler<
+//    freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
+//    MotorMoveMessage, MotorHardware>;
+// might need to change MotorMoveMessage to include sensormove
 template <template <class> class QueueImpl, class StatusClient,
           typename MotorMoveMessage, typename MotorHardware, class SensorClient>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
 class PipetteMotorInterruptHandler
-    : public motor_handler::MotorInterruptHandler<
-          freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
-          MotorMoveMessage, MotorHardware> {
+: public motor_handler::MotorInterruptHandler<
+    freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
+    MotorMoveMessage, MotorHardware> {
   public:
     using MoveQueue = QueueImpl<MotorMoveMessage>;
-    using InterruptHandler = motor_handler::MotorInterruptHandler<
-        freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
-        MotorMoveMessage, MotorHardware>;
     using UpdatePositionQueue =
         QueueImpl<can::messages::UpdateMotorPositionEstimationRequest>;
     PipetteMotorInterruptHandler() = delete;
@@ -26,7 +27,9 @@ class PipetteMotorInterruptHandler
         MotorHardware& hardware_iface, stall_check::StallCheck& stall,
         UpdatePositionQueue& incoming_update_position_queue,
         SensorClient& sensor_queue_client) :
-          InterruptHandler(incoming_move_queue, outgoing_queue, hardware_iface,
+            motor_handler::MotorInterruptHandler<
+    freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
+    MotorMoveMessage, MotorHardware>(incoming_move_queue, outgoing_queue, hardware_iface,
                            stall, incoming_update_position_queue),
           sensor_client(sensor_queue_client) {
 //        sensor_client = sensor_tasks::QueueClient{};
@@ -49,44 +52,95 @@ class PipetteMotorInterruptHandler
 
     // need to write a new kind of move that contains the stuff thats gonna be sent in the new can message
 
+    void run_interrupt() {
+        // handle various error states
+        if (this->clear_queue_until_empty) {
+            // If we were executing a move when estop asserted, and
+            // what's in the queue is the remaining enqueued moves from
+            // that group, then we will have called
+            // cancel_and_clear_moves and clear_queue_until_empty will
+            // be true. That means we should pop out the queue.
+            // clear_queue_until_empty will also be true if we were in
+            // the steady-state estop asserted; got new messages; and
+            // the other branch of this if asserted. In either case, an
+            // error message has been sent, so we need to just keep
+            // clearing the queue.
+            this->clear_queue_until_empty = this->pop_and_discard_move();
+        } else if (this->in_estop) {
+            this->handle_update_position_queue();
+            this->in_estop = this->estop_update();
+        } else if (this->estop_triggered()) {
+            this->cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
+            this->in_estop = true;
+        } else if (this->hardware.has_cancel_request()) {
+            this->cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
+                                   can::ids::ErrorSeverity::warning);
+        } else {
+            // Normal Move logic
+            this->run_normal_interrupt();
+        }
+    }
+
+    void handle_move_type(motor_messages::Move m) {
+    }
+
+    void handle_move_type(motor_messages::SensorSyncMove m) {
+        auto msg = can::messages::BindSensorOutputRequest{
+            .message_index = m.message_index,
+            .sensor = can::ids::SensorType::pressure,
+            .sensor_id = m.sensor_id,
+            .binding = can::ids::SensorOutputBinding::report
+        };
+    }
+
     void update_move() {
         // do stuff in here
-        _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();
+        this->_has_active_move = this->move_queue.try_read_isr(&this->buffered_move);
+        if (this->_has_active_move) {
+            handle_move_type(this->buffered_move);
+            this->hardware.enable_encoder();
+            this->buffered_move.start_encoder_position =
+                    this->hardware.get_encoder_pulses();
         }
-        if (set_direction_pin()) {
-            hardware.positive_direction();
+        if (this->set_direction_pin()) {
+            this->hardware.positive_direction();
         } else {
-            hardware.negative_direction();
+            this->hardware.negative_direction();
         }
-        if (_has_active_move && buffered_move.check_stop_condition(
+        if (this->_has_active_move && this->buffered_move.check_stop_condition(
                 MoveStopCondition::limit_switch)) {
-            position_tracker = 0x7FFFFFFFFFFFFFFF;
-            update_hardware_step_tracker();
-            hardware.position_flags.clear_flag(
+            this->position_tracker = 0x7FFFFFFFFFFFFFFF;
+            this->update_hardware_step_tracker();
+            this->hardware.position_flags.clear_flag(
                     can::ids::MotorPositionFlags::stepper_position_ok);
-            hardware.position_flags.clear_flag(
+            this->hardware.position_flags.clear_flag(
                     can::ids::MotorPositionFlags::encoder_position_ok);
         }
     }
 
     void finish_current_move(
             AckMessageId ack_msg_id = AckMessageId::complete_without_condition) {
-        _has_active_move = false;
-        tick_count = 0x0;
-        stall_handled = false;
-        build_and_send_ack(ack_msg_id);
-        set_buffered_move(MotorMoveMessage{});
+        this->_has_active_move = false;
+        this->tick_count = 0x0;
+        this->stall_handled = false;
+        this->build_and_send_ack(ack_msg_id);  // might want to overwrite this func
+        (MotorMoveMessage{});
         // update the stall check ideal encoder counts based on
         // last known location
 
         // check if queue is empty in here and send bindsensoroutputrequest
 
-        if (!has_move_messages()) {
-            stall_checker.reset_itr_counts(hardware.get_step_tracker());
+        if (!this->has_move_messages()) {
+            this->stall_checker.reset_itr_counts(this->hardware.get_step_tracker());
+        }
+    }
+
+    void send_to_pressure_sensor_queue(can::messages::SensorOutputBinding& m) {
+        if(this->buffered_move.sensor_id == can::ids::SensorId::S1) {
+            sensor_client.send_pressure_sensor_queue_front(m);
+        }
+        else {
+            sensor_client.send_pressure_sensor_queue_rear(m);
         }
     }