From 18b51d1918ae95763270abe12aa9f12b2b57d685 Mon Sep 17 00:00:00 2001 From: foswret Date: Sun, 22 Jun 2025 09:30:49 -0500 Subject: removed gui.py, added code.py --- README.md | 6 ++- code.py | 107 +++++++++++++++++++++++++++++++++++++++++ gui.py | 46 ------------------ main.py | 132 --------------------------------------------------- maps/exampleRobotMap | 1 - pathfind.py | 98 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 210 insertions(+), 180 deletions(-) create mode 100644 code.py delete mode 100644 gui.py delete mode 100644 main.py create mode 100644 pathfind.py diff --git a/README.md b/README.md index d0ca97a..4de9340 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ This is a program designed for use on a robot for the "Robot Tour" Event for the Positions on the course are determined by their X value and Y value. Y values are in reverse (going down on the coordinate plane means the Y value increases). Map dimensions must be rectangular and are defined in `MAPWIDTH` and `MAPHEIGHT` -Start and end points can be determined by changing the values in `STARTPOINT` and `ENDPOINT` in `main.py`. The order of points that the robot travels to is determined by `checkpointList[]`. +Start and end points can be determined by changing the values in `STARTPOINT` and `ENDPOINT` in `pathfind.py`. The order of points that the robot travels to is determined by `checkpointList[]`. ```python checkpointList = [ @@ -39,3 +39,7 @@ wallList = [ ``` The program is instructed to solve the course using the `solveCourse` function. + +The actual pathfinding and motor commands are split up between the `pathfind.py` and `code.py` files. `pathfind.py` simply finds an optimal route and `code.py` is the file that must be put on the robot. in `code.py`, you must copy the output produced by `pathfind.py` into the source code file. Eventually, this may be automated where instructions are output to a text file where `code.py` can read it. + + diff --git a/code.py b/code.py new file mode 100644 index 0000000..cb72603 --- /dev/null +++ b/code.py @@ -0,0 +1,107 @@ +import time +import board +from analogio import AnalogIn +from adafruit_motorkit import MotorKit +kit = MotorKit() + +analog_in = AnalogIn(board.A1) + +commandList = (180, 90, 0, 0, 270, 0, + 180, 90, 180, 180, 270, + 180, 90, 270, 0, 90, 90, 90, + 0, 180, 270, 180, 90, 270, 0, + 270, 270, 180, 90) + +DIRECTIONS = {0: 'Up', + 90: 'Right', + 180: 'Down', + 270: 'Left'} + +robotDirection = 90 + + +def get_voltage(pin): + return (pin.value * 3.3) / 65536 + + +def turn(direction, amount): + turnDuration = 0.6 + turnSpeed = 0.7 + if amount == 2: + turnDuration = turnDuration * 2 + if direction == 'counterClockwise': + turnSpeed = -(turnSpeed) + kit.motor3.throttle = -turnSpeed + kit.motor4.throttle = -turnSpeed + time.sleep(turnDuration) + kit.motor3.throttle = 0.0 + kit.motor4.throttle = -0.0 + time.sleep(1.0) + + +def move(): + kit.motor3.throttle = -0.55 + kit.motor4.throttle = 0.85 + time.sleep(0.9) + kit.motor3.throttle = 0.0 + kit.motor4.throttle = 0.0 + time.sleep(1.0) + + +def centerAtStart(): + kit.motor3.throttle = 0.6 + kit.motor4.throttle = -0.6 + time.sleep(0.45) + kit.motor3.throttle = 0.0 + kit.motor4.throttle = 0.0 + time.sleep(1.0) + + +def runInstructions(): + global robotDirection + v = 0 + while (v < 1.3): + v = get_voltage(analog_in) + print(v) + time.sleep(0.05) + + print("Exited while loop.") + + for i in range(len(commandList)): + turnIncrement = 0 + if commandList[i] == robotDirection: + move() + # print("Move Foward (Facing %s)" % (DIRECTIONS[robotDirection])) + else: + while robotDirection != commandList[i]: + robotDirection += 90 + turnIncrement += 1 + if robotDirection == 360: + robotDirection = 0 + if turnIncrement == 0: + # print("Move.") + move() + elif turnIncrement == 1: + # print("Turn 90° clockwise to face %s." % (DIRECTIONS[robotDirection])) + turn('clockwise', 1) # 1 represents 90 degrees + # print("Move.") + move() + elif turnIncrement == 2: + # print("Turn 180° clockwise to face %s." % (DIRECTIONS[robotDirection])) + turn('clockwise', 2) + # print("Move.") + move() + elif turnIncrement == 3: + # print("Turn 90° counterclockwise to face %s." % (DIRECTIONS[robotDirection])) + turn('counterClockwise', 1) + # print("Move.") + move() + + +# centerAtStart() +# turn('clockwise', 1) +# print("Hello World") +# runInstructions() +# move() + + diff --git a/gui.py b/gui.py deleted file mode 100644 index 3f9400c..0000000 --- a/gui.py +++ /dev/null @@ -1,46 +0,0 @@ -import pygame -import sys -from pygame.locals import * - -pygame.init() - -BLACK = (0, 0, 0) -GRAY = (99, 99, 99) -WHITE = (255, 255, 255) -WINDOW_HEIGHT = 800 -WINDOW_WIDTH = 800 -MARGIN = 20 -MATRIX_HEIGHT = 5 -MATRIX_WIDTH = 4 - -square_size = 100 - - -GridWidthpx = (MATRIX_WIDTH * square_size) + (MATRIX_WIDTH * square_size // 10) -GridHeightpx = (MATRIX_HEIGHT * square_size) + (MATRIX_HEIGHT * square_size // 10) -square_x = GridWidthpx // 4 - square_size // 4 -square_y = GridHeightpx // 4 - square_size // 4 - -square_x_orig = square_x - -displaySurface = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT), 0, 32) -pygame.display.set_caption('tourob GUI') - -displaySurface.fill(WHITE) - -for y in range(MATRIX_HEIGHT): - square_x = square_x_orig - pygame.draw.rect(displaySurface, BLACK, (square_x, square_y, MARGIN, square_size)) - for x in range(MATRIX_WIDTH): - pygame.draw.rect(displaySurface, BLACK, (square_x - MARGIN, square_y, MARGIN, square_size)) - pygame.draw.rect(displaySurface, GRAY, (square_x, square_y, square_size, square_size)) - square_x += square_size + MARGIN - pygame.draw.rect(displaySurface, BLACK, (square_x - MARGIN, square_y, MARGIN, square_size)) - square_y += square_size + MARGIN -while True: - for event in pygame.event.get(): - if event.type == QUIT: - pygame.quit() - sys.exit() - - pygame.display.update() diff --git a/main.py b/main.py deleted file mode 100644 index 097d8bd..0000000 --- a/main.py +++ /dev/null @@ -1,132 +0,0 @@ -import time - -# Constants -MAPWIDTH = 4 -MAPHEIGHT = 5 - -STARTPOINT = [0, 2] -ENDPOINT = [1, 4] - -checkpointList = [ - STARTPOINT, - [0, 0], - [1, 4], - [3, 2], - [3, 4], - ENDPOINT - ] - -wallList = [ # Each of these walls' locations are based on their neighboring squares (Ex. wall #1 is between (2, 0) and (3, 0) - [[2, 0], [3, 0]], - [[1, 0], [1, 1]], - [[3, 0], [3, 1]], - [[0, 1], [0, 2]], - [[0, 2], [1, 2]], - [[2, 2], [3, 2]], - [[1, 3], [1, 4]], - [[3, 3], [3, 4]], - [[1, 4], [2, 4]] - ] - -courseMap = [] - - -def populateCourse(course, width, height): - for x in range(width): - for y in range(height): - course.append([x, y]) - - -def queryNeighbors(node): - # Right Down Left Up - directions = [[1, 0], [0, 1], [-1, 0], [0, -1]] - result = [] - for dir in directions: # for x in range(4) - neighbor = [node[0] + dir[0], node[1] + dir[1]] # [nodeX + DirectionModifierX, nodeY + DirectionModifierY] - if neighbor in courseMap: # Makes sure the neighbor is within bounds - result.append(neighbor) - for x in range(len(wallList)): - if node in wallList[x] and neighbor in wallList[x]: # TBD: Optimize determining if wall is present - result.remove(neighbor) - return result - - -def getShortestPath(courseMap, startPos, endPos): - searchPaths = [[startPos]] - visitedCoordinates = [startPos] - - while searchPaths != []: - currentPath = searchPaths.pop(0) - currentPos = currentPath[-1] - - if currentPos == endPos: - return currentPath - - for nextPos in queryNeighbors(currentPos): - if nextPos in visitedCoordinates: - continue - searchPaths.append(currentPath + [nextPos]) - visitedCoordinates += nextPos - - -def move(direction): - # if direction = 'right': - # moving = True - # while moving: - # Motors.moving etc - # when motors.stop: - # moving = False - # if direction = 'left': - # moving = True - # while moving: - # Motors.moving etc - # when motors.stop: - # moving = False - # if direction = 'right': - # moving = True - # while moving: - # Motors.moving etc - # when motors.stop: - # moving = False - # if direction = 'up': - # moving = True - # while moving: - # Motors.moving etc - # when motors.stop: - # moving = False - return 0 - - -def solveCourse(course, obstacles, checkpoints): - stepsLength = 0 - - for x in range(len(checkpoints) - 1): - shortestPath = getShortestPath(course, checkpoints[x], checkpoints[x + 1]) - # if x != 0: - # del shortestPath[0] - for i in range(len(shortestPath) - 1): - diffX = shortestPath[i + 1][0] - shortestPath[i][0] - if diffX == 1: - move('right') - elif diffX == -1: - move('left') - - diffY = shortestPath[i + 1][1] - shortestPath[i][1] - if diffY == 1: - move('down') - elif diffY == -1: - move('up') - - stepsLength += 1 - print(shortestPath) - print("Amount of steps: %d" % (stepsLength)) - - -populateCourse(courseMap, MAPWIDTH, MAPHEIGHT) - -startTime = time.time() -solveCourse(courseMap, wallList, checkpointList) -endTime = time.time() -executionTime = endTime - startTime - -print("Execution Time: %.4f seconds" % executionTime) diff --git a/maps/exampleRobotMap b/maps/exampleRobotMap index 940d0ef..c95848f 100644 --- a/maps/exampleRobotMap +++ b/maps/exampleRobotMap @@ -24,4 +24,3 @@ wallList = [[3, 3], [3, 4]], [[1, 4], [2, 4]] ] - diff --git a/pathfind.py b/pathfind.py new file mode 100644 index 0000000..4060ea0 --- /dev/null +++ b/pathfind.py @@ -0,0 +1,98 @@ +import time + +# Constants +MAPWIDTH = 4 +MAPHEIGHT = 5 + +STARTPOINT = [0, 2] +ENDPOINT = [1, 4] + +checkpointList = [ + STARTPOINT, + [0, 0], + [1, 4], + [3, 2], + [3, 4], + ENDPOINT + ] + +wallList = [ # Each of these walls' locations are based on their neighboring squares (Ex. wall #1 is between (2, 0) and (3, 0) + [[2, 0], [3, 0]], + [[1, 0], [1, 1]], + [[3, 0], [3, 1]], + [[0, 1], [0, 2]], + [[0, 2], [1, 2]], + [[2, 2], [3, 2]], + [[1, 3], [1, 4]], + [[3, 3], [3, 4]], + [[1, 4], [2, 4]] + ] + +courseMap = [] + + +def populateCourse(course, width, height): + for x in range(width): + for y in range(height): + course.append([x, y]) + + +def queryNeighbors(node): + # Right Down Left Up + directions = [[1, 0], [0, 1], [-1, 0], [0, -1]] + result = [] + for dir in directions: # for x in range(4) + neighbor = [node[0] + dir[0], node[1] + dir[1]] # [nodeX + DirectionModifierX, nodeY + DirectionModifierY] + if neighbor in courseMap: # Makes sure the neighbor is within bounds + result.append(neighbor) + for x in range(len(wallList)): + if node in wallList[x] and neighbor in wallList[x]: # TBD: Optimize determining if wall is present + result.remove(neighbor) + return result + + +def getShortestPath(courseMap, startPos, endPos): + searchPaths = [[startPos]] + visitedCoordinates = [startPos] + + while searchPaths != []: + currentPath = searchPaths.pop(0) + currentPos = currentPath[-1] + + if currentPos == endPos: + return currentPath + + for nextPos in queryNeighbors(currentPos): + if nextPos in visitedCoordinates: + continue + searchPaths.append(currentPath + [nextPos]) + visitedCoordinates += nextPos + + +def solveCourse(course, obstacles, checkpoints): + stepsLength = 0 + courseDirections = () + + for x in range(len(checkpoints) - 1): + shortestPath = getShortestPath(course, checkpoints[x], checkpoints[x + 1]) + # if x != 0: + # del shortestPath[0] + for i in range(len(shortestPath) - 1): + diffX = shortestPath[i + 1][0] - shortestPath[i][0] + if diffX == 1: + courseDirections = courseDirections + (90, ) + elif diffX == -1: + courseDirections = courseDirections + (270, ) + diffY = shortestPath[i + 1][1] - shortestPath[i][1] + if diffY == 1: + courseDirections = courseDirections + (180, ) + elif diffY == -1: + courseDirections = courseDirections + (0, ) + stepsLength += 1 + print(shortestPath) + print("Amount of steps: %d" % (stepsLength)) + print(courseDirections) + + +populateCourse(courseMap, MAPWIDTH, MAPHEIGHT) +solveCourse(courseMap, wallList, checkpointList) -- cgit v1.2.3