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

Upload from yesterday

Browse Source
This commit is contained in:
Rudi klein 2025-01-08 15:25:41 +01:00
parent 95e836f21c
commit d6b2ca92f5
3 changed files with 25 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Examples/Adafruit_Servokit_servo_driver.py
View File

@ -23,6 +23,8 @@ kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
kit.servo[0].angle = 90 kit.servo[0].angle = 90
# property throttle: float # property throttle: float
# How much power is being delivered to the motor. # How much power is being delivered to the motor.
# Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.) # Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.)

27
control_functions.py
View File

@ -15,7 +15,7 @@ TRIGGER_PIN = board.D4 # GPIO pin xx
ECHO_PIN = board.D17 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx
PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change
RUN_TIMEOUT: float = 0.0 # Sleep time in function RUN_TIMEOUT: float = 0.0 # Sleep time in function
MIN_DISTANCE: int = 4 # Minimum sensor distance to be considered valid (1 on bar) 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 = 39 # Maximum sensor distance to be considered valid (35 on bar)
# Variables to control servo # Variables to control servo
@ -69,9 +69,6 @@ previous_error: float = 0.0
current_time: float = 0 current_time: float = 0
integral: float = 0 integral: float = 0
# Init array, used in read_distance_sensor()
sample_array: list = []
# Error sum array # Error sum array
error_sum_array: list = [] error_sum_array: list = []
error_sum_counter: int = 0 error_sum_counter: int = 0
@ -94,6 +91,8 @@ def log_data(data_file: str, data_line: str, remark: str|None):
data_writer.writerow([log_stamp,data_line, remark]) data_writer.writerow([log_stamp,data_line, remark])
def read_distance_sensor(): def read_distance_sensor():
# Init array, used in read_distance_sensor()
sample_array: list = []
# Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long a 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.
@ -110,22 +109,24 @@ def read_distance_sensor():
if MIN_DISTANCE < distance < MAX_DISTANCE: if MIN_DISTANCE < distance < MAX_DISTANCE:
log_data(data_file="sensor", data_line=str(distance), remark="") if LOG 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 # 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(),
'%Y%m%d%H%M%S.%f')[:-3]) '%Y%m%d%H%M%S.%f')[:-3])
if samples == max_samples - 1: timestamp_last = float(datetime.strftime(datetime.now(), if samples == max_samples - 1:
timestamp_last = float(datetime.strftime(datetime.now(),
'%Y%m%d%H%M%S.%f')[:-3]) '%Y%m%d%H%M%S.%f')[:-3])
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 median_distance: float = 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("Distance_median: ", median_distance) if SCREEN else None
print("Timestamp_mean: ", mean_timestamp) if SCREEN else None
print("Distance_in_range: ", distance) if SCREEN else None
print("Distance_median: ", median_distance) if SCREEN else None samples: int = samples + 1
print("Timestamp_mean: ", mean_timestamp) if SCREEN else None
else: 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="") if LOG else None

18
main.py
View File

@ -1,12 +1,12 @@
from datetime import datetime from datetime import datetime
import control_functions as cf import control_functions as cf
import board
from adafruit_hcsr04 import HCSR04 as hcsr04 # Ultrasound sensor
with open("pid-balancer_" + "time_file.txt", "w") as time_file: try:
time_file.write(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]) 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:
cf.read_distance_sensor()
cf.read_distance_sensor() except RuntimeError:
print()