#This program makes the ER1 robot traverse the path it found by starting at the #start cell and searching for adjacent cells that = 4 (cells that are part of #the path equal 4). This program is specific to the ER1 robot, so some #knowledge of it and its API commands is needed to fully understand it. #Run the A* program by using the runpy module (a VERY useful module). import runpy runpy.run_module('A_star_algorithm_V2', run_name="__main__", alter_sys=True) #Some variables and the map are imported. import A_star_pathfinding_variables StartX = A_star_pathfinding_variables.sx StartY = A_star_pathfinding_variables.sy map = A_star_pathfinding_variables.map maxValX = A_star_pathfinding_variables.maxValX maxValY = A_star_pathfinding_variables.maxValY #The heading variable is extracted from variable modual. curHeading = A_star_pathfinding_variables.cH #--------------------------------------------------------------------------- #this part of the code establishes the telnet connection to the RCC. It moves #the robot along the path that the A* program found. import telnetlib import time import string HOST = "localhost" print "\nOpening Telnet session to ER1 GUI." tn = telnetlib.Telnet(HOST, 9000) print "Opened session.\n" def evocon (cmd, timeout=None): a='' if cmd=='events': tn.write("%s\n" % cmd) time.sleep(.1) a=tn.read_until("\n", timeout) print a if a != '': while a!= '': a=tn.read_until("\n", timeout) print a if a[0] == 'p' or 'm': a = '' timeout=0 tn.write("%s\n" % cmd) return tn.read_until("\n", timeout) #--------------------------------------------------------------------------- #Some movement functions. Currently, I cannot use the 'events' command in a #predictable manner, so I set up a basic pause command. So the program will wait #for the robot to complete its move (the values are all estimates are related #to the size of each cell, so if you change the cell size, you'll have to change #the values of the 'w' constants below). #The number 18 below is the dimension of each cell. To move to the adjacent cell, #you move 18 inches. Please feel free to change the sizes of the cells by #changing this number. def forward(x): d = x * 18 evocon ('move ' + str(d) + ' i') evocon ('events') return def rotateCW(x): evocon ('move -' + str(x) + ' d') evocon ('events') return def rotateCCW(x): evocon ('move ' + str(x) + ' d') evocon ('events') return #--------------------------------------------------------------------------- #The actual program. checkedCells = [] #List of cells that have been checked #(none at the moment). curCellX = StartX #Start analysis with the start cell. curCellY = StartY moveDistance = 0 #How far the robot needs to move #forward. Set at 0, since this value #has not been found yet. rest = 0 #dummy variable loop again. while not rest: celltrack = 0 #Kind of a "3-strike, you're out" counter, so the #path's end can be found. NcellX = curCellX NcellY = curCellY + 1 ScellX = curCellX ScellY = curCellY - 1 EcellX = curCellX + 1 EcellY = curCellY WcellX = curCellX -1 WcellY = curCellY #Search surrounding cells that are part of the path. Rotate and move forward #as needed, until no more path cells are found (path ends and the ER1 has #reached its goal cell). if NcellX >= 0 and NcellX <= maxValX and NcellY >= 0 and NcellY <= maxValY: if map[NcellY][NcellX] == 4 and checkedCells.count([NcellX, NcellY]) == 0: if curHeading != 'north': if curHeading == 'west': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(90) curHeading = 'north' print "turned 90 deg from west to north" elif curHeading == 'east': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCCW(90) curHeading = 'north' print "turned -90 deg from east to north" else: if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(180) curHeading = 'north' print "turned 180 deg from south to north" else: moveDistance = moveDistance + 1 checkedCells.append([curCellX, curCellY]) curCellX = NcellX curCellY = NcellY else: celltrack = celltrack + 1 else: celltrack = celltrack + 1 if ScellX >= 0 and ScellX <= maxValX and ScellY >= 0 and ScellY <= maxValY: if map[ScellY][ScellX] == 4 and checkedCells.count([ScellX, ScellY]) == 0: if curHeading != 'south': if curHeading == 'east': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(90) curHeading = 'south' print "turned 90 deg from East to south" elif curHeading == 'west': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCCW(90) curHeading = 'south' print "turned -90 deg from west to south" else: if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(180) curHeading = 'south' print "turned 180 deg from north to south" else: moveDistance = moveDistance + 1 checkedCells.append([curCellX, curCellY]) curCellX = ScellX curCellY = ScellY else: celltrack = celltrack + 1 else: celltrack = celltrack + 1 if EcellX >= 0 and EcellX <= maxValX and EcellY >= 0 and EcellY <= maxValY: if map[EcellY][EcellX] == 4 and checkedCells.count([EcellX, EcellY]) == 0: if curHeading != 'east': if curHeading == 'north': if moveDistance > 0: forward(moveDistance) forward(moveDistance) moveDistance = 0 rotateCW(90) curHeading = 'east' print "turned 90 deg from north to east" elif curHeading == 'south': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCCW(90) curHeading = 'east' print "turned -90 deg from south to east" else: if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(180) curHeading = 'east' print "turned 180 deg from west to east" else: moveDistance = moveDistance + 1 checkedCells.append([curCellX, curCellY]) curCellX = EcellX curCellY = EcellY else: celltrack = celltrack + 1 else: celltrack = celltrack + 1 if WcellX >= 0 and WcellX <= maxValX and WcellY >= 0 and WcellY <= maxValY: if map[WcellY][WcellX] == 4 and checkedCells.count([WcellX, WcellY]) == 0: if curHeading != 'west': if curHeading == 'south': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCW(90) curHeading = 'west' print "turned 90 deg from south to west" elif curHeading == 'north': if moveDistance > 0: forward(moveDistance) moveDistance = 0 rotateCCW(90) curHeading = 'west' print "turned -90 deg from north to west" else: if moveDistance > 0: Forward(moveDistance) moveDistance = 0 rotateCW(180) curHeading = 'west' print "turned 180 deg from east to west" else: moveDistance = moveDistance + 1 checkedCells.append([curCellX, curCellY]) curCellX = WcellX curCellY = WcellY else: celltrack = celltrack + 1 else: celltrack = celltrack + 1 if celltrack == 4: forward(moveDistance) print "Reached end of path" A_star_pathfinding_variables.cH = curHeading A_star_pathfinding_variables.sx = A_star_pathfinding_variables.tx A_star_pathfinding_variables.sy = A_star_pathfinding_variables.ty break