#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 pathplan._current_waypoint_number = 0 pathplan.update_current_waypoint(waypoints,x,y)
