242 lines
9.9 KiB
Python
242 lines
9.9 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 pandas as pd
|
|
|
|
# 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 function
|
|
MIN_DISTANCE: int = 6 # Minimum sensor distance to be considered valid (1 on bar)
|
|
MAX_DISTANCE: int = 39 # 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
|
|
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
|
|
|
|
# Variables to control logging.
|
|
LOG: bool = True # Log data to files
|
|
SCREEN: bool = True # Log data to screen
|
|
DEBUG: bool = True # More data to display
|
|
TWIN_MODE: bool = False
|
|
|
|
# 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 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
|
|
|
|
# Variables to assist pid_calculations()
|
|
current_time: float = 0
|
|
integral: float = 0
|
|
|
|
# Error sum array
|
|
error_sum_array: list = []
|
|
error_sum_counter: int = 0
|
|
error_sum_max: int = 100
|
|
|
|
# Digital twin
|
|
previous_speed:float = 0.0
|
|
start_loop = True
|
|
previous_measurement: float = 0.0
|
|
|
|
# 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)
|
|
data_writer.writerow([log_stamp,data_line, remark])
|
|
|
|
def read_distance_sensor():
|
|
# 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)
|
|
try:
|
|
distance: float = sonar.distance
|
|
if MIN_DISTANCE < distance < MAX_DISTANCE:
|
|
|
|
log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
|
|
# print("Distance_in_range: ", 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)
|
|
median_distance: float = st.median(sample_array)
|
|
mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
|
|
print("Distance_median: ", median_distance) if SCREEN else None
|
|
print("Timestamp_mean: ", mean_timestamp) if SCREEN else None
|
|
print("Distance_in_range: ", distance) if SCREEN else None
|
|
|
|
samples: int = samples + 1
|
|
|
|
else:
|
|
log_data(data_file="sensor", data_line=str(distance), remark="") 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
|
|
|
|
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(data_file="potmeter", data_line=log_line, remark="") if LOG else None
|
|
|
|
cm_rounded: int = int(round(scaled_value * POT_PCM, 0))
|
|
|
|
if SCREEN:
|
|
print('Scaled_rounded = ' , round(scaled_value, 4), ' CM_rounded= ', cm_rounded)
|
|
print('Scaled_raw= ' , scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM))
|
|
|
|
sleep(POT_INT)
|
|
return cm_rounded
|
|
|
|
def calculate_acceleration():
|
|
|
|
position_1, timestamp_1 = read_distance_sensor()
|
|
position_2, timestamp_2 = read_distance_sensor()
|
|
position_3, timestamp_3 = read_distance_sensor()
|
|
|
|
initial_velocity: float = (position_2 - position_1) / (timestamp_2 - timestamp_1)
|
|
final_velocity: float = ((position_3 - position_2) / (timestamp_3 - timestamp_2))
|
|
acceleration: float = (final_velocity - initial_velocity) / (timestamp_3 - timestamp_1)
|
|
|
|
print(initial_velocity, " ", final_velocity, " ", acceleration) if SCREEN else None
|
|
|
|
data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
|
|
log_data(data_file="acceleration", data_line=data_line, remark="") 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.
|
|
global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
|
|
offset_value: int = 0
|
|
if TWIN_MODE:
|
|
measurement, measurement_time = digital_twin()
|
|
else:
|
|
measurement, measurement_time = read_distance_sensor()
|
|
|
|
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
|
|
|
|
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 SCREEN:
|
|
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:
|
|
error_sum_counter = error_sum_counter + 1
|
|
else:
|
|
error_sum_counter = 0
|
|
|
|
return pid_result
|
|
|
|
def control_server_angle(angle):
|
|
KIT.servo[0].angle = angle # Set angle
|
|
log_line = str(angle)
|
|
log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
|
|
print(angle) if SCREEN else None
|
|
|
|
def digital_twin(pid_angle):
|
|
global start_loop
|
|
measurement_time = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])
|
|
|
|
if start_loop:
|
|
delta_t = measurement_time - (measurement_time - 0.002)
|
|
start_loop = False
|
|
else:
|
|
delta_t = measurement_time - previous_time
|
|
|
|
twin_data = pd.read_csv('twin_data_file.csv')
|
|
twin_data.set_index('Arm angle', inplace=True)
|
|
acceleration = twin_data.loc[pid_angle, 'Acceleration']
|
|
|
|
# previous acceleration to speed.
|
|
new_speed = previous_speed + (acceleration*delta_t)
|
|
measurement = new_speed * delta_t + previous_measurement
|
|
|
|
print(measurement)
|
|
print(new_speed)
|
|
print(previous_speed)
|
|
return measurement, measurement_time |