Testing

2 files changed, 21 insertions, 12 deletions

diff --git a/DWM.py b/DWM.py
index 3861ed4..30a7721 100755
--- a/DWM.py
+++ b/DWM.py
@@ -1,9 +1,11 @@
 import serial
 from time import sleep
 
+updRate = "10 10" # Active Idle
+
 with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
 
-    print(f"Opened serial port {s.name}")
+    # print(f"Opened serial port {s.name}")
 
     sleep(1)
     s.write(b"\r")
@@ -15,14 +17,14 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
     s.write(b'0x1234')
     sleep(0.1)
     s.write(b'\r')
-    print(f"Set PAN ID to 0x1234")
+    # print(f"Set PAN ID to 0x1234")
 
     sleep(1)
     
     s.write(b'nmt')
     sleep(0.1)
     s.write(b'\r')
-    print("Configured node as tag")
+    # print("Configured node as tag")
 
     sleep(1)
 
@@ -34,10 +36,10 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
 
     s.write(b"aurs ")
     sleep(0.1)
-    s.write(b"10 10")
+    s.write(updRate.encode())
     sleep(0.1)
     s.write(b'\r')
-    print("Set update rate to 1 1")
+    # print("Set update rate to " + updRate)
 
     sleep(0.1)
     s.write(b"lep")
diff --git a/main.py b/main.py
index 5a666dd..c9760b0 100755
--- a/main.py
+++ b/main.py
@@ -12,7 +12,6 @@ sleep(2)
 BluetoothProcess = subprocess.Popen(["python", "-u", "/home/pi/TNE107-RPI/bt.py"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, stdin=subprocess.PIPE, text=True)
 
 # Waiting on connection
-# AGVlcd.lcd_display_string("Waiting for Bluetooth connection...", 1)
 NANO.write(b"Bluetooth up\n")
 print(BluetoothProcess.stdout.readline().strip())
 
@@ -21,13 +20,14 @@ print(BluetoothProcess.stdout.readline().strip())
 bto = BluetoothProcess.stdout.readline().strip()
 print(bto)
 
-
 newCommand = False
 rightTurnIndex = 50
 rightTurn = 0
 leftTurnIndex = 20
 leftTurn = 0
 
+dwm = ""
+
 if bto.find("Connected") != -1:
     nanoBtMessage = "Bluetooth connected " + bto[13:].translate({ord(c): None for c in ':'}) # Address to send to nano
     NANO.write(nanoBtMessage.encode())
@@ -38,20 +38,23 @@ if bto.find("Connected") != -1:
     print(f"LIDAR PID: {LIDARProcess.pid}")
 
     # DWM Process Code
-    DWMProcess = subprocess.Popen(["python", "/home/pi/TNE107-RPI/DWM.py"], stdout=open(os.devnull, 'wb'))
+    DWMProcess = subprocess.Popen(["python", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, stdin=subprocess.PIPE, text=True)
     print(f"DWM PID: {DWMProcess.pid}")
     
-    # sleep(3)
-    
     while bto != "1000":
-        ready, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.001)
+        btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005)
 
-        if ready:
+        if btReady:
             bto = BluetoothProcess.stdout.readline().strip()
             newCommand = True
         else:
             newCommand = False
 
+        dwmReady, _, _ = select.select([DWMProcess.stdout], [], [], 0.0005)
+
+        if dwmReady:
+            dwm = DWMProcess.stdout.readline()
+
         # Send commands to Arduino based on bto
         if rightTurn > 0:
             NANO.write(b"Right\n")
@@ -67,8 +70,10 @@ if bto.find("Connected") != -1:
 
         if newCommand:
             if bto == "11":
+                # Store current position => old pos
                 NANO.write(b"Forward\n")
             elif bto == "22":
+                # Store current position => old pos
                 NANO.write(b"Backward\n")
             elif bto == "33":
                 # NANO.write(b"Right\n")
@@ -79,6 +84,8 @@ if bto.find("Connected") != -1:
                 leftTurn = leftTurnIndex
                 rightTurn = 0
             else:
+                # Store current position => cur pos
+                # Calculate angle from old pos, a = math.atan2(y, x) + math.pi
                 NANO.write(b"Stop\n")
 
         # Print current status (current command, heading? position? Could we include a cool progress bar? TO MISSION COMPLETEION?????)