- maybe works

server/constants.py

@@ -9,3 +9,17 @@ class Directions(Enum):
     LEAN_LEFT = "Lean-L"
 
 SYMBOLS = [".", "█", "s", "+"]
+
+class Orientation(Enum):
+    N = (0, 1)
+    S = (0, -1)
+    W = (-1, 0)
+    E = (1, 0)
+    NW = (-1, 1)
+    NE = (1, 1)
+    SW = (-1, -1)
+    SE = (1, -1)
+
+    @staticmethod
+    def is_cardinal(direction):
+        return direction == Orientation.N.value or direction == Orientation.S.value or direction == Orientation.W.value or direction == Orientation.E.value
\ No newline at end of file

server/mapping.py

 import time
 import numpy as np
 import heapq
-from constants import SYMBOLS, Directions
+from constants import SYMBOLS, Directions, Orientation
 import SpiderG
 from ultrasonic import measure_distance
 
-DIRECTIONS = [(0, 1), (1, 0), (0, -1), (-1, 0), (1, 1), (-1, -1), (1, -1), (-1, 1)]
+DIRECTIONS = [direction.value for direction in Orientation]
 TURN_TIME = 0.05
 
 # Scan and create a 2D grid
@@ -76,7 +76,7 @@ def a_star(grid, start, goal):
                 if grid[neighbor[0], neighbor[1]] == 1:  # Obstacle
                     continue
 
-                tentative_g_score = g_score[current] + 1  # Cost to neighbor is always 1 (or a fixed cost)
+                tentative_g_score = g_score[current] + 1
 
                 if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                     came_from[neighbor] = current

server/test.py

-from ultrasonic import gpio_clean_up
-from SpiderG import move_init, servoStop
-from mapping import measure_distance, scan_360_to_grid, print_grid, print_grid_with_path, a_star
+from collections import deque
+from time import sleep
 
-GOAL = (0, 0) # (0,0) - (19, 19)
+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 = (0, 0)  # (0,0) - (19, 19)
 
 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
 
-def go():
-    print("here we go")
-    # while current_location != goal:
-        # get next step in path
-        # orient robot to move to next square
-        # check for obstacle 1 square in front
 
-        # if obstacle,
-            # make 1 on grid, rerun a*
-        # if no obstacle,
-            # move forward 1 square
-            # update grid with new robot location
+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
 
-# Example usage of the scan_360_to_grid function
-grid_size = 20  # X and Y size
-try:
-    # TODO aim for ~20 steps
-    move_init()
-    grid = scan_360_to_grid(grid_size, degree_step=20, distance_threshold=DISTANCE_THRESHOLD, max_distance=MAX_DISTANCE)
-    start = (grid_size // 2, grid_size // 2)  # Starting point in center
-    path = a_star(grid, start, GOAL)
-    print("\n\n")
-    print_grid_with_path(grid, path)
+        # 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)
 
-    go()
+                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:
-    print("clean up")
-    gpio_clean_up()
-    servoStop()
\ No newline at end of file
+    spider.clean_up()