#kalfilter.plot_robot(grid.walls,waypoints) #Now that we know where we are we need to steer now pathplan.update_current_waypoint(waypoints,x,y) #Get the robot outputs vel, ang_vel = pathplan.get_outputs(x,y,theta) if(vel > .5 or ang_vel > math.pi/3): print 'Velocity too high!!' else: if(colorseg_debug): botcontrol.drive(0,0) else: botcontrol.drive(vel,ang_vel) #Set robot velocity and angular velocity here if(pathplan.reached_end(x,y)): while(finish == old_finish): botcontrol.stop_robot() ###TODO Enter in code to get new finish here!!! finish = wrapper.dest #### if(old_finish != finish): start = (current_x,current_y) start = grid.find_nearest_neighbor(start) arrows, cost = pathplan.a_star_search(grid,start,finish) star_path = pathplan.get_star_path(grid,cost,start,finish) waypoints = pathplan.set_waypoints(grid,star_path,start,finish) #pathplan._zero_turn = True pathplan.new_path = True
# kalfilter.plot_robot(grid.walls,waypoints)
# Now that we know where we are we need to steer now
pathplan.update_current_waypoint(waypoints, x, y)
# Get the robot outputs
vel, ang_vel = pathplan.get_outputs(x, y, theta)
if vel > 0.5 or ang_vel > math.pi / 3:
print "Velocity too high!!"
else:
if colorseg_debug:
botcontrol.drive(0, 0)
else:
botcontrol.drive(vel, ang_vel)
# Set robot velocity and angular velocity here
if pathplan.reached_end(x, y):
while finish == old_finish:
botcontrol.stop_robot()
###TODO Enter in code to get new finish here!!!
finish = wrapper.dest
####
if old_finish != finish:
start = (current_x, current_y)
start = grid.find_nearest_neighbor(start)
arrows, cost = pathplan.a_star_search(grid, start, finish)
star_path = pathplan.get_star_path(grid, cost, start, finish)
waypoints = pathplan.set_waypoints(grid, star_path, start, finish)
# pathplan._zero_turn = True
pathplan.new_path = True
