diff options
| -rwxr-xr-x | DWM.py | 57 | ||||
| -rwxr-xr-x | bt.py | 18 | ||||
| -rwxr-xr-x | main.py | 45 |
3 files changed, 63 insertions, 57 deletions
@@ -1,38 +1,36 @@ import serial from time import sleep from filterpy.kalman import UnscentedKalmanFilter +from filterpy.kalman import MerweScaledSigmaPoints import numpy as np import re from collections import deque -def h(x): +def fx(x, t): return x -# x_mu = -0.02163601775523146 -# x_std = 0.07074315964054628 -# y_mu = 0.02645106742760512 -# y_std = 0.07415316805017082 +def hx(x): + return x + +x_mu = -0.02163601775523146 +x_std = 0.07074315964054628 +y_mu = 0.02645106742760512 +y_std = 0.07415316805017082 -# kf = UnscentedKalmanFilter(dim_x=2, dim_z=2, alpha=1.05) +points = MerweScaledSigmaPoints(n=2, alpha=1, beta=2, kappa=0) -# # Initial position -# kf.x = np.array([[0.], -# [0.]]) +kf = UnscentedKalmanFilter(dim_x=2, dim_z=2, dt=0.1, fx=fx, hx=hx, points=points) -# # State transition matrix -# kf.F = np.array([[1., 0.], -# [0., 1.]]) +# Initial position +kf.x = np.array([0., 0.]) -# # Measurement function -# kf.H = np.array([[1., 0.], -# [0., 1.]]) +# Initial error +kf.P *= 1000 -# # Covariance matrix -# kf.P = np.array([[x_std**2, 0.], -# [0., y_std**2]]) +# Noise matrix +kf.R = np.diag([25, 25]) -# kf.R = np.array([[x_std**2, 0.], -# [0., y_std**2]]) +kf.Q = np.eye(2) position = deque([(0, 0)]) @@ -82,11 +80,10 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: s.write(b"\r") sleep(0.1) - for i in range(10): + for i in range(30): s.readline() for i in range(50): - print(i) position.append((0, 0)) while True: @@ -97,12 +94,11 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: _, x, y, z, qf = dstr.split(",") x = int(float(x) * 1000) y = int(float(y) * 1000) - # kf.predict() - # kf.update(np.array([[x], - # [y]])) + kf.predict() + kf.update(np.array([x, y])) - # x = int(kf.x[0] * 1000) - # y = int(kf.x[1] * 1000) + kfx = int(kf.x[0] * 1000) + kfy = int(kf.x[1] * 1000) position.popleft() position.append((x, y)) @@ -114,15 +110,16 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: xmean += p_i[0] ymean += p_i[1] - xmean = xmean / 50 - ymean = ymean / 50 + xmean = int(xmean / 50) + ymean = int(ymean / 50) with open("position.txt", "w") as f: - print(f"{x},{y}") + print(f"{kfx},{kfy}") f.write(f"{x},{y}\n") f.close() with open("position_mean.txt", "w") as f: + #print(f"xmean: {xmean}, ymean: {ymean}") f.write(f"{xmean},{ymean}\n") f.close() @@ -41,15 +41,15 @@ while read != "1000": # Send DWM data if read == "10": sleep(1) - with open("position_mean.txt", "r") as f: - for line in f: - line = line.strip("\n") - x, y = line.split(",") - x = int(x) - y = int(y) - recv_sock.send(f"{x}, {y}, ".encode()) - # message = message + f"{x}, {y}, " - f.close() + with open("position_mean.txt", "r") as f: + for line in f: + line = line.strip("\n") + x, y = line.split(",") + x = int(x) + y = int(y) + recv_sock.send(f"{x}, {y}, ".encode()) + # message = message + f"{x}, {y}, " + f.close() else: with open("position.txt", "r") as f: for line in f: @@ -37,8 +37,8 @@ print(bto) newCommand = False dwm = "" -x = "" -y = "" +x = 0 +y = 0 oldX = 0 oldY = 0 @@ -47,6 +47,7 @@ backward = False curDir = 0 desDir = 0 +normDir = 0 if bto.find("Connected") != -1: GPIO.output(LED, True) @@ -57,6 +58,8 @@ if bto.find("Connected") != -1: # LIDAR Process Code LIDARProcess = subprocess.Popen(["/home/pi/TNE107-RPI/LIDARProg", "--channel", "--serial", "/dev/ttyUSB0", "460800"], stdout=open(os.devnull, 'wb')) print(f"LIDAR PID: {LIDARProcess.pid}") + + sleep(2) while bto != "1000": btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005) @@ -76,28 +79,27 @@ if bto.find("Connected") != -1: y = int(y) #print("Quality factor: " + qf) # if float(qf) > 80: - print(f"x: {x}, y: {y}") - print(f"Current direction: {curDir}") - print(f"Desired direction: {desDir}") if newCommand: print(f"Command: {bto}") if bto == "11": # Store current position => old pos + NANO.write(b"Forward\n") if forward == False: oldX = x oldY = y + sleep(2) forward = True backward = False - NANO.write(b"Forward\n") elif bto == "22": + # Store current position => old pos + NANO.write(b"Backward\n") if backward == False: oldX = x oldY = y + sleep(2) forward = False backward = True - # Store current position => old pos - NANO.write(b"Backward\n") elif bto == "33": desDir = desDir - 90 if desDir < 0: @@ -143,10 +145,14 @@ if bto.find("Connected") != -1: desDir = 0 elif bto == "0": NANO.write(b"Stop\n") + forward = False + backward = False elif bto == "10": NANO.write(b"Stop\n") + forward = False + backward = False - curDir = math.atan2(y - oldY, x - oldX) * (180/math.pi) + 180 + curDir = 340# math.atan2(y - oldY, x - oldX) * (180/math.pi) if curDir < 0: curDir = curDir + 360 if curDir >= 360: @@ -155,27 +161,30 @@ if bto.find("Connected") != -1: normDir = (desDir - curDir) % 360 if normDir > 180: - x -= 360 + normDir -= 360 if forward or backward: - if normDir < -10: - print("Adjusting to the right") - NANO.write(b"Right\n") - sleep(1/normDir) - if normDir > 10: print("Adjusting to the left") NANO.write(b"Left\n") - sleep(1/normDir) + sleep(normDir / 1000) + + if normDir < -10: + print("Adjusting to the right") + NANO.write(b"Right\n") + sleep(normDir / 1000) if forward: - NANO.write(b"Fowrad\n") + NANO.write(b"Forward\n") if backward: NANO.write(b"Backward\n") - sleep(0.1) + print(f"current pos ({x}, {y}) -> old pos ({oldX}, {oldY})") + print(f"Current direction: {curDir}") + print(f"Normalized direction: {normDir}") + print(f"Desired direction: {desDir}") with open("angle.txt", "w") as f: f.write(f"{desDir}" + '\n') |