Skip to content
Snippets Groups Projects
Commit 35e9b140 authored by Darren0822's avatar Darren0822
Browse files

Merge branch 'UWB' of https://git.soton.ac.uk/plw1g21/robobin into UWB

parents 2795981c ed6cb435
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 142 additions and 48 deletions
Wireless_Communication/UWB/Anchor/Anchor1/Anchor1.ino
+ 26
− 9
View file @ 35e9b140
......@@ -28,7 +28,7 @@
#define MY_ADDR ANCHOR1
#define RX_TIMEOUT 5000
#define INTER_RANGING_DELAY 0
#define INTER_RANGING_DELAY 5
#define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR
......@@ -139,22 +139,39 @@ void loop()
// range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
addresses[i] = i + MY_ADDR + 1; // store destination address
addresses[i] = i + MY_ADDR + 1; // store destination addresss
for (uint8_t retries = 0; retries < 10; retries++) {
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
// distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
if (distances[i] != 0.0) {break;}
delay(INTER_RANGING_DELAY);
}
}
// Serial.println("Sending distances to tag");
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR);
// ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
if (MY_ADDR < ANCHOR3) {
// Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1);
ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
}
}
state = RESPONDER;
break;
......
Wireless_Communication/UWB/Anchor/Anchor2/Anchor2.ino
+ 26
− 9
View file @ 35e9b140
......@@ -28,7 +28,7 @@
#define MY_ADDR ANCHOR2
#define RX_TIMEOUT 5000
#define INTER_RANGING_DELAY 0
#define INTER_RANGING_DELAY 5
#define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR
......@@ -139,22 +139,39 @@ void loop()
// range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
addresses[i] = i + MY_ADDR + 1; // store destination address
addresses[i] = i + MY_ADDR + 1; // store destination addresss
for (uint8_t retries = 0; retries < 10; retries++) {
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
// distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
if (distances[i] != 0.0) {break;}
delay(INTER_RANGING_DELAY);
}
}
// Serial.println("Sending distances to tag");
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR);
// ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
if (MY_ADDR < ANCHOR3) {
// Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1);
ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
}
}
state = RESPONDER;
break;
......
Wireless_Communication/UWB/Anchor/Anchor3/Anchor3.ino
+ 26
− 9
View file @ 35e9b140
......@@ -28,7 +28,7 @@
#define MY_ADDR ANCHOR3
#define RX_TIMEOUT 5000
#define INTER_RANGING_DELAY 0
#define INTER_RANGING_DELAY 5
#define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR
......@@ -139,22 +139,39 @@ void loop()
// range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
addresses[i] = i + MY_ADDR + 1; // store destination address
addresses[i] = i + MY_ADDR + 1; // store destination addresss
for (uint8_t retries = 0; retries < 10; retries++) {
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
// distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
if (distances[i] != 0.0) {break;}
delay(INTER_RANGING_DELAY);
}
}
// Serial.println("Sending distances to tag");
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR);
// ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
if (MY_ADDR < ANCHOR3) {
// Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1);
ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
}
}
state = RESPONDER;
break;
......
Wireless_Communication/UWB/Anchor/Anchor4/Anchor4.ino
+ 26
− 9
View file @ 35e9b140
......@@ -28,7 +28,7 @@
#define MY_ADDR ANCHOR4
#define RX_TIMEOUT 5000
#define INTER_RANGING_DELAY 0
#define INTER_RANGING_DELAY 5
#define NUMBER_OF_BEACONS_TO_RANGE 4 - MY_ADDR
......@@ -139,22 +139,39 @@ void loop()
// range the remaining beacons
for (uint8_t i = 0; i < NUMBER_OF_BEACONS_TO_RANGE; i++) { // anchor shouldn't range itself
addresses[i] = i + MY_ADDR + 1; // store destination address
addresses[i] = i + MY_ADDR + 1; // store destination addresss
for (uint8_t retries = 0; retries < 10; retries++) {
distances[i] = getRangingDistance(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
// distances[i] = getRangingDistanceDelayedTX(&txMHR, PAN_ID, MY_ADDR, addresses[i], &rxMHR); // range beacon at addresses[i]
if (distances[i] != 0.0) {break;}
delay(INTER_RANGING_DELAY);
}
}
// Serial.println("Sending distances to tag");
ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR);
// ack = sendDistancesWithAck(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, &rxMHR, 5);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendDistances(&txMHR, PAN_ID, MY_ADDR, TAG1, NUMBER_OF_BEACONS_TO_RANGE, addresses, distances, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
if (MY_ADDR < ANCHOR3) {
// Serial.printf("Anchor %d passing bpm to anchor %d\n", MY_ADDR, MY_ADDR + 1);
ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// ack = sendCommandWithAck(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, &rxMHR, 3);
// delay(INTER_RANGING_DELAY);
for (uint8_t retries = 0; retries < 10; retries++) {
sendCommand(&txMHR, PAN_ID, MY_ADDR, MY_ADDR + 1, BPM_COMMAND, true);
ack = receiveAcknowledgement(&rxMHR);
if (ack) {break;}
delay(INTER_RANGING_DELAY);
}
}
}
state = RESPONDER;
break;
......
Wireless_Communication/UWB/Beacons_tag_position/realtime_location_with_serial_gui.py
+ 28
− 8
View file @ 35e9b140
......@@ -116,6 +116,10 @@ class AnchorTagGUI:
self.tag_distances = {anchor: tk.StringVar() for anchor in ["A1", "A2", "A3", "A4"]}
self.tag2_distances = {anchor: tk.StringVar() for anchor in ["A1", "A2", "A3", "A4"]}
self.windowSize = 20
self.tag1Window = []
self.tag2Window = []
# # If testing, load the predefined test values
# if testing:
# # Set anchor distances
......@@ -361,9 +365,25 @@ class AnchorTagGUI:
tag1_x, tag1_y, tag1_z = self.calculate_tag_coordinates(self.tag_distances)
tag2_x, tag2_y, tag2_z = self.calculate_tag_coordinates(self.tag2_distances)
print(f"Tag 1 coordinates: ({tag1_x:.2f}, {tag1_y:.2f}, {tag1_z:.2f})")
# print(f"Tag 1 coordinates: ({tag1_x:.2f}, {tag1_y:.2f}, {tag1_z:.2f})")
if len(self.tag1Window) < self.windowSize:
self.tag1Window.append((tag1_x, tag1_y, tag1_z))
else:
self.tag1Window = self.tag1Window[1:] + [(tag1_x, tag1_y, tag1_z)]
if tag2_x is not None and tag2_y is not None and tag2_z is not None:
print(f"Tag 2 coordinates: ({tag2_x:.2f}, {tag2_y:.2f}, {tag2_z:.2f})")
# print(f"Tag 2 coordinates: ({tag2_x:.2f}, {tag2_y:.2f}, {tag2_z:.2f})")
if len(self.tag2Window) < self.windowSize:
self.tag2Window.append((tag2_x, tag2_y, tag2_z))
else:
self.tag2Window = self.tag2Window[1:] + [(tag2_x, tag2_y, tag2_z)]
tag2_x, tag2_y, tag2_z = map(lambda l: sum(l) / len(l), zip(*self.tag2Window))
else:
self.tag2Window = []
tag1_x, tag1_y, tag1_z = map(lambda l: sum(l) / len(l), zip(*self.tag1Window))
# Clear the canvas before drawing
self.canvas.delete("all")
......@@ -415,11 +435,11 @@ class AnchorTagGUI:
# Draw anchorss
if hasattr(self, 'anchors'):
print("Drawing anchors")
# print("Drawing anchors")
for name, (x, y, z) in self.anchors.items():
print(f"location {x} {y} {z}")
# print(f"location {x} {y} {z}")
x_scaled, y_scaled = transform_coordinates(x, y)
print(f"location {x_scaled} {y_scaled} {z}")
# print(f"location {x_scaled} {y_scaled} {z}")
rounded_x, rounded_y = round(x, 2), round(y, 2)
self.canvas.create_oval(
x_scaled - 5, y_scaled - 5, x_scaled + 5, y_scaled + 5, fill="blue"
......@@ -473,9 +493,9 @@ class AnchorTagGUI:
def update_gui_distances(self):
try:
if self.anchors_coords_known:
print('Updating GUI distances')
# print('Updating GUI distances')
ranging_distances = self.serial_buffer.getRangingDistances()
print(ranging_distances)
# print(ranging_distances)
#ranging_distances = [TEST_TAG1_DISTANCES, TEST_TAG2_DISTANCES]
if ranging_distances:
# Update tag 1 distances
......@@ -491,7 +511,7 @@ class AnchorTagGUI:
self.tag2_distances[anchor].set(distance)
# Now calculate and draw both tags
print("Calculating and drawing tags")
# print("Calculating and drawing tags")
self.calculate_and_draw_tags()
except Exception as e:
print(f"Error updating GUI distances: {e}")
......
Wireless_Communication/UWB/Tag/Tag1/Tag1.ino
+ 10
− 4
View file @ 35e9b140
......@@ -28,8 +28,8 @@
#define MY_ADDR TAG1
#define RX_TIMEOUT 5000
#define INTER_RANGING_DELAY 0
#define TIMEOUT_MILLIS 200
#define INTER_RANGING_DELAY 5
#define TIMEOUT_MILLIS 300
unsigned long startTime;
......@@ -286,6 +286,12 @@ void loop()
//read distances into full array of distances
readDistancesBPM(senderAnchor, rxBuffer, BPMDistances);
startTime = millis(); //reset start time
// Serial.printf("Received distances from %d\n", senderAnchor);
// for (int i = 0; i < 6; i++) {
// Serial.printf("%lf ", BPMDistances[i] * 100.0);
// }
// Serial.println();
}
else {
Serial.println(3); // did not receive a send distances frame
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment