this version works

control_functions.py

@@ -11,10 +11,11 @@ import pandas as pd
 from datetime import datetime
 import busio
 import adafruit_vl6180x
+import math
 
 # laser sensor controls.
-i2c = busio.I2C(board.SCL, board.SDA)
+# i2c = busio.I2C(board.SCL, board.SDA)
-laser = adafruit_vl6180x.VL6180X(i2c)
+# laser = adafruit_vl6180x.VL6180X(i2c)
 
 # Variables to control sensor
 TRIGGER_PIN = board.D4      # GPIO pin xx
@@ -35,7 +36,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 = False         # More data to display
-TWIN_MODE: bool = False     # Run in live or twin mode
+TWIN_MODE: bool = True     # Run in live or twin mode
 
 # Control the number of samples for single distance measurement (average from burst)
 MAX_SAMPLES: int = 1
@@ -80,15 +81,20 @@ error_sum_counter: int = 0
 
 # Digital twin
 previous_speed:float = 0.0
-start_loop = True
+previous_position: float = 0.0
-previous_measurement: float = 0.0
+previous_angle: int = 90
 
 #maximum angle the servo can move away from steady position. With 10 the range is between 80 and 100, with steady at 90
-max_angle = 6
+max_angle = 10
 
 # servo slower
 current_angle:int = 90
 
+watch_variable: int = 0
+
+# base time of the system
+base_time: float = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+
 # 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]
@@ -190,34 +196,66 @@ def read_setpoint():
 
         return cm_rounded
 
-def calculate_acceleration():
+def digital_twin():
+    # a: acceleration
+    # g: gravity (9.81 m/s^2)
+    # theta: angle of the inclined plane
+    # u: coefficient of the friction between the cart and the inclined plane.
+    acceleration: float = 0.0
+    global previous_position, previous_speed, base_time, watch_variable
+    gravity: float = 9.81
+    friction: float = 0.1
+    delta_t: float = 0.1
 
-    print("calc is active")
+    angle = (previous_angle - 90)
+    acceleration = gravity * math.sin(math.radians(angle))
+    friction_force = friction * gravity * math.cos(math.radians(angle)) * delta_t
 
-    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+    friction_force = abs(friction_force)
 
-    position_1, timestamp_1 = read_distance_sensor()
+    work_speed = previous_speed + acceleration * delta_t
-    position_2, timestamp_2 = read_distance_sensor()
+    watch_variable = watch_variable + 1
-    position_3, timestamp_3 = read_distance_sensor()
 
-    initial_velocity: float = (position_2 - position_1) / (timestamp_2 - timestamp_1)
+    if watch_variable >= 150:
-    final_velocity: float  = ((position_3 - position_2) / (timestamp_3 - timestamp_2))
+        print("breakpoint")
-    acceleration: float  = (final_velocity - initial_velocity) / (timestamp_3 - timestamp_1)
 
-    print(initial_velocity, " ", final_velocity, " ", acceleration)  if SCREEN  else None
+    print("watch_variable", watch_variable)
+    if friction_force < work_speed:
+        if work_speed > 0:
+             work_speed = work_speed - friction_force
+        elif work_speed < 0:
+            work_speed = work_speed + friction_force
+        else:
+            work_speed = work_speed
 
-    data_line: str = str(position_1) + ',' + str(position_2) + ',' + str(position_3) + ',' + str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
+    current_speed = work_speed
-    log_data(data_file="acceleration", data_line=data_line, remark="") if LOG else None
 
-    end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+    current_position = previous_position + (current_speed * delta_t)
-    data_line = str(start_time - end_time)
+
-    log_data(data_file="function", data_line=data_line, remark="calculate_acceleration") if LOG else None
+
+    print("angle", angle)
+    print("friction", friction)
+    print("acceleration", acceleration)
+    print("current speed", current_speed)
+    print("current position", current_position)
+    print("")
+    print("----------------")
+    print("")
+
+    base_time = base_time + delta_t
+
+
+    previous_speed = current_speed
+    previous_position = current_position
+
+    return current_position, base_time
 
 def pid_calculations():
     start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
 
     global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
     global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
+    global previous_angle
     offset_value: int = 0
     if TWIN_MODE:
         measurement, measurement_time = digital_twin()
@@ -272,6 +310,7 @@ def pid_calculations():
     log_data(data_file="function", data_line=data_line, remark="pid_calculations") if LOG else None
 
     output_angle = round(output_angle)
+    previous_angle = output_angle
 
     return output_angle
 
@@ -289,36 +328,6 @@ def control_server_angle(angle):
     data_line = str(start_time - end_time)
     log_data(data_file="function", data_line=data_line, remark="control_server_angle") if LOG else None
 
-def digital_twin(pid_angle):
-    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
-
-    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)
-
-    end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
-    data_line = str(start_time - end_time)
-    log_data(data_file="function", data_line=data_line, remark="digital_twin") if LOG else None
-
-    return measurement, measurement_time
-
 def servo_slower():
     start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
 
@@ -340,11 +349,9 @@ def servo_slower():
     return servo_angle
 
 try:
-    with open("pid-balancer_" + "time_file.txt", "w") as time_file:
-        time_file.write(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3])
     KIT.servo[0].angle = 90
     while True:
-        calculate_acceleration()
+        # digital_twin()
-        # control_server_angle(pid_calculations())
+        control_server_angle(pid_calculations())
 except RuntimeError:
     print("bbbb")