diff --git a/server/camera.py b/server/camera.py
deleted file mode 100644
index 7a094616cddfd04edf09d7e3b7fcd8b171546358..0000000000000000000000000000000000000000
--- a/server/camera.py
+++ /dev/null
@@ -1,50 +0,0 @@
-import cv2
-import numpy as np
-
-# Load pre-trained MobileNet SSD model and class labels
-net = cv2.dnn.readNetFromCaffe('path_to_deploy.prototxt', 'path_to_weights.caffemodel')
-
-# COCO or ImageNet classes that MobileNet was trained on
-CLASSES = ["background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus",
- "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike",
- "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"]
-
-# Initialize the camera
-cap = cv2.VideoCapture(0)
-
-while True:
- # Capture frame-by-frame
- ret, frame = cap.read()
-
- # Prepare the frame for object detection
- (h, w) = frame.shape[:2]
- blob = cv2.dnn.blobFromImage(cv2.resize(frame, (300, 300)), 0.007843, (300, 300), 127.5)
- net.setInput(blob)
- detections = net.forward()
-
- # Loop over the detections
- for i in range(detections.shape[2]):
- confidence = detections[0, 0, i, 2]
-
- # Filter out weak detections
- if confidence > 0.4: # Adjust confidence threshold as needed
- idx = int(detections[0, 0, i, 1])
- label = CLASSES[idx]
-
- # If the detected object is a bird, process it
- if label == "bird":
- box = detections[0, 0, i, 3:7] * np.array([w, h, w, h])
- (startX, startY, endX, endY) = box.astype("int")
- cv2.rectangle(frame, (startX, startY), (endX, endY), (0, 255, 0), 2)
- cv2.putText(frame, label, (startX, startY - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
-
- # Display the resulting frame
- cv2.imshow('Bird Detection', frame)
-
- # Break the loop if 'q' is pressed
- if cv2.waitKey(1) & 0xFF == ord('q'):
- break
-
-# Release the capture and close windows
-cap.release()
-cv2.destroyAllWindows()
\ No newline at end of file
diff --git a/server/go.py b/server/go.py
new file mode 100644
index 0000000000000000000000000000000000000000..afe83d0f7c74aee0bfb3aebae9657539375426e2
--- /dev/null
+++ b/server/go.py
@@ -0,0 +1,131 @@
+from collections import deque
+from time import sleep
+
+import numpy as np
+
+import SpiderG
+from constants import Orientation, Directions
+from mapping import scan_360_to_grid, print_grid_with_path, a_star
+from ultrasonic import gpio_clean_up, measure_distance
+
+GOAL = (19, 10) # (0,0) - (19, 19)
+
+MAX_DISTANCE = 300
+DISTANCE_THRESHOLD = 300
+TURN_TIME = 6 # Time it takes to turn 1 compass direction, fiddle with this
+CARDINAL_MOVE_TIME = 5 # Time it takes to move in a cardinal direction, fiddle with this too
+INTERCARDINAL_MOVE_TIME = 6 # Time it takes to move in an intercardinal direction, fiddle with this too
+OBSTACLE_DISTANCE = 30 # mess with this
+DEGREE_STEP = 20 # degrees per turn
+GRID_SIZE = 20
+
+
+class Spider:
+ def __init__(self, grid_size, degree_step):
+ self.grid_size = grid_size
+ self.degree_step = degree_step
+
+ # Spider starts facing south at first
+ self.orientation_queue = deque(
+ [Orientation.E.value, Orientation.NE.value, Orientation.N.value, Orientation.NW.value, Orientation.W.value,
+ Orientation.SW.value, Orientation.S.value, Orientation.SE.value])
+
+ self.grid = np.zeros((grid_size, grid_size))
+
+ # Start at center
+ self.current_position = (grid_size // 2, grid_size // 2)
+
+ # Starting position
+ SpiderG.move_init()
+
+ def scan(self):
+ self.grid = scan_360_to_grid(self.grid_size, degree_step=self.degree_step,
+ distance_threshold=DISTANCE_THRESHOLD, max_distance=MAX_DISTANCE)
+
+ def move_to(self, goal):
+ print("we go to", goal)
+ path = a_star(self.grid, self.current_position, goal)
+ path.pop(0)
+ print(path)
+ print_grid_with_path(self.grid, path)
+
+ while path and self.current_position != goal:
+ try:
+ next_step = path[0]
+
+ # Turn to new orientation
+ new_orientation = self.get_path_direction(next_step)
+ print(self.orientation_queue[0], print(new_orientation))
+ self.turn_to(new_orientation)
+
+ if self.is_blocked():
+ SpiderG.servoStop()
+ # Path blocked, mark obstacle and find new path
+ self.grid[next_step[0]][next_step[1]] = 1
+ path = a_star(self.grid, self.current_position, goal)
+ if path is None:
+ print(self.grid)
+ raise Exception("No path found. Giving up")
+ else:
+ # Path clear, go and remove step from path, update grid
+ self.grid[self.current_position[0]][self.current_position[1]] = 0
+ self.grid[next_step[0]][next_step[1]] = 2
+ self.current_position = next_step
+ path.pop(0)
+ self.advance(new_orientation)
+
+ print_grid_with_path(self.grid, path)
+ except Exception as e:
+ print(e)
+ break
+
+ @staticmethod
+ def is_blocked():
+ distance = measure_distance()
+ return distance <= OBSTACLE_DISTANCE
+
+ @staticmethod
+ def advance(direction):
+ SpiderG.walk(Directions.FORWARD.value)
+ # moving diagonally takes a bit more time
+ move_time = CARDINAL_MOVE_TIME if Orientation.is_cardinal(direction) else INTERCARDINAL_MOVE_TIME
+ sleep(move_time)
+
+ def get_path_direction(self, new_position):
+ coord_difference = (new_position[0] - self.current_position[0], new_position[1] - self.current_position[1])
+ if coord_difference[0] > 1 or coord_difference[1] > 1:
+ raise Exception(f"Next step out of reach. {self.current_position} to {new_position}")
+ return coord_difference
+
+ def turn_to(self, new_orientation):
+ turns = 0
+ while self.orientation_queue[0] != new_orientation:
+ old_orientation = self.orientation_queue.popleft()
+ self.orientation_queue.append(old_orientation)
+ self.turn()
+ turns += 1
+ if turns == 8:
+ raise Exception(f"360 turn without matching direction, check path direction. {new_orientation}")
+
+ @staticmethod
+ def turn():
+ SpiderG.walk(Directions.TURN_LEFT.value)
+ sleep(TURN_TIME)
+ SpiderG.servoStop()
+
+ @staticmethod
+ def clean_up():
+ print("clean up")
+ gpio_clean_up()
+ SpiderG.servoStop()
+ SpiderG.move_init()
+
+
+spider = Spider(GRID_SIZE, DEGREE_STEP)
+try:
+ spider.scan() # Optional 360 scan
+ spider.move_to(GOAL)
+except KeyboardInterrupt:
+ spider.clean_up()
+finally:
+ spider.clean_up()
diff --git a/server/mapping.py b/server/mapping.py
index e89bfaab2d5345e04e45bf9e79cc03be0cbfb965..19ca175b54a4707865ea88d151b84421f9a23652 100644
--- a/server/mapping.py
+++ b/server/mapping.py
@@ -20,7 +20,7 @@ def scan_360_to_grid(grid_size, degree_step, distance_threshold, max_distance):
print("step", step, "/", total_steps)
# Measure the distance
distance = measure_distance()
- angle = step * degree_step
+ angle = 360 - (step * degree_step) # Left turn scan
# Calculate the position in Cartesian coordinates (relative to the center of the grid)
if distance < max_distance: # Only map distances within the sensor range
diff --git a/server/test.py b/server/test.py
old mode 100755
new mode 100644
index 5f158c2fb644376ab9239d58608b4037d2c069a8..01ef59400ccfa082e8920cce9e6d7b28cc008c6f
--- a/server/test.py
+++ b/server/test.py
@@ -1,3 +1,4 @@
+import time
from collections import deque
from time import sleep
@@ -6,121 +7,8 @@ from constants import Orientation, Directions
from mapping import scan_360_to_grid, print_grid_with_path, a_star
from ultrasonic import gpio_clean_up, measure_distance
-GOAL = (0, 0) # (0,0) - (19, 19)
+SpiderG.move_init()
-MAX_DISTANCE = 300
-DISTANCE_THRESHOLD = 300
-TURN_TIME = 2 # Time it takes to turn 1 compass direction, fiddle with this
-CARDINAL_MOVE_TIME = 2 # Time it takes to move in a cardinal direction, fiddle with this too
-INTERCARDINAL_MOVE_TIME = 2 # Time it takes to move in an intercardinal direction, fiddle with this too
-OBSTACLE_DISTANCE = 10 # mess with this
-DEGREE_STEP = 20 # degrees per turn
-GRID_SIZE = 20
-
-
-class Spider:
- def __init__(self, grid_size, degree_step):
- self.grid_size = grid_size
- self.degree_step = degree_step
-
- # Spider starts facing south at first
- self.orientation_queue = deque(
- [Orientation.S.value, Orientation.SE.value, Orientation.E.value, Orientation.NE.value, Orientation.N.value,
- Orientation.NW.value, Orientation.W.value, Orientation.SW.value])
-
- self.grid = None
-
- # Start at center
- self.current_position = (grid_size // 2, grid_size // 2)
-
- # Starting position
- SpiderG.move_init()
-
- # Start with scan
- self.scan()
-
- def scan(self):
- self.grid = scan_360_to_grid(self.grid_size, degree_step=self.degree_step,
- distance_threshold=DISTANCE_THRESHOLD, max_distance=MAX_DISTANCE)
-
- def move_to(self, goal: tuple[int, int]):
- print("we go to", goal)
- path = a_star(self.grid, self.current_position, goal)
- print(path)
- print_grid_with_path(self.grid, path)
-
- while path and self.current_position != goal:
- try:
- next_step = path[0]
-
- # Turn to new orientation
- new_orientation = self.get_path_direction(next_step)
- self.turn_to(new_orientation)
-
- if self.is_blocked():
- # Path blocked, mark obstacle and find new path
- self.grid[next_step[0]][next_step[1]] = 1
- path = a_star(self.grid, self.current_position, goal)
- if path is None:
- print(self.grid)
- raise Exception("No path found. Giving up")
- else:
- # Path clear, go and remove step from path, update grid
- self.grid[self.current_position[0]][self.current_position[1]] = 0
- self.grid[next_step[0]][next_step[1]] = 2
- self.current_position = next_step
- path.pop(0)
- self.advance(new_orientation)
-
- print_grid_with_path(self.grid, path)
- except Exception as e:
- print(e)
- break
-
- @staticmethod
- def is_blocked():
- distance = measure_distance()
- return distance <= OBSTACLE_DISTANCE
-
- @staticmethod
- def advance(direction: tuple[int, int]):
- SpiderG.walk(Directions.FORWARD.value)
- # moving diagonally takes a bit more time
- move_time = CARDINAL_MOVE_TIME if Orientation.is_cardinal(direction) else INTERCARDINAL_MOVE_TIME
- sleep(move_time)
-
- def get_path_direction(self, new_position: tuple[int, int]):
- coord_difference = (new_position[0] - self.current_position[0], new_position[1] - self.current_position[1])
- if coord_difference[0] > 1 or coord_difference[1] > 1:
- raise Exception(f"Next step out of reach. {self.current_position} to {new_position}")
- return coord_difference
-
- def turn_to(self, new_orientation: tuple[int, int]):
- turns = 0
- while self.orientation_queue[0] != new_orientation:
- old_orientation = self.orientation_queue.popleft()
- self.orientation_queue.append(old_orientation)
- self.turn()
- turns += 1
- if turns == 8:
- raise Exception(f"360 turn without matching direction, check path direction. {new_orientation}")
-
- @staticmethod
- def turn():
- SpiderG.walk(Directions.TURN_LEFT.value)
- sleep(TURN_TIME)
-
- @staticmethod
- def clean_up():
- print("clean up")
- gpio_clean_up()
- SpiderG.servoStop()
-
-
-spider = Spider(GRID_SIZE, DEGREE_STEP)
-try:
- spider.move_to(GOAL)
-except KeyboardInterrupt:
- spider.clean_up()
-finally:
- spider.clean_up()
+SpiderG.walk(Directions.TURN_LEFT.value)
+time.sleep(2)
+SpiderG.servoStop()
\ No newline at end of file