Example #1
0
class EV3D4RemoteControlled(RemoteControlledTank):
    def __init__(self,
                 medium_motor=OUTPUT_A,
                 left_motor=OUTPUT_C,
                 right_motor=OUTPUT_B):
        RemoteControlledTank.__init__(self, left_motor, right_motor)
        self.medium_motor = MediumMotor(medium_motor)
        self.medium_motor.reset()
Example #2
0
def doLifter():
    """Test code for operating lifter"""

    lifter = MediumMotor()
    lifter.reset()
    lifter.run_to_rel_pos(position_sp=60, speed_sp=100, stop_action='hold')
    lifter.wait_while('running')
    sleep(1)
    lifter.run_to_rel_pos(position_sp=-60, speed_sp=100, stop_action='hold')
    lifter.wait_while('running')
class El3ctricGuitar:
    NOTES = [1318, 1174, 987, 880, 783, 659, 587, 493, 440, 392, 329, 293]
    N_NOTES = len(NOTES)

    def __init__(
            self, lever_motor_port: str = OUTPUT_D,
            touch_sensor_port: str = INPUT_1, ir_sensor_port: str = INPUT_4):
        self.lever_motor = MediumMotor(address=lever_motor_port)

        self.touch_sensor = TouchSensor(address=touch_sensor_port)

        self.ir_sensor = InfraredSensor(address=ir_sensor_port)
        
        self.leds = Leds()

        self.speaker = Sound()

    def start_up(self):
        self.leds.animate_flash(
            color='ORANGE',
            groups=('LEFT', 'RIGHT'),
            sleeptime=0.5,
            duration=3,
            block=True)

        self.lever_motor.on_for_seconds(
            speed=5,
            seconds=1,
            brake=False,
            block=True)

        self.lever_motor.on_for_degrees(
            speed=-5,
            degrees=30,
            brake=True,
            block=True)

        sleep(0.1)

        self.lever_motor.reset()

    def play_music(self):
        if self.touch_sensor.is_released:
            raw = sum(self.ir_sensor.proximity for _ in range(4)) / 4

            self.speaker.tone(
                self.NOTES[min(round(raw / 5), self.N_NOTES - 1)]
                - 11 * self.lever_motor.position,
                100,
                play_type=Sound.PLAY_WAIT_FOR_COMPLETE)

    def main(self):
        self.start_up()
        while True:
            self.play_music()
Example #4
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)
        self.medium_motor.reset()
Example #5
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()
Example #6
0
class EV3D4RemoteControlled(RemoteControlledTank):
    def __init__(
            self,
            left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C,
            medium_motor_port: str = OUTPUT_A,
            speed: float = 400,
            ir_beacon_channel: int = 1):
        super().__init__(
            left_motor=left_motor_port, right_motor=right_motor_port,
            polarity='inversed',
            speed=speed,
            channel=ir_beacon_channel)
            
        self.medium_motor = MediumMotor(address=medium_motor_port)
        self.medium_motor.reset()
Example #7
0
            m2.on(-15)
        while g.angle - g_off > degrees - 8:
            if command_handler(bob.commands):
                reset_motors()
                return
            f.write("offset: {} angle: {} delta: {}\n".format(
                g_off, g.angle, g.angle - g_off))
            m1.on((10 - (5 * (g.angle - g_off) / degrees)))
            m2.on((-10 + (5 * (g.angle - g_off) / degrees)))
    m1.off()
    m2.off()
    g_off = 0
    time.sleep(1)


m4.reset()


def dance():
    global bob
    while True:
        m1.on(100)
        m2.on(-100)
        if command_handler(bob.commands):
            reset_motors()
            return
        time.sleep(0.5)
        m1.on(-100)
        m2.on(100)
        time.sleep(0.5)
Example #8
0
LeftAction = MediumMotor(OUTPUT_A)
RightAction = MediumMotor(OUTPUT_D)
LeftWheel = LargeMotor(OUTPUT_B)
RightWheel = LargeMotor(OUTPUT_C)

btn = Button()
contin = True
print('*** ATHENA ROCKS! ***')
print('1 - Left for Trip 1')
print('2 - Up for Trip 2')
print('3 - Right for Trip 3')
print('4 - Down for Trip 4')
print('5 - Center for Trip 5')
sound.beep()
while contin:
    RightAction.reset()
    LeftAction.reset()
    LeftWheel.reset()
    RightWheel.reset()
    if btn.left == True:
        runTrip1()
        sound.beep()
    elif btn.up == True:
        runTrip2()
        sound.beep()
    elif btn.right == True:
        runTrip3()
        sound.beep()
    elif btn.down == True:
        runTrip4()
        sound.beep()
