mindstorms_turing.py

#!/usr/bin/env python3

import enum
import sys
import time
from typing import Tuple

from ev3dev2.motor import OUTPUT_A, OUTPUT_D, LargeMotor, MediumMotor, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import ColorSensor

m_motor = MediumMotor(OUTPUT_D)
motor = LargeMotor(OUTPUT_A)
sensor = ColorSensor(INPUT_1)

NUM_BITS = 15
STEP = 0.122


class Bit(enum.Enum):
    ZERO = 0
    ONE = 1
    END = None

    @classmethod
    def read(cls) -> Tuple[int, "Bit"]:
        color = sensor.color
        time.sleep(0.5)

        bit = Bit.ZERO
        if color == 5:
            bit = Bit.ONE
        elif color == 6:
            bit = Bit.END

        print(color, bit)
        return (color, bit)


def negate(bit: Bit, *args) -> Bit:
    rotations = 0.4
    speed = SpeedPercent(100)
    if bit == Bit.ONE:
        r = rotations
        neg = Bit.ZERO
        m_motor.on_for_rotations(speed, rotations)
        m_motor.on_for_rotations(speed, -rotations)
    elif bit == Bit.ZERO:
        r = -rotations
        neg = Bit.ONE
        m_motor.on_for_rotations(speed, -rotations)
        m_motor.on_for_rotations(speed, rotations)
    else:
        r = rotations
        neg = Bit.END
    time.sleep(0.5)
    m_motor.on_for_rotations(SpeedPercent(100), r / 4)
    return neg


def and_(bit, *args):
    pass


def increment(bits: int) -> None:
    carry = Bit.ONE
    i = 0
    while i < bits:
        (color, bit) = Bit.read()
        motor.on_for_rotations(SpeedPercent(25), STEP)
        if bit == Bit.ONE and (bit.value & carry.value):
            negate(bit)
            carry = Bit.ONE
        elif bit == Bit.ZERO and carry == Bit.ONE:
            negate(bit)
            carry = Bit.ZERO
        else:
            carry = Bit.ZERO

        i += 1
    # motor.on_for_rotations(SpeedPercent(25), -STEP * bits)
    scan_back(bits)


def scan_back(bits: int) -> None:
    print("## SCANNING ##")
    scanned_bits = []
    for a in range(bits):
        (color, bit) = Bit.read()
        scanned_bits.append(bit)
        motor.on_for_rotations(SpeedPercent(25), -STEP)

    (color, bit) = Bit.read()
    scanned_bits.append(bit)
    res = [a.value for a in scanned_bits]
    res_int = int("".join(map(str, res)), 2)
    print(res, res_int)


def move_read(operation=None, bits=20) -> None:
    for a in range(bits):
        (color, bit) = Bit.read()

        if color == 6:
            motor.on_for_rotations(SpeedPercent(50), -STEP * (NUM_BITS + 1))
            print("## REWIND ##")

        motor.on_for_rotations(SpeedPercent(25), STEP)

        if operation and bit != Bit.END:
            operation(bit)


def reset_motors() -> None:
    print("Reset!")
    for mot in range(5):
        try:
            with open(
                "/sys/class/tacho-motor/motor{}/command".format(mot), "w"
            ) as desc:
                desc.write("reset")
        except FileNotFoundError:
            pass


if __name__ == "__main__":
    print(STEP)
    try:
        # move_read(operation=negate)

        bits = int(sys.argv[1])
        print("Bits: ", bits)
        increment(bits)
    except KeyboardInterrupt:
        reset_motors()
    reset_motors()