86 lines
2.1 KiB
Python
86 lines
2.1 KiB
Python
|
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
|
||
|
|
|
||
|
|
# Variables to control sensor
|
||
|
|
TRIGGER_PIN = board.D4
|
||
|
|
ECHO_PIN = board.D17
|
||
|
|
TIMEOUT = 0.1
|
||
|
|
MIN_DISTANCE = 4
|
||
|
|
MAX_DISTANCE = 40
|
||
|
|
|
||
|
|
# Variables to control logging.
|
||
|
|
LOG = True
|
||
|
|
SCREEN = True
|
||
|
|
|
||
|
|
# PID measurements
|
||
|
|
time = 0
|
||
|
|
integral = 0
|
||
|
|
time_prev = -1e-6
|
||
|
|
e_prev = 0
|
||
|
|
|
||
|
|
|
||
|
|
def initial():
|
||
|
|
...
|
||
|
|
|
||
|
|
|
||
|
|
def read_distance_sensor(fixed_file_stamp):
|
||
|
|
|
||
|
|
with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
|
||
|
|
|
||
|
|
while True:
|
||
|
|
try:
|
||
|
|
distance = sonar.distance
|
||
|
|
if MIN_DISTANCE < distance < MAX_DISTANCE:
|
||
|
|
|
||
|
|
dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
|
||
|
|
print("Distance: ", sonar.distance) if SCREEN else None
|
||
|
|
else:
|
||
|
|
dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
|
||
|
|
print("Distance: ", distance) if SCREEN else None
|
||
|
|
|
||
|
|
except RuntimeError:
|
||
|
|
dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
|
||
|
|
print("Timeout") if SCREEN else None
|
||
|
|
|
||
|
|
def read_setpoint():
|
||
|
|
...
|
||
|
|
|
||
|
|
|
||
|
|
def calculate_velocity():
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def PID_calculations(Kp, Ki, Kd, setpoint, measurement):
|
||
|
|
|
||
|
|
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():
|
||
|
|
...
|
||
|
|
|
||
|
|
|
||
|
|
def send_data_to_servo():
|
||
|
|
|
||
|
|
kit = ServoKit(channels=16)
|
||
|
|
kit.servo[0].set_pulse_width_range(500, 2500)
|
||
|
|
|
||
|
|
kit.servo[0].angle = 180
|
||
|
|
|