grid.walls = initial_grid #Define the starting point of the robot #start = (2,1) start = (17,8) #while wrapper.dest is -1: # time.sleep(1) #print wrapper.dest #finish = wrapper.dest finish = (19,8) #Use a*star to plan the path arrows, cost = pathplan.a_star_search(grid,start,finish) pathplan.print_cost_grid_float(width,height,cost,finish) star_path = pathplan.get_star_path(grid,cost,start,finish) pathplan.print_path(grid,star_path,width,height,start,finish) waypoints = pathplan.set_waypoints(grid,star_path,start,finish) kalfilter = KalmanFilter(start[0]*GRID_SIZE+(GRID_SIZE/2),start[1]*GRID_SIZE+(GRID_SIZE/2),math.pi/2) kalfilter.GRID_SIZE = GRID_SIZE pathplan.GRID_SIZE = GRID_SIZE time.sleep(1) #Wait a second for everything to wake up print 'Starting Up!!!' prior_time = time.clock() #kalfilter.plot_robot(grid.walls,waypoints) delay_time = .001 first = True second = True third = False old_finish = finish while(True): old_finish = finish
