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 alignToWall(wall):
if wall == 0 or wall == "left":
servo_initial = SERVO_LEFT - ALIGN_DELTA / 2
servo_final = SERVO_LEFT + ALIGN_DELTA / 2
elif wall == 1 or wall == "forward":
servo_initial = SERVO_FORWARD - ALIGN_DELTA / 2
servo_final = SERVO_FORWARD + ALIGN_DELTA / 2
else:
servo_initial = SERVO_RIGHT - ALIGN_DELTA / 2
servo_final = SERVO_RIGHT + ALIGN_DELTA / 2
movement.setServo(servo_initial)
time.sleep(0.5)
distance_first = movement.getDistance()
movement.setServo(servo_final)
time.sleep(0.5)
distance_second = movement.getDistance()
if wall == 0 or wall == "left":
b = distance_first
c = distance_second
A = ALIGN_DELTA / 180 * math.pi
a = math.sqrt(b**2 + c**2 - 2 * b * c * math.cos(A))
if b > c:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = -1 * (B - math.pi / 2) + A + (
(servo_initial - SERVO_FORWARD) * math.pi / 180)
else:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = (B - math.pi / 2) + A + (
(servo_initial - SERVO_FORWARD) * math.pi / 180)
return math.pi / 2 - theta
if wall == 1 or wall == "forward":
b = distance_second
c = distance_first
A = ALIGN_DELTA / 180 * math.pi
a = math.sqrt(b**2 + c**2 - 2 * b * c * math.cos(A))
if b > c:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = math.pi / 2 - (B - (
(servo_initial - SERVO_FORWARD) * math.pi / 180))
else:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = (B - (
(servo_initial - SERVO_FORWARD) * math.pi / 180)) - math.pi / 2
return theta
if wall == 2 or wall == "right":
b = distance_first
c = distance_second
A = ALIGN_DELTA / 180 * math.pi
a = math.sqrt(b**2 + c**2 - 2 * b * c * math.cos(A))
if b > c:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = -1 * (B - math.pi / 2) + A + (
(servo_initial - SERVO_FORWARD) * math.pi / 180)
else:
B = math.asin((math.sin(A) * b) / a)
C = (math.pi - B) - A
theta = (B - math.pi / 2) + A + (
(servo_initial - SERVO_FORWARD) * math.pi / 180)
return theta * -1 - math.pi / 2
return 0
def checkWalls(location, maze):
options = []
avgError = 0
errorweight = 0
def set_wall(maze, location, offset, value):
angle = location[2] + offset
angle %= 4
if angle == 0:
maze[(location[0], location[1] + 1, "bottom")]["wall"] = value
return maze[(location[0], location[1] + 1, "bottom")]["visits"]
elif angle == 1:
maze[(location[0], location[1], "left")]["wall"] = value
return maze[(location[0], location[1], "left")]["visits"]
elif angle == 2:
maze[(location[0], location[1], "bottom")]["wall"] = value
return maze[(location[0], location[1], "bottom")]["visits"]
elif angle == 3:
maze[(location[0] + 1, location[1], "left")]["wall"] = value
return maze[(location[0] + 1, location[1], "left")]["visits"]
print("Got invalid angle {}".format(angle))
return False
# Check the right wall
movement.setServo(SERVO_RIGHT, blocking=True)
distance = movement.getDistance()
if distance > GRID_SCAN_SIDE_DISTANCE:
if set_wall(maze, location, -1, True) < 2:
options.append("right")
else:
avgError += calibrate.alignToWall("right")
errorweight += 1
avgError += calibrate.alignToWall("right")
errorweight += 1
set_wall(maze, location, -1, False)
# Check the front wall
movement.setServo(SERVO_FORWARD, blocking=True)
distance = movement.getDistance()
if distance > GRID_SCAN_FORWARD_DISTANCE:
if set_wall(maze, location, 0, True) < 2:
options.append("forward")
else:
avgError += calibrate.alignToWall("forward")
errorweight += 1
avgError += calibrate.alignToWall("forward")
errorweight += 1
set_wall(maze, location, 0, False)
# Check the right wall
movement.setServo(SERVO_LEFT, blocking=True)
distance = movement.getDistance()
if distance > GRID_SCAN_SIDE_DISTANCE:
if set_wall(maze, location, 1, False) < 2:
options.append("left")
else:
avgError += calibrate.alignToWall("left")
errorweight += 1
avgError += calibrate.alignToWall("left")
errorweight += 1
set_wall(maze, location, 1, True)
# If we saw any walls (meaning that direction isn't an option to move) then
# divide the avgError by the number of walls we saw.
if errorweight:
avgError /= errorweight
avgError = calibrate.deg(
avgError) # convert avgError from radians to degrees
return options, avgError
#!/usr/bin/python3
import movement
while True:
angle = int(input(":"))
movement.setServo(angle)
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)