Today's nightmare

Examples/Adafruit_Servokit_servo_driver.py

@@ -1,5 +1,6 @@
 import math
 from adafruit_servokit import ServoKit
+from time import sleep
 
 kit = ServoKit(channels=16)
 MIN_PULSE = 400             # Defines angle 80, for current PID setup -- 550
@@ -14,13 +15,17 @@ kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
 # Pulse width expressed as fraction between 0.0 (`min_pulse`) and 1.0 (`max_pulse`).
 # For conventional servos, corresponds to the servo position as a fraction
 # of the actuation range. Is None when servo is disabled (pulsewidth of 0ms)
-# kit.servo[0].fraction = 0.5
+# kit.servo[0].angle = 110
 
 # property angle: float | None
 # The servo angle in degrees. Must be in the range 0 to actuation_range.
 # Is None when servo is disabled.
-
+while True:
+    kit.servo[0].angle = 110
+    sleep(1)
     kit.servo[0].angle = 90
+    sleep(0.1)
+    print("test")
 
 
 

control_functions.py

@@ -1,3 +1,5 @@
+from csv import excel
+
 from adafruit_hcsr04 import HCSR04 as hcsr04        # Ultrasound sensor
 import board                                        # General board pin mapper
 from adafruit_servokit import ServoKit              # Servo libraries for PWM driver board
@@ -9,6 +11,7 @@ import csv                                          # CSV handling
 from datetime import datetime                       # Date and time formatting
 from time import sleep                              # Sleep/pause
 import pandas as pd
+from datetime import datetime
 
 # Variables to control sensor
 TRIGGER_PIN = board.D4      # GPIO pin xx
@@ -16,22 +19,23 @@ ECHO_PIN = board.D17        # GPIO pin xx
 PIN_TIMEOUT: float = 0.1    # Timeout for echo wait -- don't change
 RUN_TIMEOUT: float = 0.0    # Sleep time in function
 MIN_DISTANCE: int = 6       # Minimum sensor distance to be considered valid (1 on bar)
-MAX_DISTANCE: int = 39      # Maximum sensor distance to be considered valid (35 on bar)
+MAX_DISTANCE: int = 40      # Maximum sensor distance to be considered valid (35 on bar)
 
 # Variables to control servo
 KIT = ServoKit(channels=16) # Define the type of board (8, 16)
 MIN_PULSE: int = 400        # Defines angle 80, for current PID setup
 MAX_PULSE: int = 2500       # Defines angle 100, for current PID setup
+OFFSET: int = -1
 KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
 
 # Variables to control logging.
 LOG: bool   = True         # Log data to files
 SCREEN: bool = True         # Log data to screen
-DEBUG: bool = True         # More data to display
+DEBUG: bool = False         # More data to display
 TWIN_MODE: bool = False
 
 # Control the number of samples for single distance measurement (average from burst)
-MAX_SAMPLES: int = 10
+MAX_SAMPLES: int = 1
 
 # Control the potentiometer
 # Description:
@@ -56,8 +60,8 @@ pcf_out = AnalogOut(pcf, PCF.OUT)
 pcf_out.value = PCF_VAL
 
 # Variables to control PID values (PID formula tweaks)
-p_value: float = 2.0
+p_value: float = 0.5
-i_value: float = 0.0
+i_value: float = 0.01
 d_value: float = 0.0
 
 # Initial variables, used in pid_calculations()
@@ -65,20 +69,23 @@ i_result: float = 0.0
 previous_time: float = 0.0
 previous_error: float = 0.0
 
-# Variables to assist pid_calculations()
-current_time: float = 0
-integral: float = 0
-
 # Error sum array
-error_sum_array: list = []
+error_sum_max: int = 10
+error_sum_array: list = [0]*error_sum_max
 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
 
+#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
+
+# servo slower
+current_angle:int = 90
+
 # 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]
@@ -91,6 +98,8 @@ def log_data(data_file: str, data_line: str, remark: str|None):
         data_writer.writerow([log_stamp,data_line, remark])
 
 def read_distance_sensor():
+    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+
     # Init array, used in read_distance_sensor()
     sample_array: list = []
 
@@ -110,7 +119,10 @@ def read_distance_sensor():
 
                     log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
                     # print("Distance_in_range: ", distance) if SCREEN else None
-
+                    if max_samples == 1:
+                        median_distance = distance
+                        mean_timestamp = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])
+                    else:
                         sample_array.append(distance)
                         if samples == 0: timestamp_first  = float(datetime.strftime(datetime.now(),
                                                                                     '%Y%m%d%H%M%S.%f')[:-3])
