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

added analogue board and cosmetics

Browse Source
This commit is contained in:
Rudi klein 2024-12-28 21:06:10 +01:00
parent b54b75036f
commit f175b29eda
4 changed files with 129 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.idea/misc.xml generated Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Black">
<option name="sdkName" value="Python 3.11 (pid-balancer)" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (pid-balancer)" project-jdk-type="Python SDK" />
</project>

181
control_functions.py
View File

@ -1,110 +1,172 @@
from OpenGL.images import returnFormat from adafruit_hcsr04 import HCSR04 as hcsr04 # PWM driver board for servo
from adafruit_hcsr04 import HCSR04 as hcsr04 import board # PWM driver board for servo
import board from adafruit_servokit import ServoKit # Servo libraries
import data_transfer_functions as dt import adafruit_pcf8591.pcf8591 as PCF # AD/DA converter board for potentiometer
from adafruit_servokit import ServoKit from adafruit_pcf8591.analog_in import AnalogIn # Analogue in pin library
import numpy as np from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library
import matplotlib.pyplot as plt import adafruit_pcf8591 as pcf8591 # AD/DA converter board for potentiometer
from scipy.integrate import odeint import numpy as np # Number handling
import numpy as np import pandas as pd # Data handling
import matplotlib.pyplot as plt import matplotlib.pyplot as plt # Plotter handling
import statistics as st 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 # Variables to control sensor
TRIGGER_PIN = board.D4 TRIGGER_PIN = board.D4 # GPIO pin xx
ECHO_PIN = board.D17 ECHO_PIN = board.D17 # GPIO pin xx
TIMEOUT = 0.1 TIMEOUT: float = 0.1 # Timout for echo wait
MIN_DISTANCE = 4 MIN_DISTANCE: int = 4 # Minimum sensor distance to considered valid
MAX_DISTANCE = 40 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. # Variables to control logging.
LOG = True LOG: bool = True # Log data to files
SCREEN = True SCREEN: bool = True # Log data to screen
DEBUG = False DEBUG: bool = False # More data to display
# PID measurements # Variables to assist PID calculations
time = 0 current_time: float = 0
integral = 0 integral: float = 0
time_prev = -1e-6 time_prev: float = -1e-6
e_prev = 0 error_prev: float = 0
# PID values # Variables to control PID values (PID formula tweaks)
P_value = 2 p_value: float = 2
I_value = 0 i_value: float = 0
D_value = 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(): 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): def read_distance_sensor(fixed_file_stamp):
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 = 0 samples: int = 0
max_samples = 10 max_samples: int = 10
timestamp_last = 0.0 timestamp_last: float = 0.0
timestamp_first = 0.0 timestamp_first: float = 0.0
while samples != max_samples: while samples != max_samples:
try: try:
distance = sonar.distance distance: float = sonar.distance
if MIN_DISTANCE < distance < MAX_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 print("Distance: ", distance) if SCREEN else None
sample_array.append(distance) sample_array.append(distance)
if samples == 0: timestamp_first, _ = dt.timestamper() if samples == 0: timestamp_first, _ = time_stamper()
if samples == max_samples - 1: timestamp_last, _ = dt.timestamper() if samples == max_samples - 1: timestamp_last, _ = time_stamper()
timestamp_first_float = float(timestamp_first) timestamp_first_float: float = float(timestamp_first)
timestamp_last_float = float(timestamp_last) timestamp_last_float: float = float(timestamp_last)
samples = samples + 1 samples: int = samples + 1
median_distance = st.median(sample_array) median_distance: list = st.median(sample_array)
mean_timestamp = st.mean([timestamp_first_float, timestamp_last_float]) mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
print(median_distance) if LOG else None print(median_distance) if SCREEN else None
print(mean_timestamp) print(mean_timestamp) if SCREEN else None
else: 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 print("Distance: ", distance) if SCREEN else None
except RuntimeError: 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 print("Timeout") if SCREEN else None
return median_distance, mean_timestamp return median_distance, mean_timestamp
def read_setpoint(): 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 calculate_velocity():
... ...
def PID_calculations(setpoint): def pid_calculations(setpoint):
global I_result, previous_time, previous_error global i_result, previous_time, previous_error
offset_value = 320 offset_value: int = 320
measurement, current_time = read_distance_sensor measurement, current_time = read_distance_sensor
error = setpoint - measurement error: float = setpoint - measurement
error_sum: float = 0.0
if previous_time is None: if previous_time is None:
previous_error: float = 0.0 previous_error: float = 0.0
previous_time: float = current_time 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 * 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: float = p_value * error
I_result: float = I_value * error_sum i_result: float = i_value * error_sum
D_result: float = D_value * ((error - previous_error) / (current_time - previous_time)) d_result: float = d_value * ((error - previous_error) / (current_time - previous_time))
PID_result: float = offset_value + P_result + I_result + D_result pid_result: float = offset_value + p_result + i_result + d_result
previous_error: float = error previous_error: float = error
previous_time: float = current_time previous_time: float = current_time
return PID_result return pid_result
def calculate_new_servo_pos(): def calculate_new_servo_pos():
@ -113,8 +175,5 @@ def calculate_new_servo_pos():
def send_data_to_servo(): def send_data_to_servo():
kit = ServoKit(channels=16) KIT.servo[0].angle = 180 # Set angle
kit.servo[0].set_pulse_width_range(500, 2500)
kit.servo[0].angle = 180

16
data_transfer_functions.py
View File

@ -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])

5
main.py
View File

@ -1,15 +1,14 @@
import control_functions as cf import control_functions as cf
import data_transfer_functions as dt
import plotter_functions as pf import plotter_functions as pf
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from scipy.integrate import odeint from scipy.integrate import odeint
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import data_transfer_functions as dt
import statistics as st 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) cf.read_distance_sensor(fixed_file_stamp)