HU-Projects/pid-balancer
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Code additions for functions

Browse Source
This commit is contained in:
Rudi klein 2025-01-05 22:02:02 +01:00
parent c92be8b49a
commit dad15449b4
3 changed files with 65 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Examples/Adafruit_Servokit_servo_driver.py
View File

@ -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. # The servo angle in degrees. Must be in the range 0 to actuation_range.
# Is None when servo is disabled. # Is None when servo is disabled.
# kit.servo[0].angle = 90 kit.servo[0].angle = 90
# property throttle: float # property throttle: float

102
control_functions.py
View File

@ -8,29 +8,25 @@ import statistics as st # Mean and median calculatio
import csv # CSV handling import csv # CSV handling
from datetime import datetime # Date and time formatting from datetime import datetime # Date and time formatting
from time import sleep # Sleep/pause 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 # Variables to control sensor
TRIGGER_PIN = board.D4 # GPIO pin xx TRIGGER_PIN = board.D4 # GPIO pin xx
ECHO_PIN = board.D17 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx
TIMEOUT: float = 0.1 # Timout for echo wait PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change
MIN_DISTANCE: int = 4 # Minimum sensor distance to considered valid RUN_TIMEOUT: float = 0.0 # Sleep time in function
MAX_DISTANCE: int = 40 # Maximum sensor distance to considered valid 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 # 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) KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE: int = 400 # Defines angle 80, for current PID setup MIN_PULSE: int = 400 # Defines angle 80, for current PID setup
MAX_PULSE: int = 2500 # Defines angle 100, 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) KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# Variables to control logging. # 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 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) # Control the number of samples for single distance measurement (average from burst)
MAX_SAMPLES: int = 10 MAX_SAMPLES: int = 10
@ -57,12 +53,6 @@ pcf_in_0 = AnalogIn(pcf, PCF.A0)
pcf_out = AnalogOut(pcf, PCF.OUT) pcf_out = AnalogOut(pcf, PCF.OUT)
pcf_out.value = PCF_VAL 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) # Variables to control PID values (PID formula tweaks)
p_value: float = 2.0 p_value: float = 2.0
i_value: float = 0.0 i_value: float = 0.0
@ -73,34 +63,42 @@ i_result: float = 0.0
previous_time: float = 0.0 previous_time: float = 0.0
previous_error: 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() # Init array, used in read_distance_sensor()
sample_array: list = [] sample_array: list = []
# Write data to any of the logfiles # 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] 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: with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file:
data_writer = csv.writer(data_file) data_writer = csv.writer(data_file)
data_writer.writerow([log_stamp,data_line, remark]) data_writer.writerow([log_stamp,data_line, remark])
def read_distance_sensor(): 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. # 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 samples: int = 0
max_samples: int = MAX_SAMPLES max_samples: int = MAX_SAMPLES
timestamp_last: float = 0.0 timestamp_last: float = 0.0
timestamp_first: float = 0.0 timestamp_first: float = 0.0
while samples != max_samples: while samples != max_samples:
sleep(RUN_TIMEOUT)
try: try:
distance: float = sonar.distance distance: float = sonar.distance
if MIN_DISTANCE < distance < MAX_DISTANCE: if MIN_DISTANCE < distance < MAX_DISTANCE:
log_data(file_stamp,"sensor", str(distance), None) if LOG else None log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
print("Distance: ", distance) if SCREEN else None print("Distance_in_range: ", distance) if SCREEN else None
sample_array.append(distance) sample_array.append(distance)
if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(), 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_first_float: float = float(timestamp_first)
timestamp_last_float: float = float(timestamp_last) timestamp_last_float: float = float(timestamp_last)
samples: int = samples + 1 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]) mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
print(median_distance) if SCREEN else None print("Distance_median: ", median_distance) if SCREEN else None
print(mean_timestamp) if SCREEN else None print("Timestamp_mean: ", mean_timestamp) if SCREEN else None
else: else:
log_data(file_stamp=file_stamp, data_file="sensor", data_line=str(distance), log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
remark=None) if LOG else None print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None
print("Distance: ", distance) if SCREEN else None
except RuntimeError: except RuntimeError:
log_data(file_stamp=file_stamp, data_file="sensor", data_line="999.999", log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None
remark="Timeout") if LOG and DEBUG else None print("Distance_timed_out") if SCREEN else None
print("Timeout") if SCREEN else None
return median_distance, mean_timestamp return median_distance, mean_timestamp
@ -136,16 +132,32 @@ def read_setpoint():
scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage
log_line = str(scaled_value) + "," + str(raw_value) + "," + str("angle") 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: 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) sleep(POT_INT)
return cm_rounded
def calculate_velocity(): def calculate_acceleration():
velocity = "0" position_1, timestamp_1 = read_distance_sensor()
log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None 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): 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 * 0.008 # sensor sampling number approximation.
error_sum: float = error_sum + (error * (current_time - previous_time)) error_sum: float = error_sum + (error * (current_time - previous_time))
p_result = p_value * error p_result: float = p_value * error
i_result = i_value * error_sum i_result: float = i_value * error_sum
d_result = d_value * ((error - previous_error) / (measurement_time - previous_time)) d_result: float = d_value * ((error - previous_error) / (measurement_time - previous_time))
pid_result = offset_value + p_result + i_result + d_result pid_result: float = offset_value + p_result + i_result + d_result
previous_error = error previous_error = error
previous_time = measurement_time previous_time = measurement_time
log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result) 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 return pid_result
def control_server_angle(angle): def control_server_angle(angle):
KIT.servo[0].angle = angle # Set angle KIT.servo[0].angle = angle # Set angle
log_line = str(angle) log_line = str(angle)
log_data(file_stamp=file_stamp, data_file="servo", data_line=log_line, remark=None) if LOG else None log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
print(angle) if SCREEN else None

7
main.py
View File

@ -1,13 +1,12 @@
from datetime import datetime from datetime import datetime
import control_functions as cf 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: while True:
print(cf.read_setpoint()) cf.calculate_acceleration()