From 45acee6bf8339ec01f06fe3701a391d0275186db Mon Sep 17 00:00:00 2001
From: Edvin <edj7467@gmail.com>
Date: Tue, 6 May 2025 14:23:48 +0200
Subject: Added mean of position

---
 DWM.py  | 23 ++++++++++++++++++++---
 bt.py   |  1 -
 main.py | 35 +++++++++++------------------------
 3 files changed, 31 insertions(+), 28 deletions(-)

diff --git a/DWM.py b/DWM.py
index ebefb3b..6c1431e 100755
--- a/DWM.py
+++ b/DWM.py
@@ -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")
diff --git a/bt.py b/bt.py
index 8de8d95..d522ee5 100755
--- a/bt.py
+++ b/bt.py
@@ -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
diff --git a/main.py b/main.py
index 01eccb4..5935957 100755
--- a/main.py
+++ b/main.py
@@ -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)
         
-- 
cgit v1.2.3