Compare commits
No commits in common. "f175b29eda285fe128cd63119940c4063fe51fb3" and "fe6c8a9fc60405be8620088d5293bc6e4a459ee8" have entirely different histories.
f175b29eda
...
fe6c8a9fc6
7
.idea/misc.xml
generated
7
.idea/misc.xml
generated
@ -1,7 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="Black">
|
||||
<option name="sdkName" value="Python 3.11 (pid-balancer)" />
|
||||
</component>
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (pid-balancer)" project-jdk-type="Python SDK" />
|
||||
</project>
|
||||
@ -1,172 +1,75 @@
|
||||
from adafruit_hcsr04 import HCSR04 as hcsr04 # PWM driver board for servo
|
||||
import board # PWM driver board for servo
|
||||
from adafruit_servokit import ServoKit # Servo libraries
|
||||
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 adafruit_pcf8591 as pcf8591 # AD/DA converter board for potentiometer
|
||||
import numpy as np # Number handling
|
||||
import pandas as pd # Data handling
|
||||
import matplotlib.pyplot as plt # Plotter handling
|
||||
from scipy.integrate import odeint # Integral calculations
|
||||
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 adafruit_hcsr04 import HCSR04 as hcsr04
|
||||
import board
|
||||
import data_transfer_functions as dt
|
||||
from adafruit_servokit import ServoKit
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from scipy.integrate import odeint
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from serial.tools.list_ports_osx import kCFNumberSInt8Type
|
||||
|
||||
######################################## Variables (start) ##################################
|
||||
# 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
|
||||
KIT = ServoKit(channels=16) # Define the type of board (8, 16)
|
||||
MIN_PULSE = 500 # Defines angle 0
|
||||
MAX_PULSE = 2500 # Defines angle 180
|
||||
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
|
||||
TRIGGER_PIN = board.D4
|
||||
ECHO_PIN = board.D17
|
||||
TIMEOUT = 0.1
|
||||
MIN_DISTANCE = 4
|
||||
MAX_DISTANCE = 40
|
||||
|
||||
# Variables to control logging.
|
||||
LOG: bool = True # Log data to files
|
||||
SCREEN: bool = True # Log data to screen
|
||||
DEBUG: bool = False # More data to display
|
||||
LOG = True
|
||||
SCREEN = True
|
||||
|
||||
# Variables to assist PID calculations
|
||||
current_time: float = 0
|
||||
integral: float = 0
|
||||
time_prev: float = -1e-6
|
||||
error_prev: float = 0
|
||||
# PID measurements
|
||||
time = 0
|
||||
integral = 0
|
||||
time_prev = -1e-6
|
||||
e_prev = 0
|
||||
|
||||
# Variables to control PID values (PID formula tweaks)
|
||||
p_value: float = 2
|
||||
i_value: float = 0
|
||||
d_value: float = 0
|
||||
|
||||
# Initial variables, used in pid_calculations()
|
||||
i_result: float = 0
|
||||
previous_time: float
|
||||
previous_error: float
|
||||
|
||||
# Init array, used in read_distance_sensor()
|
||||
sample_array: list = []
|
||||
|
||||
######################################## Variables (end) ##################################
|
||||
|
||||
def initial():
|
||||
...
|
||||
|
||||
# Create timestamp strings for logs and screen
|
||||
def time_stamper():
|
||||
log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
|
||||
file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
|
||||
return log_timestamp, file_timestamp
|
||||
|
||||
# Write data to any of the logfiles
|
||||
def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
|
||||
log_stamp, _ = time_stamper()
|
||||
|
||||
with open("pid-balancer_" + data_file + "_data_" + fixed_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(fixed_file_stamp):
|
||||
|
||||
with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
|
||||
samples: int = 0
|
||||
max_samples: int = 10
|
||||
timestamp_last: float = 0.0
|
||||
timestamp_first: float = 0.0
|
||||
while samples != max_samples:
|
||||
|
||||
while True:
|
||||
try:
|
||||
distance: float = sonar.distance
|
||||
distance = sonar.distance
|
||||
if MIN_DISTANCE < distance < MAX_DISTANCE:
|
||||
|
||||
log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
|
||||
print("Distance: ", distance) if SCREEN else None
|
||||
|
||||
sample_array.append(distance)
|
||||
if samples == 0: timestamp_first, _ = time_stamper()
|
||||
if samples == max_samples - 1: timestamp_last, _ = time_stamper()
|
||||
|
||||
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
|
||||
|
||||
dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
|
||||
print("Distance: ", sonar.distance) if SCREEN else None
|
||||
else:
|
||||
log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
|
||||
dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
|
||||
print("Distance: ", distance) if SCREEN else None
|
||||
|
||||
except RuntimeError:
|
||||
log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None
|
||||
dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
|
||||
print("Timeout") if SCREEN else None
|
||||
|
||||
return median_distance, mean_timestamp
|
||||
|
||||
def read_setpoint():
|
||||
|
||||
|
||||
############# AnalogOut & AnalogIn Example ##########################
|
||||
#
|
||||
# This example shows how to use the included AnalogIn and AnalogOut
|
||||
# classes to set the internal DAC to output a voltage and then measure
|
||||
# it with the first ADC channel.
|
||||
#
|
||||
# Wiring:
|
||||
# Connect the DAC output to the first ADC channel, in addition to the
|
||||
# normal power and I2C connections
|
||||
#
|
||||
#####################################################################
|
||||
i2c = board.I2C()
|
||||
pcf = PCF.PCF8591(i2c)
|
||||
|
||||
pcf_in_0 = AnalogIn(pcf, PCF.A0)
|
||||
pcf_out = AnalogOut(pcf, PCF.OUT)
|
||||
|
||||
while True:
|
||||
print("Setting out to ", 65535)
|
||||
pcf_out.value = 65535
|
||||
raw_value = pcf_in_0.value
|
||||
scaled_value = (raw_value / 65535) * pcf_in_0.reference_voltage
|
||||
|
||||
print("Pin 0: %0.2fV" % (scaled_value))
|
||||
print("")
|
||||
time.sleep(1)
|
||||
...
|
||||
|
||||
|
||||
def calculate_velocity():
|
||||
...
|
||||
|
||||
def pid_calculations(setpoint):
|
||||
|
||||
global i_result, previous_time, previous_error
|
||||
offset_value: int = 320
|
||||
measurement, current_time = read_distance_sensor
|
||||
error: float = setpoint - measurement
|
||||
error_sum: float = 0.0
|
||||
|
||||
if previous_time is None:
|
||||
previous_error: float = 0.0
|
||||
previous_time: float = current_time
|
||||
i_result: float = 0.0
|
||||
error_sum: float = error * 0.008 # sensor sampling number approximation.
|
||||
def PID_calculations(Kp, Ki, Kd, setpoint, measurement):
|
||||
|
||||
error_sum: float = error_sum + (error * (current_time - previous_time))
|
||||
p_result: float = p_value * error
|
||||
i_result: float = i_value * error_sum
|
||||
d_result: float = d_value * ((error - previous_error) / (current_time - previous_time))
|
||||
pid_result: float = offset_value + p_result + i_result + d_result
|
||||
previous_error: float = error
|
||||
previous_time: float = current_time
|
||||
|
||||
return pid_result
|
||||
global cur_time, integral, prev_time, prev_error
|
||||
offset_value = 320
|
||||
# PID calculations
|
||||
error = setpoint - measurement
|
||||
P = Kp * error
|
||||
integral = integral + Ki * error * (cur_time - prev_time)
|
||||
D = Kd * (error - prev_error) / (cur_time - prev_time)
|
||||
PID_result = offset_value + P + integral + D
|
||||
prev_error = error
|
||||
prev_time = cur_time
|
||||
return PID_result
|
||||
|
||||
|
||||
def calculate_new_servo_pos():
|
||||
@ -175,5 +78,8 @@ def calculate_new_servo_pos():
|
||||
|
||||
def send_data_to_servo():
|
||||
|
||||
KIT.servo[0].angle = 180 # Set angle
|
||||
kit = ServoKit(channels=16)
|
||||
kit.servo[0].set_pulse_width_range(500, 2500)
|
||||
|
||||
kit.servo[0].angle = 180
|
||||
|
||||
|
||||
16
data_transfer_functions.py
Normal file
16
data_transfer_functions.py
Normal file
@ -0,0 +1,16 @@
|
||||
import csv
|
||||
from datetime import datetime
|
||||
|
||||
# Create timestamp strings for logs and screen
|
||||
def timestamper():
|
||||
log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
|
||||
file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
|
||||
return log_timestamp, file_timestamp
|
||||
|
||||
# Write data to any of the logfiles
|
||||
def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
|
||||
log_stamp, _ = timestamper()
|
||||
|
||||
with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file:
|
||||
data_writer = csv.writer(data_file)
|
||||
data_writer.writerow([log_stamp,data_line, remark])
|
||||
6
main.py
6
main.py
@ -1,14 +1,14 @@
|
||||
import control_functions as cf
|
||||
import data_transfer_functions as dt
|
||||
import plotter_functions as pf
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from scipy.integrate import odeint
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import statistics as st
|
||||
from adafruit_hcsr04 import HCSR04 as hcsr04
|
||||
import data_transfer_functions as dt
|
||||
|
||||
_, fixed_file_stamp = cf.time_stamper()
|
||||
_, fixed_file_stamp = dt.timestamper()
|
||||
|
||||
|
||||
cf.read_distance_sensor(fixed_file_stamp)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user