From 973d2c26abde257c842548c4af0ff4c3a3a8162c Mon Sep 17 00:00:00 2001 From: Edvin Date: Fri, 25 Apr 2025 08:42:55 +0200 Subject: Implemented collected data transfer, graphical control interface and improved README --- DWM.py | 4 +- README.md | 25 ++++++++- bt.py | 81 +++++++++++++++++++++++++++++ control.py | 0 main.py | 169 ++++++++++++++++++++++++++++++------------------------------- rpi.c | 50 ------------------ 6 files changed, 191 insertions(+), 138 deletions(-) mode change 100644 => 100755 DWM.py create mode 100755 bt.py mode change 100644 => 100755 control.py mode change 100644 => 100755 main.py delete mode 100644 rpi.c diff --git a/DWM.py b/DWM.py old mode 100644 new mode 100755 index 86cb07e..ccee6a7 --- a/DWM.py +++ b/DWM.py @@ -45,12 +45,12 @@ with serial.Serial('/dev/ttyACM0', 115200, timeout = 1) as s: sleep(0.1) s.write(b"\r") - str = f"{s.readline().decode('utf-8').strip('\n')}" + str = s.readline().decode('utf-8').strip('\n') if "apg: x:" in str: str = str.replace("apg: ", "") print(str) - with open("posititon.txt", "w") as f: + with open("position.txt", "w") as f: f.write(str + '\n') f.close() diff --git a/README.md b/README.md index 34e5407..ba9e538 100644 --- a/README.md +++ b/README.md @@ -1 +1,24 @@ -# TNE107-RPI \ No newline at end of file +# TNE107-RPI + +## control.py + +This control program is not needed for operation and is strictly used to test implementation. + +## bt.py + +This program controls the Bluetooth behaviour of the system. + +## DWM.py + +This program interfaces with the DWM1001 UWB transciever via serial to get location data. + +## LIDAR + +This program interfaces with the SLAMTEC RPLIDAR C1. To use this there are a couple steps to take. + +1. Compile the SDK in its respective folder. +2. Compile the actual LIDAR program and move it from output to the source directory with name `LIDARProg`. + +## main.py + +This program starts all the other programs at the correct times for communication to occur and for data handling to be done properly. This program is run as a systemd service on the Raspberry Pi. \ No newline at end of file diff --git a/bt.py b/bt.py new file mode 100755 index 0000000..4ec97b9 --- /dev/null +++ b/bt.py @@ -0,0 +1,81 @@ +import RPi.GPIO as GPIO +from time import sleep + +import os +import sys +import glob +import bluetooth +import re + +server_sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM) +server_sock.bind(("", bluetooth.PORT_ANY)) +server_sock.listen(1) + +port = server_sock.getsockname()[1] + +print(f"Waiting for connection on RFCOMM channel {port}...\r") + +recv_sock, client_info = server_sock.accept() + +print(f"Connected to {client_info[0]}.\r") + +read = "0" +message = "" + +while read != "1000": + read = recv_sock.recv(1024) + read = read.decode("utf-8").strip('\n') + print(f"{read}\r") + + message = "" + + # 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 + + # Send DWM data + with open("position.txt", "r") as f: + for line in f: + line = line.strip("\n") + x, y, z, qf = line.split() + x = x.translate({ord(c): None for c in 'x:'}) + y = y.translate({ord(c): None for c in 'y:'}) + recv_sock.send(f"{x}, {y}, ".encode()) + # message = message + f"{x}, {y}, " + f.close() + + # Send angle data + recv_sock.send("0, ".encode()) + # message = message + "0, " + + # Send LIDAR data + distances = [] + for i in range(360): + distances.append(0) + + with open("distances.txt", "r") as f: + for line in f: + a, d = line.split() + distances[int(a)] = int(d) + f.close() + + for i in range(359): + # print(f"({i}, {distances[i]})") + recv_sock.send(f"{distances[i]}, ".encode()) + # message = message + f"{distances[i]}, " + recv_sock.send(f"{distances[i]}\n".encode()) + # message = message + f"{distances[360]}\n" + # recv_sock.send(message.encode()) + + + # Send LIDAR data + # if read == "77": + # # Remove for KTS + # recv_sock.send("EOD\n".encode()) + + # Send DWM data + # if read == "88": + +print("Shutting down...") +server_sock.close() diff --git a/control.py b/control.py old mode 100644 new mode 100755 diff --git a/main.py b/main.py old mode 100644 new mode 100755 index 44f592e..811e982 --- a/main.py +++ b/main.py @@ -1,87 +1,86 @@ -import RPi.GPIO as GPIO +import os +import subprocess +import signal +import serial +import select from time import sleep -import os -import sys -import glob -import bluetooth -import re - -# 3 Pins to communicate with Arduino -P1 = 17 -P2 = 27 -P3 = 23 - -GPIO.setmode(GPIO.BCM) -GPIO.setup(P1, GPIO.OUT) -GPIO.setup(P2, GPIO.OUT) -GPIO.setup(P3, GPIO.OUT) -GPIO.output(P1, False) -GPIO.output(P2, False) -GPIO.output(P3, False) - -server_sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM) -server_sock.bind(("", bluetooth.PORT_ANY)) -server_sock.listen(1) - -port = server_sock.getsockname()[1] - -print("Waiting for connection on RFCOMM channel %d..." % port) - -recv_sock, client_info = server_sock.accept() - -print("Connected to %s." % client_info[0]) - -read = "0" - -while read != "1000": - read = recv_sock.recv(1024) - read = read.decode("utf-8").strip('\n') - print("Received: %s" % read) - - print(f"Sending: 'Acknowledge: {read}'") - recv_sock.send(f"Acknowledge: {read}\n".encode()) # Remember NEW LINE for ÖS to be able to read lines. - - if read == "0": # Do nothing - GPIO.output(P1, False) - GPIO.output(P2, False) - GPIO.output(P3, False) - elif read == "11": # Drive forward - GPIO.output(P1, True) - GPIO.output(P2, False) - GPIO.output(P3, False) - elif read == "22": # Drive backward - GPIO.output(P1, False) - GPIO.output(P2, True) - GPIO.output(P3, False) - elif read == "33": # Rotate right, 90 degrees - GPIO.output(P1, True) - GPIO.output(P2, True) - GPIO.output(P3, False) - elif read == "44": # Rotate left, 90 degrees - GPIO.output(P1, False) - GPIO.output(P2, False) - GPIO.output(P3, True) - elif read == "55": # Rotate right, 45 degrees - GPIO.output(P1, True) - GPIO.output(P2, False) - GPIO.output(P3, True) - elif read == "66": # Rotate left, 45 degrees - GPIO.output(P1, False) - GPIO.output(P2, True) - GPIO.output(P3, True) - elif read == "98": # Turn on light at landmark - GPIO.output(P1, True) - GPIO.output(P2, True) - GPIO.output(P3, True) - else: # Do nothing if command is not recognized - GPIO.output(P1, False) - GPIO.output(P2, False) - GPIO.output(P3, False) - -GPIO.output(P1, False) -GPIO.output(P2, False) -GPIO.output(P3, False) - -print("Shutting down...") -server_sock.close() +BluetoothProcess = subprocess.Popen(["python", "-u", "bt.py"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, stdin=subprocess.PIPE, text=True) + +# Waiting on connection +print(BluetoothProcess.stdout.readline().strip()) + +# Connected? +# Bluetooth output +bto = BluetoothProcess.stdout.readline().strip() +print(bto) + +newCommand = False +rightTurnIndex = 5 +rightTurn = 0 +leftTurnIndex = 2 +leftTurn = 0 + +if bto.find("Connected") != -1: + # Open serial port to Arduino Nano + NANO = serial.Serial('/dev/ttyUSB0', 9600, timeout=1) + + # LIDAR Process Code + LIDARProcess = subprocess.Popen(["./LIDARProg", "--channel", "--serial", "/dev/ttyUSB1", "460800"], stdout=open(os.devnull, 'wb')) + print(f"LIDAR PID: {LIDARProcess.pid}") + + # DWM Process Code + DWMProcess = subprocess.Popen(["python", "DWM.py"], stdout=open(os.devnull, 'wb')) + print(f"DWM PID: {DWMProcess.pid}") + + while bto != "1000": + ready, _, _ = select.select([BluetoothProcess.stdout], [], [], 0.01) + + if ready: + bto = BluetoothProcess.stdout.readline().strip() + newCommand = True + else: + newCommand = False + + # Send commands to Arduino based on bto + if rightTurn > 0: + NANO.write(b"Right\n") + rightTurn = rightTurn - 1 + if rightTurn == 0: + NANO.write(b"00") + + if leftTurn > 0: + NANO.write(b"Left\n") + leftTurn = leftTurn - 1 + if leftTurn == 0: + NANO.write(b"00") + + if newCommand: + if bto == "11": + NANO.write(b"Forward\n") + elif bto == "22": + NANO.write(b"Backward\n") + elif bto == "33": + # NANO.write(b"Right\n") + rightTurn = rightTurnIndex + leftTurn = 0 + elif bto == "44": + # NANO.write(b"Left\n") + leftTurn = leftTurnIndex + rightTurn = 0 + elif bto == "0": + NANO.write(b"Stop\n") + + + print("Terminating Serial port to Arduino Nano") + NANO.write(b"Stop\n") + + print("Terminating LIDAR process") + os.kill(LIDARProcess.pid, signal.SIGINT) + + print("Terminating DWM Process") + os.kill(DWMProcess.pid, signal.SIGINT) + +print("Terminating Bluetooth Process") +os.kill(BluetoothProcess.pid, signal.SIGINT) + diff --git a/rpi.c b/rpi.c deleted file mode 100644 index 3f6fa08..0000000 --- a/rpi.c +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include -#include -#include -#include - -int main(int argc, char **argv) -{ - struct sockaddr_rc loc_addr = { 0 }, rem_addr = { 0 }; - char buf[1024] = { 0 }; - int s, client, bytes_read; - socklen_t opt = sizeof(rem_addr); - - // allocate socket - s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); - - // bind socket to port 1 of the first available - // local bluetooth adapter - loc_addr.rc_family = AF_BLUETOOTH; - loc_addr.rc_bdaddr = *BDADDR_ANY; - loc_addr.rc_channel = (uint8_t) 1; - bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr)); - - // get local address - // ba2str( &loc_addr.rc_bdaddr, buf ); - // fprintf(stdout, "local %s\n", buf); - - // put socket into listening mode - fprintf(stdout, "Waiting on connection on RFCOMM channel %i...\n", loc_addr.rc_channel); - listen(s, loc_addr.rc_channel); - - // accept one connection - client = accept(s, (struct sockaddr *)&rem_addr, &opt); - ba2str( &rem_addr.rc_bdaddr, buf ); - fprintf(stdout, "Accepted connection from %s\n", buf); - - memset(buf, 0, sizeof(buf)); - - // read data from the client - bytes_read = read(client, buf, sizeof(buf)); - - if( bytes_read > 0 ) { - printf("received [%s]\n", buf); - } - - // close connection - close(client); - close(s); - return 0; -} -- cgit v1.2.3