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)