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?!")