Exemplo n.º 1
0
class TRACK3R(RemoteControlledTank):
    """
    Base class for all TRACK3R variations. The only difference in the child
    classes are in how the medium motor is handled.

    To enable the medium motor toggle the beacon button on the EV3 remote.
    """
    def __init__(self,
                 medium_motor,
                 left_motor,
                 right_motor,
                 speed_sp=400,
                 channel=1):
        RemoteControlledTank.__init__(self,
                                      left_motor,
                                      right_motor,
                                      speed_sp=speed_sp,
                                      channel=channel)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 2
0
    def __init__(self, medium_motor=OUTPUT_A, left_motor=OUTPUT_C, right_motor=OUTPUT_B):
        WebControlledTank.__init__(self, left_motor, right_motor)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 3
0
class EV3D4WebControlled(WebControlledTank):

    def __init__(self, medium_motor=OUTPUT_A, left_motor=OUTPUT_C, right_motor=OUTPUT_B):
        WebControlledTank.__init__(self, left_motor, right_motor)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 4
0
 def __init__(self):
     self.shutdown = False
     self.flipper = LargeMotor(OUTPUT_A)
     self.turntable = LargeMotor(OUTPUT_B)
     self.colorarm = MediumMotor(OUTPUT_C)
     self.color_sensor = ColorSensor()
     self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
     self.infrared_sensor = InfraredSensor()
     self.cube = {}
     self.init_motors()
     self.state = ['U', 'D', 'F', 'L', 'B', 'R']
     self.rgb_solver = None
     signal.signal(signal.SIGTERM, self.signal_term_handler)
     signal.signal(signal.SIGINT, self.signal_int_handler)
Exemplo n.º 5
0
class TRACK3R(RemoteControlledTank):
    """
    Base class for all TRACK3R variations. The only difference in the child
    classes are in how the medium motor is handled.

    To enable the medium motor toggle the beacon button on the EV3 remote.
    """

    def __init__(self, medium_motor, left_motor, right_motor):
        RemoteControlledTank.__init__(self, left_motor, right_motor)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 6
0
    def __init__(self,
                 medium_motor,
                 left_motor,
                 right_motor,
                 speed_sp=400,
                 channel=1):
        RemoteControlledTank.__init__(self,
                                      left_motor,
                                      right_motor,
                                      speed_sp=speed_sp,
                                      channel=channel)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 7
0
    def __init__(self, medium_motor, left_motor, right_motor):
        RemoteControlledTank.__init__(self, left_motor, right_motor)
        self.medium_motor = MediumMotor(medium_motor)

        if not self.medium_motor.connected:
            log.error("%s is not connected" % self.medium_motor)
            sys.exit(1)

        self.medium_motor.reset()
