From dad15449b4692f1911b35db4d4a3b6d3dc302f46 Mon Sep 17 00:00:00 2001 From: rudi Date: Sun, 5 Jan 2025 22:02:02 +0100 Subject: [PATCH] Code additions for functions --- Examples/Adafruit_Servokit_servo_driver.py | 2 +- control_functions.py | 102 ++++++++++++--------- main.py | 7 +- 3 files changed, 65 insertions(+), 46 deletions(-) diff --git a/Examples/Adafruit_Servokit_servo_driver.py b/Examples/Adafruit_Servokit_servo_driver.py index afc71e6..92d32de 100644 --- a/Examples/Adafruit_Servokit_servo_driver.py +++ b/Examples/Adafruit_Servokit_servo_driver.py @@ -20,7 +20,7 @@ kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE) # The servo angle in degrees. Must be in the range 0 to actuation_range. # Is None when servo is disabled. -# kit.servo[0].angle = 90 +kit.servo[0].angle = 90 # property throttle: float diff --git a/control_functions.py b/control_functions.py index 4807c3e..5f30307 100644 --- a/control_functions.py +++ b/control_functions.py @@ -8,29 +8,25 @@ 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 os # OS environment - -file_stamp = os.environ.get("PID_TIMESTAMP") # Get file timestamp from OS variable # Variables to control sensor TRIGGER_PIN = board.D4 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx -TIMEOUT: float = 0.1 # Timout for echo wait -MIN_DISTANCE: int = 4 # Minimum sensor distance to considered valid -MAX_DISTANCE: int = 40 # Maximum sensor distance to considered valid +PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change +RUN_TIMEOUT: float = 0.0 # Sleep time in function +MIN_DISTANCE: int = 4 # Minimum sensor distance to be considered valid (1 on bar) +MAX_DISTANCE: int = 39 # Maximum sensor distance to be considered valid (35 on bar) # Variables to control servo -# MIN_PULSE = 750 # Defines angle 0, actual minimum for this servo -# MAX_PULSE = 2150 # Defines angle 180, actual maximum for this 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 KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE) # Variables to control logging. -LOG: bool = False # Log data to files +LOG: bool = True # Log data to files SCREEN: bool = True # Log data to screen -DEBUG: bool = False # More data to display +DEBUG: bool = True # More data to display # Control the number of samples for single distance measurement (average from burst) MAX_SAMPLES: int = 10 @@ -57,12 +53,6 @@ pcf_in_0 = AnalogIn(pcf, PCF.A0) pcf_out = AnalogOut(pcf, PCF.OUT) pcf_out.value = PCF_VAL -# Variables to assist PID calculations -current_time: float = 0 -integral: float = 0 -time_prev: float = -1e-6 -error_prev: float = 0 - # Variables to control PID values (PID formula tweaks) p_value: float = 2.0 i_value: float = 0.0 @@ -73,34 +63,42 @@ 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 + # Init array, used in read_distance_sensor() sample_array: list = [] # Write data to any of the logfiles -def log_data(file_stamp: str, data_file: str, data_line: str, remark: str|None): +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] + with open("pid-balancer_" + "time_file.txt", "r") as time_file: + file_stamp: str = time_file.readline() + with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file: data_writer = csv.writer(data_file) data_writer.writerow([log_stamp,data_line, remark]) def read_distance_sensor(): - # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long distance) + # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long a distance) # Return the mean timestamp and median distance. - with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar: + with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=PIN_TIMEOUT) as sonar: samples: int = 0 max_samples: int = MAX_SAMPLES timestamp_last: float = 0.0 timestamp_first: float = 0.0 while samples != max_samples: + sleep(RUN_TIMEOUT) try: distance: float = sonar.distance if MIN_DISTANCE < distance < MAX_DISTANCE: - log_data(file_stamp,"sensor", str(distance), None) if LOG else None - print("Distance: ", distance) if SCREEN else None + log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None + print("Distance_in_range: ", distance) if SCREEN else None sample_array.append(distance) if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(), @@ -111,21 +109,19 @@ def read_distance_sensor(): timestamp_first_float: float = float(timestamp_first) timestamp_last_float: float = float(timestamp_last) samples: int = samples + 1 - median_distance: list = st.median(sample_array) + median_distance: float = st.median(sample_array) mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float]) - print(median_distance) if SCREEN else None - print(mean_timestamp) if SCREEN else None + print("Distance_median: ", median_distance) if SCREEN else None + print("Timestamp_mean: ", mean_timestamp) if SCREEN else None else: - log_data(file_stamp=file_stamp, data_file="sensor", data_line=str(distance), - remark=None) if LOG else None - print("Distance: ", distance) if SCREEN else None + log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None + print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None except RuntimeError: - log_data(file_stamp=file_stamp, data_file="sensor", data_line="999.999", - remark="Timeout") if LOG and DEBUG else None - print("Timeout") if SCREEN else None + 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 return median_distance, mean_timestamp @@ -136,16 +132,32 @@ def read_setpoint(): scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage log_line = str(scaled_value) + "," + str(raw_value) + "," + str("angle") - log_data(file_stamp=file_stamp, data_file="potmeter", data_line=log_line, remark=None) if LOG else None + log_data(data_file="potmeter", data_line=log_line, remark="") if LOG else None + + cm_rounded: int = int(round(scaled_value * POT_PCM, 0)) if SCREEN: - print('scaled= ' , round(scaled_value, 4), ' cm= ', int(round(scaled_value * POT_PCM, 0))) + print('Scaled_rounded = ' , round(scaled_value, 4), ' CM_rounded= ', cm_rounded) + print('Scaled_raw= ' , scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM)) + sleep(POT_INT) + return cm_rounded -def calculate_velocity(): +def calculate_acceleration(): - velocity = "0" - log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None + position_1, timestamp_1 = read_distance_sensor() + 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) + final_velocity: float = ((position_2 - position_3) / (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) + log_data(data_file="acceleration", data_line=data_line, remark="") if LOG else None def pid_calculations(setpoint): @@ -162,18 +174,26 @@ def pid_calculations(setpoint): error_sum: float = error * 0.008 # sensor sampling number approximation. error_sum: float = error_sum + (error * (current_time - previous_time)) - p_result = p_value * error - i_result = i_value * error_sum - d_result = d_value * ((error - previous_error) / (measurement_time - previous_time)) - pid_result = offset_value + p_result + i_result + d_result + p_result: float = p_value * error + i_result: float = i_value * error_sum + d_result: float = d_value * ((error - previous_error) / (measurement_time - previous_time)) + pid_result: float = offset_value + p_result + i_result + d_result previous_error = error previous_time = measurement_time log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result) - log_data(file_stamp=file_stamp, data_file="pid", data_line=log_line, remark=None) if LOG else None + log_data(data_file="pid", data_line=log_line, remark="") if LOG else None + + if SCREEN: + print("P_result: ", p_result) + print("D_result: ", d_result) + print("I_result: ", i_result) + print("PDI_result: ", pid_result) + return pid_result def control_server_angle(angle): KIT.servo[0].angle = angle # Set angle log_line = str(angle) - log_data(file_stamp=file_stamp, data_file="servo", data_line=log_line, remark=None) if LOG else None \ No newline at end of file + log_data(data_file="servo", data_line=log_line, remark="") if LOG else None + print(angle) if SCREEN else None \ No newline at end of file diff --git a/main.py b/main.py index ad3906b..4b35747 100644 --- a/main.py +++ b/main.py @@ -1,13 +1,12 @@ from datetime import datetime import control_functions as cf -import os - -os.environ["PID_TIMESTAMP"] = datetime.strftime(datetime.now(), '%Y%m%d%I%M') # Set file timestamp as OS variable. +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]) while True: - print(cf.read_setpoint()) + cf.calculate_acceleration()