Additions for PID en twin

control_functions.py

@@ -8,6 +8,7 @@ import statistics as st                             # Mean and median calculatio
 import csv                                          # CSV handling
 from datetime import datetime                       # Date and time formatting
 from time import sleep                              # Sleep/pause
+import pandas as pd
 
 # Variables to control sensor
 TRIGGER_PIN = board.D4      # GPIO pin xx
@@ -27,6 +28,7 @@ KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
 LOG: bool   = True         # Log data to files
 SCREEN: bool = True         # Log data to screen
 DEBUG: bool = True         # More data to display
+TWIN_MODE: bool = False
 
 # Control the number of samples for single distance measurement (average from burst)
 MAX_SAMPLES: int = 10
@@ -70,6 +72,16 @@ integral: float = 0
 # Init array, used in read_distance_sensor()
 sample_array: list = []
 
+# Error sum array
+error_sum_array: list = []
+error_sum_counter: int = 0
+error_sum_max: int = 100
+
+# Digital twin
+previous_speed:float = 0.0
+start_loop = True
+previous_measurement: float = 0.0
+
 # Write data to any of the logfiles
 def log_data(data_file: str, data_line: str, remark: str|None):
     log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
@@ -149,35 +161,37 @@ def calculate_acceleration():
     position_2, timestamp_2 = read_distance_sensor()
     position_3, timestamp_3 = read_distance_sensor()
 
-    initial_velocity: float = (position_1 - position_2) / (timestamp_2 - timestamp_1)
+    initial_velocity: float = (position_2 - position_1) / (timestamp_2 - timestamp_1)
-    final_velocity: float  = ((position_2 - position_3) / (timestamp_3 - timestamp_2))
+    final_velocity: float  = ((position_3 - position_2) / (timestamp_3 - timestamp_2))
     acceleration: float  = (final_velocity - initial_velocity) / (timestamp_3 - timestamp_1)
-    delta_t: float = timestamp_3 - timestamp_1
 
     print(initial_velocity, " ", final_velocity, " ", acceleration)  if SCREEN  else None
 
-    data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration) + ',' + str(delta_t)
+    data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
     log_data(data_file="acceleration", data_line=data_line, remark="") if LOG else None
 
 def pid_calculations(setpoint):
 
     global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
-    offset_value: int = 320
+    global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
-    measurement, measurement_time = read_distance_sensor()
+    offset_value: int = 0
+    if TWIN_MODE:
+        measurement, measurement_time = digital_twin()
+    else:
+        measurement, measurement_time = read_distance_sensor()
+
     error = setpoint - measurement
-    error_sum: float = 0.0
 
     if previous_time is None:
         previous_error = 0.0
-        previous_time = current_time
+        previous_time = measurement_time
         i_result = 0.0
-        error_sum: float = error * 0.008 # sensor sampling number approximation.
 
-    error_sum: float = error_sum +  (error * (current_time - previous_time))
+    error_sum_array[error_sum_counter] = (error * (measurement_time - previous_time))
-    p_result: float = p_value * error
+    p_result = p_value * error
-    i_result: float =  i_value * error_sum
+    i_result =  i_value * sum(error_sum_array)
-    d_result: float   = d_value * ((error - previous_error) / (measurement_time - previous_time))
+    d_result = d_value * ((error - previous_error) / (measurement_time - previous_time))
-    pid_result: float   = offset_value + p_result + i_result + d_result
+    pid_result = offset_value + p_result + i_result + d_result
     previous_error = error
     previous_time = measurement_time
 
@@ -188,7 +202,12 @@ def pid_calculations(setpoint):
         print("P_result: ", p_result)
         print("D_result: ", d_result)
         print("I_result: ", i_result)
-        print("PDI_result: ", pid_result)
+        print("PID_result: ", pid_result)
+
+    if error_sum_counter <= error_sum_max:
+        error_sum_counter = error_sum_counter + 1
+    else:
+        error_sum_counter = 0
 
     return pid_result
 
@@ -196,4 +215,27 @@ def control_server_angle(angle):
     KIT.servo[0].angle = angle # Set angle
     log_line = str(angle)
     log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
-    print(angle) if SCREEN else None
+    print(angle) if SCREEN else None
+
+def digital_twin(pid_angle):
+    global start_loop
+    measurement_time = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])
+
+    if start_loop:
+        delta_t = measurement_time - (measurement_time - 0.002)
+        start_loop = False
+    else:
+        delta_t = measurement_time - previous_time
+
+    twin_data = pd.read_csv('twin_data_file.csv')
+    twin_data.set_index('Arm angle', inplace=True)
+    acceleration = twin_data.loc[pid_angle, 'Acceleration']
+
+    # previous acceleration to speed.
+    new_speed = previous_speed + (acceleration*delta_t)
+    measurement = new_speed * delta_t + previous_measurement
+
+    print(measurement)
+    print(new_speed)
+    print(previous_speed)
+    return measurement, measurement_time

main.py

@@ -6,7 +6,7 @@ with open("pid-balancer_" + "time_file.txt", "w") as time_file:
 
 while True:
 
-    cf.calculate_acceleration()
+    cf.read_distance_sensor()