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

437 lines
18 KiB
Python
Raw Permalink Blame History

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
from adafruit_hcsr04 import HCSR04 as hcsr04 # Ultrasound sensor
import board # General board pin mapper
import statistics as st # Mean and median calculations
import csv # CSV handling
from time import sleep # Sleep/pause
import pandas as pd # Pandas for data manipulation
from datetime import datetime # Datetime for timestamps
import math # Math for particular calculations
import matplotlib.pyplot as plt # Mathplotlib for graphs
# Variables to control sensor
TRIGGER_PIN = board.D4 # GPIO pin xx
ECHO_PIN = board.D17 # GPIO pin xx
PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change
RUN_TIMEOUT: float = 0.0 # Sleep time in read_distance() function
MIN_DISTANCE: int = 2 # Minimum sensor distance to be considered valid (1 on bar)
MAX_DISTANCE: int = 36 # Maximum sensor distance to be considered valid (35 on bar)
# Variables to control servo
KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE: int = 400 # Defines angle 80, for current PID setup
MAX_PULSE: int = 2500 # Defines angle 100, for current PID setup
OFFSET: int = -2 # Correction nominal angle versus physical angle of the arm
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# Variables to control logging.
LOG: bool = False # Log data to files
LOG_GRAPH: bool = True # Log graph creation
SCREEN: bool = True # Log data to screen
DEBUG: bool = False # More data to display
TWIN_MODE: bool = True # Run in live or twin mode
# Control the number of samples for single distance measurement (average from sample burst)
MAX_SAMPLES: int = 8
# Control the potentiometer
# Description:
# POT_MIN = min_scaled: 0.012890821698329136 (0.01V)
# POT_MAX = max_scaled: 3.28715953307393000 (3.29V)
# POT_RNG = range_scaled: 3.274268711375600864 (3.28V) -> POT_MAX - POT_MIN
# POT_ARM = usable_arm_range: 35cm
# POT_PCM = 35 / 3.274268711375600864 = 10.689409784359341315326937965383 -> POT_ARM / POT_RNG
PCF_VAL: int = 65535
POT_MIN: float = 0.012890821698329136
POT_MAX: float = 3.287159533073930000
POT_RNG: float = 3.274268711375600864
POT_ARM: int = 35
POT_PCM: float = 10.689409784359341315326937965383
POT_INT: float = 0.1
# Pin control potentiometer board
i2c = board.I2C()
pcf = PCF.PCF8591(i2c)
pcf_in_0 = AnalogIn(pcf, PCF.A0)
pcf_out = AnalogOut(pcf, PCF.OUT)
pcf_out.value = PCF_VAL
# Variables to control PID values (PID formula tweaks)
p_value: float = 1.0
i_value: float = 0.0
d_value: float = 0.1
# Initial variables, used in pid_calculations()
i_result: float = 0.0
previous_time: float = 0.0
previous_error: float = 0.0
# Error sum array values
error_sum_max: int = 10
error_sum_array: list = [0] * error_sum_max
error_sum_counter: int = 0
# Digital twin parameters
previous_speed: float = 0.0
previous_position: float = 0.0
previous_angle: int = 90
# a: acceleration
# g: gravity (9.81 m/s^2)
# theta: angle of the inclined plane
# u: coefficient of the friction between the cart and the inclined plane.
acceleration: float = 0.0
gravity: float = 9.81
friction: float = 0.05
delta_t: float = 0.2
# Maximum angle the servo can move away from steady position. With 10 the range is between 80 (-10) and 100 (+10),
# with steady at 90 (0)
max_angle: int = 5
# Servo slower
current_angle: int = 90
# Servo memory for boosting the cart if its stuck due to friction
servo_memory_1: int = 0
servo_memory_2: int = 0
memory_max: int = 5
# Current time of the system, used as base for file creation)
base_time: float = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
# Write base_time in file, to be used by other functions.
with open("pid-balancer_" + "time_file.txt", "w") as time_file:
time_file.write(datetime.strftime(datetime.now(), '%Y-%m-%d %H:%M:%S.%f')[:-3])
# Write data to any of the logfiles
def log_data(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_" + "time_file.txt", "r") as time_file:
file_stamp: str = time_file.readline()
with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file:
data_writer = csv.writer(data_file, delimiter=';', quoting=csv.QUOTE_MINIMAL)
data_writer.writerow([log_stamp, data_line, remark])
# Write data to any of the logfiles. This is specifically for one type of logfile that uses multiple data columns
def log_data2(data_file: str, data_line: str, data_line2: str | None):
log_stamp: str = datetime.strftime(datetime.now(), '%Y-%m-%d %H:%M:%S.%f')[:-3]
with open("pid-balancer_" + "time_file.txt", "r") as time_file:
file_stamp: str = time_file.readline()
with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file:
data_writer = csv.writer(data_file, delimiter=';', quoting=csv.QUOTE_MINIMAL)
data_writer.writerow([log_stamp, data_line, data_line2])
# Function to read the SR05 ultrasound sensor data
def read_distance_sensor():
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
# Init array, used in read_distance_sensor()
sample_array: list = []
# Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long a distance)
# Return the mean timestamp and median distance.
with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=PIN_TIMEOUT) as sonar:
samples: int = 0
max_samples: int = MAX_SAMPLES
timestamp_last: float = 0.0
timestamp_first: float = 0.0
while samples != max_samples:
sleep(RUN_TIMEOUT) # Fixes some sensor driver crashes
try:
distance: float = sonar.distance # Reading distance from the sonic sensor
if MIN_DISTANCE < distance < MAX_DISTANCE: # Only process distances within expected range.
# This drops erroneous readings.
log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
print("Distance_in_range: ", distance) if SCREEN else None # For testing
if max_samples == 1:
median_distance: float = distance
mean_timestamp = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3])
samples: int = samples + 1
print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None # For testing
else:
sample_array.append(distance)
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)
median_distance: float = st.median(sample_array)
mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
if DEBUG:
print("Distance_median: ", median_distance)
print("Timestamp_mean: ", mean_timestamp)
print("Distance_in_range: ", distance)
data_line = str(sample_array) + ',' + str(median_distance)
log_data(data_file="sensor_array", data_line=data_line, remark="")
print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None
samples: int = samples + 1
else:
log_data(data_file="sensor", data_line=str(distance),
remark="Distance_out_of_range") if LOG else None
print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None
except RuntimeError:
log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None
print("Distance_timed_out") if SCREEN else None
# Function process time recorder
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="read_distance_sensor") if LOG else None
# Median distance and Mean time to log writer
data_line = str(median_distance)
data_line2 = str(mean_timestamp)
log_data2(data_file="median_sensor", data_line=data_line, data_line2=data_line2) if LOG_GRAPH else None
return median_distance, mean_timestamp
def read_setpoint():
# Read the resistance of the potentiometer and convert to centimeters for use with setpoint distance
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
while True:
raw_value: int = pcf_in_0.value
scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage
log_line = str(scaled_value) + ";" + str(raw_value) + ";" + str("angle")
log_data(data_file="potmeter", data_line=log_line, remark="") if LOG else None
cm_rounded: int = int(round(scaled_value * POT_PCM, 0))
if DEBUG:
print('Scaled_rounded = ', round(scaled_value, 4), ' CM_rounded= ', cm_rounded)
print('Scaled_raw= ', scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM))
print('Setpoint in cm: ', cm_rounded) if SCREEN else None
sleep(POT_INT) # Fix for driver crashes
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="read_setpoint") if LOG else None
return cm_rounded
def digital_twin():
# Digital model of the physical model.
global previous_position, previous_speed, base_time
angle = (previous_angle - 90)
acceleration = gravity * math.sin(math.radians(angle))
friction_force = abs(friction * gravity * math.cos(math.radians(angle)) * delta_t)
work_speed = previous_speed + acceleration * delta_t
# To avoid the friction setting the work_speed to a negative value, forced the friction to be lower than the speed.
if friction_force < work_speed * 0.8:
if work_speed > 0:
work_speed = work_speed - friction_force
elif work_speed < 0:
work_speed = work_speed + friction_force
else:
work_speed = work_speed
current_speed: float = work_speed
current_position: float = previous_position + (current_speed * delta_t)
if SCREEN:
print("Angle", angle)
print("Friction", friction)
print("Acceleration", acceleration)
print("Current speed", current_speed)
print("Current position", current_position)
print("")
print("----------------------------------------------")
print("")
base_time = base_time + delta_t
previous_speed = current_speed
previous_position = current_position
if LOG_GRAPH:
# PID position logging
data_line = str(current_position)
log_data(data_file="twin_current_position", data_line=data_line, remark="")
# PID acceleration logging
data_line = str(acceleration)
log_data(data_file="twin_acceleration", data_line=data_line, remark="")
# PID speed logging
data_line = str(current_speed)
log_data(data_file="twin_current_speed", data_line=data_line, remark="")
return current_position, base_time
def pid_calculations():
# Do all the PID calculations and return the new angle for the servo
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
global previous_angle
offset_value: int = 0
if TWIN_MODE:
measurement, measurement_time = digital_twin()
else:
measurement, measurement_time = read_distance_sensor()
setpoint = read_setpoint()
error = setpoint - measurement
if previous_time is None:
previous_error = 0.0
previous_time = measurement_time
i_result = 0.0
error_sum_array[error_sum_counter] = (error * (measurement_time - previous_time))
p_result = p_value * error
i_result = i_value * sum(error_sum_array)
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
# Code to set the max angles. Or set the angle to a specific number = pid_result * max movement + correction
if pid_result >= max_angle:
output_angle = (90 + max_angle)
elif pid_result <= -max_angle:
output_angle = (90 - max_angle)
elif -max_angle < pid_result < max_angle:
output_angle = pid_result + 90
else:
output_angle = 90
log_line = str(p_result) + ";" + str(i_result) + ";" + str(d_result) + ";" + str(pid_result)
log_data(data_file="pid", data_line=log_line, remark="") if LOG else None
if DEBUG:
print("P_result: ", p_result)
print("D_result: ", d_result)
print("I_result: ", i_result)
print("PID_result: ", pid_result)
if error_sum_counter <= error_sum_max - 2: # Correction tweak for error sum
error_sum_counter = error_sum_counter + 1
else:
error_sum_counter = 0
print("error sum counter", error_sum_counter) if DEBUG else None
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="pid_calculations") if LOG else None
output_angle = round(output_angle)
previous_angle = output_angle
# PID angle logging
data_line = str(output_angle)
log_data(data_file="pid_output_angle", data_line=data_line, remark="") if LOG_GRAPH and TWIN_MODE == False else None
log_data(data_file="pid_output_angle_twin", data_line=data_line,
remark="") if LOG_GRAPH and TWIN_MODE == True else None
return output_angle
def control_server_angle(angle):
# Tell the servo to set its position
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
print("Current angle: ", angle) if SCREEN else None
servo_angle = angle + OFFSET
print("Offset angle: ", servo_angle) if SCREEN else None
KIT.servo[0].angle = servo_angle # Send angle instruction to the servo
log_line = str(angle)
log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="control_server_angle") if LOG else None
def servo_slower():
# This function restricts the servo to +/- 5 degrees in order to prevent launching the cart
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
global current_angle
pid_angle = pid_calculations()
if (pid_angle - current_angle) > 5:
servo_angle = current_angle + 5
elif (pid_angle - current_angle) < -5:
servo_angle = current_angle - 5
else:
servo_angle = pid_angle
current_angle = servo_angle
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="servo_slower") if LOG else None
return servo_angle
def graph_plotter(file_name):
# Creates the graphs with Pandas and Mathplotlib using the logiles as input. It must be run manually.
plt.rcParams['figure.figsize'] = [12, 8] # Set the size of the plot canvas
picture_name = file_name + '.png' # User the name of the logfile as input for the graphical image
file_name_plotter = file_name + ".csv" # Use the logfile as input
# Run one set of the graph code.
# df = pd.read_csv(file_name_plotter,delimiter=';', header=None, skiprows=0, decimal=".", names=['Timestamp', 'Distance', 'Timestamp2','Remarks'])
# df = df.drop(columns = ['Timestamp2'])
df = pd.read_csv(file_name_plotter, delimiter=';', header=None, skiprows=0, decimal=".",
names=['Timestamp', 'Distance', 'Remarks'])
df = df.drop(columns=['Remarks'])
plt.figure(figsize=(30, 60))
df.plot(x='Timestamp', y='Distance')
plt.savefig(picture_name)
plt.show()
# -------------------- Main ----------------------------------
try:
KIT.servo[0].angle = 90
# graph_plotter("pid-balancer_pid_output_angle_twin_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_acceleration_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_current_position_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_current_speed_data_2025-01-17 14:29:29.624")
while True:
control_server_angle(pid_calculations())
print("------------------------------------------\n")
except RuntimeError:
print("What's up?!")
Reference in New Issue View Git Blame Copy Permalink