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

Cosmetic changes, potmeter code fixed, cleanup code.

Browse Source
This commit is contained in:
Rudi klein 2025-01-04 17:50:27 +01:00
parent a359707bc7
commit c92be8b49a
4 changed files with 74 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Examples/Adafruit_PCA9685_servo_driver.py
View File

@ -17,7 +17,7 @@ pwm.set_pwm_freq(50)
channel = 0 channel = 0
# Minimum and maximum pulse lengths. 100-510 translates to 0-180 degree. # Minimum and maximum pulse lengths. 100-510 translates to 0-180 degree.
# The formula for angel to pulse length is: 41/9 * <angle> /2 +100. MUST BE ROUNDED en set to INT() # The formula for angle to pulse length is: 41/9 * <angle> /2 +100. MUST BE ROUNDED en set to INT()
min_pulse = 100 # Min pulse length = 0deg min_pulse = 100 # Min pulse length = 0deg
max_pulse = 510 # Max pulse length = 180deg max_pulse = 510 # Max pulse length = 180deg
@ -26,3 +26,4 @@ set_angle = 180
print("Angle:", set_angle, "> Pulse:", angle_to_pulse(set_angle)) print("Angle:", set_angle, "> Pulse:", angle_to_pulse(set_angle))
pwm.set_pwm(channel, 0, angle_to_pulse(set_angle)) pwm.set_pwm(channel, 0, angle_to_pulse(set_angle))

23
Examples/Adafruit_Servokit_servo_driver.py
View File

@ -1,12 +1,15 @@
from time import sleep import math
from adafruit_servokit import ServoKit from adafruit_servokit import ServoKit
kit = ServoKit(channels=16) kit = ServoKit(channels=16)
MIN_PULSE = 400 # Defines angle 80, for current PID setup -- 550
MAX_PULSE = 2500 # Defines angle 100, for current PID setup -- 2450
kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# Control the minimum and maximum range of the servo. # Control the minimum and maximum range of the servo.
# min_pulse (int) – The minimum pulse width of the servo in microseconds. # min_pulse (int) – The minimum pulse width of the servo in microseconds.
# max_pulse (int) – The maximum pulse width of the servo in microseconds. # max_pulse (int) – The maximum pulse width of the servo in microseconds.
kit.servo[0].set_pulse_width_range(500, 2500) # kit.servo[0].set_pulse_width_range(500, 2500)
# Pulse width expressed as fraction between 0.0 (`min_pulse`) and 1.0 (`max_pulse`). # Pulse width expressed as fraction between 0.0 (`min_pulse`) and 1.0 (`max_pulse`).
# For conventional servos, corresponds to the servo position as a fraction # For conventional servos, corresponds to the servo position as a fraction
@ -16,23 +19,17 @@ kit.servo[0].set_pulse_width_range(500, 2500)
# property angle: float | None # property angle: float | None
# The servo angle in degrees. Must be in the range 0 to actuation_range. # The servo angle in degrees. Must be in the range 0 to actuation_range.
# Is None when servo is disabled. # Is None when servo is disabled.
kit.servo[0].angle = 180
# kit.servo[0].angle = 90
# property throttle: float # property throttle: float
# How much power is being delivered to the motor. # How much power is being delivered to the motor.
# Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.) # Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.)
# 0 will stop the motor from spinning. # 0 will stop the motor from spinning.
# kit.continuous_servo[0].throttle = 1 # kit.continuous_servo[0].throttle = 0.5
# property actuation range: float | None # property actuation range: float | None
# The servo angle in degrees. Must be in the range 0 to actuation_range. # The servo angle in degrees. Must be in the range 0 to actuation_range.
# Is None when servo is disabled # Is None when servo is disabled
#kit.servo[0].actuation_range = 120 #kit.servo[0].actuation_range = 120

108
control_functions.py
View File

