diff --git a/programs/dmatest_v2.py b/programs/dmatest_v2.py
index 410d54362d84801a879707deb7b2a8475b66b3d7..3a5904aac49aa4c5f3d70a787098342493f5e5a4 100644
--- a/programs/dmatest_v2.py
+++ b/programs/dmatest_v2.py
@@ -88,17 +88,19 @@ class AdcDevice:
                                              read_err=True)
 
     def set_sampling_rate(self, rate):
-        async with self.__lock:
-            self._rate = rate
-            self.__adc_device.DIV_REG = (48000000 // self._rate - 1) << 8
+        self._rate = int(rate)
+        self.__adc_device.DIV_REG = (48000000 // self._rate - 1) << 8
 
     def get_sampling_rate(self):
         return self._rate
+    
+    def get_sample_count(self):
+        return self._sample_count
 
     def set_sample_count(self, samples):
-        async with self.__lock:
-            self._sample_count = samples
-            self._buffer = array.array("H", (0 for _ in range(samples)))
+
+        self._sample_count = samples
+        self._buffer = array.array("H", (0 for _ in range(samples)))
 
     def __stop_adc(self):
         dev = self.__adc_device
@@ -183,7 +185,7 @@ def calculate_frequency(voltages_centered, sampling_rate):
     consecutive_sign = 0
     counts = []
 
-    old_sign = voltages_centered > 0
+    old_sign = voltages_centered[0] > 0
     for v in voltages_centered:
         sign = v > 0
         if sign != old_sign:
@@ -193,10 +195,11 @@ def calculate_frequency(voltages_centered, sampling_rate):
         old_sign = sign
         consecutive_sign += 1
 
-    if len(counts) < 3:
+    if len(counts) < 5:
         return SAMPLING_FREQ_TO_HIGH
 
-    counts = counts[1:-1]
+    counts = counts[3:-1]
+    #print(counts)
     count_mean = sum(counts) / len(counts)
     if count_mean < 5:
         return SAMPLING_FREQ_TO_LOW
@@ -205,20 +208,20 @@ def calculate_frequency(voltages_centered, sampling_rate):
     return f
 
 
-def calculate_peak_to_peak(voltages_centered):
+def calculate_peak_to_peak(voltages_centered, voltage_mean):
     positive_peaks = []
     negative_peaks = []
     maximum = -9999
     minimum = 9999
-
-    old_sign = voltages_centered > 0
-    for v in voltages_centered:
+    start_idx = 0
+    old_sign = voltages_centered[start_idx] > 0
+    for v in voltages_centered[start_idx:]:
         sign = v > 0
         if sign != old_sign:
             if maximum > -9999:
-                positive_peaks.append(maximum)
+                positive_peaks.append(maximum + voltage_mean)
             if minimum < 9999:
-                negative_peaks.append(minimum)
+                negative_peaks.append(minimum + voltage_mean)
             maximum = -9999
             minimum = 9999
 
@@ -226,12 +229,15 @@ def calculate_peak_to_peak(voltages_centered):
             maximum = v
         if v < 0 and v < minimum:
             minimum = v
+        old_sign = sign
 
     if len(positive_peaks) < 3 or len(negative_peaks) < 3:
+        print(positive_peaks)
         return 0
-
+    
     negative_peaks = negative_peaks[1: -1]
     positive_peaks = positive_peaks[1: -1]
+    
     return sum(positive_peaks) / len(positive_peaks) - sum(negative_peaks) / len(negative_peaks)
 
 
@@ -242,28 +248,28 @@ async def main():
 
     while True:
         voltages = [v / 4096 * 3.3 for v in await device.measure()]
-        voltage_mean = sum(voltages) / SAMPLES
+        voltage_mean = sum(voltages) / device.get_sample_count()
         voltages_centered = [v - voltage_mean for v in voltages]
-        freq = calculate_frequency(voltages_centered, sampling_rate=SAMPLING_RATE_HZ)
+        freq = calculate_frequency(voltages_centered, sampling_rate=device.get_sampling_rate())
         if freq == SAMPLING_FREQ_TO_HIGH:
             rate = device.get_sampling_rate()
             if rate < 3:
-                print(f"Cannot reduce sampling frequency rate further than {rate}")
+                # print(f"Cannot reduce sampling frequency rate further than {rate}")
                 continue
             device.set_sampling_rate(rate / 2.0)
             print(f"Reducing sampling rate to {device.get_sampling_rate()}")
             continue
         elif freq == SAMPLING_FREQ_TO_LOW:
             rate = device.get_sampling_rate()
-            if rate > 100000:
-                print(f"Cannot reduce increase frequency rate further than {rate}")
+            if rate > 40000:
+                # print(f"Cannot reduce increase frequency rate further than {rate}")
                 continue
             device.set_sampling_rate(rate * 2)
             print(f"Increasing sampling rate to {device.get_sampling_rate()}")
             continue
 
-        v_to_v = calculate_peak_to_peak(voltages_centered)
-        print(f"{freq:0.2f} Hz, {v_to_v:0.2f} V peak to peak")
+        v_to_v = calculate_peak_to_peak(voltages_centered, voltage_mean)
+        print(f"{freq:0.2f} Hz, {v_to_v:0.2f} V peak to peak, sampling at {device.get_sampling_rate()}")
         gc.collect()