Example #9
0
class MindCuber(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 = 450
    flip_speed_push = 400

    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.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)

        filename_max_rgb = 'max_rgb.txt'

        if os.path.exists(filename_max_rgb):
            with open(filename_max_rgb, 'r') as fh:
                for line in fh:
                    (color, value) = line.strip().split()

                    if color == 'red':
                        self.color_sensor.red_max = int(value)
                        log.info("red max is %d" % self.color_sensor.red_max)
                    elif color == 'green':
                        self.color_sensor.green_max = int(value)
                        log.info("green max is %d" %
                                 self.color_sensor.green_max)
                    elif color == 'blue':
                        self.color_sensor.blue_max = int(value)
                        log.info("blue max is %d" % self.color_sensor.blue_max)

    def init_motors(self):

        for x in (self.flipper, self.turntable, self.colorarm):
            x.reset()

        log.info("Initialize flipper %s" % self.flipper)
        self.flipper.on(SpeedDPS(-50), block=True)
        self.flipper.off()
        self.flipper.reset()

        log.info("Initialize colorarm %s" % self.colorarm)
        self.colorarm.on(SpeedDPS(500), block=True)
        self.colorarm.off()
        self.colorarm.reset()

        log.info("Initialize turntable %s" % self.turntable)
        self.turntable.off()
        self.turntable.reset()

    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):
            # We are shutting down so do not 'hold' the motors
            x.stop_action = 'brake'
            x.off(False)

    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.on_to_position(SpeedDPS(MindCuber.rotate_speed),
                                      final_pos)

        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 MindCuber.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.on_to_position(SpeedDPS(MindCuber.rotate_speed),
                                      temp_pos)
        self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
                                      final_pos)

    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 <= MindCuber.hold_cube_pos - 10
                or current_position >= MindCuber.hold_cube_pos + 10):

            self.flipper.ramp_down_sp = 400
            self.flipper.on_to_position(SpeedDPS(speed),
                                        MindCuber.hold_cube_pos)
            sleep(0.05)

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

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

        We have to sleep after the 2nd on_to_position() 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.ramp_up_sp = 200
        self.flipper.ramp_down_sp = 0
        self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
        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.ramp_up_sp = 200
        self.flipper.ramp_down_sp = 400
        self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
                                    MindCuber.hold_cube_pos)
        sleep(0.05)

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

    def colorarm_middle(self):
        log.info("colorarm_middle()")
        self.colorarm.on_to_position(SpeedDPS(600), -750)

    def colorarm_corner(self, square_index):
        """
        The lower the number the closer to the center
        """
        log.info("colorarm_corner(%d)" % square_index)
        position_target = -580

        if square_index == 1:
            position_target -= 10

        elif square_index == 3:
            position_target -= 30

        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.on_to_position(SpeedDPS(600), position_target)

    def colorarm_edge(self, square_index):
        """
        The lower the number the closer to the center
        """
        log.info("colorarm_edge(%d)" % square_index)
        position_target = -640

        if square_index == 2:
            position_target -= 20

        elif square_index == 4:
            position_target -= 40

        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.on_to_position(SpeedDPS(600), position_target)

    def colorarm_remove(self):
        log.info("colorarm_remove()")
        self.colorarm.on_to_position(SpeedDPS(600), 0)

    def colorarm_remove_halfway(self):
        log.info("colorarm_remove_halfway()")
        self.colorarm.on_to_position(SpeedDPS(600), -400)

    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(100)

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

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

        # The gear ratio is 3:1 so 1080 is one full rotation
        self.turntable.reset()
        self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
                                      1080,
                                      block=False)
        self.turntable.wait_until('running')

        while True:

            # 135 is 1/8 of full rotation
            if self.turntable.position >= target_pos:
                current_color = self.color_sensor.rgb
                self.colors[int(MindCuber.scan_order[self.k])] = 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)

                    if i == 1:
                        target_pos = 115
                    elif i == 3:
                        target_pos = 380
                    else:
                        target_pos = i * 135

                else:
                    self.colorarm_edge(i)

                    if i == 2:
                        target_pos = 220
                    elif i == 8:
                        target_pos = 1060
                    else:
                        target_pos = i * 135

            if self.shutdown:
                return

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

        self.turntable.wait_until_not_moving()
        self.turntable.off()
        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)
        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 self.shutdown:
            return

        cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
        output = check_output(cmd).decode('ascii')

        if 'ERROR' in output:
            msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
                                                               output)
            log.error(msg)
            print(msg)
            sys.exit(1)

        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)
