From 07515e3ac7da2e023ad1cabe7356011ee2a549a4 Mon Sep 17 00:00:00 2001 From: Edvin Date: Fri, 9 May 2025 08:50:14 +0200 Subject: Fully implemented goal communication. --- arduino/arduino.ino | 88 ++++++++++++++++++-------- bt.py | 2 +- main.py | 174 +++++++++++++++++++++++++++------------------------- 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) - -- cgit v1.2.3