@@ -123,22 +135,32 @@ def read_distance_sensor():
                             median_distance: float = st.median(sample_array)
                             mean_timestamp: float  = st.mean([timestamp_first_float, timestamp_last_float])
                             print("Distance_median: ", median_distance) if SCREEN else None
-                        print("Timestamp_mean: ", mean_timestamp) if SCREEN else None
+                            print("Timestamp_mean: ", mean_timestamp) if DEBUG else None
                             print("Distance_in_range: ", distance) if SCREEN else None
 
+                            data_line = str(sample_array) + ',' + str(median_distance)
+                            log_data(data_file="sensor_array", data_line= data_line,
+                                     remark="") if LOG else None
+                            print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None
+
                         samples: int = samples + 1
 
                 else:
-                    log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
+                    log_data(data_file="sensor", data_line=str(distance), remark="Distance_out_of_range") if LOG else None
                     print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None
 
             except RuntimeError:
                 log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None
                 print("Distance_timed_out") if SCREEN else None
 
+    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="read_distance_sensor") if LOG else None
+
     return median_distance, mean_timestamp
 
 def read_setpoint():
+    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
 
     while True:
         raw_value: int = pcf_in_0.value
@@ -149,15 +171,26 @@ def read_setpoint():
 
         cm_rounded: int = int(round(scaled_value * POT_PCM, 0))
 
-        if SCREEN:
+        if DEBUG:
             print('Scaled_rounded = ' , round(scaled_value, 4), ' CM_rounded= ', cm_rounded)
             print('Scaled_raw= ' , scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM))
 
+        print('setpoing in cm: ', cm_rounded) if SCREEN else None
+
         sleep(POT_INT)
+
+        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="read_setpoint") if LOG else None
+
         return cm_rounded
 
 def calculate_acceleration():
 
+    print("calc is active")
+
+    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+
     position_1, timestamp_1 = read_distance_sensor()
     position_2, timestamp_2 = read_distance_sensor()
     position_3, timestamp_3 = read_distance_sensor()
@@ -168,10 +201,15 @@ def calculate_acceleration():
 
     print(initial_velocity, " ", final_velocity, " ", acceleration)  if SCREEN  else None
 
-    data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
+    data_line: str = str(position_1) + ',' + str(position_2) + ',' + str(position_3) + ',' + 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):
+    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="calculate_acceleration") if LOG else None
+
+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
@@ -181,6 +219,7 @@ def pid_calculations(setpoint):
     else:
         measurement, measurement_time = read_distance_sensor()
 
+    setpoint = read_setpoint()
     error = setpoint - measurement
 
     if previous_time is None:
@@ -196,29 +235,58 @@ def pid_calculations(setpoint):
     previous_error = error
     previous_time = measurement_time
 
+
+    #function to set the 2 max angles. Or set the angle to a specific number = pid_result * max movement + correction
+    if pid_result >= max_angle: # if PID result is greater than 1, set to 1. 1 = max upward angle
+        output_angle = (90 + max_angle)
+    elif pid_result <= -max_angle:  # if PID result is greater than 1, set to 1. 1 = max downward angle
+        output_angle = (90-max_angle)
+    elif -max_angle < pid_result < max_angle:
+        output_angle = pid_result + 90
+    else:
+        output_angle = 90
+
     log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result)
     log_data(data_file="pid", data_line=log_line, remark="") if LOG else None
 
-    if SCREEN:
+    if DEBUG:
         print("P_result: ", p_result)
         print("D_result: ", d_result)
         print("I_result: ", i_result)
         print("PID_result: ", pid_result)
 
-    if error_sum_counter <= error_sum_max:
+    if error_sum_counter <= error_sum_max-2:
         error_sum_counter = error_sum_counter + 1
     else:
         error_sum_counter = 0
 
-    return pid_result
+    print("error sum counter", error_sum_counter)  if DEBUG else None
+
+    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="pid_calculations") if LOG else None
+
+    output_angle = round(output_angle)
+
+    return output_angle
 
 def control_server_angle(angle):
-    KIT.servo[0].angle = angle # Set angle
+    start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
+
+    servo_angle = angle + OFFSET
+
+    KIT.servo[0].angle = servo_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: ", servo_angle) if SCREEN else None
+
+    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="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])
 
@@ -239,4 +307,40 @@ def digital_twin(pid_angle):
     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'))
+
+    global current_angle
+    pid_angle = pid_calculations()
+    if (pid_angle - current_angle) > 5:
+        servo_angle = current_angle + 5
+    elif (pid_angle - current_angle) < -5:
+        servo_angle = current_angle - 5
+    else:
+        servo_angle = pid_angle
+
+    current_angle = servo_angle
+
+    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="servo_slower") if LOG else None
+
+    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()
+        # control_server_angle(pid_calculations())
+except RuntimeError:
+    print("bbbb")