Fully implemented goal communication.

3 files changed, 153 insertions, 111 deletions

diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index 5667315..aba1ba6 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -12,6 +12,7 @@ const int D4 = 5;
 char c;
 String received = "";
 
+
 // LCD
 LiquidCrystal_I2C lcd(0x27, 20, 4);
 int emptySpace = 0;
@@ -39,27 +40,20 @@ void setup() {
   digitalWrite(D4, LOW);
 
   Serial.begin(115200);
-
   Wire.begin();
+
+  sensor.init();
+  sensor.setMeasurementTimingBudget(10000);
+
   lcd.init();
   lcd.backlight();
   lcd.setCursor(2, 0);
   lcd.print("AGV booting!");
   received = "Dot dot dot dot";
-
-  
-  sensor.setTimeout(500);
-  if (!sensor.init())
-  {
-    Serial.println("Failed to detect and initialize sensor!");
-    while (1) {}
-  }
-  sensor.setMeasurementTimingBudget(10000);
 }
 
 void loop() {
-  dist = sensor.readRangeSingleMillimeters();
-
+  
   if (Serial.available() > 0) {
     received = Serial.readStringUntil('\n');
     //Serial.println(received);
@@ -117,16 +111,16 @@ void loop() {
   }
 
   if (received.indexOf("Forward") >= 0) { // Forward
-    digitalWrite(D1, HIGH);
+    analogWrite(D1, 150);
     digitalWrite(D2, LOW);
-    digitalWrite(D3, HIGH);
+    analogWrite(D3, 150);
     digitalWrite(D4, LOW);
   }
   else if (received.indexOf("Backward") >= 0) { // Backward
     digitalWrite(D1, LOW);
-    digitalWrite(D2, HIGH);
+    analogWrite(D2, 150);
     digitalWrite(D3, LOW);
-    digitalWrite(D4, HIGH);
+    analogWrite(D4, 150);
   }
   else if (received.indexOf("Right") >= 0) { // Right
     digitalWrite(D1, HIGH);
@@ -141,24 +135,36 @@ void loop() {
     digitalWrite(D4, LOW);
   }
   else if (received.indexOf("Goal") >= 0) { // Pair with goal
-    if (((digitalRead(LIR)) && digitalRead(RIR)) || (dist < 100)) {
-      Serial.println("Stopped");
+    dist = sensor.readRangeSingleMillimeters();
+    if (dist < 50) {
+      Serial.println("Something in front!");
+      received = "Check1";
+      analogWrite(D1, 255);
+      digitalWrite(D2, LOW);
+      digitalWrite(D3, LOW);
+      analogWrite(D4, 255);
+      delay(100);
       digitalWrite(D1, LOW);
       digitalWrite(D2, LOW);
       digitalWrite(D3, LOW);
       digitalWrite(D4, LOW);
+      delay(500);
     }
     else if (digitalRead(RIR)) {
-      analogWrite(D1, 100);
+      delay(50);
+      analogWrite(D1, 255);
       digitalWrite(D2, LOW);
       digitalWrite(D3, LOW);
-      analogWrite(D4, 100);
+      analogWrite(D4, 255);
+      delay(100);
     }
     else if (digitalRead(LIR)) {
+      delay(50);
       digitalWrite(D1, LOW);
-      analogWrite(D2, 100);
-      analogWrite(D3, 100);
+      analogWrite(D2, 255);
+      analogWrite(D3, 255);
       digitalWrite(D4, LOW);
+      delay(100);
     }
     else {
       analogWrite(D1, 50);
@@ -166,14 +172,46 @@ void loop() {
       analogWrite(D3, 50);
       digitalWrite(D4, LOW);
     }
-  } 
+  }
+  else if (received.indexOf("Check1") >= 0) {
+    received = "Check2";
+    digitalWrite(D1, LOW);
+    analogWrite(D2, 100);
+    analogWrite(D3, 100);
+    digitalWrite(D4, LOW);
+    delay(100 + i);
+    digitalWrite(D1, LOW);
+    digitalWrite(D2, LOW);
+    digitalWrite(D3, LOW);
+    digitalWrite(D4, LOW);
+    delay(500);
+
+    i += 10;
+  }
+  else if (received.indexOf("Check2") >= 0) {
+    received = "Check1";
+    analogWrite(D1, 100);
+    digitalWrite(D2, LOW);
+    digitalWrite(D3, LOW);
+    analogWrite(D4, 100);
+    delay(100 + i);
+    digitalWrite(D1, LOW);
+    digitalWrite(D2, LOW);
+    digitalWrite(D3, LOW);
+    digitalWrite(D4, LOW);
+    delay(500);
+
+    i += 10;
+  }
   else {
+    i = 0;
     digitalWrite(D1, LOW);
     digitalWrite(D2, LOW);
     digitalWrite(D3, LOW);
     digitalWrite(D4, LOW);
   }
 
-  
-  
+  if (i > 100) {
+    received = "Stop";
+  }
 }
diff --git a/bt.py b/bt.py
index 95506d5..3892fbe 100755
--- a/bt.py
+++ b/bt.py
@@ -24,7 +24,7 @@ message = ""
 while read != "1000":
     read = recv_sock.recv(1024)
     read = read.decode("utf-8").strip('\n')
-    print(f"{read}\r")
+    print(f"{read}")
 
     sleep(1)
 
diff --git a/main.py b/main.py
index 01ccc4d..cbc7cc5 100755
--- a/main.py
+++ b/main.py
@@ -7,6 +7,56 @@ from time import sleep
 import math
 import RPi.GPIO as GPIO
 
+# Takes desired direction and current direction to get angle difference from desired direction
+# Returns this angle difference with positive differences being left turns and negative ones being right turns
+def calcAngleDiff(a1, a2):
+    diff = a1 - a2
+
+    if diff < -180:
+        diff += 360
+
+    print(f"Angle diff: {diff}")
+    return diff
+
+def calcCurDir(x, y, xo, yo):
+    curDir = math.atan2(y - yo, x - xo) * (180/math.pi)
+    if curDir < 0:
+        curDir = curDir + 360
+    if curDir >= 360:
+        curDir = curDir - 360
+
+    print(f"Current angle: {curDir}")
+    return curDir
+
+def adjustAngle(adiff, f, b):
+    if f == True:
+        print("Going forward")
+        if adiff < -10:
+            print("Adjusting to the right")
+            NANO.write(b"Right\n")
+            sleep(abs(adiff) / 90)
+        elif adiff > 10:
+            print("Adjusting to the left")
+            NANO.write(b"Left\n")
+            sleep(abs(adiff) / 90)
+    elif b == True:
+        print("Going backward")
+        if adiff > 10:
+            adiff -= 180
+            print(f"Adjusting to the right {adiff}")
+            # NANO.write(b"Right\n")
+            # sleep(abs(adiff) / 90)
+        elif adiff < -10:
+            adiff += 180
+            print(f"Adjusting to the left {adiff}")
+            # NANO.write(b"Left\n")
+            # sleep(abs(adiff) / 90)
+
+def writeAngleToFile(a):
+    with open("angle_buf.txt", "w") as f:
+        f.write(f"{a}" + '\n')
+    os.rename("angle_buf.txt", "angle.txt")
+
 LED = 17 # Light emitting diode
 PT = 27 # Phototransistor
 
@@ -20,7 +70,7 @@ DWMProcess = subprocess.Popen(["/home/pi/filterpy/bin/python", "-u", "/home/pi/T
 print(f"DWM PID: {DWMProcess.pid}")
 
 # Open serial port to Arduino Nano
-NANO = serial.Serial('/dev/ttyUSB1', 115200, timeout=1)
+NANO = serial.Serial('/dev/ttyUSB1', 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)
@@ -49,7 +99,7 @@ curDir = 0
 desDir = 0
 normDir = 0
 
-iteration = 0
+writeAngleToFile(desDir)
 
 if bto.find("Connected") != -1:
     nanoBtMessage = "Bluetooth connected " + bto[13:].translate({ord(c): None for c in ':'}) # Address to send to nano
@@ -83,110 +133,115 @@ if bto.find("Connected") != -1:
 
         if newCommand:
             print(f"Command: {bto}")
-            if bto == "11":
-                # Store current position => old pos
-                # NANO.write(b"Forward\n")
+            if bto == "11": # Go forward
                 if forward == False:
                     oldX = x
                     oldY = y
                 forward = True
                 backward = False
-            elif bto == "22":
-                # Store current position => old pos
-                # NANO.write(b"Backward\n")
+                
+            elif bto == "22": # Go backward
                 if backward == False:
                     oldX = x
                     oldY = y
                 forward = False
                 backward = True
-            elif bto == "33":
+                
+            elif bto == "33": # Turn right 90 degrees
                 desDir = desDir - 90
                 if desDir < 0:
                     desDir = desDir + 360
 
                 curDir = desDir
 
-                with open("angle.txt", "w") as f:
-                    f.write(f"{desDir}" + '\n')
-                    f.close()
+                writeAngleToFile(desDir)
                     
                 NANO.write(b"Stop\n")
                 sleep(0.1)    
                 NANO.write(b"Right\n")
                 sleep(0.95)
                 NANO.write(b"Stop\n")
-            elif bto == "44":
+                
+            elif bto == "44": # Turn left 90 degrees
                 desDir = desDir + 90
                 if desDir >= 360:
                     desDir = desDir - 360
 
                 curDir = desDir
 
-                with open("angle.txt", "w") as f:
-                    f.write(f"{desDir}" + '\n')
-                    f.close()
+                writeAngleToFile(desDir)
 
                 NANO.write(b"Stop\n")
                 sleep(0.1)
                 NANO.write(b"Left\n")
                 sleep(0.9)
                 NANO.write(b"Stop\n")
-            elif bto == "55":
+                
+            elif bto == "55": # Turn right 45 degrees
                 desDir = desDir - 45
                 if desDir < 0:
                     desDir = desDir + 360
 
                 curDir = desDir
 
-                with open("angle.txt", "w") as f:
-                    f.write(f"{desDir}" + '\n')
-                    f.close()
+                writeAngleToFile(desDir)
 
                 NANO.write(b"Stop\n")
                 sleep(0.1)
                 NANO.write(b"Right\n")
                 sleep(0.45)
                 NANO.write(b"Stop\n")
-            elif bto == "66":
+                
+            elif bto == "66": # Turn left 45 degrees
                 desDir = desDir + 45
                 if desDir >= 360:
                     desDir = desDir - 360
                 
                 curDir = desDir
 
-                with open("angle.txt", "w") as f:
-                    f.write(f"{desDir}" + '\n')
-                    f.close()
+                writeAngleToFile(desDir)
                     
                 NANO.write(b"Stop\n")
                 sleep(0.1)
                 NANO.write(b"Left\n")
                 sleep(0.45)
                 NANO.write(b"Stop\n")
-            elif bto == "420":
+                
+            elif bto == "420": # Communicate with goal
+                # Start goal communication
                 NANO.write(b"Goal\n")
-                print(NANO.readline())
+
+                # Wait for AGV to have detected something in front of it.
+                print(NANO.readline().decode("utf8-").strip())
+
+                # AGV has now started wiggling and we check the phototransistor
+                # to see if the LED has triggered the goal. 
                 GPIO.output(LED, True)
                 goal_read = False
                 while goal_read == False:
                     goal_read = GPIO.input(PT)
+                print("Found goal")
                 GPIO.output(LED, False)
-            elif bto == "0":
                 NANO.write(b"Stop\n")
-                forward = False
-                backward = False
-            elif bto == "10":
+                
+            elif bto == "0": # Stop
                 NANO.write(b"Stop\n")
                 forward = False
                 backward = False
-                sleep(2)
+                
+            elif bto == "10": # Stop and find current position
+                NANO.write(b"Stop\n")
 
-                for i in range(30):
+                # Change number of iterations to change wait time
+                # Readings come every 0.1 seconds, so 30 iterations
+                # means 3 second delay. 
+                for i in range(10):
                     dwm = DWMProcess.stdout.readline().strip("\n")
                 x, y = dwm.split(",")
                 x = int(x)
                 y = int(y)
 
+                print(f"Desired direction {desDir}")
                 curDir = calcCurDir(x, y, oldX, oldY)
 
                 adiff = calcAngleDiff(desDir, curDir)
@@ -196,11 +251,12 @@ if bto.find("Connected") != -1:
                 forward = False
                 backward = False
 
-            elif bto == "98":
+            elif bto == "98": # Turn on LED
                 GPIO.output(LED, True)
                 sleep(1)
                 GPIO.output(LED, False)
 
+        # Check LIDAR data to see if anything is in front or behind AGV
         with open("distances.txt", "r") as f:
             for line in f:
                 a, d = line.split()
@@ -222,17 +278,6 @@ if bto.find("Connected") != -1:
 
         if backward:
             NANO.write(b"Backward\n")
-        
-        print(f"current pos ({x}, {y}) <- old pos ({oldX}, {oldY})")
-        print(f"Current direction: {curDir}")
-        print(f"Normalized direction: {normDir}")
-        print(f"Desired direction: {desDir}")
-        
-        with open("angle.txt", "w") as f:
-            f.write(f"{desDir}" + '\n')
-            f.close()
-
-        # Print current status (current command, heading? position? Could we include a cool progress bar? TO MISSION COMPLETETION?????)
 
     print("Terminating LIDAR process")
     os.kill(LIDARProcess.pid, signal.SIGINT)
@@ -249,44 +294,3 @@ sleep(1)
 NANO.close()
 
 
-# Takes desired direction and current direction to get angle difference from desired direction
-# Returns this angle difference with positive differences being left turns and negative ones being right turns
-def calcAngleDiff(a1, a2):
-    diff = a1 - a2
-
-    if diff < -180:
-        diff += 360
-
-    return diff
-
-def calcCurDir(x, y, xo, yo):
-    curDir = math.atan2(y - yo, x - xo) * (180/math.pi)
-    if curDir < 0:
-        curDir = curDir + 360
-    if curDir >= 360:
-        curDir = curDir - 360
-
-    return curDir
-
-def adjustAngle(adiff, f, b):
-    if f:
-        if adiff > 10:
-            print("Adjusting to the right")
-            NANO.write(b"Right\n")
-            sleep(abs(adiff) / 90)
-
-        if normDir < -10:
-            print("Adjusting to the left")
-            NANO.write(b"Left\n")
-            sleep(abs(adiff) / 90)
-    elif b:
-        if adiff < -10:
-            print("Adjusting to the right")
-            NANO.write(b"Right\n")
-            sleep(abs(adiff) / 90)
-
-        if normDir > 10:
-            print("Adjusting to the left")
-            NANO.write(b"Left\n")
-            sleep(abs(adiff) / 90)
-