diff --git a/control_functions.py b/control_functions.py index 2239d47..125a7af 100644 --- a/control_functions.py +++ b/control_functions.py @@ -4,16 +4,13 @@ from adafruit_servokit import ServoKit # Servo libraries for PWM dr 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 +import main + # Variables to control sensor TRIGGER_PIN = board.D4 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx @@ -39,7 +36,7 @@ MAX_SAMPLES = 10 PCF_VALUE = 65535 POT_MAX = 65280 POT_MIN = 256 -POT_INTERVAL = 0.1 +POT_INTERVAL = 0.01 # Variables to assist PID calculations current_time: float = 0 @@ -64,7 +61,7 @@ def initial(): ... # Write data to any of the logfiles -def log_data(file_stamp: str, data_file: str, data_line: float, remark: str|None): +def log_data(file_stamp: str, data_file: str, data_line: str, 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_" + file_stamp + ".csv", "a") as data_file: @@ -85,7 +82,7 @@ def read_distance_sensor(file_stamp): distance: float = sonar.distance if MIN_DISTANCE < distance < MAX_DISTANCE: - log_data(file_stamp,"sensor", distance, None) if LOG else None + log_data(file_stamp,"sensor", str(distance), None) if LOG else None print("Distance: ", distance) if SCREEN else None sample_array.append(distance) @@ -103,16 +100,16 @@ def read_distance_sensor(file_stamp): print(mean_timestamp) if SCREEN else None else: - log_data(file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None + log_data(file_stamp=file_stamp, data_file="sensor", data_line=str(distance), remark=None) if LOG else None print("Distance: ", distance) if SCREEN else None except RuntimeError: - log_data(file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None + log_data(file_stamp=file_stamp, data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None print("Timeout") if SCREEN else None return median_distance, mean_timestamp -def read_setpoint(): +def read_setpoint(file_stamp): i2c = board.I2C() pcf = PCF.PCF8591(i2c) @@ -121,28 +118,34 @@ def read_setpoint(): pcf_out.value = PCF_VALUE while True: - raw_value: float = pcf_in_0.value + raw_value: int = 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) + angle = round(((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN),0) + log_line = str(scaled_value) + "," + str(raw_value) + "," + str(angle) + log_data(file_stamp=file_stamp, data_file="potmeter", data_line=log_line, remark=None) if LOG else None if SCREEN: - print('pin 0 ', pcf.read(0)) - print('raw_value ',raw_value) - print("pin 0: %0.2fV" % scaled_value) + print('pin 0= ', pcf.read(0)) + print('raw_value= ',raw_value) + print("pin 0= %0.2fV" % scaled_value) + print('Scaled= ' , scaled_value) print(angle) time.sleep(POT_INTERVAL) - send_data_to_servo(set_angle=angle) + send_servo_angle(set_angle=angle) -def calculate_velocity(): - ... +def calculate_velocity(file_stamp): -def pid_calculations(setpoint): + velocity = "0" + log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None + + +def pid_calculations(file_stamp, setpoint): global i_result, previous_time, previous_error offset_value: int = 320 - measurement, measurement_time = read_distance_sensor() + measurement, measurement_time = read_distance_sensor(file_stamp=main.file_stamp) # todo Check logging error: float = setpoint - measurement error_sum: float = 0.0 @@ -160,16 +163,15 @@ def pid_calculations(setpoint): previous_error = error previous_time = measurement_time + log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result) + log_data(file_stamp=file_stamp, data_file="pid", data_line=log_line, remark=None) if LOG else None return pid_result -def calculate_new_servo_position(): +def calculate_servo_position(): ... -def send_data_to_servo(set_angle): +def send_servo_angle(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 5cca24a..f4b79b4 100644 --- a/main.py +++ b/main.py @@ -4,6 +4,8 @@ import control_functions as cf file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M') cf.read_distance_sensor(file_stamp) +cf.read_setpoint(file_stamp) +cf.send_servo_angle(file_stamp) diff --git a/plotter_functions.py b/plotter_functions.py index b9925de..b330e93 100644 --- a/plotter_functions.py +++ b/plotter_functions.py @@ -1,5 +1,7 @@ -import pandas as pd -import matplotlib.pyplot as plt +import pandas as pd # Data wrangling +import numpy as np # Number handling +import matplotlib.pyplot as plt # Plotter handling +from scipy.integrate import odeint # Integral calculations # Variables to control logging. LOG: bool = True # Log data to files