diff --git a/Examples/Adafruit_Servokit_servo_driver.py b/Examples/Adafruit_Servokit_servo_driver.py index b9aad53..e6e6e3e 100644 --- a/Examples/Adafruit_Servokit_servo_driver.py +++ b/Examples/Adafruit_Servokit_servo_driver.py @@ -1,5 +1,6 @@ import math from adafruit_servokit import ServoKit +from time import sleep kit = ServoKit(channels=16) MIN_PULSE = 400 # Defines angle 80, for current PID setup -- 550 @@ -14,13 +15,17 @@ kit.servo[0].set_pulse_width_range(MIN_PULSE, 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 # of the actuation range. Is None when servo is disabled (pulsewidth of 0ms) -# kit.servo[0].fraction = 0.5 +# kit.servo[0].angle = 110 # property angle: float | None # The servo angle in degrees. Must be in the range 0 to actuation_range. # Is None when servo is disabled. - -kit.servo[0].angle = 90 +while True: + kit.servo[0].angle = 110 + sleep(1) + kit.servo[0].angle = 90 + sleep(0.1) + print("test") diff --git a/control_functions.py b/control_functions.py index e270484..faa60c3 100644 --- a/control_functions.py +++ b/control_functions.py @@ -1,3 +1,5 @@ +from csv import excel + 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 @@ -9,6 +11,7 @@ import csv # CSV handling from datetime import datetime # Date and time formatting from time import sleep # Sleep/pause import pandas as pd +from datetime import datetime # Variables to control sensor TRIGGER_PIN = board.D4 # GPIO pin xx @@ -16,22 +19,23 @@ 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) +MAX_DISTANCE: int = 40 # 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 = -1 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 +DEBUG: bool = False # More data to display TWIN_MODE: bool = False # Control the number of samples for single distance measurement (average from burst) -MAX_SAMPLES: int = 10 +MAX_SAMPLES: int = 1 # Control the potentiometer # Description: @@ -56,8 +60,8 @@ 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 +p_value: float = 0.5 +i_value: float = 0.01 d_value: float = 0.0 # Initial variables, used in pid_calculations() @@ -65,20 +69,23 @@ 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_max: int = 10 +error_sum_array: list = [0]*error_sum_max 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 +#maximum angle the servo can move away from steady position. With 10 the range is between 80 and 100, with steady at 90 +max_angle = 6 + +# servo slower +current_angle:int = 90 + # 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] @@ -91,6 +98,8 @@ def log_data(data_file: str, data_line: str, remark: str|None): data_writer.writerow([log_stamp,data_line, remark]) 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 = [] @@ -110,35 +119,48 @@ def read_distance_sensor(): log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None # print("Distance_in_range: ", distance) if SCREEN else None + if max_samples == 1: + median_distance = distance + mean_timestamp = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3]) + 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]) - 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 DEBUG else None + print("Distance_in_range: ", distance) if SCREEN else None - 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 + data_line = str(sample_array) + ',' + str(median_distance) + log_data(data_file="sensor_array", data_line= data_line, + remark="") if LOG else None + print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None - samples: int = samples + 1 + samples: int = samples + 1 else: - log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None + 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 + 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 + return median_distance, mean_timestamp def read_setpoint(): + start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) while True: raw_value: int = pcf_in_0.value @@ -149,15 +171,26 @@ def read_setpoint(): cm_rounded: int = int(round(scaled_value * POT_PCM, 0)) - if SCREEN: + 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('setpoing in cm: ', cm_rounded) if SCREEN else None + sleep(POT_INT) + + 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 calculate_acceleration(): + print("calc is active") + + start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) + position_1, timestamp_1 = read_distance_sensor() position_2, timestamp_2 = read_distance_sensor() position_3, timestamp_3 = read_distance_sensor() @@ -168,10 +201,15 @@ def calculate_acceleration(): print(initial_velocity, " ", final_velocity, " ", acceleration) if SCREEN else None - data_line: str = str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration) + data_line: str = str(position_1) + ',' + str(position_2) + ',' + str(position_3) + ',' + 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): + 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="calculate_acceleration") if LOG else None + +def pid_calculations(): + 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 @@ -181,6 +219,7 @@ def pid_calculations(setpoint): else: measurement, measurement_time = read_distance_sensor() + setpoint = read_setpoint() error = setpoint - measurement if previous_time is None: @@ -196,29 +235,58 @@ def pid_calculations(setpoint): previous_error = error previous_time = measurement_time + + #function to set the 2 max angles. Or set the angle to a specific number = pid_result * max movement + correction + if pid_result >= max_angle: # if PID result is greater than 1, set to 1. 1 = max upward angle + output_angle = (90 + max_angle) + elif pid_result <= -max_angle: # if PID result is greater than 1, set to 1. 1 = max downward 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 SCREEN: + 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: + if error_sum_counter <= error_sum_max-2: error_sum_counter = error_sum_counter + 1 else: error_sum_counter = 0 - return pid_result + 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) + + return output_angle def control_server_angle(angle): - KIT.servo[0].angle = angle # Set angle + start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) + + servo_angle = angle + OFFSET + + KIT.servo[0].angle = servo_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 + print("angle: ", servo_angle) if SCREEN 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 digital_twin(pid_angle): + start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) + global start_loop measurement_time = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3]) @@ -239,4 +307,40 @@ def digital_twin(pid_angle): print(measurement) print(new_speed) print(previous_speed) - return measurement, measurement_time \ No newline at end of file + + 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="digital_twin") if LOG else None + + return measurement, measurement_time + +def servo_slower(): + 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 + + +try: + 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]) + KIT.servo[0].angle = 90 + while True: + calculate_acceleration() + # control_server_angle(pid_calculations()) +except RuntimeError: + print("bbbb") \ No newline at end of file