diff options
| -rwxr-xr-x | bt.py | 4 | ||||
| -rwxr-xr-x | main.py | 75 |
2 files changed, 53 insertions, 26 deletions
@@ -28,10 +28,10 @@ while read != "1000": sleep(1) - # print(f"Sending: 'Acknowledge: {read}'") # Acknowledge ÖS command recv_sock.send(f"{read}, ".encode()) # Remember NEW LINE for ÖS to be able to read lines. - # message = message + read + + # TODO Maybe obstruction ack can be sent here? # Send DWM data if read == "10": @@ -43,13 +43,13 @@ def adjustAngle(adiff, f, b): NANO.write(b"Stop\n") elif b == True: print("Going backward") - if adiff > 0: + if adiff < 0: adiff -= 180 print(f"Adjusting to the right {adiff}") NANO.write(b"Right\n") sleep(abs(adiff) / 90) NANO.write(b"Stop\n") - elif adiff < 0: + elif adiff > 0: adiff += 180 print(f"Adjusting to the left {adiff}") NANO.write(b"Left\n") @@ -108,6 +108,8 @@ writeAngleToFile(desDir) iteration = 1 delay = 100 # Delay status command to every 20 loops +calibrationBudget = 10 + 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()) @@ -218,22 +220,6 @@ if bto.find("Connected") != -1: sleep(0.5) NANO.write(b"Stop\n") - elif bto == "420": # Communicate with goal - # Start goal communication - NANO.write(b"Start goal\n") - - # 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) - GPIO.output(LED, False) - NANO.write(b"Found goal\n") - elif bto == "0": # Stop NANO.write(b"Stop\n") forward = False @@ -242,15 +228,23 @@ if bto.find("Connected") != -1: elif bto == "10": # Stop and find current position NANO.write(b"Stop\n") + xmean = 0 + ymean = 0 # 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): + for i in range(calibrationBudget): dwm = DWMProcess.stdout.readline().strip("\n") - x, y = dwm.split(",") - x = int(x) - y = int(y) + x, y = dwm.split(",") + x = int(x) + y = int(y) + + xmean += x + ymean += y + xmean = xmean / calibrationBudget + ymean = ymean / calibrationBudget + print(f"Desired direction {desDir}") curDir = calcCurDir(x, y, oldX, oldY) @@ -261,10 +255,43 @@ if bto.find("Connected") != -1: forward = False backward = False - elif bto == "98": # Turn on LED + elif "98" in bto: + curDir = calcCurDir(x, y, oldX, oldY) + + _, gx, gy, gDir = bto.split() + + gDir += 180 + if gDir > 360: + gDir -= 360 + + # N = 90 + + gx += 15 * math.cos(math.radians(gDir)) + gy += 15 * math.sin(math.radians(gDir)) + + gDir = calcCurDir(gx, gy, x, y) + + adiff = calcAngleDiff(gDir, curDir) + + # We want to go forward towards the goal + adjustAngle(adiff, True, False) + + # Start goal communication + NANO.write(b"Start goal\n") + + # 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) - sleep(1) + goal_read = False + while goal_read == False: + goal_read = GPIO.input(PT) GPIO.output(LED, False) + NANO.write(b"Found goal\n") + + # Send something to ÖS # Check LIDAR data to see if anything is in front or behind AGV with open("distances.txt", "r") as f: |