@ -7,9 +7,10 @@ from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library
import statistics as st # Mean and median calculations import statistics as st # Mean and median calculations
import csv # CSV handling import csv # CSV handling
from datetime import datetime # Date and time formatting from datetime import datetime # Date and time formatting
import time # Time formatting from time import sleep # Sleep/pause
import os # OS environment
import main file_stamp = os.environ.get("PID_TIMESTAMP") # Get file timestamp from OS variable
# Variables to control sensor # Variables to control sensor
TRIGGER_PIN = board.D4 # GPIO pin xx TRIGGER_PIN = board.D4 # GPIO pin xx
@ -19,24 +20,42 @@ MIN_DISTANCE: int = 4 # Minimum sensor distance to considered valid
MAX_DISTANCE: int = 40 # Maximum sensor distance to considered valid MAX_DISTANCE: int = 40 # Maximum sensor distance to considered valid
# Variables to control servo # Variables to control servo
# MIN_PULSE = 750 # Defines angle 0, actual minimum for this servo
# MAX_PULSE = 2150 # Defines angle 180, actual maximum for this servo
KIT = ServoKit(channels=16) # Define the type of board (8, 16) KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE = 500 # Defines angle 0 MIN_PULSE: int = 400 # Defines angle 80, for current PID setup
MAX_PULSE = 2500 # Defines angle 180 MAX_PULSE: int = 2500 # Defines angle 100, for current PID setup
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE) KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# Variables to control logging. # Variables to control logging.
LOG: bool = True # Log data to files LOG: bool = False # Log data to files
SCREEN: bool = True # Log data to screen SCREEN: bool = True # Log data to screen
DEBUG: bool = False # More data to display DEBUG: bool = False # More data to display
# Control the number of samples for single measurement # Control the number of samples for single distance measurement (average from burst)
MAX_SAMPLES = 10 MAX_SAMPLES: int = 10
# Control the number of samples for the potentiometer # Control the potentiometer
PCF_VALUE = 65535 # Description:
POT_MAX = 65280 # POT_MIN = min_scaled: 0.012890821698329136 (0.01V)
POT_MIN = 256 # POT_MAX = max_scaled: 3.28715953307393000 (3.29V)
POT_INTERVAL = 0.01 # 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 assist PID calculations # Variables to assist PID calculations
current_time: float = 0 current_time: float = 0
@ -45,7 +64,7 @@ time_prev: float = -1e-6
error_prev: float = 0 error_prev: float = 0
# Variables to control PID values (PID formula tweaks) # Variables to control PID values (PID formula tweaks)
p_value : float = 2.0 p_value: float = 2.0
i_value: float = 0.0 i_value: float = 0.0
d_value: float = 0.0 d_value: float = 0.0
@ -57,9 +76,6 @@ previous_error: float = 0.0
# Init array, used in read_distance_sensor() # Init array, used in read_distance_sensor()
sample_array: list = [] sample_array: list = []
def initial():
...
# Write data to any of the logfiles # Write data to any of the logfiles
def log_data(file_stamp: str, data_file: str, data_line: str, 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] log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
@ -68,7 +84,7 @@ def log_data(file_stamp: str, data_file: str, data_line: str, remark: str|None):
data_writer = csv.writer(data_file) data_writer = csv.writer(data_file)
data_writer.writerow([log_stamp,data_line, remark]) data_writer.writerow([log_stamp,data_line, remark])
def read_distance_sensor(file_stamp): def read_distance_sensor():
# Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long distance) # 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. # Return the mean timestamp and median distance.
@ -77,6 +93,7 @@ def read_distance_sensor(file_stamp):
max_samples: int = MAX_SAMPLES max_samples: int = MAX_SAMPLES
timestamp_last: float = 0.0 timestamp_last: float = 0.0
timestamp_first: float = 0.0 timestamp_first: float = 0.0
while samples != max_samples: while samples != max_samples:
try: try:
distance: float = sonar.distance distance: float = sonar.distance
@ -96,64 +113,53 @@ def read_distance_sensor(file_stamp):
samples: int = samples + 1 samples: int = samples + 1
median_distance: list = st.median(sample_array) median_distance: list = st.median(sample_array)
mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float]) mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
print(median_distance) if SCREEN else None print(median_distance) if SCREEN else None
print(mean_timestamp) if SCREEN else None print(mean_timestamp) if SCREEN else None
else: else:
log_data(file_stamp=file_stamp, data_file="sensor", data_line=str(distance), remark=None) if LOG 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 print("Distance: ", distance) if SCREEN else None
except RuntimeError: except RuntimeError:
log_data(file_stamp=file_stamp, data_file="sensor", data_line="999.999", remark="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 print("Timeout") if SCREEN else None
return median_distance, mean_timestamp return median_distance, mean_timestamp
def read_setpoint(file_stamp): def read_setpoint():
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: while True:
raw_value: int = pcf_in_0.value raw_value: int = pcf_in_0.value
scaled_value: float = (raw_value / PCF_VALUE) * pcf_in_0.reference_voltage scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage
# Calculate angle in reference to raw pot values
angle = round(((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN),0) log_line = str(scaled_value) + "," + str(raw_value) + "," + str("angle")
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 log_data(file_stamp=file_stamp, data_file="potmeter", data_line=log_line, remark=None) if LOG else None
if SCREEN: if SCREEN:
print('pin 0= ', pcf.read(0)) print('scaled= ' , round(scaled_value, 4), ' cm= ', int(round(scaled_value * POT_PCM, 0)))
print('raw_value= ',raw_value) sleep(POT_INT)
print("pin 0= %0.2fV" % scaled_value)
print('Scaled= ' , scaled_value)
print(angle)
time.sleep(POT_INTERVAL) def calculate_velocity():
send_servo_angle(set_angle=angle)
def calculate_velocity(file_stamp):
velocity = "0" velocity = "0"
log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None
def pid_calculations(setpoint):
def pid_calculations(file_stamp, setpoint): global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
global i_result, previous_time, previous_error
offset_value: int = 320 offset_value: int = 320
measurement, measurement_time = read_distance_sensor(file_stamp=main.file_stamp) # todo Check logging measurement, measurement_time = read_distance_sensor()
error: float = setpoint - measurement error = setpoint - measurement
error_sum: float = 0.0 error_sum: float = 0.0
if previous_time is None: if previous_time is None:
previous_error = 0.0 previous_error = 0.0
previous_time = current_time previous_time = current_time
i_result = 0.0 i_result = 0.0
error_sum = 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 = p_value * error p_result = p_value * error
@ -167,11 +173,7 @@ def pid_calculations(file_stamp, setpoint):
log_data(file_stamp=file_stamp, data_file="pid", data_line=log_line, remark=None) if LOG else None log_data(file_stamp=file_stamp, data_file="pid", data_line=log_line, remark=None) if LOG else None
return pid_result return pid_result
def control_server_angle(angle):
def calculate_servo_position(): KIT.servo[0].angle = angle # Set angle
... log_line = str(angle)
log_data(file_stamp=file_stamp, data_file="servo", data_line=log_line, remark=None) if LOG else None
def send_servo_angle(set_angle):
KIT.servo[0].angle = set_angle # Set angle

12
main.py
View File

@ -1,13 +1,13 @@
from datetime import datetime from datetime import datetime
import control_functions as cf import control_functions as cf
import os
file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M') os.environ["PID_TIMESTAMP"] = datetime.strftime(datetime.now(), '%Y%m%d%I%M') # Set file timestamp as OS variable.
cf.read_distance_sensor(file_stamp)
cf.read_setpoint(file_stamp)
cf.send_servo_angle(file_stamp)
while True:
print(cf.read_setpoint())