pathplan.new_path = True pathplan._current_waypoint_number = 0 pathplan.update_current_waypoint(waypoints,x,y) pathplan.print_path(grid,star_path,width,height,start,finish) #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)
