Beispiel #1
0
def turn_to(heading, error=math.radians(4)):
    pid = PID(3, 0.1, 5, -50, 50)
    pid.set_target(heading)
    pid.difference = compass.angleDifference

    h = compass.getHeading()
    while True:
        ret = pid.update(h)
        if ret < 0:
            motors.right(50-ret)
        elif ret > 0:
            motors.left(50+ret)

        if abs(compass.angleDifference(h, heading)) <= error:
            motors.stop()
            time.sleep(0.5)
            if abs(compass.angleDifference(compass.getHeading(), heading)) <= error:
                break

        h = compass.getHeading()
        time.sleep(0.05)
Beispiel #2
0
def turn_to(heading, error=math.radians(4)):
    pid = PID(3, 0.1, 5, -50, 50)
    pid.set_target(heading)
    pid.difference = compass.angleDifference

    h = compass.getHeading()
    while True:
        ret = pid.update(h)
        if ret < 0:
            motors.right(50 - ret)
        elif ret > 0:
            motors.left(50 + ret)

        if abs(compass.angleDifference(h, heading)) <= error:
            motors.stop()
            time.sleep(0.5)
            if abs(compass.angleDifference(compass.getHeading(),
                                           heading)) <= error:
                break

        h = compass.getHeading()
        time.sleep(0.05)
Beispiel #3
0
            last = compass.getHeading()
            distance.start_measuring(count)
            motors.forward()
        elif middle <= 50:
            distance.stop_measuring()
            motors.stop()
            motors.right()
            time.sleep(0.5)
            motors.stop()
            time.sleep(0.1)
            last = compass.getHeading()
            distance.start_measuring(count)
            motors.forward()

        if time.time() - t >= 0.5:
            if abs(compass.angleDifference(compass.getHeading(), last)) >= math.radians(15):
                distance.stop_measuring()
                motors.stop()
                done = True
                logger.join()
                a = compass.getHeading()
                compass.calibrate(3)
                done = False
                logger = threading.Thread(target=supermagiskt)
                logger.start()
                robot.turn_to(a, math.radians(20))
                time.sleep(1)
                distance.start_measuring(count)
                motors.forward()
            t = time.time()
            last = compass.getHeading()
Beispiel #4
0
            last = compass.getHeading()
            distance.start_measuring(count)
            motors.forward()
        elif middle <= 50:
            distance.stop_measuring()
            motors.stop()
            motors.right()
            time.sleep(0.5)
            motors.stop()
            time.sleep(0.1)
            last = compass.getHeading()
            distance.start_measuring(count)
            motors.forward()

        if time.time() - t >= 0.5:
            if abs(compass.angleDifference(compass.getHeading(),
                                           last)) >= math.radians(15):
                distance.stop_measuring()
                motors.stop()
                done = True
                logger.join()
                a = compass.getHeading()
                compass.calibrate(3)
                done = False
                logger = threading.Thread(target=supermagiskt)
                logger.start()
                robot.turn_to(a, math.radians(20))
                time.sleep(1)
                distance.start_measuring(count)
                motors.forward()
            t = time.time()
            last = compass.getHeading()