def get_nearest_obstacle(): obstacle_range = make_sweep() nearest_obstacle_index = numpy.argmin(obstacle_range) if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT): no_obstacles_found = True else: no_obstacles_found = False #Look for an obstacle search_angle = 0 while no_obstacles_found: # Turn clockwise by the step degrees to check there search_angle += SEARCH_LANDMARK_STEP MoveRobot.turn_in_place(SEARCH_LANDMARK_STEP) # Look for obstacles obstacle_range = make_sweep() nearest_obstacle_index = numpy.argmin(obstacle_range) if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT): no_obstacles_found = True else: no_obstacles_found = False # if no obstacle found anywhere, move forward and search again if no_obstacles_found and search_angle >= MoveRobot.FULL_REVOLUTION_DEGREES : search_angle = 0 print("Going to move ", MoveRobot.ROBOT_LENGTH_CM) MoveRobot.go_forward(MoveRobot.ROBOT_LENGTH_CM) return get_angle_to_obstacle(nearest_obstacle_index), obstacle_range[nearest_obstacle_index]
def get_nearest_obstacle(): obstacle_range = make_sweep() nearest_obstacle_index = numpy.argmin(obstacle_range) if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT): no_obstacles_found = True else: no_obstacles_found = False #Look for an obstacle search_angle = 0 while no_obstacles_found: # Turn clockwise by the step degrees to check there search_angle += SEARCH_LANDMARK_STEP MoveRobot.turn_in_place(SEARCH_LANDMARK_STEP) # Look for obstacles obstacle_range = make_sweep() nearest_obstacle_index = numpy.argmin(obstacle_range) if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT): no_obstacles_found = True else: no_obstacles_found = False # if no obstacle found anywhere, move forward and search again if no_obstacles_found and search_angle >= MoveRobot.FULL_REVOLUTION_DEGREES: search_angle = 0 print("Going to move ", MoveRobot.ROBOT_LENGTH_CM) MoveRobot.go_forward(MoveRobot.ROBOT_LENGTH_CM) return get_angle_to_obstacle( nearest_obstacle_index), obstacle_range[nearest_obstacle_index]
def calibrate_move(): count = 0 while (True): count += 1 if count > NUMBER_OF_ITERATIONS: break MoveRobot.go_forward(MoveRobot.ROBOT_LENGTH_CM) raw_input("Press Enter to continue")
def calibrate_turn(): angle = 5 count = 0 while (True): count += 1 if count > NUMBER_OF_ITERATIONS: break print("Going to turn ", angle, " degrees.") MoveRobot.turn_in_place(angle) angle += 5 raw_input("Press Enter to continue")
import MoveRobot import RobotPosePrediction import SenseLandmarks import gopigo import time import numpy #MoveRobot.go_towards_nearest_obstacle() #MoveRobot.go_forward(22.86) #MoveRobot.go_backwards(100) MoveRobot.go_towards_nearest_obstacle() print("Pose is now ", RobotPosePrediction.currentRobotPose) print("Uncertainty is now ", RobotPosePrediction.currentRobotLocationUncertainty) print("Voltage ", gopigo.volt())