From 3a61b4a6accb64cfe712629b50b27cf07d6c45a1 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 8 Oct 2018 19:40:44 -0700
Subject: [PATCH] lockin with enc sense working

---
 Firmware/MotorControl/axis.cpp    |  6 ++++--
 Firmware/MotorControl/axis.hpp    | 11 ++++++++++-
 Firmware/MotorControl/encoder.cpp |  2 ++
 Firmware/MotorControl/encoder.hpp |  4 +++-
 4 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 2136ebcb..276aa8ca 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -134,6 +134,7 @@ float Axis::get_temp() {
 
 bool Axis::run_lockin_spin() {
     // Spiral up current for softer rotor lock-in
+    lockin_state_ = LOCKIN_STATE_RAMP;
     float x = 0.0f;
     run_control_loop([&]() {
         float phase = wrap_pm_pi(config_.lockin_ramp_time * x);
@@ -143,8 +144,6 @@ bool Axis::run_lockin_spin() {
             return false;
         return x < 1.0f;
     });
-    if (error_ != ERROR_NONE)
-        return false;
     
     // Spin states
     float distance = config_.lockin_ramp_time;
@@ -164,6 +163,7 @@ bool Axis::run_lockin_spin() {
     };
 
     // Accelerate
+    lockin_state_ = LOCKIN_STATE_ACCELERATE;
     run_control_loop([&]() {
         vel += config_.lockin_accel * current_meas_period;
         distance += vel * current_meas_period;
@@ -176,6 +176,7 @@ bool Axis::run_lockin_spin() {
 
     // Constant speed
     if (!spin_done()) {
+        lockin_state_ = LOCKIN_STATE_CONST_VEL;
         vel = config_.lockin_vel; // reset to actual specified vel to avoid small integration error
         run_control_loop([&]() {
             distance += vel * current_meas_period;
@@ -187,6 +188,7 @@ bool Axis::run_lockin_spin() {
         });
     }
 
+    lockin_state_ = LOCKIN_STATE_INACTIVE;
     return check_for_errors();
 }
 
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index 31dc1488..36edaab7 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -55,7 +55,7 @@ public:
         float lockin_ramp_distance = 4 * M_PI;  // [rad]
         float lockin_accel = 400.0f;     // [rad/s^2]
         float lockin_vel = 400.0f; // [rad/s]
-        bool lockin_finish_on_vel = false;
+        bool lockin_finish_on_vel = true;
         bool lockin_finish_on_distance = false;
         bool lockin_finish_on_enc_idx = false;
         float lockin_finish_distance = 1000.0f;  // [rad]
@@ -65,6 +65,13 @@ public:
         M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
     };
 
+    enum LockinState_t {
+        LOCKIN_STATE_INACTIVE,
+        LOCKIN_STATE_RAMP,
+        LOCKIN_STATE_ACCELERATE,
+        LOCKIN_STATE_CONST_VEL,
+    };
+
     Axis(const AxisHardwareConfig_t& hw_config,
             Config_t& config,
             Encoder& encoder,
@@ -176,6 +183,7 @@ public:
     State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
     State_t& current_state_ = task_chain_[0];
     uint32_t loop_counter_ = 0;
+    LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
 
     // Communication protocol definitions
     auto make_protocol_definitions() {
@@ -185,6 +193,7 @@ public:
             make_protocol_ro_property("current_state", &current_state_),
             make_protocol_property("requested_state", &requested_state_),
             make_protocol_ro_property("loop_counter", &loop_counter_),
+            make_protocol_ro_property("lockin_state", &lockin_state_),
             make_protocol_object("config",
                 make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
                 make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index f8e1e530..45885c29 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -43,6 +43,8 @@ bool Encoder::do_checks(){
 // TODO: disable interrupt once we found the index
 void Encoder::enc_index_cb() {
     if (config_.use_index && !index_found_) {
+        if (config_.find_idx_on_lockin && axis_->lockin_state_ != Axis::LOCKIN_STATE_CONST_VEL)
+            return;
         set_circular_count(0, false);
         set_linear_count(0); // Avoid position control transient after search
         if (config_.pre_calibrated) {
diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp
index 6eca0536..baa32ad0 100644
--- a/Firmware/MotorControl/encoder.hpp
+++ b/Firmware/MotorControl/encoder.hpp
@@ -36,6 +36,7 @@ public:
         float offset_float = 0.0f; // Sub-count phase alignment offset
         float calib_range = 0.02f;
         float bandwidth = 1000.0f;
+        bool find_idx_on_lockin = false;
     };
 
     Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -104,7 +105,8 @@ public:
                 make_protocol_property("offset_float", &config_.offset_float),
                 make_protocol_property("bandwidth", &config_.bandwidth,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
-                make_protocol_property("calib_range", &config_.calib_range)
+                make_protocol_property("calib_range", &config_.calib_range),
+                make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin)
             )
         );
     }