Working on goal pairing and fully autonomous drivemain

3 files changed, 152 insertions, 42 deletions

diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index 35ebe17..010b31b 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -233,7 +233,7 @@ void loop() {
       // Get TOF sensor reading in front of AGV
       dist = sensor.readRangeSingleMillimeters();
 
-      if (dist < 100) {
+      if (dist < 60) {
         Serial.println("Something in front!");
         command = checkLeft;
         analogWrite(D1, 255);
diff --git a/bt.py b/bt.py
index bafa5f2..4142669 100755
--- a/bt.py
+++ b/bt.py
@@ -26,13 +26,26 @@ while read != "1000":
     read = read.decode("utf-8").strip('\n')
     print(f"{read}")
 
-    sleep(1)
+    if "98" in read:
+        while read == "98":
+            read = sys.stdin.readline().strip('\n')
+            print(f"{read}")
+            with open("position.txt", "r") as f:
+                for line in f:
+                    line = line.strip("\n")
+                    x, y = line.split(",")
+                    x = int(x)
+                    y = int(y)
+                    recv_sock.send(f"98, {x}, {y}\n".encode())
+                    # message = message + f"{x}, {y}, "
+                f.close()
+        read = "99"
+    else:
+        sleep(0.5)
 
     # Acknowledge ÖS command
     recv_sock.send(f"{read}, ".encode()) # Remember NEW LINE for ÖS to be able to read lines.
 
-    # TODO Maybe obstruction ack can be sent here?
-
     # Send DWM data
     if read == "10":
         sleep(1)
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()
-
-