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