179 lines
7.8 KiB
Python
179 lines
7.8 KiB
Python
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
|
|
import statistics as st # Mean and median calculations
|
|
import csv # CSV handling
|
|
from datetime import datetime # Date and time formatting
|
|
from time import sleep # Sleep/pause
|
|
import os # OS environment
|
|
|
|
file_stamp = os.environ.get("PID_TIMESTAMP") # Get file timestamp from OS variable
|
|
|
|
# Variables to control sensor
|
|
TRIGGER_PIN = board.D4 # GPIO pin xx
|
|
ECHO_PIN = board.D17 # GPIO pin xx
|
|
TIMEOUT: float = 0.1 # Timout for echo wait
|
|
MIN_DISTANCE: int = 4 # Minimum sensor distance to considered valid
|
|
MAX_DISTANCE: int = 40 # Maximum sensor distance to considered valid
|
|
|
|
# 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)
|
|
MIN_PULSE: int = 400 # Defines angle 80, for current PID setup
|
|
MAX_PULSE: int = 2500 # Defines angle 100, for current PID setup
|
|
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
|
|
|
|
# Variables to control logging.
|
|
LOG: bool = False # 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 distance measurement (average from burst)
|
|
MAX_SAMPLES: int = 10
|
|
|
|
# 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 assist PID calculations
|
|
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: float = 2.0
|
|
i_value: float = 0.0
|
|
d_value: float = 0.0
|
|
|
|
# Initial variables, used in pid_calculations()
|
|
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 = []
|
|
|
|
# Write data to any of the logfiles
|
|
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:
|
|
data_writer = csv.writer(data_file)
|
|
data_writer.writerow([log_stamp,data_line, remark])
|
|
|
|
def read_distance_sensor():
|
|
|
|
# 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 = MAX_SAMPLES
|
|
timestamp_last: float = 0.0
|
|
timestamp_first: float = 0.0
|
|
|
|
while samples != max_samples:
|
|
try:
|
|
distance: float = sonar.distance
|
|
if MIN_DISTANCE < distance < MAX_DISTANCE:
|
|
|
|
log_data(file_stamp,"sensor", str(distance), None) if LOG else None
|
|
print("Distance: ", distance) if SCREEN else None
|
|
|
|
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)
|
|
samples: int = samples + 1
|
|
median_distance: list = st.median(sample_array)
|
|
mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
|
|
|
|
print(median_distance) if SCREEN else None
|
|
print(mean_timestamp) if SCREEN else None
|
|
|
|
else:
|
|
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=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():
|
|
|
|
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(file_stamp=file_stamp, data_file="potmeter", data_line=log_line, remark=None) if LOG else None
|
|
|
|
if SCREEN:
|
|
print('scaled= ' , round(scaled_value, 4), ' cm= ', int(round(scaled_value * POT_PCM, 0)))
|
|
sleep(POT_INT)
|
|
|
|
def calculate_velocity():
|
|
|
|
velocity = "0"
|
|
log_data(file_stamp=file_stamp, data_file="velocity", data_line=velocity, remark=None) if LOG else None
|
|
|
|
def pid_calculations(setpoint):
|
|
|
|
global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
|
|
offset_value: int = 320
|
|
measurement, measurement_time = read_distance_sensor()
|
|
error = setpoint - measurement
|
|
error_sum: float = 0.0
|
|
|
|
if previous_time is None:
|
|
previous_error = 0.0
|
|
previous_time = current_time
|
|
i_result = 0.0
|
|
error_sum: float = error * 0.008 # sensor sampling number approximation.
|
|
|
|
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) / (measurement_time - previous_time))
|
|
pid_result = offset_value + p_result + i_result + d_result
|
|
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 control_server_angle(angle):
|
|
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 |