diff --git a/Examples/Adafruit_PCA9685_servo_driver.py b/Examples/Adafruit_PCA9685_servo_driver.py index 60e55ba..258e605 100644 --- a/Examples/Adafruit_PCA9685_servo_driver.py +++ b/Examples/Adafruit_PCA9685_servo_driver.py @@ -17,7 +17,7 @@ pwm.set_pwm_freq(50) channel = 0 # Minimum and maximum pulse lengths. 100-510 translates to 0-180 degree. -# The formula for angel to pulse length is: 41/9 * /2 +100. MUST BE ROUNDED en set to INT() +# The formula for angle to pulse length is: 41/9 * /2 +100. MUST BE ROUNDED en set to INT() min_pulse = 100 # Min pulse length = 0deg max_pulse = 510 # Max pulse length = 180deg @@ -26,3 +26,4 @@ set_angle = 180 print("Angle:", set_angle, "> Pulse:", angle_to_pulse(set_angle)) pwm.set_pwm(channel, 0, angle_to_pulse(set_angle)) + diff --git a/Examples/Adafruit_Servokit_servo_driver.py b/Examples/Adafruit_Servokit_servo_driver.py index c38db1d..afc71e6 100644 --- a/Examples/Adafruit_Servokit_servo_driver.py +++ b/Examples/Adafruit_Servokit_servo_driver.py @@ -1,12 +1,15 @@ -from time import sleep +import math from adafruit_servokit import ServoKit 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. # min_pulse (int) – The minimum 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`). # 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 # The servo angle in degrees. Must be in the range 0 to actuation_range. # Is None when servo is disabled. -kit.servo[0].angle = 180 + +# kit.servo[0].angle = 90 + # property throttle: float # How much power is being delivered to the motor. # Values range from -1.0 (full throttle reverse) to 1.0 (full throttle forwards.) # 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 # The servo angle in degrees. Must be in the range 0 to actuation_range. # Is None when servo is disabled #kit.servo[0].actuation_range = 120 - - - - - - - - diff --git a/control_functions.py b/control_functions.py index 125a7af..4807c3e 100644 --- a/control_functions.py +++ b/control_functions.py @@ -7,9 +7,10 @@ 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 -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 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 # 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 = 500 # Defines angle 0 -MAX_PULSE = 2500 # Defines angle 180 +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 +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 measurement -MAX_SAMPLES = 10 +# Control the number of samples for single distance measurement (average from burst) +MAX_SAMPLES: int = 10 -# Control the number of samples for the potentiometer -PCF_VALUE = 65535 -POT_MAX = 65280 -POT_MIN = 256 -POT_INTERVAL = 0.01 +# 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 @@ -45,7 +64,7 @@ time_prev: float = -1e-6 error_prev: float = 0 # Variables to control PID values (PID formula tweaks) -p_value : float = 2.0 +p_value: float = 2.0 i_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() sample_array: list = [] -def initial(): - ... - # 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] @@ -68,15 +84,16 @@ def log_data(file_stamp: str, data_file: str, data_line: str, remark: str|None): data_writer = csv.writer(data_file) 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) # 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_last: float = 0.0 timestamp_first: float = 0.0 + while samples != max_samples: try: distance: float = sonar.distance @@ -96,64 +113,53 @@ def read_distance_sensor(file_stamp): 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 + 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 + 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(file_stamp): - - 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 +def read_setpoint(): while True: raw_value: int = pcf_in_0.value - scaled_value: float = (raw_value / PCF_VALUE) * 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) + 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('pin 0= ', pcf.read(0)) - print('raw_value= ',raw_value) - print("pin 0= %0.2fV" % scaled_value) - print('Scaled= ' , scaled_value) - print(angle) + print('scaled= ' , round(scaled_value, 4), ' cm= ', int(round(scaled_value * POT_PCM, 0))) + sleep(POT_INT) - time.sleep(POT_INTERVAL) - send_servo_angle(set_angle=angle) - -def calculate_velocity(file_stamp): +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): -def pid_calculations(file_stamp, setpoint): - - global i_result, previous_time, previous_error + 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(file_stamp=main.file_stamp) # todo Check logging - error: float = setpoint - measurement + 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 = 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)) 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 return pid_result - -def calculate_servo_position(): - ... - - -def send_servo_angle(set_angle): - - KIT.servo[0].angle = set_angle # Set angle +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 \ No newline at end of file diff --git a/main.py b/main.py index f4b79b4..ad3906b 100644 --- a/main.py +++ b/main.py @@ -1,13 +1,13 @@ from datetime import datetime import control_functions as cf +import os -file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M') - -cf.read_distance_sensor(file_stamp) -cf.read_setpoint(file_stamp) -cf.send_servo_angle(file_stamp) - +os.environ["PID_TIMESTAMP"] = datetime.strftime(datetime.now(), '%Y%m%d%I%M') # Set file timestamp as OS variable. +while True: + + print(cf.read_setpoint()) +