From 82d58a4ec50a8bee76cbe300ef9a4db4fa9df1be Mon Sep 17 00:00:00 2001 From: Edvin Date: Mon, 12 May 2025 13:41:55 +0200 Subject: Fixed status messages and added direction --- arduino/arduino.ino | 127 +++++++++++++++++++++++++++++----------------------- main.py | 7 ++- 2 files changed, 78 insertions(+), 56 deletions(-) diff --git a/arduino/arduino.ino b/arduino/arduino.ino index d925703..5ab3c43 100644 --- a/arduino/arduino.ino +++ b/arduino/arduino.ino @@ -14,6 +14,7 @@ String received = ""; String hostAddr = ""; String xstatus = "0000"; String ystatus = "0000"; +String dir = "000"; enum statusDialog { booting, btUp, btConnected, shutDown, startingGoal, foundGoal, currentStatus, obstruction }; enum availCommand { forward, backward, right, left, goal, checkRight, checkLeft, stop }; @@ -63,6 +64,8 @@ void setup() { lcd.init(); lcd.backlight(); lcd.clear(); + lcd.setCursor(2, 0); + lcd.print("AGV booting!"); dots = true; status = booting; @@ -73,40 +76,106 @@ void loop() { if (Serial.available() > 0) { received = Serial.readStringUntil('\n'); // Serial.println(received); - lcd.clear(); } // Status states handling if (received == "Bluetooth up") { status = btUp; dots = true; + + lcd.clear(); + lcd.setCursor(2, 0); + lcd.print("Bluetooth up"); + lcd.setCursor(6, 1); } else if (received.indexOf("Bluetooth connected") >= 0) { status = btConnected; hostAddr = received.substring(20, 32); dots = false; + + lcd.clear(); + lcd.setCursor(2, 0); + lcd.print("Connected to"); + lcd.setCursor(2, 1); + lcd.print(hostAddr); // Message as 'Bluetooth connected XXXXXXXXXXXX' with MAC-address. } else if (received == "Closing down") { status = shutDown; dots = true; + + lcd.clear(); + lcd.setCursor(1, 0); + lcd.print("Shutting down!"); + + digitalWrite(D1, LOW); + digitalWrite(D2, LOW); + digitalWrite(D3, LOW); + digitalWrite(D4, LOW); + + delay(5000); + + lcd.clear(); + lcd.setCursor(2, 0); + lcd.print("AGV booting!"); } else if (received == "Start goal") { status = startingGoal; dots = true; + + lcd.clear(); + lcd.setCursor(0, 0); + lcd.print("Looking for goal"); } else if (received == "Found goal") { status = foundGoal; dots = false; + + lcd.clear(); + lcd.setCursor(3, 0); + lcd.print("Found goal"); } else if (received.indexOf("Current status") >= 0) { status = currentStatus; - xstatus = received.substring(15, 18); - ystatus = received.substring(20, 23); + xstatus = received.substring(15, 19); + ystatus = received.substring(20, 24); + dir = received.substring(25, 28); dots = false; + + lcd.clear(); + lcd.setCursor(0, 0); + // lcd.print("Status"); + switch (command) { + case forward: + lcd.print("Forward"); + break; + case backward: + lcd.print("Backward"); + break; + case left: + lcd.print("Left"); + break; + case right: + lcd.print("Right"); + break; + case stop: + lcd.print("Stop"); + break; + } + lcd.setCursor(13, 0); + lcd.print(dir); + lcd.setCursor(0, 1); + lcd.print("x:"); + lcd.print(xstatus); + lcd.print(", y:"); + lcd.print(ystatus); } else if (received == "Obstruction found") { status = obstruction; dots = false; + + lcd.clear(); + lcd.setCursor(0, 0); + lcd.print("Obstruction"); } // Command state handling @@ -134,58 +203,6 @@ void loop() { received = ""; - // Status state logic - switch (status) { - case booting: - lcd.setCursor(2, 0); - lcd.print("AGV booting!"); - break; - case btUp: - lcd.setCursor(2, 0); - lcd.print("Bluetooth up"); - lcd.setCursor(6, 1); - lcd.print("...."); - emptySpace = 0; - break; - case btConnected: - lcd.setCursor(2, 0); - lcd.print("Connected to"); - lcd.setCursor(2, 1); - lcd.print(hostAddr); // Message as 'Bluetooth connected XXXXXXXXXXXX' with MAC-address. - break; - case shutDown: - lcd.setCursor(1, 0); - lcd.print("Shutting down!"); - - digitalWrite(D1, LOW); - digitalWrite(D2, LOW); - digitalWrite(D3, LOW); - digitalWrite(D4, LOW); - break; - case startingGoal: - lcd.setCursor(0, 0); - lcd.print("Looking for goal"); - break; - case foundGoal: - lcd.setCursor(3, 0); - lcd.print("Found goal"); - break; - case currentStatus: - lcd.setCursor(0, 0); - lcd.print("Status"); - lcd.setCursor(0, 1); - lcd.print(xstatus); - lcd.print(ystatus); - break; - case obstruction: - lcd.setCursor(0, 0); - lcd.print("Obstruction"); - break; - default: - dots = false; - break; - } - // Command state logic switch (command) { case forward: diff --git a/main.py b/main.py index 3f5f5c3..ae57298 100755 --- a/main.py +++ b/main.py @@ -162,6 +162,7 @@ if bto.find("Connected") != -1: NANO.write(b"Stop\n") sleep(0.1) NANO.write(b"Right\n") + NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode()) sleep(0.9) NANO.write(b"Stop\n") @@ -177,6 +178,7 @@ if bto.find("Connected") != -1: NANO.write(b"Stop\n") sleep(0.1) NANO.write(b"Left\n") + NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode()) sleep(0.9) NANO.write(b"Stop\n") @@ -192,6 +194,7 @@ if bto.find("Connected") != -1: NANO.write(b"Stop\n") sleep(0.1) NANO.write(b"Right\n") + NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode()) sleep(0.5) NANO.write(b"Stop\n") @@ -207,6 +210,7 @@ if bto.find("Connected") != -1: NANO.write(b"Stop\n") sleep(0.1) NANO.write(b"Left\n") + NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode()) sleep(0.5) NANO.write(b"Stop\n") @@ -283,8 +287,9 @@ if bto.find("Connected") != -1: xstatus = str(x).zfill(4) ystatus = str(y).zfill(4) + dirstatus = str(curDir).zfill(3) if iteration > delay: - NANO.write(f"Current status {xstatus} {ystatus}\n".encode()) + NANO.write(f"Current status {xstatus} {ystatus} {dirstatus}\n".encode()) iteration += 1 print("Terminating LIDAR process") -- cgit v1.2.3