def draw_tilemap(self, game, screen):
'''build a map with number of tiles and TILESIZE'''
self.board_image.draw(screen)
# draw a grid
for i in range(BOARDHEIGHT):
for j in range(BOARDWIDTH):
rect = pg.Rect(self.offset[0] + j * self.tilesize,
self.offset[1] + i * self.tilesize, self.tilesize, self.tilesize)
pg.draw.rect(screen, DARKGREY, (rect), 1)
# draw the objects on the screen
for player in self.players.keys():
for building in self.players[player]["buildings"]:
building.draw(self, screen)
for player in self.players.keys():
for unit in self.players[player]["units"]:
unit.draw(self, screen)
# draw movement path of the selected unit
for unit in self.players[self.current_player]["units"]:
if unit.selected and self.get_mouse_pos() != unit.pos:
self.path = a_star_search(
self, self.get_mouse_pos(), unit.pos)
self.draw_path(screen, unit, self.get_mouse_pos())
if game.show_path:
self.draw_explored_tiles(screen)
# draw tooltips
self.draw_tooltips(game, screen)
def staticGridSimulation():
wMap = grid.Grid(tile_num_height, tile_num_width, tile_size)
# Generates random enviroment on the grid
generator = RandomObjects(wMap)
# You can change the number of every type of object you want
generator.create_env(20, 0, 0, 20, 7)
generator.bloatTiles(robot_radius, bloat_factor)
# Starting location
topLeftX = 2.0
topLeftY = 2.0
# Ending Location
botRightX = tile_size * tile_num_width - 2.0
botRightY = tile_size * tile_num_height - 2.0
# Run algorithm to get path
try:
dists, path = search.a_star_search(wMap, (topLeftX, topLeftY),
(botRightX, botRightY),
search.euclidean)
root = Tk()
# start GUI and run animation
simulation = MapPathGUI(root, wMap, path)
simulation.runSimulation(True)
except:
print("C1C0: \"There is no path to the desired location. Beep Boop\"")
if __name__ == "__main__":
staticGridSimulation()
def dynamicGridSimulation(endPoint):
"""
Run a dynamic grid simulation that navigates C1C0 to endpoint
Parameter endPoint: (int, int) that represents the (x,y) coordinate
"""
emptyMap = grid.Grid(tile_num_height, tile_num_width, tile_size)
fullMap = grid.Grid(tile_num_height, tile_num_width, tile_size)
# Generates random enviroment on the grid
generator = RandomObjects(fullMap)
# You can change the number of every type of object you want
generator.create_env(22, 0, 0, 22, 9)
# starting location for middle
midX = tile_size * tile_num_width / 2
midY = tile_size * tile_num_height / 2
print(endPoint)
# Run algorithm to get path
dists, path = search.a_star_search(emptyMap, (midX, midY), endPoint,
search.euclidean)
# start GUI and run animation
simulation = DynamicGUI(Tk(), fullMap, emptyMap,
search.segment_path(emptyMap, path), endPoint, 180)
simulation.runSimulation()
def commandExecute(command):
"""
Run A command execution with C1C0 that executes the command
Prameter command: a tuple of (type, amount) where type is the type of command 'forward' 'turn' and amount is the
corresponding distance or degree to execute for the robot.
"""
emptyMap = grid.Grid(tile_num_height, tile_num_width, tile_size)
fullMap = grid.Grid(tile_num_height, tile_num_width, tile_size)
# Generates random enviroment on the grid
generator = RandomObjects(fullMap)
# You can change the number of every type of object you want
generator.create_env(22, 0, 0, 22, 9)
# starting location for middle
midX = tile_size * tile_num_width / 2
midY = tile_size * tile_num_height / 2
if command[0] == 'move forward':
# TODO: THIS IS FILLER CODE FOR WHEN WE CAN
# Run algorithm to get path
first_num = 0
second_num = command[1] * 100
first_num = first_num + tile_num_width * tile_size / 2
second_num = -second_num + tile_num_height * tile_size / 2
endPoint = (first_num + tile_size, second_num + tile_size)
dists, path = search.a_star_search(emptyMap, (midX, midY), endPoint,
search.euclidean)
# start GUI and run animation
simulation = DynamicGUI(Tk(), fullMap, emptyMap,
search.segment_path(emptyMap, path), endPoint,
0)
simulation.runSimulation()
if command[0] == 'turn':
endPoint = (tile_num_width * tile_size / 2,
tile_num_height * tile_size / 2)
dists, path = search.a_star_search(emptyMap, (midX, midY), endPoint,
search.euclidean)
# start GUI and run animation
simulation = DynamicGUI(Tk(), fullMap, emptyMap,
search.segment_path(emptyMap, path), endPoint,
(command[1] + 180) % 360)
simulation.runSimulation()
def main():
try:
sm = sys.argv[1].lower()
file_to_read = sys.argv[2]
file_to_write = sys.argv[3]
except IndexError:
print(
"Enter valid command arguments !Usage : python bw.py <method> <problem file> <solution file>"
)
exit(0)
data_folder = os.path.join("input_files")
file_to_open = os.path.join(data_folder, file_to_read)
try:
with open(file_to_open, 'r') as f:
objects, begin_config, goal_config = parse_file(f)
initial_state = BlockState(begin_config, len(begin_config),
objects)
if sm == "breadth":
state, nodes, max_depth, running_time = s.bfs_search(
initial_state, goal_config)
elif sm == "depth":
state, nodes, max_depth, running_time = s.dfs_search(
initial_state, goal_config)
elif sm == "best":
state, nodes, max_depth, running_time = s.best_first_search(
initial_state, goal_config)
elif sm == "astar":
state, nodes, max_depth, running_time = s.a_star_search(
initial_state, goal_config)
else:
print(
"Enter valid command arguments !Usage : python bw.py <method> <problem file> <solution file>"
)
exit(0)
moves = s.calculate_path_to_goal(state)
write_in_file(moves, file_to_write)
print("cost_of_path:", state.cost)
print("nodes_expanded:", nodes)
print("max_search_depth:", max_depth)
print("running_time:", running_time)
valid = s.is_valid(initial_state, moves, goal_config)
if valid:
print('valid_solution: true')
else:
print('valid_solution: false')
except EnvironmentError:
print("File not found!")
def cover_closest_target(self, current_state, target):
"""
:param current_state:
:param target:
:return:
"""
target_cover_problem = BlokusExistingBoardCoverProblem(
current_state, [target])
actions = a_star_search(target_cover_problem, blokus_cover_heuristic)
self.expanded += target_cover_problem.expanded
return actions
def _compute_astar(self, level):
self.level = level
self.world = World(level=self.level)
exit_position, exit_cell = self.level.get_exit()
came_from, cost_so_far, current, n_steps = a_star_search(
graph=WorldGraph(self.world),
start=self.world.init_state,
exit_definition=exit_position,
extract_definition=self.world.get_player_position)
self.path = []
while True:
if current is None:
break
self.path.append(self.world.get_player_position(current))
current = came_from[current]
self.path.reverse()
def BuscaAEstrela(self):
table = self.get_table()
print(table)
initial_state = StateNode(Game8(table), None, None, 0, 0)
start = int(round(time.time() * 1000))
path = a_star_search(initial_state)
end = int(round(time.time() * 1000))
for state in path:
self.depth_label['text'] = 'Profundidade: ' + str(state.depth)
b = state.game.get_b_position()
state.game.table[b[0]][b[1]] = ''
self.set_table(state.game.table)
state.game.show_table()
time.sleep(1)
self.generated_nodes_label['text'] = 'Nós gerados: ' + str(
s.generated_nodes)
self.execution_time['text'] = 'Tempo de execução (ms): ' + str(end -
start)
def calculateFitness(self, individual):
#print("calculating...")
phen = individual.getPhenotype()
world = World(phen.level)
state = world.init_state
exit_position, exit_cell = phen.level.get_exit()
#Calculate the cost of traversing the level
try:
came_from, cost_so_far, current, n_steps = a_star_search(
graph=WorldGraph(world),
start=state,
exit_definition=exit_position,
extract_definition=world.get_player_position)
except OverflowError:
# A* failure.
return 0
return n_steps
def main():
""" main function
"""
from search import a_star_search
# get filename
filename = sys.argv[1]
# read state
state = read_state_from_json(filename)
# test a* running time
from datetime import datetime
# print("#")
# print("# a* start at", datetime.now())
search_res = a_star_search(state)
# print("# a* end at", datetime.now())
# print(len(search_res) - 1)
print("# solution path length =", len(search_res) - 1, "\n#")
# submission output
print_result(search_res, True)
def __init__(self):
"""Create agent."""
self.search_function = lambda prob: (search.a_star_search(
prob, corners_heuristic))
self.search_type = CornersProblem
def __init__(self):
"""Create agent."""
self.search_function = lambda prob: (search.a_star_search(
prob, food_heuristic))
self.search_type = FoodSearchProblem
def main():
execution_times = []
nodes_problem = []
list_files = []
movements = []
all_files = []
data_for_excel = []
while True:
try:
print(
"\nEscoja que tipo de algoritmos desea realizar: "
"\n[D]esinformados: Breadth-First Search & Depth-First Search"
"\n[I]nformados: A-star (3 heuristicas)")
enter = input().lower()
if enter == "d":
busqueda = ["breadth", "depth"]
path = "bd-input_files/"
hoja = "Desinformed"
break
elif enter == "i":
busqueda = ["heuristica_1", "heuristica_2", "ambas"]
path = "astar-input_files/"
hoja = "A-star"
break
except Exception:
print("Solo ingrese una letra, porfavor")
exit(0)
for sm in busqueda:
for file in listdir(path):
# obtener datos iniciales del problema
objects, begin_config, goal_config = parse_file(path + file)
# obtener el estado inicial
initial_state = BlockState(begin_config, len(begin_config),
objects)
# realizar el tipo de busqueda especificado
if sm == "breadth":
state, nodes, running_time = s.bfs_search(
initial_state, goal_config)
elif sm == "depth":
state, nodes, running_time = s.dfs_search(
initial_state, goal_config)
elif enter == "i":
state, nodes, running_time = s.a_star_search(
initial_state, goal_config, sm)
# es valida nuestra solución
moves = s.calculate_path_to_goal(state)
valid = s.is_valid(initial_state, moves, goal_config)
# imprimir resultados en consola del problema
print("\nArchivo: ", file.replace(".pddl", ""))
print("Método de Busqueda: ", sm)
print("Se alcanzó el estado final?: ", "Si" if valid else "No")
print("Cantidad de movimientos (Profundidad maxima recorrida): ",
len(moves))
print("Nodos expandidos: ", nodes)
print("Tiempo de Ejecución: {0:.06f} segundos\n".format(
running_time))
# escribir que movimientos hacer para resolver el problema
write_in_file("solution_files/" + file.replace(".pddl", ".txt"),
moves)
# datos para los plot
list_files.append(file.replace(".pddl", "")[11:])
movements.append(len(moves))
nodes_problem.append(nodes)
execution_times.append(running_time)
# datos para el excel
all_files.append(file.replace(".pddl", "")[11:])
fila = [running_time, nodes, len(moves), sm]
data_for_excel.append(fila)
# imprimimos los tres tipos de plot para cada tipo de busqueda
print_plot(list_files, execution_times, 'Archivo vs Tiempo',
'Tiempo (segundos)')
print_plot(list_files, nodes_problem, 'Archivo vs Espacio',
'Cantidad de nodos')
print_plot(list_files, movements, 'Archivo vs Movimientos',
'Cantidad de Movimientos')
# re-inicializando arreglos
list_files = []
execution_times = []
nodes_problem = []
movements = []
#subir datos a un excel
columna = ["Tiempo", "Nodos", "Movimientos", "Busqueda"]
create_Excel(data_for_excel, all_files, columna, hoja)
# imprimir datos para comparar tipos de busqueda
print_box("A-star", "Tiempo")
print_box("A-star", "Nodos")
print_box("A-star", "Movimientos")
from search import Node, Heuristic, a_star_search, listToString, get_moves
from alpha_sokoban import alpha_sokoban
import sys
import os.path
if __name__ == "__main__":
if len(sys.argv) >= 1:
path_to_file = sys.argv[-1]
if os.path.isfile(path_to_file):
sokoban = alpha_sokoban(path_to_file)
init_node = Node(state=sokoban,
parent=None,
action=None,
g=0,
h=Heuristic(sokoban))
soln_node, nodes_exp = a_star_search(init_node)
if isinstance(soln_node, Node):
moves = get_moves(soln_node)
print(str(len(moves)) + " " + listToString(moves))
else:
print(
"ERROR: Input file {} could not be found".format(path_to_file))
else:
print(
"ERROR: run_search.py requires as a command line argument a path to a sokoban map file (i.e. sokoban01.txt)."
)
def updateGridSmoothed(self):
"""
updates the grid in a smoothed fashion
"""
try:
if self.desired_heading is not None and self.heading == self.desired_heading:
self.output_state = "move forward"
print(self.output_state)
if self.desired_heading is not None and self.heading != self.desired_heading:
if self.heading < self.desired_heading:
self.draw_headings()
cw_turn_degrees = 360 + self.heading - self.desired_heading
ccw_turn_degrees = self.desired_heading - self.heading
else:
self.draw_headings()
cw_turn_degrees = self.heading - self.desired_heading
ccw_turn_degrees = 360 - self.heading + self.desired_heading
if abs(self.desired_heading - self.heading) < turn_speed:
if self.just_turn != 0:
self.draw_headings()
print("C1C0 has turned to face the correct angle")
return
self.heading = self.desired_heading
else:
if cw_turn_degrees < ccw_turn_degrees: # turn clockwise
self.heading = self.heading - turn_speed
print('turn left')
self.output_state = "turn right"
else: # turn counter clockwise
self.heading = self.heading + turn_speed
print('turn right')
self.output_state = "turn left"
if self.heading < 0:
self.heading = 360 + self.heading
elif self.heading >= 360:
self.heading = self.heading - 360
if self.desired_heading == self.heading and self.just_turn != 0:
print("C1C0 has turned to face the correct angle")
return
self.master.after(speed_dynamic, self.updateGridSmoothed)
elif self.initPhase:
curr_tile = self.path[0]
self.curr_tile = curr_tile
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.visitedSet.add(curr_tile)
self.getPathSet()
lidar_data = self.generate_sensor.generateLidar(
degree_freq, curr_tile.row, curr_tile.col)
self.getPathSet()
if (self.gridEmpty.updateGridLidar(curr_tile.x, curr_tile.y,
lidar_data, robot_radius,
bloat_factor, self.pathSet,
self.gridFull)):
self.recalc = True
if self.path is not None and len(self.path) > 1:
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.initPhase = False
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we need to iterate through a brokenPath
elif self.brokenPathIndex < len(self.brokenPath):
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
if (self.gridEmpty.updateGridLidar(self.curr_tile.x,
self.curr_tile.y,
lidar_data, robot_radius,
bloat_factor, self.pathSet,
self.gridFull)):
self.recalc = True
# Relcalculate the path if needed
if self.recalc:
# print('recalculating!')
dists, self.path = search.a_star_search(
self.gridEmpty, (self.curr_tile.x, self.curr_tile.y),
self.endPoint, search.euclidean)
self.path = search.segment_path(self.gridEmpty, self.path)
self.pathIndex = 0
self.smoothed_window.path = self.path
self.smoothed_window.drawPath()
self.draw_line(self.curr_x, self.curr_y, self.path[0].x,
self.path[0].y)
self.curr_tile = self.path[0]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.updateDesiredHeading()
self.getPathSet()
self.visibilityDraw(lidar_data)
self.pathSet = set()
self.getPathSet()
self.pathIndex = 0
self.brokenPathIndex = 0
self.recalc = False
else:
if self.brokenPathIndex == 0:
x1 = self.curr_x
y1 = self.curr_y
else:
x1 = self.brokenPath[self.brokenPathIndex - 1][0]
y1 = self.brokenPath[self.brokenPathIndex - 1][1]
x2 = self.brokenPath[self.brokenPathIndex][0]
y2 = self.brokenPath[self.brokenPathIndex][1]
# MAYBE CHANGE WIDTH TO SEE IF IT LOOKS BETTER?
self.draw_line(x1, y1, x2, y2)
self.curr_x = x2
self.curr_y = y2
self.curr_tile = self.gridEmpty.get_tile((x2, y2))
self.visitedSet.add(self.curr_tile)
self.visibilityDraw(lidar_data)
self.brokenPathIndex += 1
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we have finished iterating through a broken path, we need to go to the
# Next tile in path, and create a new broken path to iterate through
else:
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
if self.curr_tile == self.gridEmpty.get_tile(self.endPoint):
print(
'C1C0 has ARRIVED AT THE DESTINATION, destination tile'
)
return
self.pathSet = set()
self.pathIndex += 1
if self.pathIndex == len(self.path) - 1:
print(
'C1C0 has ARRIVED AT THE DESTINATION, destination tile'
)
return
self.draw_line(self.curr_x, self.curr_y,
self.path[self.pathIndex].x,
self.path[self.pathIndex].y)
self.curr_tile = self.path[self.pathIndex]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[self.pathIndex + 1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.master.after(speed_dynamic, self.updateGridSmoothed)
except Exception as e:
print(e)
print(
"C1C0: \"There is no path to the desired location. Beep Boop\""
)
def _initialize_level(self):
self.game_objects = []
for x in range(self.level_width):
for y in range(self.level_height):
objs = []
cell = self.level.get_cell(x, y)
if type(cell) is EmptyCell:
objs.append(Empty(x, y))
elif type(cell) is BlockCell:
objs.append(Block(x, y))
elif type(cell) is StartPositionCell:
objs.append(Empty(x, y))
obj = Player(x, y)
self.player = obj
elif type(cell) is ExitCell:
obj = Exit(x, y)
self.exit = obj
objs.append(obj)
elif type(cell) is WineCell:
objs.append(Empty(x, y))
objs.append(Wine(x, y))
elif type(cell) is CheeseCell:
objs.append(Empty(x, y))
objs.append(Cheese(x, y))
elif type(cell) is TornadoCell:
objs.append(Empty(x, y))
objs.append(Tornado(x, y))
elif type(cell) is IceCell:
objs.append(Empty(x, y))
objs.append(Ice(x, y))
for obj in objs:
self.game_objects.append(obj)
self.game_objects.append(self.player)
self.world = World(self.level)
self.state = self.world.init_state
# Compute path from start to exit with A*.
exit_position, exit_cell = self.level.get_exit()
came_from, cost_so_far, current, n_steps = a_star_search(
graph=WorldGraph(self.world),
start=self.state,
exit_definition=exit_position,
extract_definition=self.world.get_player_position)
search_path = []
while True:
if current is None:
break
search_path.append(current)
current = came_from[current]
self.search_path = []
for state in search_path:
point = self.world.get_player_position(state)
x = GameEngine.MARGIN_LEFT + point[
0] * GameEngine.CELL_SIZE + GameEngine.CELL_SIZE / 2 + 2
y = GameEngine.MARGIN_TOP + point[
1] * GameEngine.CELL_SIZE + GameEngine.CELL_SIZE / 2 + 2
self.search_path.append([x, y])
trajectory_path = self.trajectory.get_path()
self.trajectory_path = []
for point in trajectory_path:
x = GameEngine.MARGIN_LEFT + point[
0] * GameEngine.CELL_SIZE + GameEngine.CELL_SIZE / 2 - 2
y = GameEngine.MARGIN_TOP + point[
1] * GameEngine.CELL_SIZE + GameEngine.CELL_SIZE / 2 - 2
self.trajectory_path.append([x, y])
# Initialize sprites.
self.sprites = pygame.sprite.Group(self.game_objects)
def create_plan(self):
print("Planning player", self.get_character(),
"certainly is planning something...")
murder_room = self._suspected_triplate[2]
murder_weapon = self._suspected_triplate[1]
murder_character = self._suspected_triplate[0]
# Creating the plan
propositions = self.create_propositions()
actions = set(self.create_actions())
domain_file_name = str(self.game_number) + murder_character.name + "_" + murder_weapon.name \
+ "_" + murder_room.name + "_" + self._character.name + "_domain.txt"
domain_file = open(domain_file_name, 'w')
domain_file.write("Propositions: \n")
for proposition in propositions:
domain_file.write(proposition + " ")
domain_file.write("\nActions: \n")
for action in actions:
domain_file.write(action + "\n")
domain_file.close()
problem_file_name = str(self.game_number) + murder_character.name + "_" + \
murder_weapon.name + "_" + murder_room.name + "_" + self._character.name + "_problem.txt"
problem_file = open(problem_file_name, 'w')
problem_file.write("Initial state: ")
for room in self._unknown_rooms:
problem_file.write(room.name + "_unknown ")
for character in self._unknown_characters:
problem_file.write(character.name + "_unknown ")
for weapon in self._unknown_weapons:
problem_file.write(weapon.name + "_unknown ")
problem_file.write(
"\nGoal state: found_character found_room found_weapon " +
self._suspected_triplate[2].name)
problem_file.close()
# As search problem
plan_problem = PlanningProblem(domain_file_name, problem_file_name)
written_plan = a_star_search(plan_problem, null_heuristic,
self._location)
# print("Planning player ", self.get_character(), "plan found is: ")
# for action in written_plan:
# print(action)
# print(plan_problem.expanded)
# print()
# Delete the files now that we have a plan
os.remove(domain_file_name)
os.remove(problem_file_name)
self._plan = []
for action in written_plan:
if "foundAll" not in action.name:
action_character, action_weapon, action_room, found = \
action.name.split("_unknown_")
self._plan.append(
(Character[action_character], Weapon[action_weapon],
Room[action_room], found))
def __init__(self):
self.search_function = lambda prob: search.a_star_search(
prob, food_heuristic)
self.search_type = FoodSearchProblem
import search
from graph import Graph
if __name__ == "__main__":
# Setting graph we initiated to search class...
graph = Graph()
search.graph = graph
search.depth_first_search()
search.breath_first_search()
search.iterative_deepening_search()
search.uniform_cost_search()
search.greedy_best_first_search()
search.a_star_search()
)
exit()
domain = 'dwrDomain.txt'
problem = 'dwrProblem.txt'
heuristic = null_heuristic
if len(sys.argv) == 4:
domain = str(sys.argv[1])
problem = str(sys.argv[2])
if str(sys.argv[3]) == 'max':
heuristic = max_level
elif str(sys.argv[3]) == 'sum':
heuristic = level_sum
elif str(sys.argv[3]) == 'zero':
heuristic = null_heuristic
else:
print(
"Usage: planning_problem.py domain_name problem_name heuristic_name[max, sum, zero]"
)
exit()
prob = PlanningProblem(domain, problem)
start = time.clock()
plan = a_star_search(prob, heuristic)
elapsed = time.clock() - start
if plan is not None:
print("Plan found with %d actions in %.2f seconds" %
(len(plan), elapsed))
else:
print("Could not find a plan in %.2f seconds" % elapsed)
print("Search nodes expanded: %d" % prob.expanded)
def solve(self):
return a_star_search(self.blokusCoverProblem, blokus_cover_heuristic)
def __init__(self):
self.search_function = lambda prob: search.a_star_search(
prob, corners_heuristic)
self.search_type = CornersProblem
endPos = (100, 100)
GlobalPosition = GPS.GPSThread(1)
MapThread = Map.MapThread(2)
# function to use for search that returns the manahttan distance between two
# points
def manhattan(first, second):
legX = abs(first[0] - second[0])
legY = abs(first[1] - second[1])
return legX + legY
path = search.a_star_search(MapThread.grid, startPos, endPos, manhattan)
MapThread.setPath(path)
# Variable to represent velocity of CICO: float in m/s
# Will need to pull inputs from pins- TEMPORARILY SET TO 1m/s
vel = 1
# Variable to represent heading of CICO: float between 0-360 degrees
# Will need to read from pins
heading = 0
sensorDataTop = []
sensorDataBot = []
lidarData = []
# Command loop to continuosuly run, we will eventually change while loop
