From f175b29eda285fe128cd63119940c4063fe51fb3 Mon Sep 17 00:00:00 2001 From: rudi Date: Sat, 28 Dec 2024 21:06:10 +0100 Subject: [PATCH] added analogue board and cosmetics --- .idea/misc.xml | 7 ++ control_functions.py | 181 ++++++++++++++++++++++++------------- data_transfer_functions.py | 16 ---- main.py | 5 +- 4 files changed, 129 insertions(+), 80 deletions(-) create mode 100644 .idea/misc.xml delete mode 100644 data_transfer_functions.py diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..e158a33 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/control_functions.py b/control_functions.py index 3e5dc47..4cbe1d1 100644 --- a/control_functions.py +++ b/control_functions.py @@ -1,110 +1,172 @@ -from OpenGL.images import returnFormat -from adafruit_hcsr04 import HCSR04 as hcsr04 -import board -import data_transfer_functions as dt -from adafruit_servokit import ServoKit -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 # PWM driver board for servo +import board # PWM driver board for servo +from adafruit_servokit import ServoKit # Servo libraries +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 +import adafruit_pcf8591 as pcf8591 # AD/DA converter board for potentiometer +import numpy as np # Number handling +import pandas as pd # Data handling +import matplotlib.pyplot as plt # Plotter handling +from scipy.integrate import odeint # Integral calculations +import statistics as st # Mean and median calculations +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 -ECHO_PIN = board.D17 -TIMEOUT = 0.1 -MIN_DISTANCE = 4 -MAX_DISTANCE = 40 +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 + +# Variables to control servo +KIT = ServoKit(channels=16) # Define the type of board (8, 16) +MIN_PULSE = 500 # Defines angle 0 +MAX_PULSE = 2500 # Defines angle 180 +KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE) # Variables to control logging. -LOG = True -SCREEN = True -DEBUG = False +LOG: bool = True # Log data to files +SCREEN: bool = True # Log data to screen +DEBUG: bool = False # More data to display -# PID measurements -time = 0 -integral = 0 -time_prev = -1e-6 -e_prev = 0 +# Variables to assist PID calculations +current_time: float = 0 +integral: float = 0 +time_prev: float = -1e-6 +error_prev: float = 0 -# PID values -P_value = 2 -I_value = 0 -D_value = 0 +# Variables to control PID values (PID formula tweaks) +p_value: float = 2 +i_value: float = 0 +d_value: float = 0 -sample_array = [] +# Initial variables, used in pid_calculations() +i_result: float = 0 +previous_time: float +previous_error: float + +# 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() + + with open("pid-balancer_" + data_file + "_data_" + fixed_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): with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar): - samples = 0 - max_samples = 10 - timestamp_last = 0.0 - timestamp_first = 0.0 + samples: int = 0 + max_samples: int = 10 + timestamp_last: float = 0.0 + timestamp_first: float = 0.0 while samples != max_samples: try: - distance = sonar.distance + distance: float = sonar.distance if MIN_DISTANCE < distance < MAX_DISTANCE: - dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None + log_data(fixed_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, _ = dt.timestamper() - if samples == max_samples - 1: timestamp_last, _ = dt.timestamper() + if samples == 0: timestamp_first, _ = time_stamper() + if samples == max_samples - 1: timestamp_last, _ = time_stamper() - timestamp_first_float = float(timestamp_first) - timestamp_last_float = float(timestamp_last) - samples = samples + 1 - median_distance = st.median(sample_array) - mean_timestamp = st.mean([timestamp_first_float, timestamp_last_float]) - print(median_distance) if LOG else None - print(mean_timestamp) + 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) + 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 else: - dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None + log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None print("Distance: ", distance) if SCREEN else None except RuntimeError: - dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None + log_data(fixed_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(): - ... + + + ############# 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) + + pcf_in_0 = AnalogIn(pcf, PCF.A0) + pcf_out = AnalogOut(pcf, PCF.OUT) + + 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 + + print("Pin 0: %0.2fV" % (scaled_value)) + print("") + time.sleep(1) def calculate_velocity(): ... -def PID_calculations(setpoint): +def pid_calculations(setpoint): - global I_result, previous_time, previous_error - offset_value = 320 + global i_result, previous_time, previous_error + offset_value: int = 320 measurement, current_time = read_distance_sensor - error = setpoint - measurement + 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 + i_result: float = 0.0 error_sum: float = 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 + 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 - return PID_result + return pid_result def calculate_new_servo_pos(): @@ -113,8 +175,5 @@ def calculate_new_servo_pos(): def send_data_to_servo(): - kit = ServoKit(channels=16) - kit.servo[0].set_pulse_width_range(500, 2500) - - kit.servo[0].angle = 180 + KIT.servo[0].angle = 180 # Set angle diff --git a/data_transfer_functions.py b/data_transfer_functions.py deleted file mode 100644 index b30820a..0000000 --- a/data_transfer_functions.py +++ /dev/null @@ -1,16 +0,0 @@ -import csv -from datetime import datetime - -# Create timestamp strings for logs and screen -def timestamper(): - 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, _ = timestamper() - - with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file: - data_writer = csv.writer(data_file) - data_writer.writerow([log_stamp,data_line, remark]) \ No newline at end of file diff --git a/main.py b/main.py index bda1ec2..2c1382d 100644 --- a/main.py +++ b/main.py @@ -1,15 +1,14 @@ import control_functions as cf -import data_transfer_functions as dt 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 data_transfer_functions as dt import statistics as st +from adafruit_hcsr04 import HCSR04 as hcsr04 -_, fixed_file_stamp = dt.timestamper() +_, fixed_file_stamp = cf.time_stamper() cf.read_distance_sensor(fixed_file_stamp)