Example #10
0
class ALBERT(object):
    def __init__(self):
        # ev3dev initialization
        self.leds = Leds()
        self.sound = Sound()
        self.arm_base = LargeMotor(OUTPUT_D)
        self.arm_shoulder = LargeMotor(OUTPUT_C)
        self.arm_wrist = LargeMotor(OUTPUT_B)
        self.arm_gripper = MediumMotor(OUTPUT_A)
        self.station = Workstation()
        self.color_sensor = ColorSensor(INPUT_1)
        self.find_indicator()
        self.rotate_base(STATION_COLOR)
        self.reset_all_motors()

    def find_indicator(self):
        ''' Search for a valid color indicator. '''
        if self.color_sensor.color in VALID_COLORS:
            return
        self.arm_base.on(10)
        while self.color_sensor.color not in VALID_COLORS:
            pass
        self.arm_base.stop()

    def reset_all_motors(self):
        ''' Reset all motor tach counts. '''
        self.arm_base.reset()
        self.arm_shoulder.reset()
        self.arm_wrist.reset()
        self.arm_gripper.reset()

    def rotate_base(self, color):
        '''
        Rotate from one color indicator to another.
        Color order is:
            YELLOW <--> BLUE   <--> RED
            STORE  <--> STATION <--> STERILE
        '''
        current_color = self.color_sensor.color
        if current_color == color:
            return
        direction = 1
        if (current_color == STATION_COLOR
                and color == STERILE_COLOR) or current_color == STORE_COLOR:
            direction = -1
        self.arm_base.on(SPEEDS[0] * direction, block=False)
        while self.color_sensor.color != color:
            pass
        self.arm_base.stop()
        self.arm_base.on_for_rotations(SPEEDS[0], direction * STRIPE_BIAS)

    def make_plate(self):
        ''' Sequence to make a plate. '''
        self.get_plate()
        self.lift_lid()
        self.swab_plate()
        self.lower_lid()
        self.store_plate()

    def check_plate(self):
        ''' Sequence to check plate. '''
        self.get_plate(from_storage=True, upside_down=True)
        self.move_to_keypoint(KP_UP_HORZ)
        refl = self.station.check_status()
        self.move_to_keypoint(KP_DOWN_HORZ)
        self.store_plate(is_upside_down=True)
        return refl

    def get_plate(self, from_storage=False, upside_down=False):
        ''' 
        Sequence to get a plate and place it in the workstation.
        Post-conditions
            Gripper: WIDE
            Arm:     DOWN
            Base:    STATION
        '''
        src = STORE_COLOR if from_storage else STERILE_COLOR
        self.move_to_keypoint(KP_UP_HORZ)
        self.rotate_base(src)
        self.set_gripper(GRIP_NARROW)
        self.move_to_keypoint(KP_DOWN_HORZ)
        self.set_gripper(GRIP_CLOSED)
        self.move_to_keypoint(KP_UP_HORZ)
        self.rotate_base(STATION_COLOR)
        dest_up = KP_UP_VERT_INVERT if upside_down else KP_UP_VERT
        dest_down = KP_DOWN_VERT_INVERT if upside_down else KP_DOWN_VERT
        self.move_to_keypoint(dest_up)
        self.move_to_keypoint(dest_down)
        self.set_gripper(GRIP_WIDE)
        self.move_to_keypoint(KP_DOWN_HORZ)

    def lift_lid(self):
        '''
        Lift the dish lid.
        Pre-condition
            Gripper: WIDE
            Arm:     DOWN
            Base:    STATION
        Post-condition
            Gripper: CLOSED
            Arm:     UP
        '''
        self.move_to_keypoint(KP_DOWN_HORZ_LID)
        self.set_gripper(GRIP_CLOSED)
        self.move_to_keypoint(KP_UP_HORZ)

    def lower_lid(self):
        '''
        Lower the dish lid.
        Pre-condition
            Gripper: CLOSED
            Arm:     UP
            Base:    STATION
        Post-condition
            Gripper: WIDE
            Arm:     DOWN
        '''
        self.move_to_keypoint(KP_DOWN_HORZ_LID)
        self.set_gripper(GRIP_WIDE)

    def swab_plate(self):
        ''' Call the Workstation swab routine. '''
        self.station.swab()

    def store_plate(self, is_upside_down=False):
        ''' Sequence to store a plate. '''
        src_down = KP_DOWN_VERT_INVERT if is_upside_down else KP_DOWN_VERT
        self.move_to_keypoint(src_down)
        self.set_gripper(GRIP_CLOSED)
        self.move_to_keypoint(KP_UP_HORZ)
        self.rotate_base(STORE_COLOR)
        self.move_to_keypoint(KP_DOWN_HORZ)
        self.set_gripper(GRIP_NARROW)
        self.move_to_keypoint(KP_UP_VERT)
        self.rotate_base(STATION_COLOR)
        self.set_gripper(GRIP_CLOSED)

    def move_to_keypoint(self, keypoint):
        ''' Move the should/wrist to a keypoint.'''
        # keypoint is [shoulder, wrist] with unit of rotations
        self.arm_shoulder.on_to_position(
            SPEEDS[1], keypoint[0] * self.arm_shoulder.count_per_rot)
        self.arm_wrist.on_to_position(
            SPEEDS[2], keypoint[1] * self.arm_wrist.count_per_rot)
        # pause to let things settle
        time.sleep(MOVE_PAUSE)

    def set_gripper(self, position):
        ''' Set the gripper position. '''
        self.arm_gripper.on_to_position(
            25, self.arm_gripper.count_per_rot * position)
        time.sleep(MOVE_PAUSE)
