diff options
| -rwxr-xr-x | DWM.py | 23 | ||||
| -rwxr-xr-x | bt.py | 1 | ||||
| -rwxr-xr-x | main.py | 35 |
3 files changed, 31 insertions, 28 deletions
@@ -3,13 +3,14 @@ from time import sleep from filterpy.kalman import KalmanFilter import numpy as np import re +from collections import deque x_mu = -0.02163601775523146 x_std = 0.07074315964054628 y_mu = 0.02645106742760512 y_std = 0.07415316805017082 -kf = KalmanFilter(dim_x=2, dim_z=2) +kf = KalmanFilter(dim_x=2, dim_z=2, alpha=10) # Initial position kf.x = np.array([[0.], @@ -30,9 +31,9 @@ kf.P = np.array([[x_std**2, 0.], kf.R = np.array([[x_mu, 0.], [0., y_mu]]) -position = np.array([0, 0]) +position = deque([(0, 0)]) -updRate = "10 10" # Active Idle +updRate = "1 1" # Active Idle with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: @@ -80,6 +81,9 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: for i in range(10): s.readline() + + for i in range(10): + position.append((0, 0)) while True: dstr = s.readline().decode('utf-8').strip('\n') @@ -96,6 +100,19 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: x = int(kf.x[0] * 1000) y = int(kf.x[1] * 1000) + position.popleft() + position.appen((x, y)) + + x = 0 + y = 0 + + for p_i in position: + x += p_i[0] + y += p_i[1] + + x = x / 10 + y = y / 10 + with open("position.txt", "w") as f: print(f"{x},{y}") f.write(f"{x},{y}\n") @@ -53,7 +53,6 @@ while read != "1000": with open("angle.txt", "r") as f: for line in f: angle = line.strip("\n") - print(angle) recv_sock.send(f"{angle}, ".encode()) recv_sock.send("0, ".encode()) # Near goal @@ -57,12 +57,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(5) while bto != "1000": - #print(GPIO.input(PT)) btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005) if btReady: @@ -90,14 +86,10 @@ if bto.find("Connected") != -1: # Store current position => old pos forward = True backward = False - oldX = x - oldY = y NANO.write(b"Forward\n") elif bto == "22": forward = False backward = True - oldX = x - oldY = y # Store current position => old pos NANO.write(b"Backward\n") elif bto == "33": @@ -144,27 +136,22 @@ if bto.find("Connected") != -1: print("reset") desDir = 0 elif bto == "0": - # Store current position => cur pos - # curX = float(x) * 1000 - # curY = float(y) * 1000 - - # oldDir = curDir - - # curDir = (math.atan2(curY - oldY, curX - oldX) + math.pi) * (180/math.pi) - # if backward == True: - # a = a - 180 - NANO.write(b"Stop\n") - curDir = math.atan2(y - oldY, x - oldX) - # if curDir < desDir: + curDir = math.atan2(y - oldY, x - oldX) * (180/math.pi) + 180 + if curDir < 0: + curDir = curDir + 360 + if curDir >= 360: + curDir = curDir - 360 + + # if curDir < desDir: # if curDir > desDir: - if forward: - NANO.write(b"Fowrad\n") + # if forward: + # NANO.write(b"Fowrad\n") - if backward: - NANO.write(b"Backward\n") + # if backward: + # NANO.write(b"Backward\n") sleep(0.1) |