diff options
| -rwxr-xr-x | bt.py | 16 | ||||
| -rwxr-xr-x | main.py | 74 |
2 files changed, 56 insertions, 34 deletions
@@ -26,7 +26,9 @@ while read != "1000": read = read.decode("utf-8").strip('\n') print(f"{read}\r") - message = "" + sleep(1) + + print("0\r") # print(f"Sending: 'Acknowledge: {read}'") # Acknowledge ÖS command @@ -38,15 +40,21 @@ while read != "1000": for line in f: line = line.strip("\n") x, y, z, qf = line.split(",") - x = float(x) * 1000 - y = float(y) * 1000 + x = int(float(x) * 1000) + y = int(float(y) * 1000) print(f"x: {x}, y: {y}") recv_sock.send(f"{x}, {y}, ".encode()) # message = message + f"{x}, {y}, " f.close() # Send angle data - recv_sock.send("0, ".encode()) + with open("angle.txt", "r") as f: + for line in f: + angle = line.strip("\n") + print(angle) + recv_sock.send(f"{angle}, ".encode()) + + recv_sock.send("0, ".encode()) # Near goal # message = message + "0, " # Send LIDAR data @@ -13,6 +13,7 @@ PT = 27 # Phototransistor GPIO.setmode(GPIO.BCM) GPIO.setup(PT, GPIO.IN) GPIO.setup(LED, GPIO.OUT) +GPIO.output(LED, False) # Open serial port to Arduino Nano NANO = serial.Serial('/dev/ttyUSB1', 115200, timeout=1) @@ -50,9 +51,7 @@ oldX = 0 oldY = 0 backward = False -oldDir = 0.0 -curDir = 0.0 -desDir = 0.0 +curDir = 0 if bto.find("Connected") != -1: GPIO.output(LED, True) @@ -71,7 +70,7 @@ if bto.find("Connected") != -1: sleep(5) while bto != "1000": - print(GPIO.input(PD)) + #print(GPIO.input(PT)) btReady, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.0005) if btReady: @@ -105,49 +104,64 @@ if bto.find("Connected") != -1: if newCommand: if bto == "11": # Store current position => old pos - oldX = float(x) * 1000 - oldY = float(y) * 1000 + # oldX = float(x) * 1000 + # oldY = float(y) * 1000 backward = False NANO.write(b"Forward\n") elif bto == "22": - oldX = float(x) * 1000 - oldY = float(y) * 1000 + # oldX = float(x) * 1000 + # oldY = float(y) * 1000 backward = True # Store current position => old pos NANO.write(b"Backward\n") elif bto == "33": - # NANO.write(b"Right\n") - rightTurn = rightTurnIndexN - leftTurn = 0 - desDir = desDir + 90 - if desDir >= 360: - desDir = desDir - 360 + curDir = curDir + 90 + if curDir >= 360: + curDir = curDir - 360 + NANO.write(b"Right\n") + sleep(0.95) + NANO.write(b"Stop\n") elif bto == "44": - # NANO.write(b"Left\n") - leftTurn = leftTurnIndexN - rightTurn = 0 - desDir = desDir - 90 - if desDir < 0: - desDir = desDir + 360 + curDir = curDir - 90 + if curDir < 0: + curDir = curDir + 360 + NANO.write(b"Left\n") + sleep(0.9) + NANO.write(b"Stop\n") + elif bto == "55": + curDir = curDir + 45 + if curDir >= 360: + curDir = curDir - 360 + NANO.write(b"Left\n") + sleep(0.45) + NANO.write(b"Stop\n") + elif bto == "66": + curDir = curDir - 45 + if curDir < 0: + curDir = curDir + 360 + NANO.write(b"Left\n") + sleep(0.45) + NANO.write(b"Stop\n") elif bto == "420": print("reset") desDir = 0 else: # Store current position => cur pos - curX = float(x) * 1000 - curY = float(y) * 1000 - - oldDir = curDir + # curX = float(x) * 1000 + # curY = float(y) * 1000 - curDir = (math.atan2(curY - oldY, curX - oldX) + math.pi) * (180/math.pi) - if backward == True: - a = a - 180 + # oldDir = curDir + # curDir = (math.atan2(curY - oldY, curX - oldX) + math.pi) * (180/math.pi) + # if backward == True: + # a = a - 180 print(f"Current direction: {curDir}") - print(f"Old Direction: {oldDir}") - print(f"Direction difference: {(curDir - oldDir) / 100}") NANO.write(b"Stop\n") + with open("angle.txt", "w") as f: + f.write(f"{curDir}" + '\n') + f.close() + # Print current status (current command, heading? position? Could we include a cool progress bar? TO MISSION COMPLETETION?????) @@ -165,4 +179,4 @@ if bto.find("Connected") != -1: print("Terminating Bluetooth Process") os.kill(BluetoothProcess.pid, signal.SIGINT) -def calc_turn(desiredDirection, currentDirection): +# def calc_turn(desiredDirection, currentDirection): |