Example #11
0
class AthenaRobot(object):
    # constructors for the robot with default parameters of wheel radius and ports
    def __init__(self,
                 wheelRadiusCm=4,
                 leftLargeMotorPort=OUTPUT_B,
                 rightLargeMotorPort=OUTPUT_C,
                 leftMediumMotorPort=OUTPUT_A,
                 rightMediumMotorPort=OUTPUT_D,
                 leftSensorPort=INPUT_1,
                 rightSensorPort=INPUT_4,
                 ultraSonicSensorPort=INPUT_2):
        #self is the current object, everything below for self are member variables
        self.wheelRadiusCm = wheelRadiusCm
        self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
        self.leftLargeMotor = LargeMotor(leftLargeMotorPort)
        self.rightLargeMotor = LargeMotor(rightLargeMotorPort)
        self.leftMediumMotor = MediumMotor(leftMediumMotorPort)
        self.rightMediumMotor = MediumMotor(rightMediumMotorPort)
        self.leftSensor = ColorSensor(leftSensorPort)
        self.rightSensor = ColorSensor(rightSensorPort)

    # run a distance in centimeters at speed of centimeters per second
    def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
        if speedCmPerSecond < 0:
            raise Exception('speed cannot be negative')
        # Calculate degrees of distances and SpeedDegreePerSecond
        degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
        speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
        print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
            degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed),
              file=sys.stderr)
        # run motors based on the calculated results
        self.leftLargeMotor.on_for_degrees(
            SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False)
        self.rightLargeMotor.on_for_degrees(
            SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block)

    # turn a angle in degrees, positive means turn right and negative means turn left.
    def turn(self, degree, speed=10, brake=True, block=True):
        # 1.9 is a scale factor from experiments
        degreesToRun = degree * 1.275
        # Turn at the speed
        self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, False)
        self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)

    def turnRight(self, degree, speed=10, brake=True, block=True):
        self.turn(degree, speed, brake, block)

    def turnLeft(self, degree, speed=10, brake=True, block=True):
        self.turn(-degree, speed, brake, block)

    def turnOnRightWheel(self, degree, speed=10, brake=True, block=True):
        degreesToRun = degree * 2.7
        self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)

    def turnRightOnRightWheel(self, degree, speed=10, brake=True, block=True):
        self.turnOnRightWheel(degree, speed, brake, block)

    def turnLeftOnRightWheel(self, degree, speed=10, brake=True, block=True):
        self.turnOnRightWheel(-degree, speed, brake, block)

    def turnOnLeftWheel(self, degree, speed=10, brake=True, block=True):
        degreesToRun = degree * 2.7
        self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, block)

    def turnRightOnLeftWheel(self, degree, speed=10, brake=True, block=True):
        self.turnOnLeftWheel(degree, speed, brake, block)

    def turnLeftOnLeftWheel(self, degree, speed=10, brake=True, block=True):
        self.turnOnLeftWheel(-degree, speed, brake, block)

    #Medium Motor Movement
    def moveMediumMotor(self, isLeft, speed, degrees, brake=True, block=True):
        #sees which motor is running
        if isLeft == False:
            self.rightMediumMotor.on_for_degrees(speed, degrees, brake, block)
        else:
            self.leftMediumMotor.on_for_degrees(speed, degrees, brake, block)

    # Following a line with one sensor
    def lineFollow(self,
                   whiteThreshold=98,
                   blackThreshold=15,
                   scale=0.2,
                   useLeftSensor=True,
                   useLeftEdge=True,
                   runDistanceCM=300):
        self.leftLargeMotor.reset()
        self.rightLargeMotor.reset()
        # Allow an attached backsensor. Ixf useBackSensor, defining back sensor and revert useLeftEdge since motor is actually going backward
        initialPos = self.leftLargeMotor.position  # remember initial position
        loop = True
        while loop:
            # use left or right sensor based on passed in useLeftSensor
            reflect = self.leftSensor.reflected_light_intensity if useLeftSensor == True else self.rightSensor.reflected_light_intensity
            # Allow an attached backsensor. If useBackSensor, use reflected_light_intensity of that sensor

            leftPower = abs(reflect - blackThreshold) * scale
            rightPower = abs(whiteThreshold - reflect) * scale
            # if we follow the right edge, need to swap left and right
            if useLeftEdge == False:
                oldLeft = leftPower
                leftPower = rightPower
                rightPower = oldLeft
            self.leftLargeMotor.on(-leftPower)
            self.rightLargeMotor.on(-rightPower)
            # Calculate the distance run in CM
            distanceRanInCM = abs((self.leftLargeMotor.position - initialPos) *
                                  (self.wheelCircumferenceCm /
                                   self.leftLargeMotor.count_per_rot))
            # Printing the reflected light intensity with the powers of the two motors
            print(
                "LineFollow - reflect: {0:3d} leftPower: {1:3f} rightPower: {2:3f} lMotorPos: {3:3d} distanceRanInCM {4:3f}"
                .format(reflect, leftPower, rightPower,
                        self.leftLargeMotor.position, distanceRanInCM),
                file=sys.stderr)

            if distanceRanInCM >= runDistanceCM:
                loop = False
        # Stopping the motor once the loop is over
        self.leftLargeMotor.off()
        self.rightLargeMotor.off()

    # run until both conditions are met
    def onUntilTwoConditions(self,
                             leftCondition,
                             rightCondition,
                             caller,
                             speed=5,
                             consecutiveHit=5,
                             sleepTime=0.01):
        # Start motor at passed speonUntilTwoConditionsed.
        self.leftLargeMotor.on(-speed)
        self.rightLargeMotor.on(-speed)

        condLeftCounter = 0
        condRightCounter = 0
        condLeftMet = False
        condRightMet = False

        while (not condLeftMet or not condRightMet):
            # check left condition
            if (leftCondition()):
                condLeftCounter += 1
            else:
                condLeftCounter = 0
                # reset to zero
            if (condLeftCounter >= consecutiveHit):
                if (condRightMet):
                    sleep(.1)
                self.leftLargeMotor.off()
                condLeftMet = True

            # check right condition
            if (rightCondition()):
                condRightCounter += 1
            else:
                condRightCounter = 0
                # reset to zero
            if (condRightCounter >= consecutiveHit):
                if (condLeftMet):
                    sleep(.1)
                self.rightLargeMotor.off()
                condRightMet = True

            print(
                "{4}-onUntilTwoConditions: left_reflected: {0:3d}, right_reflected: {1:3d}, leftHit: {2:3d}, rightHit: {3:3d}"
                .format(self.leftSensor.reflected_light_intensity,
                        self.rightSensor.reflected_light_intensity,
                        condLeftCounter, condRightCounter, caller),
                file=sys.stderr)
            sleep(sleepTime)
        self.leftLargeMotor.off()
        self.rightLargeMotor.off()

    def onUntilWhiteLine(self,
                         consecutiveHit=5,
                         speed=5,
                         sleepTime=0.01,
                         white_threshold=85):
        self.onUntilTwoConditions(
            lambda: self.leftSensor.reflected_light_intensity >
            white_threshold, lambda: self.rightSensor.reflected_light_intensity
            > white_threshold, "onUntilWhiteLine", speed, consecutiveHit,
            sleepTime)

    def onUntilBlackLine(self,
                         consecutiveHit=5,
                         speed=5,
                         sleepTime=0.01,
                         black_threshold=30):
        self.onUntilTwoConditions(
            lambda: self.leftSensor.reflected_light_intensity <
            black_threshold, lambda: self.rightSensor.reflected_light_intensity
            < black_threshold, "onUntilBlackLine", speed, consecutiveHit,
            sleepTime)

    # run until find a game line
    def onUntilGameLine(self,
                        consecutiveHit=5,
                        speed=5,
                        sleepTime=0.01,
                        white_threshold=85,
                        black_threshold=30):
        self.onUntilWhiteLine(consecutiveHit, speed, sleepTime,
                              white_threshold)
        self.onUntilBlackLine(consecutiveHit, speed, sleepTime,
                              black_threshold)

    # run until condition is met
    def onUntilCondition(self,
                         condition,
                         caller,
                         leftSpeed=5,
                         rightSpeed=5,
                         consecutiveHit=5,
                         sleepTime=0.01,
                         revert=False):
        # Start motor at passed speonUntilTwoConditionsed.
        self.leftLargeMotor.on(-leftSpeed if revert == False else leftSpeed)
        self.rightLargeMotor.on(-rightSpeed if revert == False else rightSpeed)
        counter = 0
        condMet = False

        while (not condMet):
            # check condition
            if (condition()):
                counter += 1
            else:
                counter = 0
                # reset to zero
            if (counter >= consecutiveHit):
                self.leftLargeMotor.off()
                self.rightLargeMotor.off()
                condMet = True

            print(
                "{4}-onUntilCondition: ColorSensor(left_reflected: {0:3d}, right_reflected: {1:3d}, hit: {2:3d}), UltraSonicSensor(distance_centimeters: {3:3f})"
                .format(self.leftSensor.reflected_light_intensity,
                        self.rightSensor.reflected_light_intensity, counter,
                        self.ultraSonicSensor.distance_centimeters, caller),
                file=sys.stderr)
            sleep(sleepTime)
        self.leftLargeMotor.off()
        self.rightLargeMotor.off()

    def onUntilLeftBlack(self,
                         speed=5,
                         consecutiveHit=5,
                         sleepTime=0.01,
                         black_threshold=30):
        self.onUntilCondition(
            lambda: self.leftSensor.reflected_light_intensity <
            black_threshold, "onUntilLeftBlack", speed, speed, consecutiveHit,
            sleepTime)

    def onUntilLeftWhite(self,
                         speed=5,
                         consecutiveHit=5,
                         sleepTime=0.01,
                         white_threshold=85):
        self.onUntilCondition(
            lambda: self.leftSensor.reflected_light_intensity >
            white_threshold, "onUntilLeftWhite", speed, speed, consecutiveHit,
            sleepTime)

    def onUntilRightBlack(self,
                          speed=5,
                          consecutiveHit=5,
                          sleepTime=0.01,
                          black_threshold=30):
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity <
            black_threshold, "onUntilRightBlack", speed, speed, consecutiveHit,
            sleepTime)

    def onUntilRightWhite(self,
                          speed=5,
                          consecutiveHit=5,
                          sleepTime=0.01,
                          white_threshold=85):
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity >
            white_threshold, "onUntilRightWhite", speed, speed, consecutiveHit,
            sleepTime)

    def turnUntilLeftBlack(self,
                           turnLeft,
                           speed,
                           consecutiveHit=2,
                           black_threshold=30):
        self.onUntilCondition(
            lambda: self.leftSensor.reflected_light_intensity <
            black_threshold, "turnUntilLeftBlack",
            0 if turnLeft == True else speed, speed if turnLeft == True else 0,
            consecutiveHit)

    def turnUntilLeftWhite(self,
                           turnLeft,
                           speed,
                           consecutiveHit=2,
                           white_threshold=85):
        self.onUntilCondition(
            lambda: self.leftSensor.reflected_light_intensity >
            white_threshold, "turnUntilLeftWhite",
            0 if turnLeft == True else speed, speed if turnLeft == True else 0,
            consecutiveHit)

    def turnUntilRightBlack(self,
                            turnLeft,
                            speed,
                            consecutiveHit=2,
                            black_threshold=30):
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity <
            black_threshold, "turnUntilRightBlack",
            0 if turnLeft == True else speed, speed if turnLeft == True else 0,
            consecutiveHit)

    def turnUntilRightWhite(self,
                            turnLeft,
                            speed,
                            consecutiveHit=2,
                            white_threshold=85):
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity >
            white_threshold, "turnUntilRightWhite",
            0 if turnLeft == True else speed, speed if turnLeft == True else 0,
            consecutiveHit)

    # Go until sensor reading has a specified offset or reach to the threshhold
    def onUntilRightDarkerBy(self,
                             difference,
                             black_threshold=20,
                             speed=10,
                             consecutiveHit=2):
        originalValue = self.rightSensor.reflected_light_intensity
        print("onUntilRightDarkerBy - originalValue: {0:3d}, diff: {1:3d}".
              format(originalValue, difference),
              file=sys.stderr)
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity - originalValue
            < -difference or self.rightSensor.reflected_light_intensity <
            black_threshold,
            "onUntilRightSensorDiff",
            consecutiveHit=consecutiveHit)

    def onUntilRightLighterBy(self,
                              difference,
                              white_threshold=80,
                              speed=10,
                              consecutiveHit=2):
        originalValue = self.rightSensor.reflected_light_intensity
        print("onUntilRightLighterBy - originalValue: {0:3d}, diff: {1:3d}".
              format(originalValue, difference),
              file=sys.stderr)
        self.onUntilCondition(
            lambda: self.rightSensor.reflected_light_intensity - originalValue
            > difference or self.rightSensor.reflected_light_intensity >
            white_threshold,
            "onUntilRightSensorDiff",
            consecutiveHit=consecutiveHit)

    def onUntilLeftDarkerBy(self,
                            difference,
                            black_threshold=20,
                            speed=10,
                            consecutiveHit=2):
        originalValue = self.leftSensor.reflected_light_intensity
        print(
            "onUntilLeftDarkerBy - originalValue: {0:3d}, diff: {1:3d}".format(
                originalValue, difference),
            file=sys.stderr)
        self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
                              - originalValue < -difference or self.leftSensor.
                              reflected_light_intensity < black_threshold,
                              "onUntilLeftSensorDiff",
                              consecutiveHit=consecutiveHit)

    def onUntilLeftLighterBy(self,
                             difference,
                             white_threshold=80,
                             speed=10,
                             consecutiveHit=2):
        originalValue = self.leftSensor.reflected_light_intensity
        print("onUntilLeftLighterBy - originalValue: {0:3d}, diff: {1:3d}".
              format(originalValue, difference),
              file=sys.stderr)
        self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
                              - originalValue > difference or self.leftSensor.
                              reflected_light_intensity > white_threshold,
                              "onUntilLeftSensorDiff",
                              consecutiveHit=consecutiveHit)

    #uses Ultrasonic sensor to see wall as going back
    def revertSafely(self,
                     speed=100,
                     distanceToStop=10,
                     consecutiveHit=1,
                     sleepTime=0.01):
        self.onUntilCondition(
            lambda: self.ultraSonicSensor.distance_centimeters <
            distanceToStop, "revertSafely", speed, speed, consecutiveHit,
            sleepTime, True)

    # Calibrating White for Sensor
    def calibrateColorSensor(self, sensorInput):
        sensor = ColorSensor(sensorInput)
        # Calibration
        sensor.calibrate_white()
        # Done Signal
        sound.beep()

    # Calibrating Color for Sensor
    def testColorSensor(self,
                        sensorInput,
                        sensorPort,
                        repeatNumber=10,
                        pauseNumber=0.2,
                        speed=0):
        sensor = ColorSensor(sensorInput)
        if (speed > 0):
            self.leftLargeMotor.on(speed)
            self.rightLargeMotor.on(speed)
        times = 0
        # For loop
        while times != repeatNumber:
            # Print
            print("testColorSensor- Port: {0:3d}: {1:3d}".format(
                sensorPort, sensor.reflected_light_intensity),
                  file=sys.stderr)
            time.sleep(pauseNumber)
            times = times + 1
        self.leftLargeMotor.off()
        self.rightLargeMotor.off()

    def resetMediumMotor():
        self.moveMediumMotor(isLeft=False, speed=50, degrees=1000)
        self.rightMediumMotor.reset()

    def testRobot(self):
        self.leftLargeMotor.on_for_degrees(20, 180)
        sleep(.1)
        self.rightLargeMotor.on_for_degrees(20, 180)
        sleep(.1)
        self.moveMediumMotor(True, 10, 180)
        sleep(.1)
        self.moveMediumMotor(False, 10, 180)
        sleep(.1)
        self.run(20, 10)
        sleep(.1)
        self.run(-20, 10)
        sleep(.1)
        self.turn(90)
        sleep(.1)
        self.turn(-90)
        sleep(.1)
        self.turnOnLeftWheel(90)
        sleep(.1)
        self.turnOnLeftWheel(-90)
        sleep(.1)
        self.turnOnRightWheel(90)
        sleep(.1)
        self.turnOnRightWheel(-90)
        sleep(.1)
        self.calibrateColorSensor(INPUT_1)
        self.calibrateColorSensor(INPUT_4)
        self.testColorSensor(INPUT_1, 1)
        self.testColorSensor(INPUT_4, 4)
