Added some direction code

3 files changed, 38 insertions, 18 deletions

diff --git a/DWM.py b/DWM.py
index 6db4582..cfe7178 100755
--- a/DWM.py
+++ b/DWM.py
@@ -30,8 +30,9 @@ kf.P = np.array([[x_std**2, 0.],
 kf.R = np.array([[x_mu, 0.],
                  [0., y_mu]])
 
-updRate = "1 1" # Active Idle
+position = np.array([0, 0])
 
+updRate = "10 10" # Active Idle
 
 with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
 
@@ -91,9 +92,10 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
 
             x = int(kf.x[0] * 1000)
             y = int(kf.x[1] * 1000)
-            print(f"x: {x}, y: {y}")
+
             with open("position.txt", "w") as f:
-                f.write(f"x,y\n")
+                print(f"{x},{y}")
+                f.write(f"{x},{y}\n")
                 f.close()
 
 print("Shutting down serial communication")
diff --git a/bt.py b/bt.py
index 2f0a493..8de8d95 100755
--- a/bt.py
+++ b/bt.py
@@ -31,7 +31,7 @@ while read != "1000":
     else:
         sleep(1)
 
-    print("0\r")
+    # print("0\r")
 
     # print(f"Sending: 'Acknowledge: {read}'")
     # Acknowledge ÖS command
@@ -42,10 +42,9 @@ while read != "1000":
     with open("position.txt", "r") as f:
         for line in f:
             line = line.strip("\n")
-            x, y, z, qf = line.split(",")
-            x = int(float(x) * 1000)
-            y = int(float(y) * 1000)
-            print(f"x: {x}, y: {y}")
+            x, y = line.split(",")
+            x = int(x)
+            y = int(y)
             recv_sock.send(f"{x}, {y}, ".encode())
             # message = message + f"{x}, {y}, "
         f.close()
diff --git a/main.py b/main.py
index 56905d0..7b04670 100755
--- a/main.py
+++ b/main.py
@@ -15,6 +15,10 @@ GPIO.setup(PT, GPIO.IN)
 GPIO.setup(LED, GPIO.OUT)
 GPIO.output(LED, False)
 
+# DWM Process Code
+DWMProcess = subprocess.Popen(["/home/pi/venv/bin/python", "-u", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, text=True)
+print(f"DWM PID: {DWMProcess.pid}")
+
 # Open serial port to Arduino Nano
 NANO = serial.Serial('/dev/ttyUSB1', 115200, timeout=1)
 sleep(2)
@@ -38,6 +42,7 @@ y = ""
 
 oldX = 0
 oldY = 0
+forward = False
 backward = False
 
 curDir = 0
@@ -53,9 +58,6 @@ if bto.find("Connected") != -1:
     LIDARProcess = subprocess.Popen(["/home/pi/TNE107-RPI/LIDARProg", "--channel", "--serial", "/dev/ttyUSB0", "460800"], stdout=open(os.devnull, 'wb'))
     print(f"LIDAR PID: {LIDARProcess.pid}")
 
-    # DWM Process Code
-    DWMProcess = subprocess.Popen(["python", "-u", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, text=True)
-    print(f"DWM PID: {DWMProcess.pid}")
 
     sleep(5)
     
@@ -73,23 +75,27 @@ if bto.find("Connected") != -1:
 
         if dwmReady:
             dwm = DWMProcess.stdout.readline().strip("\n")
-            x, y, z, qf = dwm.split(",")
+            x, y = dwm.split(",")
+            x = int(x)
+            y = int(y)
             #print("Quality factor: " + qf)
             # if float(qf) > 80:
-            print(x + ", " + y)
+            print(f"x: {x}, y: {y}")
 
         if newCommand:
             print(f"Command: {bto}")
             if bto == "11":
                 # Store current position => old pos
-                oldX = float(x) * 1000
-                oldY = float(y) * 1000
+                forward = True
                 backward = False
+                oldX = x
+                oldY = y
                 NANO.write(b"Forward\n")
             elif bto == "22":
-                oldX = float(x) * 1000
-                oldY = float(y) * 1000
+                forward = False
                 backward = True
+                oldX = x
+                oldY = y
                 # Store current position => old pos
                 NANO.write(b"Backward\n")
             elif bto == "33":
@@ -146,10 +152,23 @@ if bto.find("Connected") != -1:
                 # if backward == True:
                 #     a = a - 180
                 print(f"Current direction: {curDir}")
+                print(f"Desired direction: {desDir}")
                 NANO.write(b"Stop\n")
 
+        curDir = math.atan2(y - oldY, x - oldX)
+        # if curDir < desDir: 
+        # if curDir > desDir:
+
+        if forward:
+            NANO.write(b"Fowrad\n")
+
+        if backward:
+            NANO.write(b"Backward\n")
+
+        sleep(0.1)
+        
         with open("angle.txt", "w") as f:
-            f.write(f"{curDir}" + '\n')
+            f.write(f"{desDir}" + '\n')
             f.close()