Final section of main.py

main.py

DISTANCE_THRESHOLD_CM = 0
while True:
    # all coordinates are in cm
    DISTANCE_THRESHOLD_CM += 10
    origin = Point(0,0)
    curr_pos = origin
    curr_orientation = Point(0,1)
    corners = []
    while True:
        if is_there_obstacle(distance_sensor_R, DISTANCE_THRESHOLD_CM):
            turn_90_deg_ccw(servo_L, servo_R, WHEEL_DISTANCE_CM)
            curr_orientation = curr_orientation.rotate(PI/2) # Counter-clockwise
        else:
            turn_90_deg_cw(servo_L, servo_R, WHEEL_DISTANCE_CM)
            curr_orientation = curr_orientation.rotate(-PI/2) # Counter-clockwise
        x = distance_cm_along_wall_till_broken(servo_L,servo_R,
                                               distance_sensor_R,
                                               distance_sensor_F,
                                               DISTANCE_THRESHOLD_CM)
        if curr_pos.distance_to(origin) < DISTANCE_THRESHOLD_CM:
            break
        curr_pos += curr_orientation*x
        corners  += [curr_pos]
        print("Corner detected:"+curr_pos)

    print("The detected corners are:")
    for p in corners:
        print(p)