Implemented collected data transfer, graphical control interface and improved README

6 files changed, 191 insertions, 138 deletions

diff --git a/DWM.py b/DWM.py
index 86cb07e..ccee6a7 100644..100755
--- 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
index f86d3bf..f86d3bf 100644..100755
--- a/control.py
+++ b/control.py
diff --git a/main.py b/main.py
index 44f592e..811e982 100644..100755
--- 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 <stdio.h>
-#include <unistd.h>
-#include <sys/socket.h>
-#include <bluetooth/bluetooth.h>
-#include <bluetooth/rfcomm.h>
-
-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;
-}