← Back to ratslair.com
aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rwxr-xr-xDWM.py4
-rw-r--r--arduino/arduino.ino399
-rwxr-xr-xmain.py20
3 files changed, 273 insertions, 150 deletions
diff --git a/DWM.py b/DWM.py
index 39c599b..45a2f6f 100755
--- a/DWM.py
+++ b/DWM.py
@@ -29,7 +29,7 @@ kf.x = np.array([0., 0.])
kf.P *= 1000
# Noise matrix
-kf.R = np.diag([25, 25])
+kf.R = np.diag([100, 100])
kf.Q = np.eye(2)
@@ -37,7 +37,7 @@ position = deque([(0, 0)])
updRate = "1 1" # Active Idle
-with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s:
+with serial.Serial('/dev/ttyACM1', 115200, timeout = 1) as s:
# print(f"Opened serial port {s.name}")
diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index aba1ba6..d925703 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -8,22 +8,34 @@ const int D2 = 3;
const int D3 = 6;
const int D4 = 5;
-// UART communication
+// UART communication and
char c;
String received = "";
+String hostAddr = "";
+String xstatus = "0000";
+String ystatus = "0000";
+enum statusDialog { booting, btUp, btConnected, shutDown, startingGoal, foundGoal, currentStatus, obstruction };
+enum availCommand { forward, backward, right, left, goal, checkRight, checkLeft, stop };
+
+statusDialog status;
+availCommand command;
// LCD
LiquidCrystal_I2C lcd(0x27, 20, 4);
int emptySpace = 0;
+int i = 0;
+bool dots = false;
// TOF
VL53L0X sensor;
int dist = 0;
// IR
+const int FLIR = 7;
const int LIR = 10;
const int RIR = 11;
+const int FRIR = 8;
void setup() {
pinMode(D1, OUTPUT);
@@ -31,8 +43,10 @@ void setup() {
pinMode(D3, OUTPUT);
pinMode(D4, OUTPUT);
+ pinMode(FLIR, INPUT);
pinMode(LIR, INPUT);
pinMode(RIR, INPUT);
+ pinMode(FRIR, INPUT);
digitalWrite(D1, LOW);
digitalWrite(D2, LOW);
@@ -40,178 +54,279 @@ void setup() {
digitalWrite(D4, LOW);
Serial.begin(115200);
+ while (!Serial) { ; }
Wire.begin();
sensor.init();
- sensor.setMeasurementTimingBudget(10000);
+ sensor.setMeasurementTimingBudget(100);
lcd.init();
lcd.backlight();
- lcd.setCursor(2, 0);
- lcd.print("AGV booting!");
- received = "Dot dot dot dot";
+ lcd.clear();
+ dots = true;
+
+ status = booting;
+ command = stop;
}
void loop() {
-
if (Serial.available() > 0) {
received = Serial.readStringUntil('\n');
- //Serial.println(received);
+ // Serial.println(received);
+ lcd.clear();
}
- if (received.indexOf("Bluetooth up") >= 0) {
- lcd.clear();
- lcd.setCursor(2, 0);
- lcd.print("Bluetooth up");
- lcd.setCursor(6, 1);
- lcd.print("....");
- emptySpace = 0;
- received = "Dot dot dot dot"; // Reset received message so that LCD does not clear and rewrite every tick
+ // Status states handling
+ if (received == "Bluetooth up") {
+ status = btUp;
+ dots = true;
+ }
+ else if (received.indexOf("Bluetooth connected") >= 0) {
+ status = btConnected;
+ hostAddr = received.substring(20, 32);
+ dots = false;
+ }
+ else if (received == "Closing down") {
+ status = shutDown;
+ dots = true;
+ }
+ else if (received == "Start goal") {
+ status = startingGoal;
+ dots = true;
+ }
+ else if (received == "Found goal") {
+ status = foundGoal;
+ dots = false;
+ }
+ else if (received.indexOf("Current status") >= 0) {
+ status = currentStatus;
+ xstatus = received.substring(15, 18);
+ ystatus = received.substring(20, 23);
+ dots = false;
+ }
+ else if (received == "Obstruction found") {
+ status = obstruction;
+ dots = false;
}
- if (received.indexOf("Dot dot dot dot") >= 0) {
- String dot;
- for (int i = 0;i < 4;i++) {
- if (i == emptySpace) {
- dot = " ";
- } else {
- dot = ".";
- }
- lcd.setCursor(6 + i, 1);
- lcd.print(dot);
- }
- emptySpace++;
- if (emptySpace > 3) {
- emptySpace = 0;
- }
- delay(500);
+ // Command state handling
+ if (received == "Forward") {
+ command = forward;
+ }
+ else if (received == "Backward") {
+ command = backward;
+ }
+ else if (received == "Right") {
+ command = right;
+ }
+ else if (received == "Left") {
+ command = left;
+ }
+ else if (received == "Stop") {
+ command = stop;
+ }
+ else if (received == "Start goal") {
+ command = goal;
+ }
+ else if (received == "Found goal") {
+ command = stop;
}
- if (received.indexOf("Bluetooth connected") >= 0) {
- lcd.clear();
- lcd.setCursor(2, 0);
- lcd.print("Connected to");
- lcd.setCursor(2, 1);
- lcd.print(received.substring(20, 32)); // Message as 'Bluetooth connected XXXXXXXXXXXX' with MAC-address.
- received = "";
- delay(2000);
- lcd.clear();
+ 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;
}
- if (received.indexOf("Closing down") >= 0) {
- lcd.clear();
- lcd.setCursor(1, 0);
- lcd.print("Shutting down!");
- received = "Dot dot dot dot";
-
- digitalWrite(D1, LOW);
- digitalWrite(D2, LOW);
- digitalWrite(D3, LOW);
- digitalWrite(D4, LOW);
- }
-
- if (received.indexOf("Forward") >= 0) { // Forward
- analogWrite(D1, 150);
- digitalWrite(D2, LOW);
- analogWrite(D3, 150);
- digitalWrite(D4, LOW);
- }
- else if (received.indexOf("Backward") >= 0) { // Backward
- digitalWrite(D1, LOW);
- analogWrite(D2, 150);
- digitalWrite(D3, LOW);
- analogWrite(D4, 150);
- }
- else if (received.indexOf("Right") >= 0) { // Right
- digitalWrite(D1, HIGH);
- digitalWrite(D2, LOW);
- digitalWrite(D3, LOW);
- digitalWrite(D4, HIGH);
- }
- else if (received.indexOf("Left") >= 0) { // Left
- digitalWrite(D1, LOW);
- digitalWrite(D2, HIGH);
- digitalWrite(D3, HIGH);
- digitalWrite(D4, LOW);
- }
- else if (received.indexOf("Goal") >= 0) { // Pair with goal
- dist = sensor.readRangeSingleMillimeters();
- if (dist < 50) {
- Serial.println("Something in front!");
- received = "Check1";
- analogWrite(D1, 255);
+ // Command state logic
+ switch (command) {
+ case forward:
+ analogWrite(D1, 150);
+ digitalWrite(D2, LOW);
+ analogWrite(D3, 150);
+ digitalWrite(D4, LOW);
+ break;
+ case backward:
+ digitalWrite(D1, LOW);
+ analogWrite(D2, 150);
+ digitalWrite(D3, LOW);
+ analogWrite(D4, 150);
+ break;
+ case right:
+ digitalWrite(D1, HIGH);
+ digitalWrite(D2, LOW);
+ digitalWrite(D3, LOW);
+ digitalWrite(D4, HIGH);
+ break;
+ case left:
+ digitalWrite(D1, LOW);
+ digitalWrite(D2, HIGH);
+ digitalWrite(D3, HIGH);
+ digitalWrite(D4, LOW);
+ break;
+ case goal:
+ // Get TOF sensor reading in front of AGV
+ dist = sensor.readRangeSingleMillimeters();
+
+ if (dist < 100) {
+ Serial.println("Something in front!");
+ command = checkLeft;
+ 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);
+
+ i = 0;
+ }
+ else if (digitalRead(RIR)) {
+ delay(50);
+ analogWrite(D1, 150);
+ digitalWrite(D2, LOW);
+ digitalWrite(D3, LOW);
+ analogWrite(D4, 150);
+ delay(100);
+ }
+ else if (digitalRead(LIR)) {
+ delay(50);
+ digitalWrite(D1, LOW);
+ analogWrite(D2, 150);
+ analogWrite(D3, 150);
+ digitalWrite(D4, LOW);
+ delay(100);
+ }
+ else if (digitalRead(FRIR)) {
+ delay(50);
+ analogWrite(D1, 255);
+ digitalWrite(D2, LOW);
+ digitalWrite(D3, LOW);
+ analogWrite(D4, 255);
+ delay(150);
+ }
+ else if (digitalRead(FLIR)) {
+ delay(50);
+ digitalWrite(D1, LOW);
+ analogWrite(D2, 255);
+ analogWrite(D3, 255);
+ digitalWrite(D4, LOW);
+ delay(150);
+ }
+ else {
+ analogWrite(D1, 50);
+ digitalWrite(D2, LOW);
+ analogWrite(D3, 50);
+ digitalWrite(D4, LOW);
+ }
+ break;
+ case checkRight:
+ command = checkLeft;
+
+ analogWrite(D1, 100);
digitalWrite(D2, LOW);
digitalWrite(D3, LOW);
- analogWrite(D4, 255);
- delay(100);
+ analogWrite(D4, 100);
+ delay(100 + i);
digitalWrite(D1, LOW);
digitalWrite(D2, LOW);
digitalWrite(D3, LOW);
digitalWrite(D4, LOW);
delay(500);
- }
- else if (digitalRead(RIR)) {
- delay(50);
- analogWrite(D1, 255);
+
+ i += 10;
+ break;
+ case checkLeft:
+ command = checkRight;
+
+ digitalWrite(D1, LOW);
+ analogWrite(D2, 100);
+ analogWrite(D3, 100);
+ digitalWrite(D4, LOW);
+ delay(100 + i);
+ digitalWrite(D1, LOW);
digitalWrite(D2, LOW);
digitalWrite(D3, LOW);
- analogWrite(D4, 255);
- delay(100);
- }
- else if (digitalRead(LIR)) {
- delay(50);
- digitalWrite(D1, LOW);
- analogWrite(D2, 255);
- analogWrite(D3, 255);
digitalWrite(D4, LOW);
- delay(100);
- }
- else {
- analogWrite(D1, 50);
+ delay(500);
+
+ i += 10;
+ break;
+ default:
+ digitalWrite(D1, LOW);
digitalWrite(D2, LOW);
- analogWrite(D3, 50);
+ digitalWrite(D3, LOW);
digitalWrite(D4, LOW);
- }
+ break;
}
- 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";
+
+ // Print dots waiting for next command
+ if (dots == true) {
+ String dot;
+ for (int i = 0;i < 4;i++) {
+ if (i == emptySpace) {
+ dot = " ";
+ } else {
+ dot = ".";
+ }
+ lcd.setCursor(6 + i, 1);
+ lcd.print(dot);
+ }
+ emptySpace++;
+ if (emptySpace > 3) {
+ emptySpace = 0;
+ }
+ // delay(500);
}
}
diff --git a/main.py b/main.py
index cbc7cc5..065b4c5 100755
--- a/main.py
+++ b/main.py
@@ -101,6 +101,9 @@ normDir = 0
writeAngleToFile(desDir)
+iteration = 1
+delay = 20 # Delay status command to every 20 loops
+
if bto.find("Connected") != -1:
nanoBtMessage = "Bluetooth connected " + bto[13:].translate({ord(c): None for c in ':'}) # Address to send to nano
NANO.write(nanoBtMessage.encode())
@@ -159,7 +162,7 @@ if bto.find("Connected") != -1:
NANO.write(b"Stop\n")
sleep(0.1)
NANO.write(b"Right\n")
- sleep(0.95)
+ sleep(0.9)
NANO.write(b"Stop\n")
elif bto == "44": # Turn left 90 degrees
@@ -189,7 +192,7 @@ if bto.find("Connected") != -1:
NANO.write(b"Stop\n")
sleep(0.1)
NANO.write(b"Right\n")
- sleep(0.45)
+ sleep(0.5)
NANO.write(b"Stop\n")
elif bto == "66": # Turn left 45 degrees
@@ -204,12 +207,12 @@ if bto.find("Connected") != -1:
NANO.write(b"Stop\n")
sleep(0.1)
NANO.write(b"Left\n")
- sleep(0.45)
+ sleep(0.5)
NANO.write(b"Stop\n")
elif bto == "420": # Communicate with goal
# Start goal communication
- NANO.write(b"Goal\n")
+ NANO.write(b"Start goal\n")
# Wait for AGV to have detected something in front of it.
print(NANO.readline().decode("utf8-").strip())
@@ -220,9 +223,8 @@ if bto.find("Connected") != -1:
goal_read = False
while goal_read == False:
goal_read = GPIO.input(PT)
- print("Found goal")
GPIO.output(LED, False)
- NANO.write(b"Stop\n")
+ NANO.write(b"Found goal\n")
elif bto == "0": # Stop
NANO.write(b"Stop\n")
@@ -279,6 +281,12 @@ if bto.find("Connected") != -1:
if backward:
NANO.write(b"Backward\n")
+ xstatus = str(x).zfill(4)
+ ystatus = str(y).zfill(4)
+ if iteration > delay:
+ NANO.write(f"Current status {xstatus} {ystatus}\n".encode())
+ iteration += 1
+
print("Terminating LIDAR process")
os.kill(LIDARProcess.pid, signal.SIGINT)
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-27 06:12:34 +0000