Example #12
0
class Robot:
    def __init__(self):
        self.sound = Sound()
        self.direction_motor = MediumMotor(OUTPUT_D)
        self.swing_motorL = LargeMotor(OUTPUT_A)
        self.swing_motorC = LargeMotor(OUTPUT_B)
        self.swing_motorR = LargeMotor(OUTPUT_C)
        self.swing_motors = [
            self.swing_motorL, self.swing_motorC, self.swing_motorR
        ]
        self.touch_sensor = TouchSensor(INPUT_1)
        self.console = Console(DEFAULT_FONT)
        self.buttons = Button()
        self.beeps_enabled = True

    def beep(self, frequency=700, wait_for_comeplete=False):
        if not self.beeps_enabled:
            return
        play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE
        self.sound.beep("-f %i" % frequency, play_type=play_type)

    def calibrate_dir(self):
        ''' Calibrate direction motor '''
        # Run motor with 25% power, and wait until it stops running
        self.direction_motor.on(SpeedPercent(-10), block=False)
        # while (not self.direction_motor.STATE_OVERLOADED):
        #     print(self.direction_motor.duty_cycle)
        self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED)

        self.direction_motor.stop_action = Motor.STOP_ACTION_COAST
        self.direction_motor.stop()

        time.sleep(.5)

        # Reset to straight
        # self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True)
        self.direction_motor.on_for_degrees(SpeedPercent(10),
                                            127,
                                            brake=True,
                                            block=True)
        self.direction_motor.reset()

        print("Motor reset, position: " + str(self.direction_motor.position))

        time.sleep(.5)

    def calibrate_swing(self):
        for m in self.swing_motors:
            m.stop_action = m.STOP_ACTION_HOLD
            m.on(SpeedPercent(6))

        self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000)

        for m in self.swing_motors:
            m.stop_action = m.STOP_ACTION_HOLD
            m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False)

        self.swing_motorC.wait_while('running')

        for m in self.swing_motors:
            m.reset()
            m.stop_action = m.STOP_ACTION_HOLD
            m.stop()

        print("Ready Angle: %i" % self.swing_motorC.position)

    def ready_swing(self, angle: int):
        right_angle = -(angle / 3)
        # adjust motors to target angle
        for m in self.swing_motors:
            m.stop_action = Motor.STOP_ACTION_HOLD
            m.on_for_degrees(SpeedPercent(2),
                             right_angle,
                             brake=True,
                             block=False)

        self.swing_motorC.wait_while('running')

        for m in self.swing_motors:
            m.stop_action = Motor.STOP_ACTION_HOLD
            m.stop()

        print("Swing Angle: %i" % self.swing_motorC.position)

    def set_direction(self, direction):
        print("Setting direction to: " + str(direction))
        #direction = self.__aim_correction(direction)
        self.direction_motor.on_for_degrees(SpeedPercent(10),
                                            round(direction * 3))
        print("Direction set to: " + str(self.direction_motor.position))

