From b54b75036ff0c1d34bc99ed687c1298635173a86 Mon Sep 17 00:00:00 2001
From: rudi <rudi.klein@rudiklein.nl>
Date: Fri, 27 Dec 2024 16:10:25 +0100
Subject: [PATCH] PID and sensor measurement code applied

---
 control_functions.py | 67 +++++++++++++++++++++++++++++++++-----------
 main.py              |  1 +
 2 files changed, 52 insertions(+), 16 deletions(-)

diff --git a/control_functions.py b/control_functions.py
index 271a64f..3e5dc47 100644
--- a/control_functions.py
+++ b/control_functions.py
@@ -1,3 +1,4 @@
+from OpenGL.images import returnFormat
 from adafruit_hcsr04 import HCSR04 as hcsr04
 import board
 import data_transfer_functions as dt
@@ -7,6 +8,7 @@ import matplotlib.pyplot as plt
 from scipy.integrate import odeint
 import numpy as np
 import matplotlib.pyplot as plt
+import statistics as st
 
 # Variables to control sensor
 TRIGGER_PIN = board.D4
@@ -18,6 +20,7 @@ MAX_DISTANCE = 40
 # Variables to control logging.
 LOG = True
 SCREEN = True
+DEBUG = False
 
 # PID measurements
 time = 0
@@ -25,6 +28,12 @@ integral = 0
 time_prev = -1e-6
 e_prev = 0
 
+# PID values
+P_value = 2
+I_value = 0
+D_value = 0
+
+sample_array = []
 
 def initial():
     ...
@@ -33,42 +42,68 @@ def initial():
 def read_distance_sensor(fixed_file_stamp):
 
     with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
-
-        while True:
+        samples = 0
+        max_samples = 10
+        timestamp_last = 0.0
+        timestamp_first = 0.0
+        while samples != max_samples:
             try:
                 distance = sonar.distance
                 if MIN_DISTANCE < distance < MAX_DISTANCE:
 
                     dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
-                    print("Distance: ", sonar.distance) if SCREEN else None
+                    print("Distance: ", distance) if SCREEN else None
+
+                    sample_array.append(distance)
+                    if samples == 0: timestamp_first, _ = dt.timestamper()
+                    if samples == max_samples - 1: timestamp_last, _ = dt.timestamper()
+
+                    timestamp_first_float = float(timestamp_first)
+                    timestamp_last_float = float(timestamp_last)
+                    samples = samples + 1
+                    median_distance = st.median(sample_array)
+                    mean_timestamp  = st.mean([timestamp_first_float, timestamp_last_float])
+                    print(median_distance) if LOG else None
+                    print(mean_timestamp)
+
                 else:
-                    dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
+                    dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
                     print("Distance: ", distance) if SCREEN else None
 
             except RuntimeError:
-                dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
+                dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None
                 print("Timeout") if SCREEN else None
 
+    return median_distance, mean_timestamp
+
 def read_setpoint():
     ...
 
 
 def calculate_velocity():
+    ...
 
+def PID_calculations(setpoint):
 
-
-def PID_calculations(Kp, Ki, Kd, setpoint, measurement):
-
-    global cur_time, integral, prev_time, prev_error
+    global I_result, previous_time, previous_error
     offset_value = 320
-    # PID calculations
+    measurement, current_time = read_distance_sensor
     error = setpoint - measurement
-    P = Kp * error
-    integral = integral + Ki * error * (cur_time - prev_time)
-    D = Kd * (error - prev_error) / (cur_time - prev_time)
-    PID_result = offset_value + P + integral + D
-    prev_error = error
-    prev_time = cur_time
+
+    if previous_time is None:
+        previous_error: float = 0.0
+        previous_time: float = current_time
+        I_result: float = 0.0
+        error_sum: float = error * 0.008 # sensor sampling number approximation.
+
+    error_sum: float = error_sum +  (error * (current_time - previous_time))
+    P_result: float = P_value * error
+    I_result: float =  I_value * error_sum
+    D_result: float = D_value * ((error - previous_error) / (current_time - previous_time))
+    PID_result: float = offset_value + P_result + I_result + D_result
+    previous_error: float = error
+    previous_time: float = current_time
+
     return PID_result
 
 
diff --git a/main.py b/main.py
index b199b4d..bda1ec2 100644
--- a/main.py
+++ b/main.py
@@ -7,6 +7,7 @@ from scipy.integrate import odeint
 import numpy as np
 import matplotlib.pyplot as plt
 import data_transfer_functions as dt
+import statistics as st
 
 _, fixed_file_stamp = dt.timestamper()