1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
|
import os
import subprocess
import signal
import serial
import select
from time import sleep
# Open serial port to Arduino Nano
NANO = serial.Serial('/dev/ttyUSB1', 9600, timeout=1)
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())
# Connected?
# Bluetooth output
bto = BluetoothProcess.stdout.readline().strip()
print(bto)
newCommand = False
rightTurnIndex = 50
rightTurn = 0
leftTurnIndex = 20
leftTurn = 0
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())
print(nanoBtMessage)
# 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}")
# DWM Process Code
DWMProcess = subprocess.Popen(["python", "/home/pi/TNE107-RPI/DWM.py"], stdout=open(os.devnull, 'wb'))
print(f"DWM PID: {DWMProcess.pid}")
# sleep(3)
while bto != "1000":
ready, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.001)
if ready:
bto = BluetoothProcess.stdout.readline().strip()
newCommand = True
else:
newCommand = False
# Send commands to Arduino based on bto
if rightTurn > 0:
NANO.write(b"Right\n")
rightTurn = rightTurn - 1
if rightTurn == 0:
NANO.write(b"00")
if leftTurn > 0:
NANO.write(b"Left\n")
leftTurn = leftTurn - 1
if leftTurn == 0:
NANO.write(b"00")
if newCommand:
if bto == "11":
NANO.write(b"Forward\n")
elif bto == "22":
NANO.write(b"Backward\n")
elif bto == "33":
# NANO.write(b"Right\n")
rightTurn = rightTurnIndex
leftTurn = 0
elif bto == "44":
# NANO.write(b"Left\n")
leftTurn = leftTurnIndex
rightTurn = 0
else:
NANO.write(b"Stop\n")
# Print current status (current command, heading? position? Could we include a cool progress bar? TO MISSION COMPLETEION?????)
print("Terminating Serial port to Arduino Nano")
NANO.write(b"Closing down\n")
sleep(1)
NANO.close()
print("Terminating LIDAR process")
os.kill(LIDARProcess.pid, signal.SIGINT)
print("Terminating DWM Process")
os.kill(DWMProcess.pid, signal.SIGINT)
print("Terminating Bluetooth Process")
os.kill(BluetoothProcess.pid, signal.SIGINT)
|
