def test():
while True:
error = deg(alignToWall(2))
movement.setServo(0, blocking=True)
dist = movement.getDistance()
print("Error: {} Dist: {}".format(error, dist))
movement.turn(-1 * error, blocking=True)
movement.driveForward(100, blocking=True)
def face(x):
global angle
print("turning to" + str(x) + "from" + str(angle))
print(str(x - angle) + "degree turn")
movement.turn(300, x - angle)
angle = x
def main():
while input("Press e to exit:") != "e":
movement.driveForward(1000, blocking=True)
time.sleep(0.5)
movement.turn(90, blocking=True)
time.sleep(0.5)
movement.driveForward(1000, blocking=True)
time.sleep(0.5)
movement.turn(90, blocking=True)
time.sleep(0.5)
movement.driveForward(1000, blocking=True)
time.sleep(0.5)
movement.turn(90, blocking=True)
time.sleep(0.5)
movement.driveForward(1000, blocking=True)
time.sleep(0.5)
movement.turn(90, blocking=True)
carryOn = True
while carryOn:
for event in pygame.event.get():
if event.type == pygame.QUIT:
carryOn = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_q:
carryOn = False
keys = pygame.key.get_pressed()
if keys[pygame.K_LEFT]:
try:
movement.read_laser_scan_data()
movement.turn()
except rospy.ROSInterruptException:
pass
elif keys[pygame.K_RIGHT]:
try:
movement.read_laser_scan_data()
movement.turn(-0.75)
except rospy.ROSInterruptException:
pass
elif keys[pygame.K_UP]:
try:
movement.read_laser_scan_data()
movement.move()
except rospy.ROSInterruptException:
pass
elif keys[pygame.K_DOWN]:
def relativeTurn(x):
global angle
movement.turn(300, x)
angle += x
def main():
maze = {
(0, 0, "bottom"): {
"wall": True,
"visits": 1
},
(0, 0, "left"): {
"wall": True,
"visits": 1
},
(0, 1, "bottom"): {
"wall": False,
"visits": 0
},
(0, 1, "left"): {
"wall": None,
"visits": 0
},
(1, 0, "bottom"): {
"wall": None,
"visits": 0
},
(1, 0, "left"): {
"wall": True,
"visits": 0
},
}
location = [0, 0, 0]
while True:
print(maze)
# Look into the next cell to figure out what moves will be available, and how
# well we are aligned to its walls
options, avgError = checkWalls(location, maze)
# If we are not parallel to the next cell then adjust our heading before we go forward
if avgError:
print("Measured {} degree error.".format(avgError))
if abs(
avgError
) > 15: # Really big errors were probably either found too late, or measured wrong
print("Error is strangely large..."
) # If you see this then something likely went wrong.
else:
time.sleep(0.5)
# Rotate in the opposite direction of the measured error
movement.turn(-1 * avgError, blocking=True)
time.sleep(0.5)
# If any walls are open in the next cell then pick which path we are taking
if options:
choice = options[0]
else: # If no walls are open then we will turn around
choice = "turn_around"
# If we have more than one option then we had a decision to make
decision_point = len(options) > 1
if decision_point:
print("Decision point found. Options: {}".format(
", ".join(options)))
print("DECISION: Let's go {}.".format(choice))
time.sleep(0.5)
# Point the servo forward to detect a wall in front of us in case we overshoot while moving
movement.setServo(SERVO_FORWARD, blocking=True)
# If you use driveForward in blocking mode then it will check the distance sensor while moving
# and abort early if it sees something too close.
movement.driveForward(GRID_MOVE_DISTANCE, blocking=True)
if location[2] == 0:
location[1] += 1
elif location[2] == 1:
location[0] -= 1
elif location[2] == 2:
location[1] -= 1
elif location[2] == 3:
location[0] += 1
else:
sys.exit("Illegal orientation {}".format(location[2]))
if not ((location[0], location[1], "bottom")) in maze:
maze[(location[0], location[1], "bottom")] = {
"wall": None,
"visits": 0
}
if not ((location[0], location[1], "left")) in maze:
maze[(location[0], location[1], "left")] = {
"wall": None,
"visits": 0
}
if not ((location[0], location[1] + 1, "bottom")) in maze:
maze[(location[0], location[1] + 1, "bottom")] = {
"wall": None,
"visits": 0
}
if not ((location[0] + 1, location[1], "left")) in maze:
maze[(location[0] + 1, location[1], "left")] = {
"wall": None,
"visits": 0
}
maze[(location[0], location[1], "bottom")]["visits"] += 1
maze[(location[0], location[1], "left")]["visits"] += 1
time.sleep(0.5)
# Now that we are in the next cell we can turn wherever we decided.
if choice == "turn_around":
movement.turn(180, blocking=True)
location[2] += 2
location[2] %= 4
elif choice == "left":
movement.turn(90)
location[2] -= 1
location[2] %= 4
elif choice == "right":
movement.turn(-90)
location[2] -= 1
location[2] %= 4
elif choice == "forward":
pass
else:
# This really shouldn't happen.
sys.exit("Unknown choice: {}".format(choice))
time.sleep(0.5)
#!/usr/bin/python3 import movement movement.turn(360)
duration = float(raw_input("Please enter a valid duration (>0):"))
direction = check_valid_direction(direction)
start_time = time()
if (direction == 1):
while time() < start_time + duration:
movement.move(speed)
movement.read_laser_scan_data()
elif (direction == 2):
while time() < start_time + duration:
movement.move(-1*speed)
movement.read_laser_scan_data()
elif (direction == 3):
while time() < start_time + duration:
movement.turn(-1*speed)
movement.read_laser_scan_data()
elif (direction == 4):
while time() < start_time + duration:
movement.turn(speed)
movement.read_laser_scan_data()
movement.stop()
answer = raw_input("Continue?")
while answer != "yes" and answer != "no":
answer = raw_input("Please enter yes/no:")
if answer == "no":