diff options
| -rwxr-xr-x | DWM.py | 12 | ||||
| -rwxr-xr-x | main.py | 21 |
2 files changed, 21 insertions, 12 deletions
@@ -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") @@ -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?????) |