Exemplo n.º 8
0
    To enable the medium motor toggle the beacon button on the EV3 remote.
    """
    def __init__(self, left_motor, right_motor, speed_sp=400, channel=1):
        RemoteControlledTank.__init__(self,
                                      left_motor,
                                      right_motor,
                                      speed_sp=speed_sp,
                                      channel=channel)


# assuming we start with both pens up
pen_shifts = [-30, +30, +30, -30]
pen_state = len(pen_shifts) - 1  # the last pen state
# from the even position, -30 is left pen down and +30 is right pen down
medmotor = MediumMotor(OUTPUT_A)
medmotor.reset()  # set motor position to 0


class TRACK3RWithPen(TRACK3R):
    def __init__(self,
                 medium_motor,
                 left_motor=OUTPUT_B,
                 right_motor=OUTPUT_C,
                 speed_sp=400,
                 channel=1):
        TRACK3R.__init__(self,
                         left_motor,
                         right_motor,
                         speed_sp=speed_sp,
                         channel=channel)
Exemplo n.º 9
0
class Rubiks(object):
    scan_order = [
        5, 9, 6, 3, 2, 1, 4, 7, 8,
        23, 27, 24, 21, 20, 19, 22, 25, 26,
        50, 54, 51, 48, 47, 46, 49, 52, 53,
        14, 10, 13, 16, 17, 18, 15, 12, 11,
        41, 43, 44, 45, 42, 39, 38, 37, 40,
        32, 34, 35, 36, 33, 30, 29, 28, 31]

    hold_cube_pos = 85
    rotate_speed = 400
    flip_speed = 300
    flip_speed_push = 400
    corner_to_edge_diff = 60

    def __init__(self):
        self.shutdown = False
        self.flipper = LargeMotor(OUTPUT_A)
        self.turntable = LargeMotor(OUTPUT_B)
        self.colorarm = MediumMotor(OUTPUT_C)
        self.color_sensor = ColorSensor()
        self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
        self.infrared_sensor = InfraredSensor()
        self.cube = {}
        self.init_motors()
        self.state = ['U', 'D', 'F', 'L', 'B', 'R']
        self.rgb_solver = None
        signal.signal(signal.SIGTERM, self.signal_term_handler)
        signal.signal(signal.SIGINT, self.signal_int_handler)

    def init_motors(self):

        for x in (self.flipper, self.turntable, self.colorarm):
            if not x.connected:
                log.error("%s is not connected" % x)
                sys.exit(1)
            x.reset()

        log.info("Initialize flipper %s" % self.flipper)
        self.flipper.run_forever(speed_sp=-50, stop_action='hold')
        self.flipper.wait_for_stop()
        self.flipper.stop()
        self.flipper.reset()
        self.flipper.stop(stop_action='hold')

        log.info("Initialize colorarm %s" % self.colorarm)
        self.colorarm.run_forever(speed_sp=500, stop_action='hold')
        self.colorarm.wait_for_stop()
        self.colorarm.stop()
        self.colorarm.reset()
        self.colorarm.stop(stop_action='hold')

        log.info("Initialize turntable %s" % self.turntable)
        self.turntable.reset()
        self.turntable.stop(stop_action='hold')

    def shutdown_robot(self):
        log.info('Shutting down')
        self.shutdown = True

        if self.rgb_solver:
            self.rgb_solver.shutdown = True

        for x in (self.flipper, self.turntable, self.colorarm):
            x.shutdown = True

        for x in (self.flipper, self.turntable, self.colorarm):
            x.stop(stop_action='brake')

    def signal_term_handler(self, signal, frame):
        log.error('Caught SIGTERM')
        self.shutdown_robot()

    def signal_int_handler(self, signal, frame):
        log.error('Caught SIGINT')
        self.shutdown_robot()

    def apply_transformation(self, transformation):
        self.state = [self.state[t] for t in transformation]

    def rotate_cube(self, direction, nb):
        current_pos = self.turntable.position
        final_pos = 135 * round((self.turntable.position + (270 * direction * nb)) / 135.0)
        log.info("rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" % (direction, nb, current_pos, final_pos))

        if self.flipper.position > 35:
            self.flipper_away()

        self.turntable.run_to_abs_pos(position_sp=final_pos,
                                      speed_sp=Rubiks.rotate_speed,
                                      stop_action='hold',
                                      ramp_up_sp=0)
        self.turntable.wait_for_running()
        self.turntable.wait_for_position(final_pos)
        self.turntable.wait_for_stop()

        if nb >= 1:
            for i in range(nb):
                if direction > 0:
                    transformation = [0, 1, 5, 2, 3, 4]
                else:
                    transformation = [0, 1, 3, 4, 5, 2]
                self.apply_transformation(transformation)

    def rotate_cube_1(self):
        self.rotate_cube(1, 1)

    def rotate_cube_2(self):
        self.rotate_cube(1, 2)

    def rotate_cube_3(self):
        self.rotate_cube(-1, 1)

    def rotate_cube_blocked(self, direction, nb):

        # Move the arm down to hold the cube in place
        self.flipper_hold_cube()

        # OVERROTATE depends on lot on Rubiks.rotate_speed
        current_pos = self.turntable.position
        OVERROTATE = 18
        final_pos = int(135 * round((current_pos + (270 * direction * nb)) / 135.0))
        temp_pos = int(final_pos + (OVERROTATE * direction))

        log.info("rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s" %
                 (direction, nb, current_pos, temp_pos, final_pos))

        self.turntable.run_to_abs_pos(position_sp=temp_pos,
                                      speed_sp=Rubiks.rotate_speed,
                                      stop_action='hold',
                                      ramp_up_sp=0)
        self.turntable.wait_for_running()
        self.turntable.wait_for_position(temp_pos)
        self.turntable.wait_for_stop()

        self.turntable.run_to_abs_pos(position_sp=final_pos,
                                      speed_sp=int(Rubiks.rotate_speed/4),
                                      stop_action='hold',
                                      ramp_up_sp=0)
        self.turntable.wait_for_running()
        self.turntable.wait_for_position(final_pos, stall_ok=True)
        self.turntable.wait_for_stop()

    def rotate_cube_blocked_1(self):
        self.rotate_cube_blocked(1, 1)

    def rotate_cube_blocked_2(self):
        self.rotate_cube_blocked(1, 2)

    def rotate_cube_blocked_3(self):
        self.rotate_cube_blocked(-1, 1)

    def flipper_hold_cube(self, speed=300):
        current_position = self.flipper.position

        # Push it forward so the cube is always in the same position
        # when we start the flip
        if (current_position <= Rubiks.hold_cube_pos - 10 or
            current_position >= Rubiks.hold_cube_pos + 10):
            self.flipper.run_to_abs_pos(position_sp=Rubiks.hold_cube_pos,
                                        ramp_down_sp=400,
                                        speed_sp=speed)
            self.flipper.wait_for_running()
            self.flipper.wait_for_position(Rubiks.hold_cube_pos)
            self.flipper.wait_for_stop()
            sleep(0.05)

    def flipper_away(self, speed=300):
        """
        Move the flipper arm out of the way
        """
        log.info("flipper_away()")
        self.flipper.run_to_abs_pos(position_sp=0,
                                    ramp_down_sp=400,
                                    speed_sp=speed)
        self.flipper.wait_for_running()

        try:
            self.flipper.wait_for_position(0)
            self.flipper.wait_for_stop()
        except MotorStall:
            self.flipper.stop()

    def flip(self):
        """
        Motors will sometimes stall if you call run_to_abs_pos() multiple
        times back to back on the same motor. To avoid this we call a 50ms
        sleep in flipper_hold_cube() and after each run_to_abs_pos() below.

        We have to sleep after the 2nd run_to_abs_pos() because sometimes
        flip() is called back to back.
        """
        log.info("flip()")

        if self.shutdown:
            return

        # Move the arm down to hold the cube in place
        self.flipper_hold_cube()

        # Grab the cube and pull back
        self.flipper.run_to_abs_pos(position_sp=190,
                                    ramp_up_sp=200,
                                    ramp_down_sp=0,
                                    speed_sp=self.flip_speed)
        self.flipper.wait_for_running()
        self.flipper.wait_for_position(190)
        self.flipper.wait_for_stop()
        sleep(0.05)

        # At this point the cube is at an angle, push it forward to
        # drop it back down in the turntable
        self.flipper.run_to_abs_pos(position_sp=Rubiks.hold_cube_pos,
                                    ramp_up_sp=200,
                                    ramp_down_sp=400,
                                    speed_sp=self.flip_speed_push)
        self.flipper.wait_for_running()
        self.flipper.wait_for_position(Rubiks.hold_cube_pos)
        self.flipper.wait_for_stop()
        sleep(0.05)

        transformation = [2, 4, 1, 3, 0, 5]
        self.apply_transformation(transformation)

    def colorarm_middle(self):
        log.info("colorarm_middle()")
        self.colorarm.run_to_abs_pos(position_sp=-750,
                                     speed_sp=600,
                                     stop_action='hold')
        self.colorarm.wait_for_running()
        self.colorarm.wait_for_position(-750)
        self.colorarm.wait_for_stop()

    def colorarm_corner(self, square_index):
        log.info("colorarm_corner(%d)" % square_index)
        position_target = -580

        if square_index == 1:
            position_target += 20
        elif square_index == 3:
            pass
        elif square_index == 5:
            position_target -= 20
        elif square_index == 7:
            pass
        else:
            raise ScanError("colorarm_corner was given unsupported square_index %d" % square_index)

        self.colorarm.run_to_abs_pos(position_sp=position_target,
                                     speed_sp=600,
                                     stop_action='hold')

    def colorarm_edge(self, square_index):
        log.info("colorarm_edge(%d)" % square_index)
        position_target = -640

        if square_index == 2:
            pass
        elif square_index == 4:
            position_target -= 20
        elif square_index == 6:
            position_target -= 20
        elif square_index == 8:
            pass
        else:
            raise ScanError("colorarm_edge was given unsupported square_index %d" % square_index)

        self.colorarm.run_to_abs_pos(position_sp=position_target,
                                     speed_sp=600,
                                     stop_action='hold')

    def colorarm_remove(self):
        log.info("colorarm_remove()")
        self.colorarm.run_to_abs_pos(position_sp=0,
                                     speed_sp=600)
        self.colorarm.wait_for_running()
        try:
            self.colorarm.wait_for_position(0)
            self.colorarm.wait_for_stop()
        except MotorStall:
            self.colorarm.stop()

    def colorarm_remove_halfway(self):
        log.info("colorarm_remove_halfway()")
        self.colorarm.run_to_abs_pos(position_sp=-400,
                                     speed_sp=600)
        self.colorarm.wait_for_running()
        self.colorarm.wait_for_position(-400)
        self.colorarm.wait_for_stop()

    def scan_middle(self, face_number):
        log.info("scan_middle() %d/6" % face_number)

        if self.flipper.position > 35:
            self.flipper_away()

        self.colorarm_middle()
        log.info(self.color_sensor.rgb())
        self.colorarm_remove_halfway()

    def scan_middles(self):
        """
        Used once to get the RGB values of the middle squares to
        populate the crayola_colors in rubiks_rgb_solver.py
        """
        log.info("scan_middle()")
        self.colors = {}
        self.k = 0
        self.scan_middle(1)
        raw_input('Paused')

        self.flip()
        self.scan_middle(2)
        raw_input('Paused')

        self.flip()
        self.scan_middle(3)
        raw_input('Paused')

        self.rotate_cube(-1, 1)
        self.flip()
        self.scan_middle(4)
        raw_input('Paused')

        self.rotate_cube(1, 1)
        self.flip()
        self.scan_middle(5)
        raw_input('Paused')

        self.flip()
        self.scan_middle(6)
        raw_input('Paused')

    def scan_face(self, face_number):
        log.info("scan_face() %d/6" % face_number)

        if self.shutdown:
            return

        if self.flipper.position > 35:
            self.flipper_away()

        self.colorarm_middle()
        self.colors[int(Rubiks.scan_order[self.k])] = tuple(self.color_sensor.rgb())

        self.k += 1
        i = 1
        self.colorarm_corner(i)

        # The gear ratio is 3:1 so 1080 is one full rotation
        self.turntable.reset()
        self.turntable.run_to_abs_pos(position_sp=1080,
                                      speed_sp=Rubiks.rotate_speed,
                                      stop_action='hold')

        while True:
            current_position = self.turntable.position

            # 135 is 1/8 of full rotation
            if current_position >= (i * 135):
                current_color = tuple(self.color_sensor.rgb())
                self.colors[int(Rubiks.scan_order[self.k])] = current_color
                # log.info("%s: i %d, current_position %d, current_color %s" %
                #          (self.turntable, i, current_position, current_color))

                i += 1
                self.k += 1

                if i == 9:
                    # Last face, move the color arm all the way out of the way
                    if face_number == 6:
                        self.colorarm_remove()

                    # Move the color arm far enough away so that the flipper
                    # arm doesn't hit it
                    else:
                        self.colorarm_remove_halfway()

                    break

                elif i % 2:
                    self.colorarm_corner(i)
                else:
                    self.colorarm_edge(i)

            if self.shutdown:
                return

        if i < 9:
            raise ScanError('i is %d..should be 9' % i)

        self.turntable.wait_for_position(1080)
        self.turntable.stop()
        self.turntable.reset()
        log.info("\n")

    def scan(self):
        log.info("scan()")
        self.colors = {}
        self.k = 0
        self.scan_face(1)

        self.flip()
        self.scan_face(2)

        self.flip()
        self.scan_face(3)

        self.rotate_cube(-1, 1)
        self.flip()
        self.scan_face(4)

        self.rotate_cube(1, 1)
        self.flip()
        self.scan_face(5)

        self.flip()
        self.scan_face(6)

        if self.shutdown:
            return

        log.info("RGB json:\n%s\n" % json.dumps(self.colors))
        self.rgb_solver = RubiksColorSolverGeneric(3)
        self.rgb_solver.enter_scan_data(self.colors)
        self.rgb_solver.crunch_colors()
        self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
        log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))

        # This is only used if you want to rotate the cube so U is on top, F is
        # in the front, etc. You would do this if you were troubleshooting color
        # detection and you want to pause to compare the color pattern on the
        # cube vs. what we think the color pattern is.
        '''
        log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
        self.rotate_cube(-1, 1)
        self.flip()
        self.flipper_away()
        self.rotate_cube(1, 1)
        raw_input('Paused')
        '''

    def move(self, face_down):
        log.info("move() face_down %s" % face_down)

        position = self.state.index(face_down)
        actions = {
            0: ["flip", "flip"],
            1: [],
            2: ["rotate_cube_2", "flip"],
            3: ["rotate_cube_1", "flip"],
            4: ["flip"],
            5: ["rotate_cube_3", "flip"]
        }.get(position, None)

        for a in actions:

            if self.shutdown:
                break

            getattr(self, a)()

    def run_kociemba_actions(self, actions):
        log.info('Action (kociemba): %s' % ' '.join(actions))
        total_actions = len(actions)

        for (i, a) in enumerate(actions):

            if self.shutdown:
                break

            if a.endswith("'"):
                face_down = list(a)[0]
                rotation_dir = 1
            elif a.endswith("2"):
                face_down = list(a)[0]
                rotation_dir = 2
            else:
                face_down = a
                rotation_dir = 3

            log.info("Move %d/%d: %s%s (a %s)" % (i, total_actions, face_down, rotation_dir, pformat(a)))
            self.move(face_down)

            if rotation_dir == 1:
                self.rotate_cube_blocked_1()
            elif rotation_dir == 2:
                self.rotate_cube_blocked_2()
            elif rotation_dir == 3:
                self.rotate_cube_blocked_3()
            log.info("\n")

    def resolve(self):

        if rub.shutdown:
            return

        output = check_output(['kociemba', ''.join(map(str, self.cube_kociemba))]).decode('ascii')
        actions = output.strip().split()
        self.run_kociemba_actions(actions)
        self.cube_done()

    def cube_done(self):
        self.flipper_away()

    def wait_for_cube_insert(self):
        rubiks_present = 0
        rubiks_present_target = 10
        log.info('wait for cube...to be inserted')

        while True:

            if self.shutdown:
                break

            dist = self.infrared_sensor.proximity

            # It is odd but sometimes when the cube is inserted
            # the IR sensor returns a value of 100...most of the
            # time it is just a value less than 50
            if dist < 50 or dist == 100:
                rubiks_present += 1
                log.info("wait for cube...distance %d, present for %d/%d" %
                         (dist, rubiks_present, rubiks_present_target))
            else:
                if rubiks_present:
                    log.info('wait for cube...cube removed (%d)' % dist)
                rubiks_present = 0

            if rubiks_present >= rubiks_present_target:
                log.info('wait for cube...cube found and stable')
                break

            time.sleep(0.1)