diff --git a/control_functions.py b/control_functions.py index 987deef..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 @@ -14,8 +14,6 @@ import csv # CSV handling from datetime import datetime # Date and time formatting import time # Time formatting - -######################################## Variables (start) ################################## # Variables to control sensor TRIGGER_PIN = board.D4 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx @@ -34,49 +32,52 @@ 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 + +# 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 = 0 -integral = 0 -time_prev = -1e-6 -error_prev = 0 +current_time: float = 0 +integral: float = 0 +time_prev: float = -1e-6 +error_prev: float = 0 # Variables to control PID values (PID formula tweaks) -p_value = 2 -i_value = 0 -d_value = 0 +p_value : float = 2.0 +i_value: float = 0.0 +d_value: float = 0.0 # Initial variables, used in pid_calculations() -i_result = 0 -previous_time = 0 -previous_error = 0 +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): - with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar): + # 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: @@ -84,12 +85,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) @@ -100,45 +103,37 @@ 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(): - - ############# 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) + 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(): ... @@ -147,7 +142,7 @@ 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 @@ -157,22 +152,24 @@ def pid_calculations(setpoint): i_result = 0.0 error_sum = error * 0.008 # sensor sampling number approximation. - error_sum = error_sum + (error * (current_time - previous_time)) - p_result = p_value * error + error_sum: float = error_sum + (error * (current_time - previous_time)) + p_result = p_value * error i_result = i_value * error_sum - d_result = d_value * ((error - previous_error) / (current_time - previous_time)) + 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 = current_time + previous_error = error + previous_time = measurement_time return pid_result -def calculate_new_servo_pos(): +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 2c1382d..5cca24a 100644 --- a/main.py +++ b/main.py @@ -1,17 +1,9 @@ +from datetime import datetime 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 -_, 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