.. _lib_Linefollower:

Linefollower Examples
*********************

Simple Linefollower
-------------------------

.. code-block:: python

    import time

    from compLib.IRWrapper import IRWrapper
    from compLib.Motor import Motor

    STRAIGHT_SPEED = 40.0
    TURN_SPEED = 40.0

    COLOR_BREAK = 500


    def drive(left, right, active_break=False, back_mult=0.0):
        left = max(min(left, 100), -100)
        right = max(min(right, 100), -100)

        print("left {} right {}".format(left, right))

        Motor.power(0, left)
        Motor.power(1, left * back_mult)
        Motor.power(2, right * back_mult)
        Motor.power(3, right)

        if active_break:
            time.sleep(0.001)

            Motor.power(0, 10)
            Motor.power(1, 10)
            Motor.power(2, 10)
            Motor.power(3, 10)

            time.sleep(0.001)


    def follow_till_line():
        while True:
            print("left: {:.2f}  middle: {:.2f}  right: {:.2f}".format(IRWrapper.bottom_left_calibrated(),
                                                                       IRWrapper.bottom_middle_calibrated(),
                                                                       IRWrapper.bottom_right_calibrated()))

            left = IRWrapper.bottom_left_calibrated()
            right = IRWrapper.bottom_right_calibrated()

            if left > COLOR_BREAK:
                drive(-TURN_SPEED, TURN_SPEED, back_mult=1.25)
                while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
                        IRWrapper.bottom_right_calibrated() < COLOR_BREAK:
                    pass
                drive(0, 0)
            elif right > COLOR_BREAK:
                drive(TURN_SPEED, -TURN_SPEED, back_mult=1.25)
                while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
                        IRWrapper.bottom_left_calibrated() < COLOR_BREAK:
                    pass
                drive(0, 0)
            else:
                drive(STRAIGHT_SPEED, STRAIGHT_SPEED, True)

            if IRWrapper.bottom_left_calibrated() > 750 and IRWrapper.bottom_right_calibrated() > 750:
                break

        print("left: {}  middle: {}  right: {}".format(IRWrapper.bottom_left_calibrated(),
                                                       IRWrapper.bottom_middle_calibrated(),
                                                       IRWrapper.bottom_right_calibrated()))

        drive(0, 0)

        time.sleep(0.2)


    if __name__ == "__main__":
        # Put bot with middle sensor onto black line
        IRWrapper.calibrate()

        # Initialize Motors, so the bot doesn't speed off instantly
        drive(1, 1)
        time.sleep(1)

        # Follow line, till the left and right sensor are on black
        follow_till_line()