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

refactor

Browse Source
This commit is contained in:
Rudi klein 2024-12-29 14:30:37 +01:00
parent f175b29eda
commit dd112f20a1
3 changed files with 78 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
control_functions.py
View File

@ -14,9 +14,6 @@ import csv # CSV handling
from datetime import datetime # Date and time formatting from datetime import datetime # Date and time formatting
import time # Time formatting import time # Time formatting
from serial.tools.list_ports_osx import kCFNumberSInt8Type
######################################## Variables (start) ##################################
# 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
@ -35,6 +32,9 @@ 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 = False # More data to display
# Control the number of samples for single measurement
MAX_SAMPLES = 10
# Variables to assist PID calculations # Variables to assist PID calculations
current_time: float = 0 current_time: float = 0
integral: float = 0 integral: float = 0
@ -42,42 +42,36 @@ time_prev: float = -1e-6
error_prev: float = 0 error_prev: float = 0
# Variables to control PID values (PID formula tweaks) # Variables to control PID values (PID formula tweaks)
p_value: float = 2 p_value : float = 2.0
i_value: float = 0 i_value: float = 0.0
d_value: float = 0 d_value: float = 0.0
# Initial variables, used in pid_calculations() # Initial variables, used in pid_calculations()
i_result: float = 0 i_result: float = 0.0
previous_time: float previous_time: float = 0.0
previous_error: float previous_error: float = 0.0
# Init array, used in read_distance_sensor() # Init array, used in read_distance_sensor()
sample_array: list = [] sample_array: list = []
######################################## Variables (end) ##################################
def initial(): def initial():
... ...
# Create timestamp strings for logs and screen
def time_stamper():
log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
return log_timestamp, file_timestamp
# Write data to any of the logfiles # Write data to any of the logfiles
def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None): def log_data(file_stamp: str, data_file: str, data_line: float, remark: str|None):
log_stamp, _ = time_stamper() log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
with open("pid-balancer_" + data_file + "_data_" + fixed_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(fixed_file_stamp): def read_distance_sensor(file_stamp):
# Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long 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=TIMEOUT) as sonar):
samples: int = 0 samples: int = 0
max_samples: int = 10 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:
@ -85,12 +79,14 @@ def read_distance_sensor(fixed_file_stamp):
distance: float = sonar.distance distance: float = sonar.distance
if MIN_DISTANCE < distance < MAX_DISTANCE: if MIN_DISTANCE < distance < MAX_DISTANCE:
log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None log_data(file_stamp,"sensor", distance, None) if LOG else None
print("Distance: ", distance) if SCREEN else None print("Distance: ", distance) if SCREEN else None
sample_array.append(distance) sample_array.append(distance)
if samples == 0: timestamp_first, _ = time_stamper() if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(),
if samples == max_samples - 1: timestamp_last, _ = time_stamper() '%Y%m%d%H%M%S.%f')[:-3])
if samples == max_samples - 1: timestamp_last = float(datetime.strftime(datetime.now(),
'%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)
@ -101,29 +97,20 @@ def read_distance_sensor(fixed_file_stamp):
print(mean_timestamp) if SCREEN else None print(mean_timestamp) if SCREEN else None
else: else:
log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None log_data(file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
print("Distance: ", distance) if SCREEN else None print("Distance: ", distance) if SCREEN else None
except RuntimeError: except RuntimeError:
log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None log_data(file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None
print("Timeout") if SCREEN else None print("Timeout") if SCREEN else None
return median_distance, mean_timestamp return median_distance, mean_timestamp
def read_setpoint(): def read_setpoint():
# Setup the internal DAC to output a voltage and then measure it with the first ADC channel.
# Wiring: Connect the DAC output to the first ADC channel, in addition to the normal power and I2C connections
############# AnalogOut & AnalogIn Example ##########################
#
# This example shows how to use the included AnalogIn and AnalogOut
# classes to set the internal DAC to output a voltage and then measure
# it with the first ADC channel.
#
# Wiring:
# Connect the DAC output to the first ADC channel, in addition to the
# normal power and I2C connections
#
#####################################################################
i2c = board.I2C() i2c = board.I2C()
pcf = PCF.PCF8591(i2c) pcf = PCF.PCF8591(i2c)
@ -148,28 +135,28 @@ def pid_calculations(setpoint):
global i_result, previous_time, previous_error global i_result, previous_time, previous_error
offset_value: int = 320 offset_value: int = 320
measurement, current_time = read_distance_sensor measurement, measurement_time = read_distance_sensor()
error: float = setpoint - measurement error: float = setpoint - measurement
error_sum: float = 0.0 error_sum: float = 0.0
if previous_time is None: if previous_time is None:
previous_error: float = 0.0 previous_error = 0.0
previous_time: float = current_time previous_time = current_time
i_result: float = 0.0 i_result = 0.0
error_sum: float = error * 0.008 # sensor sampling number approximation. error_sum = 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: float = p_value * error p_result = p_value * error
i_result: float = i_value * error_sum i_result = i_value * error_sum
d_result: float = d_value * ((error - previous_error) / (current_time - previous_time)) d_result = d_value * ((error - previous_error) / (measurement_time - previous_time))
pid_result: float = offset_value + p_result + i_result + d_result pid_result = offset_value + p_result + i_result + d_result
previous_error: float = error previous_error = error
previous_time: float = current_time previous_time = measurement_time
return pid_result return pid_result
def calculate_new_servo_pos(): def calculate_new_servo_position():
... ...

14
main.py
View File

@ -1,17 +1,11 @@
import control_functions as cf import control_functions as cf
import plotter_functions as pf import plotter_functions as pf
import numpy as np import twin_functions as tw
import matplotlib.pyplot as plt from datetime import datetime
from scipy.integrate import odeint
import numpy as np
import matplotlib.pyplot as plt
import statistics as st
from adafruit_hcsr04 import HCSR04 as hcsr04
_, fixed_file_stamp = cf.time_stamper() file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
cf.read_distance_sensor(file_stamp)
cf.read_distance_sensor(fixed_file_stamp)

41
plotter_functions.py
View File

@ -1,7 +1,40 @@
def read_data_file(): import pandas as pd
pass import matplotlib.pyplot as plt
# Variables to control logging.
LOG: bool = True # Log data to files
SCREEN: bool = True # Log data to screen
DEBUG: bool = False # More data to display
def plot_graphs(): def read_data_file(data_file):
pass data_frame = pd.read_csv(data_file)
first_row_time = data_frame['Timestamp'].iloc[1]
last_row_time = data_frame['Timestamp'].iloc[-1]
first_row_value = data_frame['Value'].iloc[1]
last_row_value = data_frame['Value'].iloc[-1]
mean_value = data_frame['Value'].mean()
median_value = data_frame['Value'].median()
sum_value = data_frame['Value'].sum()
if SCREEN:
print('first_row_value ',first_row_value)
print('last_row_value ',last_row_value)
print('first_row_time ', first_row_time)
print('last_row_time ', last_row_time)
print('elapsed_time ', (last_row_time - first_row_time))
print('mean_value ', mean_value)
print('median_value ', median_value)
print('sum_value ', sum_value)
return data_frame
def plot_data_frame(data_file):
data_frame = read_data_file(data_file)
plt.plot(data_frame['Timestamp'], data_frame['Value'])
# plt.savefig(data_file + '.png')
# img = plt.imread(data_file + '.png')
# plt.imshow(img)
plt.show()
plot_data_frame(data_file = 'pid-balancer_twin_test_data.csv')