#
#    def __aim_correction(self, direction):
#        x = direction
#        y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000
#
#        return y

    def shoot(self, power):

        print("SHOOT, power: %i" % power)

        for m in self.swing_motors:
            m.duty_cycle_sp = -power

        for m in self.swing_motors:
            m.run_direct()

        time.sleep(.5)

        self.swing_motorC.wait_until_not_moving()

        for m in self.swing_motors:
            m.reset()

    def wait_for_button(self):
        self.touch_sensor.wait_for_bump()

    def __set_display(self, str):
        self.console.set_font(font=LARGER_FONT, reset_console=True)
        self.console.text_at(str, column=1, row=1, reset_console=True)

    def wait_for_power_select(self, power=0, angle=0, steps=1):
        self.__set_display("Pow: %i\nAngle: %i" % (power, angle))

        def left():
            power -= steps
            if power < 0:
                power = 0
            self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
            self.buttons.wait_for_released(buttons=['left'], timeout_ms=150)

        def right():
            power += steps
            if power > 100:
                power = 100
            self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
            self.buttons.wait_for_released(buttons=['right'], timeout_ms=150)

        def up():
            angle += steps
            if angle > 110:
                angle = 110
            self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
            self.buttons.wait_for_released(buttons=['up'], timeout_ms=150)

        def down():
            angle -= steps
            if angle < 0:
                angle = 0
            self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
            self.buttons.wait_for_released(buttons=['down'], timeout_ms=150)

        while not self.touch_sensor.is_pressed:
            if self.buttons.left:
                left()
            elif self.buttons.right:
                right()
            elif self.buttons.up:
                up()
            elif self.buttons.down:
                down()

        self.console.set_font(font=DEFAULT_FONT, reset_console=True)
        return (power, angle)

    def select_connection_mode(self):
        self.__set_display("Enable Connection\nLeft: True - Right: False")

        enabled = True

        while not self.touch_sensor.is_pressed:
            if self.buttons.left:
                enabled = True
                self.buttons.wait_for_released(buttons=['left'])
                break
            elif self.buttons.right:
                enabled = False
                self.buttons.wait_for_released(buttons=['right'])
                break

        self.console.set_font(font=DEFAULT_FONT, reset_console=True)
        return enabled

    def print(self, string):
        print(string)
