3 files changed, 65 insertions, 24 deletions

diff --git a/DWM.py b/DWM.py
index a828e22..42a89c0 100755
--- a/DWM.py
+++ b/DWM.py
@@ -1,16 +1,19 @@
 import serial
 from time import sleep
-from filterpy.kalman import KalmanFilter
+from filterpy.kalman import UnscentedKalmanFilter
 import numpy as np
 import re
 from collections import deque
 
+def h(x):
+    return x
+
 # x_mu = -0.02163601775523146
 # x_std = 0.07074315964054628
 # y_mu = 0.02645106742760512
 # y_std = 0.07415316805017082
 
-# kf = KalmanFilter(dim_x=2, dim_z=2, alpha=10)
+# kf = UnscentedKalmanFilter(dim_x=2, dim_z=2, alpha=1.05)
 
 # # Initial position
 # kf.x = np.array([[0.],
@@ -82,7 +85,7 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
     for i in range(10):
         s.readline()
 
-    for i in range(10):
+    for i in range(50):
         print(i)
         position.append((0, 0))
     
@@ -104,19 +107,23 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
             position.popleft()
             position.append((x, y))
 
-            x = 0
-            y = 0
+            xmean = 0
+            ymean = 0
             
             for p_i in position:
-                x += p_i[0]
-                y += p_i[1]
+                xmean += p_i[0]
+                ymean += p_i[1]
 
-            x = x / 10
-            y = y / 10
+            xmean = xmean / 50
+            ymean = ymean / 50
 
             with open("position.txt", "w") as f:
                 print(f"{x},{y}")
                 f.write(f"{x},{y}\n")
                 f.close()
 
+            with open("position_mean.txt", "w") as f:
+                f.write(f"{xmean},{ymean}\n")
+                f.close()
+
 print("Shutting down serial communication")
diff --git a/bt.py b/bt.py
index d522ee5..98fc45e 100755
--- a/bt.py
+++ b/bt.py
@@ -39,15 +39,27 @@ while read != "1000":
     # message = message + read
 
     # Send DWM data
-    with open("position.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()
+    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()
+    else:
+        with open("position.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()
 
     # Send angle data
     with open("angle.txt", "r") as f:
diff --git a/main.py b/main.py
index 5935957..08ddc78 100755
--- a/main.py
+++ b/main.py
@@ -84,10 +84,16 @@ if bto.find("Connected") != -1:
             print(f"Command: {bto}")
             if bto == "11":
                 # Store current position => old pos
+                if forward == False:
+                    oldX = x
+                    oldY = y
                 forward = True
                 backward = False
                 NANO.write(b"Forward\n")
             elif bto == "22":
+                if backward == False:
+                    oldX = x
+                    oldY = y
                 forward = False
                 backward = True
                 # Store current position => old pos
@@ -137,21 +143,37 @@ if bto.find("Connected") != -1:
                 desDir = 0
             elif bto == "0":
                 NANO.write(b"Stop\n")
+            elif bto == "10":
+                NANO.write(b"Stop\n")
 
         curDir = math.atan2(y - oldY, x - oldX) * (180/math.pi) + 180
         if curDir < 0:
             curDir = curDir + 360
         if curDir >= 360:
             curDir = curDir - 360
+
+
+        normDir = (desDir - curDir) % 360
+        if normDir > 180:
+            x -= 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)
         
-        # 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)