← Back to ratslair.com
aboutsummaryrefslogtreecommitdiff
path: root/main.py
diff options
context:
space:
mode:
Diffstat (limited to 'main.py')
-rwxr-xr-xmain.py173
1 files changed, 135 insertions, 38 deletions
diff --git a/main.py b/main.py
index 8762366..b46b7a8 100755
--- a/main.py
+++ b/main.py
@@ -15,7 +15,7 @@ def calcAngleDiff(a1, a2):
if diff < -180:
diff += 360
- print(f"Angle diff: {diff}")
+ # print(f"Angle diff: {diff}")
return diff
def calcCurDir(x, y, xo, yo):
@@ -25,12 +25,12 @@ def calcCurDir(x, y, xo, yo):
if curDir >= 360:
curDir = curDir - 360
- print(f"Current angle: {curDir}")
+ # print(f"Current angle: {curDir}")
return curDir
def adjustAngle(adiff, f, b):
if f == True:
- print("Going forward")
+ # print("Going forward")
if adiff < 0:
print("Adjusting to the right")
NANO.write(b"Right\n")
@@ -42,14 +42,13 @@ def adjustAngle(adiff, f, b):
sleep(abs(adiff) / 90)
NANO.write(b"Stop\n")
elif b == True:
- print("Going backward")
+ # print("Going backward")
if adiff < 0:
adiff += 180
print(f"Adjusting to the left {adiff}")
NANO.write(b"Left\n")
sleep(abs(adiff) / 90)
NANO.write(b"Stop\n")
-
elif adiff > 0:
adiff -= 180
print(f"Adjusting to the right {adiff}")
@@ -71,14 +70,15 @@ GPIO.setup(LED, GPIO.OUT)
GPIO.output(LED, False)
# DWM Process Code
-DWMProcess = subprocess.Popen(["/home/pi/filterpy/bin/python", "-u", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, stderr=open(os.devnull, 'wb'), text=True)
+DWMProcess = subprocess.Popen(["/home/pi/filterpy/bin/python", "-u", "/home/pi/TNE107-RPI/DWM.py"], stdout=subprocess.PIPE, text=True) # stderr=open(os.devnull, 'wb'),
print(f"DWM PID: {DWMProcess.pid}")
# Open serial port to Arduino Nano
NANO = serial.Serial('/dev/ttyACM0', 115200, timeout=30)
sleep(2)
-BluetoothProcess = subprocess.Popen(["python", "-u", "/home/pi/TNE107-RPI/bt.py"], stdout=subprocess.PIPE, stderr=open(os.devnull, 'wb'), text=True)
+BluetoothProcess = subprocess.Popen(["python", "-u", "/home/pi/TNE107-RPI/bt.py"], stdout=subprocess.PIPE, stdin=subprocess.PIPE, text=True) # stderr=open(os.devnull, 'wb'),
+
# Waiting on connection
NANO.write(b"Bluetooth up\n")
@@ -100,6 +100,9 @@ oldY = 0
forward = False
backward = False
+obstructF = False
+obstructB = False
+
curDir = 0
desDir = 0
normDir = 0
@@ -109,7 +112,7 @@ writeAngleToFile(desDir)
iteration = 1
delay = 100 # Delay status command to every 20 loops
-calibrationBudget = 10
+calibrationBudget = 20
if bto.find("Connected") != -1:
nanoBtMessage = "Bluetooth connected " + bto[13:].translate({ord(c): None for c in ':'}) # Address to send to nano
@@ -123,7 +126,9 @@ if bto.find("Connected") != -1:
sleep(2)
while bto != "1000":
- btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005)
+ i = 0
+
+ btReady = bool(select.select([BluetoothProcess.stdout], [], [], 0.0005)[0])
if btReady:
bto = BluetoothProcess.stdout.readline().strip()
@@ -131,10 +136,12 @@ if bto.find("Connected") != -1:
else:
newCommand = False
- dwmReady, _, _ = select.select([DWMProcess.stdout], [], [], 0.0005)
+ dwmReady = bool(select.select([DWMProcess.stdout], [], [], 0.0005)[0])
if dwmReady:
- dwm = DWMProcess.stdout.readline().strip("\n")
+ while dwmReady:
+ dwmReady = bool(select.select([DWMProcess.stdout], [], [], 0.05)[0])
+ dwm = DWMProcess.stdout.readline().strip("\n")
x, y = dwm.split(",")
x = int(x)
y = int(y)
@@ -227,6 +234,11 @@ if bto.find("Connected") != -1:
backward = False
elif bto == "10": # Stop and find current position
+ dwmReady = True
+ while dwmReady:
+ dwmReady, _, _ = select.select([DWMProcess.stdout], [], [], 0.0005)
+ dwm = DWMProcess.stdout.readline().strip("\n")
+
NANO.write(b"Stop\n")
xmean = 0
@@ -243,8 +255,8 @@ if bto.find("Connected") != -1:
xmean += x
ymean += y
- xmean = xmean / calibrationBudget
- ymean = ymean / calibrationBudget
+ x = xmean / calibrationBudget
+ y = ymean / calibrationBudget
print(f"Desired direction {desDir}")
curDir = calcCurDir(x, y, oldX, oldY)
@@ -257,25 +269,98 @@ if bto.find("Connected") != -1:
backward = False
elif "98" in bto:
- curDir = calcCurDir(x, y, oldX, oldY)
+ NANO.write(b"Stop\n")
+
+ dwmReady = True
+ while dwmReady:
+ dwmReady = bool(select.select([DWMProcess.stdout], [], [], 0.05)[0])
+ dwm = DWMProcess.stdout.readline().strip("\n")
+
+ xmean = 0
+ ymean = 0
+
+ for i in range(calibrationBudget):
+ dwm = DWMProcess.stdout.readline().strip("\n")
+ x, y = dwm.split(",")
+ x = int(x)
+ y = int(y)
+
+ xmean += x
+ ymean += y
+
+ x = xmean / calibrationBudget
+ y = ymean / calibrationBudget
+ oldX = x
+ oldY = y
+
+ curDir = desDir
+ # curDir = calcCurDir(x, y, oldX, oldY)
_, gx, gy, gDir = bto.split()
- gDir += 180
- if gDir > 360:
- gDir -= 360
+ gx = int(gx)
+ gy = int(gy)
+ gDir = int(gDir)
+
+ print(f"AGV: (x:{x}, y:{y}, dir:{curDir})")
+ print(f"goal: (x:{gx}, y:{gy}, dir:{gDir})")
- # N = 90
+ # Flips gDir so that N = 270 => 90 etc.
+ # gDir += 180
+ # if gDir > 360:
+ # gDir -= 360
gx += 15 * math.cos(math.radians(gDir))
gy += 15 * math.sin(math.radians(gDir))
- gDir = calcCurDir(gx, gy, x, y)
+ tapeDir = calcCurDir(gx, gy, x, y)
- adiff = calcAngleDiff(gDir, curDir)
+ print(f"goal tape end: (x:{gx}, y:{gy}, dir:{tapeDir})")
+
+ adiff = calcAngleDiff(curDir, tapeDir)
+
+ print(f"Angle diff: {adiff}")
# We want to go forward towards the goal
- adjustAngle(adiff, True, False)
+ adjustAngle(-adiff, True, False)
+
+ # NANO.write(b"Forward\n")
+ # sleep(1)
+
+ # NANO.write(b"Stop\n")
+ # sleep(1)
+
+ # xmean = 0
+ # ymean = 0
+
+ # dwmReady = True
+ # while dwmReady:
+ # dwmReady = bool(select.select([DWMProcess.stdout], [], [], 0.05)[0])
+ # DWMProcess.stdout.readline().strip("\n")
+
+ # i = 0
+ # for i in range(calibrationBudget):
+ # dwm = DWMProcess.stdout.readline().strip("\n")
+ # x, y = dwm.split(",")
+ # x = int(x)
+ # y = int(y)
+ #
+ # xmean += x
+ # ymean += y
+
+ # x = xmean / calibrationBudget
+ # y = ymean / calibrationBudget
+
+ # curDir = calcCurDir(x, y, oldX, oldY)
+
+ # tapeDir = calcCurDir(gx, gy, x, y)
+
+ # adiff = calcAngleDiff(curDir, tapeDir)
+
+ # print(f"Angle diff: {adiff}")
+
+ # # We want to go forward towards the goal
+ # adjustAngle(-adiff, True, False)
# Start goal communication
NANO.write(b"Start goal\n")
@@ -289,10 +374,18 @@ if bto.find("Connected") != -1:
goal_read = False
while goal_read == False:
goal_read = GPIO.input(PT)
+ BluetoothProcess.stdin.write("98\n")
+ BluetoothProcess.stdin.flush()
+ sleep(0.1)
GPIO.output(LED, False)
NANO.write(b"Found goal\n")
- # Send something to ÖS
+ curDir = gDir
+ writeAngleToFile(curDir)
+
+ BluetoothProcess.stdin.write("99\n")
+ BluetoothProcess.stdin.flush()
+ print("Sent bluetooth 99")
# Check LIDAR data to see if anything is in front or behind AGV
with open("distances.txt", "r") as f:
@@ -301,20 +394,16 @@ if bto.find("Connected") != -1:
a = int(a)
d = int(d)
- if (a < 25 or a > 335) and (d > 0 and d < 150):
+ if (a < 25 or a > 335) and (d > 0 and d < 200):
forward = False
if backward == False:
- NANO.write(b"Stop\n")
- NANO.write(b"Backward\n")
- sleep(0.5)
+ obstructF = True
NANO.write(b"Stop\n")
- if ((a > 155 and a < 165) or (a > 195 and a < 205)) and (d > 0 and d < 150):
+ if ((a > 155 and a < 165) or (a > 195 and a < 205)) and (d > 0 and d < 200):
backward = False
if forward == False:
- NANO.write(b"Stop\n")
- NANO.write(b"Forward\n")
- sleep(0.5)
+ obstructB = True
NANO.write(b"Stop\n")
if forward:
@@ -323,16 +412,26 @@ if bto.find("Connected") != -1:
if backward:
NANO.write(b"Backward\n")
+ if obstructF:
+ print("Obstruction front")
+ obstructF = False
+ NANO.write(b"Backward\n")
+ sleep(0.5)
+ NANO.write(b"Stop\n")
+
+ if obstructB:
+ print("Obstruction back")
+ obstructB = False
+ NANO.write(b"Forward\n")
+ sleep(0.5)
+ NANO.write(b"Stop\n")
+
xstatus = str(x).zfill(4)
ystatus = str(y).zfill(4)
- dirstatus = int(curDir)
- if backward:
- dirstatus -= 180
- if dirstatus < 0:
- dirstatus += 360
- dirstatus = str(dirstatus).zfill(3)
+ dirstatus = str(int(curDir)).zfill(3)
if iteration > delay:
NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode())
+ print(f"Current status {xstatus} {ystatus} {dirstatus}")
iteration = 0
iteration += 1
@@ -349,5 +448,3 @@ print("Terminating Serial port to Arduino Nano")
NANO.write(b"Closing down\n")
sleep(1)
NANO.close()
-
-
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-27 05:26:56 +0000