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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
|
import os
import subprocess
import signal
import serial
import select
from time import sleep
import math
# 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
NANO.write(b"Bluetooth up\n")
print(BluetoothProcess.stdout.readline().strip())
# Connected?
# Bluetooth output
bto = BluetoothProcess.stdout.readline().strip()
print(bto)
newCommand = False
rightTurnIndexN = 100
rightTurnIndexF = 50
rightTurnIndexR = 0
rightTurn = 0
leftTurnIndexN = 100
leftTurnIndexF = 50
leftTurn = 0
angle = 0.0
dwm = ""
x = ""
y = ""
curX = 0
curY = 0
oldX = 0
oldY = 0
backward = False
oldDir = 0.0
curDir = 0.0
desDir = 0.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", "-u", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, stdin=subprocess.PIPE, text=True)
print(f"DWM PID: {DWMProcess.pid}")
sleep(5)
while bto != "1000":
btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005)
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().strip("\n")
x, y, z, qf = dwm.split(",")
#print("Quality factor: " + qf)
# if float(qf) > 80:
print(x + ", " + y)
# 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":
# Store current position => old pos
oldX = float(x) * 1000
oldY = float(y) * 1000
backward = False
NANO.write(b"Forward\n")
elif bto == "22":
oldX = float(x) * 1000
oldY = float(y) * 1000
backward = True
# Store current position => old pos
NANO.write(b"Backward\n")
elif bto == "33":
# NANO.write(b"Right\n")
rightTurn = rightTurnIndexN
leftTurn = 0
desDir = desDir + 90
if desDir >= 360:
desDir = desDir - 360
elif bto == "44":
# NANO.write(b"Left\n")
leftTurn = leftTurnIndexN
rightTurn = 0
desDir = desDir - 90
if desDir < 0:
desDir = desDir + 360
elif bto == "420":
print("reset")
desDir = 0
else:
# Store current position => cur pos
# Calculate angle from old pos, a = math.atan2(y, x) + math.pi
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
print(f"Current direction: {curDir}")
print(f"Old Direction: {oldDir}")
print(f"Direction difference: {(curDir - oldDir) / 100}")
NANO.write(b"Stop\n")
# Print current status (current command, heading? position? Could we include a cool progress bar? TO MISSION COMPLETETION?????)
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)
def calc_turn(desiredDirection, currentDirection):
|
