From c632c1cc164c67cbafa73366cf9a3409666fbb6c Mon Sep 17 00:00:00 2001
From: Edvin <edj7467@gmail.com>
Date: Thu, 8 May 2025 11:40:26 +0200
Subject: Added goal communication

---
 arduino/arduino.ino | 58 +++++++++++++++++++++++++++++++++++++++++++++++++++--
 main.py             |  9 +++++++--
 2 files changed, 63 insertions(+), 4 deletions(-)

diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index 425d82d..5667315 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -1,4 +1,6 @@
+#include <Wire.h>
 #include <LiquidCrystal_I2C.h>
+#include <VL53L0X.h>
 
 // To H-bridges
 const int D1 = 9;
@@ -12,15 +14,25 @@ String received = "";
 
 // LCD
 LiquidCrystal_I2C lcd(0x27, 20, 4);
-
 int emptySpace = 0;
 
-void setup() {
+// TOF
+VL53L0X sensor;
+int dist = 0;
 
+// IR
+const int LIR = 10;
+const int RIR = 11;
+
+void setup() {
   pinMode(D1, OUTPUT);
   pinMode(D2, OUTPUT);
   pinMode(D3, OUTPUT);
   pinMode(D4, OUTPUT);
+
+  pinMode(LIR, INPUT);
+  pinMode(RIR, INPUT);
+
   digitalWrite(D1, LOW);
   digitalWrite(D2, LOW);
   digitalWrite(D3, LOW);
@@ -28,14 +40,26 @@ void setup() {
 
   Serial.begin(115200);
 
+  Wire.begin();
   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);
@@ -116,10 +140,40 @@ void loop() {
     digitalWrite(D3, HIGH);
     digitalWrite(D4, LOW);
   }
+  else if (received.indexOf("Goal") >= 0) { // Pair with goal
+    if (((digitalRead(LIR)) && digitalRead(RIR)) || (dist < 100)) {
+      Serial.println("Stopped");
+      digitalWrite(D1, LOW);
+      digitalWrite(D2, LOW);
+      digitalWrite(D3, LOW);
+      digitalWrite(D4, LOW);
+    }
+    else if (digitalRead(RIR)) {
+      analogWrite(D1, 100);
+      digitalWrite(D2, LOW);
+      digitalWrite(D3, LOW);
+      analogWrite(D4, 100);
+    }
+    else if (digitalRead(LIR)) {
+      digitalWrite(D1, LOW);
+      analogWrite(D2, 100);
+      analogWrite(D3, 100);
+      digitalWrite(D4, LOW);
+    }
+    else {
+      analogWrite(D1, 50);
+      digitalWrite(D2, LOW);
+      analogWrite(D3, 50);
+      digitalWrite(D4, LOW);
+    }
+  } 
   else {
     digitalWrite(D1, LOW);
     digitalWrite(D2, LOW);
     digitalWrite(D3, LOW);
     digitalWrite(D4, LOW);
   }
+
+  
+  
 }
diff --git a/main.py b/main.py
index d1a05a7..06b27cd 100755
--- a/main.py
+++ b/main.py
@@ -164,8 +164,13 @@ if bto.find("Connected") != -1:
                 sleep(0.45)
                 NANO.write(b"Stop\n")
             elif bto == "420":
-                print("reset")
-                desDir = 0
+                NANO.write(b"Goal\n")
+                print(NANO.readline())
+                GPIO.output(LED, True)
+                goal_read = False
+                while goal_read == False:
+                    goal_read = GPIO.input(PT)
+                GPIO.output(LED, False)
             elif bto == "0":
                 NANO.write(b"Stop\n")
                 forward = False
-- 
cgit v1.2.3