From dd112f20a176f7b3e5490894a88edc4d8ad93cc6 Mon Sep 17 00:00:00 2001 From: rudi Date: Sun, 29 Dec 2024 14:30:37 +0100 Subject: [PATCH 1/2] refactor --- control_functions.py | 87 +++++++++++++++++++------------------------- main.py | 14 ++----- plotter_functions.py | 41 +++++++++++++++++++-- 3 files changed, 78 insertions(+), 64 deletions(-) diff --git a/control_functions.py b/control_functions.py index 4cbe1d1..26f6ef3 100644 --- a/control_functions.py +++ b/control_functions.py @@ -14,9 +14,6 @@ import csv # CSV handling from datetime import datetime # Date and time formatting import time # Time formatting -from serial.tools.list_ports_osx import kCFNumberSInt8Type - -######################################## Variables (start) ################################## # Variables to control sensor TRIGGER_PIN = board.D4 # 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 DEBUG: bool = False # More data to display +# Control the number of samples for single measurement +MAX_SAMPLES = 10 + # Variables to assist PID calculations current_time: float = 0 integral: float = 0 @@ -42,42 +42,36 @@ time_prev: float = -1e-6 error_prev: float = 0 # Variables to control PID values (PID formula tweaks) -p_value: float = 2 -i_value: float = 0 -d_value: float = 0 +p_value : float = 2.0 +i_value: float = 0.0 +d_value: float = 0.0 # Initial variables, used in pid_calculations() -i_result: float = 0 -previous_time: float -previous_error: float +i_result: float = 0.0 +previous_time: float = 0.0 +previous_error: float = 0.0 # Init array, used in read_distance_sensor() sample_array: list = [] -######################################## Variables (end) ################################## - 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 -def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None): - log_stamp, _ = time_stamper() +def log_data(file_stamp: str, data_file: str, data_line: float, remark: str|None): + 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.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): samples: int = 0 - max_samples: int = 10 + max_samples: int = MAX_SAMPLES timestamp_last: float = 0.0 timestamp_first: float = 0.0 while samples != max_samples: @@ -85,12 +79,14 @@ def read_distance_sensor(fixed_file_stamp): distance: float = sonar.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 sample_array.append(distance) - if samples == 0: timestamp_first, _ = time_stamper() - if samples == max_samples - 1: timestamp_last, _ = time_stamper() + if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(), + '%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_last_float: float = float(timestamp_last) @@ -101,29 +97,20 @@ def read_distance_sensor(fixed_file_stamp): print(mean_timestamp) if SCREEN else None 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 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 return median_distance, mean_timestamp 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() pcf = PCF.PCF8591(i2c) @@ -148,28 +135,28 @@ def pid_calculations(setpoint): global i_result, previous_time, previous_error offset_value: int = 320 - measurement, current_time = read_distance_sensor + measurement, measurement_time = read_distance_sensor() error: float = setpoint - measurement error_sum: float = 0.0 if previous_time is None: - previous_error: float = 0.0 - previous_time: float = current_time - i_result: float = 0.0 - error_sum: float = error * 0.008 # sensor sampling number approximation. + previous_error = 0.0 + previous_time = current_time + i_result = 0.0 + error_sum = error * 0.008 # sensor sampling number approximation. error_sum: float = error_sum + (error * (current_time - previous_time)) - p_result: float = p_value * error - i_result: float = i_value * error_sum - d_result: float = d_value * ((error - previous_error) / (current_time - previous_time)) - pid_result: float = offset_value + p_result + i_result + d_result - previous_error: float = error - previous_time: float = current_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 + previous_error = error + previous_time = measurement_time return pid_result -def calculate_new_servo_pos(): +def calculate_new_servo_position(): ... diff --git a/main.py b/main.py index 2c1382d..97337b3 100644 --- a/main.py +++ b/main.py @@ -1,17 +1,11 @@ import control_functions as cf import plotter_functions as pf -import numpy as np -import matplotlib.pyplot as plt -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 +import twin_functions as tw +from datetime import datetime -_, fixed_file_stamp = cf.time_stamper() +file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M') - -cf.read_distance_sensor(fixed_file_stamp) +cf.read_distance_sensor(file_stamp) diff --git a/plotter_functions.py b/plotter_functions.py index df4dc39..b9925de 100644 --- a/plotter_functions.py +++ b/plotter_functions.py @@ -1,7 +1,40 @@ -def read_data_file(): - pass +import pandas as pd +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(): - pass +def read_data_file(data_file): + 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') \ No newline at end of file -- 2.47.2 From bd388bf94e27e79165fcf4089618e3c8e8d5dfb0 Mon Sep 17 00:00:00 2001 From: rudi Date: Sun, 29 Dec 2024 21:58:19 +0100 Subject: [PATCH 2/2] Fixed sensor issue and implemented potentiometer --- .idea/misc.xml | 2 +- control_functions.py | 43 ++++++++++++++++++++++++++----------------- main.py | 4 +--- 3 files changed, 28 insertions(+), 21 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index e158a33..116cb80 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -3,5 +3,5 @@ - + \ No newline at end of file diff --git a/control_functions.py b/control_functions.py index 26f6ef3..2239d47 100644 --- a/control_functions.py +++ b/control_functions.py @@ -1,6 +1,6 @@ -from adafruit_hcsr04 import HCSR04 as hcsr04 # PWM driver board for servo -import board # PWM driver board for servo -from adafruit_servokit import ServoKit # Servo libraries +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 import adafruit_pcf8591.pcf8591 as PCF # AD/DA converter board for potentiometer from adafruit_pcf8591.analog_in import AnalogIn # Analogue in pin library from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library @@ -35,6 +35,12 @@ DEBUG: bool = False # More data to display # Control the number of samples for single measurement MAX_SAMPLES = 10 +# Control the number of samples for the potentiometer +PCF_VALUE = 65535 +POT_MAX = 65280 +POT_MIN = 256 +POT_INTERVAL = 0.1 + # Variables to assist PID calculations current_time: float = 0 integral: float = 0 @@ -69,7 +75,7 @@ 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 max_samples: int = MAX_SAMPLES timestamp_last: float = 0.0 @@ -108,25 +114,26 @@ def read_distance_sensor(file_stamp): 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 - i2c = board.I2C() pcf = PCF.PCF8591(i2c) - pcf_in_0 = AnalogIn(pcf, PCF.A0) pcf_out = AnalogOut(pcf, PCF.OUT) + pcf_out.value = PCF_VALUE while True: - print("Setting out to ", 65535) - pcf_out.value = 65535 - raw_value = pcf_in_0.value - scaled_value = (raw_value / 65535) * pcf_in_0.reference_voltage + raw_value: float = pcf_in_0.value + scaled_value: float = (raw_value / PCF_VALUE) * pcf_in_0.reference_voltage + # Calculate angle in reference to raw pot values + angle: float = ((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN) - print("Pin 0: %0.2fV" % (scaled_value)) - print("") - time.sleep(1) + if SCREEN: + print('pin 0 ', pcf.read(0)) + print('raw_value ',raw_value) + print("pin 0: %0.2fV" % scaled_value) + print(angle) + time.sleep(POT_INTERVAL) + send_data_to_servo(set_angle=angle) def calculate_velocity(): ... @@ -160,7 +167,9 @@ def calculate_new_servo_position(): ... -def send_data_to_servo(): +def send_data_to_servo(set_angle): - KIT.servo[0].angle = 180 # Set angle + KIT.servo[0].angle = set_angle # Set angle +read_distance_sensor(file_stamp=123) +read_setpoint() \ No newline at end of file diff --git a/main.py b/main.py index 97337b3..5cca24a 100644 --- a/main.py +++ b/main.py @@ -1,7 +1,5 @@ -import control_functions as cf -import plotter_functions as pf -import twin_functions as tw from datetime import datetime +import control_functions as cf file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M') -- 2.47.2