Example #13
0
class Gripp3r(RemoteControlledTank):
    """
    To enable the medium motor toggle the beacon button on the EV3 remote.
    """
    CLAW_DEGREES_OPEN = 225
    CLAW_DEGREES_CLOSE = 920
    CLAW_SPEED_PCT = 50

    def __init__(self, left_motor_port=OUTPUT_B, right_motor_port=OUTPUT_C, medium_motor_port=OUTPUT_A):
        RemoteControlledTank.__init__(self, left_motor_port, right_motor_port)
        self.set_polarity(MediumMotor.POLARITY_NORMAL)
        self.medium_motor = MediumMotor(medium_motor_port)
        self.ts = TouchSensor()
        self.mts = MonitorTouchSensor(self)
        self.mrc = MonitorRemoteControl(self)
        self.shutdown_event = Event()

        # Register our signal_term_handler() to be called if the user sends
        # a 'kill' to this process or does a Ctrl-C from the command line
        signal.signal(signal.SIGTERM, self.signal_term_handler)
        signal.signal(signal.SIGINT, self.signal_int_handler)

        self.claw_open(True)
        self.remote.on_channel4_top_left = self.claw_close
        self.remote.on_channel4_bottom_left = self.claw_open

    def shutdown_robot(self):

        if self.shutdown_event.is_set():
            return

        self.shutdown_event.set()
        log.info('shutting down')
        self.mts.shutdown_event.set()
        self.mrc.shutdown_event.set()
        self.remote.on_channel4_top_left = None
        self.remote.on_channel4_bottom_left = None
        self.left_motor.off(brake=False)
        self.right_motor.off(brake=False)
        self.medium_motor.off(brake=False)
        self.mts.join()
        self.mrc.join()

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

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

    def claw_open(self, state):
        if state:

            # Clear monitor_ts while we are opening the claw. We do this because
            # the act of opening the claw presses the TouchSensor so we must
            # tell mts to ignore that press.
            self.mts.monitor_ts.clear()
            self.medium_motor.on(speed=self.CLAW_SPEED_PCT * -1, block=True)
            self.medium_motor.off()
            self.medium_motor.reset()
            self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT,
                                             position=self.CLAW_DEGREES_OPEN,
                                             brake=False, block=True)
            self.mts.monitor_ts.set()

    def claw_close(self, state):
        if state:
            self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT,
                                             position=self.CLAW_DEGREES_CLOSE)

    def main(self):
        self.mts.start()
        self.mrc.start()
        self.shutdown_event.wait()
