diff --git a/gateware/logic/autolock.py b/gateware/logic/autolock.py
index a6dc764..975b23f 100644
--- a/gateware/logic/autolock.py
+++ b/gateware/logic/autolock.py
@@ -148,14 +148,17 @@ class RobustAutolock(Module, AutoCSR):
         final_waited_for = Signal(bits_for(N_points))
 
         # this is the signal that's used for detecting peaks
-        sum_diff = Signal((len(self.sum_diff_calculator.output), True))
-        abs_sum_diff = Signal.like(sum_diff)
+        self.sum_diff = Signal((len(self.sum_diff_calculator.output), True))
+        abs_sum_diff = Signal.like(self.sum_diff)
         self.comb += [
             self.sum_diff_calculator.writing_data_now.eq(self.writing_data_now),
             self.sum_diff_calculator.restart.eq(self.at_start),
             self.sum_diff_calculator.input.eq(self.input),
             self.sum_diff_calculator.delay_value.eq(self.time_scale.storage),
-            sum_diff.eq(self.sum_diff_calculator.output),
+        ]
+
+        self.sync += [
+            self.sum_diff.eq(self.sum_diff_calculator.output),
         ]
 
         # has this signal at the moment the same sign as the peak we are looking for?
@@ -167,36 +170,41 @@ class RobustAutolock(Module, AutoCSR):
         # have we detected all peaks (and can turn on the lock)?
         all_instructions_triggered = Signal()
 
-        self.comb += [
-            sign_equal.eq((sum_diff > 0) == (current_peak_height > 0)),
-            If(sum_diff >= 0, abs_sum_diff.eq(sum_diff)).Else(
-                abs_sum_diff.eq(-1 * sum_diff)
-            ),
-            If(
-                current_peak_height >= 0,
+        self.sync += [
+            If(current_peak_height >= 0, 
                 abs_current_peak_height.eq(current_peak_height),
             ).Else(abs_current_peak_height.eq(-1 * current_peak_height)),
-            over_threshold.eq(abs_sum_diff >= abs_current_peak_height),
-            waited_long_enough.eq(waited_for > current_wait_for),
             all_instructions_triggered.eq(
                 self.current_instruction_idx >= self.N_instructions.storage
             ),
+        ]
+        self.comb += [
+            sign_equal.eq((self.sum_diff > 0) == (current_peak_height > 0)),
+            If(self.sum_diff >= 0, abs_sum_diff.eq(self.sum_diff)).Else(
+                abs_sum_diff.eq(-1 * self.sum_diff)
+            ),
+            over_threshold.eq(abs_sum_diff >= abs_current_peak_height),
+            waited_long_enough.eq(waited_for > current_wait_for),
             self.turn_on_lock.eq(
                 all_instructions_triggered
                 & (final_waited_for >= self.final_wait_time.storage)
             ),
         ]
 
+        watching_reg = Signal()
         self.sync += [
+            watching.eq(watching_reg),
             If(
                 self.at_start,
-                waited_for.eq(0),
+                # Compensate pipeline delay
+                waited_for.eq(1),
                 # fpga robust autolock algorithm registeres trigger events delayed.
                 # Therefore, we give it a head start for `final_waited_for`
                 final_waited_for.eq(ROBUST_AUTOLOCK_FPGA_DELAY),
                 self.current_instruction_idx.eq(0),
-                If(self.request_lock, watching.eq(1)).Else(watching.eq(0)),
+                If(self.request_lock, watching_reg.eq(1)).Else(watching.eq(0), watching_reg.eq(0)),
             ).Else(
+                # Compensate pipeline delay
                 # not at start
                 If(
                     ~self.request_lock,
@@ -213,7 +221,8 @@ class RobustAutolock(Module, AutoCSR):
                         self.current_instruction_idx.eq(
                             self.current_instruction_idx + 1
                         ),
-                        waited_for.eq(0),
+                        # Compensate pipeline delay
+                        waited_for.eq(1),
                     ).Else(waited_for.eq(waited_for + 1)),
                 ),
                 If(