From d59c90468833aeb8b6da2a0a126453d6ce9e4c43 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Bourdeauducq?= <sb@m-labs.hk>
Date: Mon, 20 Jan 2025 21:01:49 +0800
Subject: [PATCH] servo WIP

---
 sndlock.cpp | 44 +++++++++++++++++++++++++++++++++++++-------
 1 file changed, 37 insertions(+), 7 deletions(-)

diff --git a/sndlock.cpp b/sndlock.cpp
index 07412fc..572c8f0 100644
--- a/sndlock.cpp
+++ b/sndlock.cpp
@@ -258,7 +258,7 @@ static std::atomic<float> init_current_cooling[SND_PCHAN];
 static std::atomic<float> init_current_heating[SND_PCHAN];
 static std::atomic<float> init_temp[SND_PCHAN];
 static std::atomic<float> leadin_current[SND_PCHAN];
-static std::atomic<float> leadin_temp[SND_PCHAN];
+static std::atomic<float> leadin_mag[SND_PCHAN];
 
 static void servo_thread(int channel)
 {
@@ -276,6 +276,34 @@ static void servo_thread(int channel)
         }
 
         servo_state[channel] = "INIT";
+        if(temp > init_temp[channel]) {
+            kirdy.set_tec_current(init_current_cooling[channel]);
+            while(servo_enable[channel] && temp > init_temp[channel]) {
+                clocker.tick();
+                laser_temp[channel] = temp = kirdy.get_laser_temp();
+            }
+        } else {
+            kirdy.set_tec_current(init_current_heating[channel]);
+            while(servo_enable[channel] && temp < init_temp[channel]) {
+                clocker.tick();
+                laser_temp[channel] = temp = kirdy.get_laser_temp();
+            }
+        }
+        if(!servo_enable[channel])
+            continue;
+
+        servo_state[channel] = "LEAD-IN";
+        kirdy.set_tec_current(leadin_current[channel]);
+        while(servo_enable[channel] && li_mag[channel] < leadin_mag[channel]) {
+            clocker.tick();
+            laser_temp[channel] = temp = kirdy.get_laser_temp();
+        }
+
+        servo_state[channel] = "LOCKING";
+        while(servo_enable[channel]) {
+            clocker.tick();
+            laser_temp[channel] = temp = kirdy.get_laser_temp();
+        }
     }
 }
 
@@ -321,10 +349,10 @@ int main(int argc, char* argv[])
         lpf_bandwidth[i] = 10.0;
         servo_state[i] = "DISABLED";
         init_current_cooling[i] = 100.0f;
-        init_current_heating[i] = 30.0f;
+        init_current_heating[i] = -30.0f;
         init_temp[i] = 25.0f;
         leadin_current[i] = 100.0f;
-        leadin_temp[i] = 24.0f;
+        leadin_mag[i] = 0.01f;
     }
 
     static std::thread dsp_thread_h = std::thread(dsp_thread);
@@ -344,6 +372,8 @@ int main(int argc, char* argv[])
 
     shutdown_threads = false;
     static auto SetShutdown = []() {
+        for(int i=0;i<SND_PCHAN;i++)
+            servo_enable[i] = false;
         shutdown_threads = true;
     };
     std::atexit(SetShutdown);
@@ -492,10 +522,10 @@ int main(int argc, char* argv[])
                 ImGui::InputFloat(str, &leadin_current_l, 1.0f, 500.0f, "%.3f");
                 leadin_current[i] = leadin_current_l;
                 ImGui::SameLine();
-                sprintf(str, "temp °C##leadin%d", i);
-                float leadin_temp_l = leadin_temp[i];
-                ImGui::InputFloat(str, &leadin_temp_l, 1.0f, 500.0f, "%.4f");
-                leadin_temp[i] = leadin_temp_l;
+                sprintf(str, "magnitude##%d", i);
+                float leadin_mag_l = leadin_mag[i];
+                ImGui::InputFloat(str, &leadin_mag_l, 0.0f, 0.001f, "%.6f");
+                leadin_mag[i] = leadin_mag_l;
                 ImGui::PopItemWidth();
             }
         }