Example #14
0
screenw = display.xres
screenh = display.yres

# hostMACAddress = '00:17:E9:B2:8A:AF' # The MAC address of a Bluetooth adapter on the server. The server might have multiple Bluetooth adapters.
# Fetch BT MAC address automatically
cmd = "hciconfig"
device_id = "hci0"
sp_result = subprocess.run(cmd, stdout=subprocess.PIPE, universal_newlines=True)
hostMACAddress = sp_result.stdout.split("{}:".format(device_id))[1].split("BD Address: ")[1].split(" ")[0].strip()
debug_print (hostMACAddress)
print (hostMACAddress)

# reset the kick motor to a known good position
kickMotor.on_for_seconds(speed=-10, seconds=0.5)
kickMotor.on_for_seconds(speed=10, seconds=2, brake=False)
kickMotor.reset()
kicking = False
kick_power = 0
max_kick = 1000

port = 3  # port number is arbitrary, but must match between server and client
backlog = 1
size = 1024
s = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
s.bind((hostMACAddress, port))
s.listen(backlog)
while True:
    try:
        reset_console()
        print (hostMACAddress)
        leds.set_color('LEFT', 'AMBER')
Example #15
0
from ev3dev2.motor import MediumMotor
from time import sleep
mA = MediumMotor('outA')
mA.reset()


def lift(direction):
    if(direction == "up"):
        mA.run_to_rel_pos(position_sp=130, speed_sp=300, stop_action="coast")
    else:
        mA.run_to_rel_pos(position_sp=-130, speed_sp=300)
    mA.wait_while('running')