Пример #1
0
class captorDistance:
    def __init__(self):
        self.infraredSensor = InfraredSensor(Port.S2)

    def getDistance(self):
        dMesurer = self.infraredSensor.distance()
        #distance = dMesurer**2*0.0177 + dMesurer*0.0167 + 7.7471
        distance = dMesurer * 1.0356 - 4.3746
        #print(distance)

        return distance
Пример #2
0
def fahre_bis_ir():
    """
    drive until ir distance drops below 50%
    """
    ev3.screen.load_image(ImageFile.PINCHED_LEFT)

    infrarot = InfraredSensor(Port.S4)
    db.drive(speed=20, turn_rate=0)
    while (infrarot.distance() > 50):
        time.sleep(0.1)
    db.stop()
    db.turn(180)
    db.straight(150)
Пример #3
0
class Remote():
    def __init__(self) -> None:
        super().__init__()

        self.inf_sensor = InfraredSensor(Port.S3)

    def get_button(self):
        pressButton = self.inf_sensor.buttons(1)
        brick.display.text(str(pressButton))
        return pressButton

    def get_distance(self):
        dist = self.inf_sensor.distance()
        brick.display.text(str(dist))
        return dist
Пример #4
0
class distanceSensor:
    """ Defines a sensor that reads distance from a specific sensor. """

    def __init__(self, sensor_input):
        """
        Initializes the ultrasonic sensor component.
        :param sensor_input: the input pin of the sensor
        """
        self._sensor = InfraredSensor(sensor_input)

    def read_distance(self):
        """
        Reads the measured distance.
        :return: the distance in centimeters
        """
        return self._sensor.distance()*1.0356 - 4.3746
Пример #5
0
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: Port = Port.D,
                 touch_sensor_port: Port = Port.S1,
                 ir_sensor_port: Port = Port.S4):
        self.ev3_brick = EV3Brick()

        self.lever_motor = Motor(port=lever_motor_port,
                                 positive_direction=Direction.CLOCKWISE)

        self.touch_sensor = TouchSensor(port=touch_sensor_port)

        self.ir_sensor = InfraredSensor(port=ir_sensor_port)

    def start_up(self):
        self.ev3_brick.screen.load_image(ImageFile.EV3)

        self.ev3_brick.light.on(color=Color.ORANGE)

        self.lever_motor.run_time(speed=50,
                                  time=1000,
                                  then=Stop.COAST,
                                  wait=True)

        self.lever_motor.run_angle(speed=50,
                                   rotation_angle=-30,
                                   then=Stop.BRAKE,
                                   wait=True)

        wait(100)

        self.lever_motor.reset_angle(angle=0)

    def play_note(self):
        if not self.touch_sensor.pressed():
            raw = sum(self.ir_sensor.distance() for _ in range(4)) / 4

            self.ev3_brick.speaker.beep(
                frequency=self.NOTES[min(int(raw / 5), self.N_NOTES - 1)] -
                11 * self.lever_motor.angle(),
                duration=100)

        wait(1)
Пример #6
0
class Wack3m:
    N_WHACK_TIMES = 10

    def __init__(
            self,
            left_motor_port: str = Port.B, right_motor_port: str = Port.C,
            middle_motor_port: str = Port.A,
            touch_sensor_port: str = Port.S1, ir_sensor_port: str = Port.S4):
        self.ev3_brick = EV3Brick()
        
        self.left_motor = Motor(port=left_motor_port,
                                positive_direction=Direction.CLOCKWISE)
        self.right_motor = Motor(port=right_motor_port,
                                 positive_direction=Direction.CLOCKWISE)
        self.middle_motor = Motor(port=middle_motor_port,
                                  positive_direction=Direction.CLOCKWISE)

        self.touch_sensor = TouchSensor(port=touch_sensor_port)

        self.ir_sensor = InfraredSensor(port=ir_sensor_port)

    def start_up(self):
        self.ev3_brick.light.on(color=Color.RED)

        self.ev3_brick.screen.print('WACK3M')

        self.left_motor.run_time(
            speed=-1000,
            time=1000,
            then=Stop.HOLD,
            wait=True)

        self.left_motor.reset_angle(angle=0)

        self.middle_motor.run_time(
            speed=-1000,
            time=1000,
            then=Stop.HOLD,
            wait=True)

        self.middle_motor.reset_angle(angle=0)

        self.right_motor.run_time(
            speed=-1000,
            time=1000,
            then=Stop.HOLD,
            wait=True)

        self.right_motor.reset_angle(angle=0)

    def play(self):
        while True:
            self.ev3_brick.speaker.play_file(file=SoundFile.START)

            self.ev3_brick.screen.load_image(ImageFile.TARGET)

            self.ev3_brick.light.on(color=Color.ORANGE)

            while not self.touch_sensor.pressed():
                wait(10)

            self.ev3_brick.speaker.play_file(file=SoundFile.GO)

            self.ev3_brick.light.on(color=Color.GREEN)

            total_response_time = 0

            sleep(1)

            for _ in range(self.N_WHACK_TIMES):
                self.ev3_brick.light.on(color=Color.GREEN)

                self.ev3_brick.screen.load_image(ImageFile.EV3_ICON)

                sleep(uniform(0.1, 3))

                which_motor = randint(1, 3)

                if which_motor == 1:
                    self.left_motor.run_angle(
                        speed=1000,
                        rotation_angle=90,
                        then=Stop.COAST,
                        wait=True)

                    start_time = time()

                    self.ev3_brick.screen.load_image(ImageFile.MIDDLE_LEFT)

                    self.left_motor.run_time(
                        speed=-1000,
                        time=500,
                        then=Stop.HOLD,
                        wait=True)

                    proximity = self.ir_sensor.distance()
                    while abs(self.ir_sensor.distance() - proximity) <= 4:
                        wait(10)

                elif which_motor == 2:
                    self.middle_motor.run_angle(
                        speed=1000,
                        rotation_angle=210,
                        then=Stop.COAST,
                        wait=True)

                    start_time = time()

                    self.ev3_brick.screen.load_image(ImageFile.NEUTRAL)

                    self.middle_motor.run_time(
                        speed=-1000,
                        time=500,
                        then=Stop.COAST,
                        wait=True)

                    proximity = self.ir_sensor.distance()
                    while abs(self.ir_sensor.distance() - proximity) <= 5:
                        wait(10)

                else:
                    self.right_motor.run_angle(
                        speed=1000,
                        rotation_angle=90,
                        then=Stop.COAST,
                        wait=True)

                    start_time = time()

                    self.ev3_brick.screen.load_image(ImageFile.MIDDLE_RIGHT)

                    self.right_motor.run_time(
                        speed=-1000,
                        time=500,
                        then=Stop.HOLD,
                        wait=True)

                    proximity = self.ir_sensor.distance()
                    while abs(self.ir_sensor.distance() - proximity) <= 5:
                        wait(10)

                response_time = time() - start_time

                self.ev3_brick.screen.load_image(ImageFile.DIZZY)

                self.ev3_brick.screen.print(response_time)

                self.ev3_brick.light.on(color=Color.RED)

                self.ev3_brick.speaker.play_file(file=SoundFile.BOING)

                total_response_time += response_time

            average_response_time = total_response_time / self.N_WHACK_TIMES

            self.ev3_brick.screen.clear()
            self.ev3_brick.screen.print(
                'Avg. Time: {:.1f}s'.format(average_response_time))

            self.ev3_brick.speaker.play_file(
                file=SoundFile.FANTASTIC
                     if average_response_time <= 1
                     else SoundFile.GOOD_JOB)

            self.ev3_brick.speaker.play_file(file=SoundFile.GAME_OVER)

            self.ev3_brick.light.on(color=Color.RED)

            sleep(4)
Пример #7
0
class Dinor3x(EV3Brick):
    """
    Challenges:
    - Can you make DINOR3X remote controlled with the IR-Beacon?
    - Can you attach a colorsensor to DINOR3X, and make it behave differently
        depending on which color is in front of the sensor
        (red = walk fast, white = walk slow, etc.)?
    """

    def __init__(
            self,
            left_motor_port: Port = Port.B, right_motor_port: Port = Port.C,
            jaw_motor_port: Port = Port.A,
            touch_sensor_port: Port = Port.S1,
            ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1):
        self.left_motor = Motor(port=left_motor_port,
                                positive_direction=Direction.CLOCKWISE)
        self.right_motor = Motor(port=right_motor_port,
                                 positive_direction=Direction.CLOCKWISE)

        self.jaw_motor = Motor(port=jaw_motor_port,
                               positive_direction=Direction.CLOCKWISE)

        self.touch_sensor = TouchSensor(port=touch_sensor_port)

        self.ir_sensor = InfraredSensor(port=ir_sensor_port)
        self.ir_beacon_channel = ir_beacon_channel

    def calibrate_legs(self):
        self.left_motor.run(speed=100)
        self.right_motor.run(speed=200)

        while self.touch_sensor.pressed():
            pass

        self.left_motor.hold()
        self.right_motor.hold()

        self.left_motor.run(speed=400)

        while not self.touch_sensor.pressed():
            pass

        self.left_motor.hold()

        self.left_motor.run_angle(
            rotation_angle=-0.2 * 360,
            speed=500,
            then=Stop.HOLD,
            wait=True)

        self.right_motor.run(speed=400)

        while not self.touch_sensor.pressed():
            pass

        self.right_motor.hold()

        self.right_motor.run_angle(
            rotation_angle=-0.2 * 360,
            speed=500,
            then=Stop.HOLD,
            wait=True)

        self.left_motor.reset_angle(angle=0)
        self.right_motor.reset_angle(angle=0)

    def roar(self):
        self.speaker.play_file(file=SoundFile.T_REX_ROAR)

        self.jaw_motor.run_angle(
            speed=400,
            rotation_angle=-60,
            then=Stop.HOLD,
            wait=True)

        # FIXME: jaw doesn't close
        for i in range(12):
            self.jaw_motor.run_time(
                speed=-400,
                time=0.05 * 1000,
                then=Stop.HOLD,
                wait=True)

            self.jaw_motor.run_time(
                speed=400,
                time=0.05 * 1000,
                then=Stop.HOLD,
                wait=True)

        self.jaw_motor.run(speed=200)

        sleep(0.5)

    def close_mouth(self):
        self.jaw_motor.run(speed=200)
        sleep(1)
        self.jaw_motor.stop()

    def walk_until_blocked(self):
        self.left_motor.run(speed=-400)
        self.right_motor.run(speed=-400)

        while self.ir_sensor.distance() >= 25:
            pass

        self.left_motor.stop()
        self.right_motor.stop()

    def run_away(self):
        self.left_motor.run_angle(
            speed=750,
            rotation_angle=3 * 360,
            then=Stop.BRAKE,
            wait=False)
        self.right_motor.run_angle(
            speed=750,
            rotation_angle=3 * 360,
            then=Stop.BRAKE,
            wait=True)

    def jump(self):
        """
        Dinor3x Mission 02 Challenge: make it jump
        """
        ...

    # TRANSLATED FROM EV3-G MY BLOCKS
    # -------------------------------

    def leg_adjust(
            self,
            cyclic_degrees: float,
            speed: float = 1000,
            leg_offset_percent: float = 0,
            mirrored_adjust: bool = False,
            brake: bool = True):
        ...

    def leg_to_pos(
            self,
            speed: float = 1000,
            left_position: float = 0,
            right_position: float = 0):
        self.left_motor.brake()
        self.right_motor.brake()

        self.left_motor.run_angle(
            speed=speed,
            rotation_angle=left_position -
                            cyclic_position_offset(
                                rotation_sensor=self.left_motor.angle(),
                                cyclic_degrees=360),
            then=Stop.BRAKE,
            wait=True)

        self.right_motor.run_angle(
            speed=speed,
            rotation_angle=right_position -
                            cyclic_position_offset(
                                rotation_sensor=self.right_motor.angle(),
                                cyclic_degrees=360),
            then=Stop.BRAKE,
            wait=True)

    def turn(self, speed: float = 1000, n_steps: int = 1):
        ...

    def walk(self, speed: float = 1000):
        ...

    def walk_steps(self, speed: float = 1000, n_steps: int = 1):
        ...
    straight_speed=750,   # milimeters per second
    straight_acceleration=750,
    turn_rate=90,   # degrees per second
    turn_acceleration=90)

IR_SENSOR = InfraredSensor(port=Port.S4)


MEDIUM_MOTOR.run_time(
    speed=-200,   # deg/s
    time=1000,   # ms
    then=Stop.HOLD,
    wait=True)

while True:
    if IR_SENSOR.distance() < 25:
        BRICK.screen.load_image(ImageFile.PINCHED_RIGHT)

        DRIVE_BASE.turn(angle=-180)

        BRICK.screen.load_image(ImageFile.ANGRY)

        MEDIUM_MOTOR.run_time(
            speed=1000,   # deg/s
            time=1000,   # ms
            then=Stop.HOLD,
            wait=True)

        BRICK.speaker.play_file(file=SoundFile.LAUGHING_2)

        MEDIUM_MOTOR.run_time(
Пример #9
0
# Clear the display
brick.display.clear()
brick.display.text("Hello", (0, 20))

# Initialize a motors and reset their angles
base_motor = Motor(Port.C)
leg_motor = Motor(Port.A)
infrared = InfraredSensor(Port.S1)
base_motor.reset_angle(0.0)
leg_motor.reset_angle(0.0)
step_angle = 10

print('Hello Robot')

while True:
    # Get current distance
    distance = infrared.distance()
    # Get current angles
    angle_base, angle_leg = utils_motor.get_curr_angle(base_motor, leg_motor)
    print('Angle base:', angle_base)
    print('Angle leg:', angle_leg)
    print('Distance sensor:', distance)
    # Move arm/leg depending on the button pressed
    if Button.UP in brick.buttons():
        utils_motor.base_arm(step_angle, base_motor, leg_motor)
    elif Button.DOWN in brick.buttons():
        utils_motor.base_arm(-step_angle, base_motor, leg_motor)
    elif Button.LEFT in brick.buttons():
        utils_motor.leg_arm(step_angle, leg_motor)
    elif Button.RIGHT in brick.buttons():
        utils_motor.leg_arm(-step_angle, leg_motor)
from random import randint
from time import sleep

BRICK = EV3Brick()

MEDIUM_MOTOR = Motor(port=Port.A, positive_direction=Direction.CLOCKWISE)
TAIL_MOTOR = Motor(port=Port.B, positive_direction=Direction.CLOCKWISE)
CHEST_MOTOR = Motor(port=Port.D, positive_direction=Direction.CLOCKWISE)

IR_SENSOR = InfraredSensor(port=Port.S4)

CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True)

while True:
    if IR_SENSOR.distance() < 30:
        BRICK.light.on(color=Color.RED)

        MEDIUM_MOTOR.stop()

        TAIL_MOTOR.stop()

        BRICK.speaker.play_file(file=SoundFile.SNAKE_HISS)

        CHEST_MOTOR.run_time(speed=1000, time=1000, then=Stop.HOLD, wait=True)

        MEDIUM_MOTOR.run(speed=1000)

        TAIL_MOTOR.run(speed=-1000)

        CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True)
Пример #11
0
# move at the correct speed when you give a motor command.
# The axle track is the distance between the points where the wheels
# touch the ground.
robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=104)

obstacleSensors = ObstacleSensors(robot=robot, color_sensor=reflection_sensor)

# Play a sound to tell us when we are ready to start moving
ev3.speaker.beep()

# def playtone():
#     for j in range(0,20):             # Do twenty times.
#         ev3.speaker.beep()  #1000Hz for 0.2s
#         sleep(0.5)

# t = Thread(target=playtone)
# t.start()
server_socket = ServerSocket()
server_socket.run()

while True:
    # Begin driving forward at 200 millimeters per second.
    robot.drive(200, 0)

    while obstacle_sensor.distance() < 10:
        robot.stop()
        robot.turn(45)
        wait(10)

    obstacleSensors.hole()
STING_MOTOR = Motor(port=Port.D, positive_direction=Direction.CLOCKWISE)

IR_SENSOR = InfraredSensor(port=Port.S4)

MEDIUM_MOTOR.run_time(speed=500, time=1000, then=Stop.HOLD, wait=True)

MEDIUM_MOTOR.run_time(speed=-500, time=0.3 * 1000, then=Stop.HOLD, wait=True)

STING_MOTOR.run_time(speed=400, time=1000, then=Stop.HOLD, wait=True)

for i in range(3):
    GO_MOTOR.run(speed=-1000)

    BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)

    while IR_SENSOR.distance() >= 40:
        pass

    GO_MOTOR.run(speed=250)

    BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)

    STING_MOTOR.run_angle(speed=-750,
                          rotation_angle=220,
                          then=Stop.HOLD,
                          wait=True)

    BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)

    STING_MOTOR.run_time(speed=-1000, time=1000, then=Stop.HOLD, wait=True)
class CrawlingRobotEnv:
    """
    This class will implement a similar environment interface as seen on openAI gym, but actually
    interface with a real world-robot (Lego)
    """
    def __init__(self,
                 invert_reward=False,
                 step_angle=35,
                 run_on_lego=True,
                 no_change_penalty=-20):
        self.n_leg_state = 3
        self.n_feet_state = 3
        self.action_space = self.n_leg_state + self.n_feet_state
        self.observation_space = self.n_leg_state * self.n_feet_state
        # Set Initial state
        self.state = (MotorState.NEUTRAL, MotorState.NEUTRAL)
        self.running_on_lego = run_on_lego
        self.no_change_penalty = no_change_penalty
        if running_on_lego:
            # Initialize a motors
            self.leg_motor = Motor(Port.C)
            self.feet_motor = Motor(Port.A)
            # Initialize sensor for getting distance
            self.infrared = InfraredSensor(Port.S1)
        else:
            self.leg_motor = None
            self.feet_motor = None
            self.infrared = None

        self.motor_step_angle = step_angle
        # Dictionary to convert action indexes to motor commands
        self.action_2_arm = {
            0: (MotorType.LEG, MotorState.NEUTRAL),
            1: (MotorType.LEG, MotorState.UP),
            2: (MotorType.LEG, MotorState.DOWN),
            3: (MotorType.FEET, MotorState.NEUTRAL),
            4: (MotorType.FEET, MotorState.UP),
            5: (MotorType.FEET, MotorState.DOWN)
        }

        # Dictionary to get the angles to move from one motor state to another, the key tuple is (previous, requested)
        self.dict_angle = {
            (MotorState.NEUTRAL, MotorState.NEUTRAL): 0,
            (MotorState.NEUTRAL, MotorState.UP): self.motor_step_angle,
            (MotorState.NEUTRAL, MotorState.DOWN): -self.motor_step_angle,
            (MotorState.UP, MotorState.UP): 0,
            (MotorState.UP, MotorState.NEUTRAL): -self.motor_step_angle,
            (MotorState.UP, MotorState.DOWN): -2 * self.motor_step_angle,
            (MotorState.DOWN, MotorState.DOWN): 0,
            (MotorState.DOWN, MotorState.NEUTRAL): self.motor_step_angle,
            (MotorState.DOWN, MotorState.UP): 2 * self.motor_step_angle,
        }

        # Dictionary that will retain samples of experience, if not on lego mode we will use this to simulate
        # if on lego mode we will capture this
        self.sampled_reward_function = {
            ((2, 1), 5): -7,
            ((0, 2), 5): -20,
            ((1, 2), 5): -20,
            ((0, 0), 2): 0,
            ((0, 1), 4): -20,
            ((2, 1), 0): -6,
            ((0, 2), 0): -20,
            ((0, 0), 5): 0,
            ((2, 2), 2): -20,
            ((0, 2), 3): 0,
            ((2, 0), 4): 0,
            ((2, 1), 2): -20,
            ((2, 1), 4): -20,
            ((0, 1), 3): 0,
            ((1, 2), 4): 0,
            ((1, 1), 1): -20,
            ((1, 1), 5): 0,
            ((1, 2), 3): 0,
            ((2, 0), 2): -20,
            ((2, 0), 3): -20,
            ((1, 0), 5): 0,
            ((1, 2), 0): 0,
            ((0, 0), 1): 0,
            ((2, 0), 0): 0,
            ((0, 0), 3): -20,
            ((2, 2), 0): 0,
            ((0, 0), 4): 0,
            ((0, 2), 4): 0,
            ((0, 2), 2): -4,
            ((2, 2), 1): 0,
            ((1, 0), 4): 0,
            ((2, 2), 3): 4,
            ((2, 2), 5): -20,
            ((1, 1), 3): 0,
            ((0, 1), 2): 0,
            ((1, 0), 3): -20,
            ((1, 1), 2): 0,
            ((0, 0), 0): -20,
            ((1, 0), 0): 0,
            ((0, 1), 0): -20,
            ((0, 1), 1): 0,
            ((1, 2), 1): -20,
            ((2, 1), 1): 0,
            ((2, 1), 3): 0,
            ((1, 2), 2): 0,
            ((1, 1), 0): 0,
            ((0, 2), 1): 0,
            ((1, 0), 2): 0,
            ((0, 1), 5): -22,
            ((2, 0), 5): -4,
            ((2, 2), 4): 13
        }

        self.sampled_transition_function = {}

        # Populate dictionary to convert state tuple to state indexes
        self.state_2_index = self.__get_tuple_2_index()
        self.invert_reward = invert_reward

    def reset(self):
        """
        Reset robot internal states, and rotor angles, before calling this functions check
        if robot leg/feet are into neutral angles
        :return:
        """
        if self.running_on_lego:
            self.leg_motor.reset_angle(0.0)
            self.feet_motor.reset_angle(0.0)

        # Set Initial internal state
        self.state = (MotorState.NEUTRAL, MotorState.NEUTRAL)
        return self.state_2_index[self.state]

    def step(self, action):
        """
        Our RL Agent will interface with our robot through this function, the agent will send an action index
        and the environment(this class) will answer back a tuple (next_state, reward, end_game, other_info)
        :param action: Action index from (0..self.action_space-1)
        :return: next_state, reward, False, other_info
        """
        motor_action = self.action_2_arm[action]
        if running_on_lego:
            # Get distance before move
            # wait(100)
            distance_before_move = self.infrared.distance()
        else:
            distance_before_move = 0

        # Send command to motors, and update state
        state = self.state
        no_state_change_penalty = self.__control_motors(motor_action)
        if running_on_lego:
            # wait(300)
            # Get distance travelled after motors did some job(reward)
            distance_after_move = self.infrared.distance()
            reward = distance_after_move - distance_before_move

            # Try to filter out noise
            if abs(reward) < 3:
                reward = 0

            # Invert the reward if needed
            if self.invert_reward:
                reward *= -1
        else:
            reward = self.mdp_immediate_reward(state, action)

        # Penalty for doing command that doesn't change state
        reward += no_state_change_penalty

        next_state = self.state_2_index[self.state]

        # Save on dictionary the pair state, action, reward
        self.record_mdp(state, action, reward, next_state)

        # Return (next_state, reward, done, some_info)
        return next_state, reward, False, {}

    def __control_motors(self, motor_action):
        """
        Receive an action tupple (Motor, Action) and apply on the motor
        This function also need to take into account the safety of the robot, for example
        if the robot motor is already UP we will ignore another UP command to avoid breaking it
        :param motor_action: Input tupple
        :return: Next state after the action
        """
        no_change_penalty = 0
        motor, position = motor_action
        # Convert state to list, make changes and bring back to tuple
        self.state = list(self.state)
        # Get current leg/feet position
        curr_leg_pos = self.state[0]
        curr_feet_pos = self.state[1]
        if motor == MotorType.LEG:
            # Only change motor for different positions
            if position != curr_leg_pos:
                # Update state for motor 0
                self.state[0] = position
                if self.running_on_lego:
                    angle = self.dict_angle[curr_leg_pos, position]
                    utils_motor.leg(angle, self.leg_motor, self.feet_motor)
            else:
                no_change_penalty = self.no_change_penalty
        elif motor == MotorType.FEET:
            # Only change motor for different positions
            if position != curr_feet_pos:
                # Update state for motor 1
                self.state[1] = position
                if self.running_on_lego:
                    angle = self.dict_angle[curr_feet_pos, position]
                    if position == MotorState.UP:
                        angle += 5
                    if position == MotorState.DOWN:
                        angle -= 5
                    utils_motor.feet(angle, self.feet_motor)
            else:
                no_change_penalty = self.no_change_penalty
        # Convert back to tuple
        self.state = tuple(self.state)
        return no_change_penalty

    def __get_tuple_2_index(self):
        """
        Utility function used to convert the internal state-space tuples ie:(NEUTRAL, UP) to state index
        0,1,2 ...
        :return: Index for state given one of possible state-space tuples.
        """
        leg_state_space, feet_state_space = self.n_leg_state, self.n_feet_state
        tuple_2_index = {}
        index = 0
        for x in range(leg_state_space):
            for y in range(feet_state_space):
                tuple_2_index[(MotorState.val(x), MotorState.val(y))] = index
                index += 1
        return tuple_2_index

    def mdp_immediate_reward(self, state, action):
        """
        Execute the immediate reward function (we get this from experiment actions manually on the robot)
        :param action:
        :param state:
        :return:
        Action table:
        0: (MotorType.LEG, MotorState.NEUTRAL),
        1: (MotorType.LEG, MotorState.UP),
        2: (MotorType.LEG, MotorState.DOWN),
        3: (MotorType.FEET, MotorState.NEUTRAL),
        4: (MotorType.FEET, MotorState.UP),
        5: (MotorType.FEET, MotorState.DOWN)

        State tuple: (LEG, FEET)
        """
        try:
            reward = self.sampled_reward_function[state, action]
        except KeyError:
            print('Experience:', state, 'not recorded')
            reward = 0
        return reward

    def __str__(self):
        """
        Return the current internal state description
        :return:
        """
        arm_idx, feet_idx = self.state
        arm_desc = MotorState.desc(arm_idx)
        feet_desc = MotorState.desc(feet_idx)
        description_state = 'leg:' + arm_desc + ' feet:' + feet_desc
        return description_state

    def state_idx_to_str(self, state_idx):
        """
        Return the description of a state index
        :param state_idx: State index
        :return:
        """
        state = list(self.state_2_index.keys())[list(
            self.state_2_index.values()).index(state_idx)]
        arm_idx, feet_idx = state
        arm_desc = MotorState.desc(arm_idx)
        feet_desc = MotorState.desc(feet_idx)
        description_state = 'leg:' + arm_desc + ' feet:' + feet_desc
        return description_state

    def action_idx_to_str(self, action_idx):
        motor, motor_state = self.action_2_arm[action_idx]
        motor_str = MotorType.desc(motor)
        action_str = MotorState.desc(motor_state)
        description_action = motor_str + ' ' + action_str + ' idx:' + str(
            action_idx)
        return description_action

    def record_mdp(self, state, action, reward, next_state):
        """
        Use the state/action/reward/next_state pairs from real-life experience to populate the MDP
        :param state: Current state
        :param action: action
        :param reward: Immediate reward of doing an "action" at state "state"
        :param next_state: Next state after doing an "action" at state "state"
        :return: None
        """
        if self.running_on_lego:
            if (state, action) in self.sampled_reward_function:
                old_reward = self.sampled_reward_function[state, action]
                if old_reward != reward:
                    print(
                        '\t\t****Reward change for same state/action pair: (OLD)',
                        old_reward, '(NEW)', reward)
                self.sampled_reward_function[state, action] = reward
            else:
                self.sampled_reward_function[state, action] = reward

            if (state, action) in self.sampled_transition_function:
                previous_next_state = self.sampled_transition_function[state,
                                                                       action]
                if previous_next_state != next_state:
                    raise ValueError(
                        'This should never happen on this robot ...')
            self.sampled_transition_function[state, action] = next_state

    def read_sensor(self):
        return self.infrared.distance()
Пример #14
0
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase

# Write your program here
brick.sound.beep(500, 500)
print("Hallo Welt!")

# Motor an Port B
motor_m = Motor(Port.B)
motor_m.run_target(500, -80)
motor_l = Motor(Port.A)
motor_l.run_target(500, 360)
motor_r = Motor(Port.D)
motor_r.run_target(500, 360)

robot = DriveBase(motor_l, motor_r, 30, 160)

sensor = InfraredSensor(Port.S4)

dist = sensor.distance()
print("Abstand: ", dist)
while dist > 10:
    robot.drive_time(50, 90, 500)
    dist = sensor.distance()
    print("Abstand: ", dist)
robot.stop()

brick.sound.beep(1500, 500)
brick.sound.beep(300, 500)
brick.sound.beep(100, 500)
Пример #15
0
    elif Button.LEFT_DOWN in buttons:
        ev3.speaker.play_file(Sound.LEFT)
        ev3.speaker.play_file(Sound.DOWN)
    elif Button.RIGHT_UP in buttons:
        ev3.speaker.play_file(Sound.RIGHT)
        ev3.speaker.play_file(Sound.UP)
    elif Button.RIGHT_DOWN in buttons:
        ev3.speaker.play_file(Sound.RIGHT)
        ev3.speaker.play_file(Sound.DOWN)

ev3.speaker.say('come to me')

dist = 100
while dist > 30:
    wait(10)
    dist = ir.distance()
    ev3.screen.print('Distance: ', dist, 'mm')

ev3.speaker.say('come to me on channel 1')

dist = 100
angle = 0
while dist > 10:
    wait(10)
    dist, angle = ir.beacon(1)
    ev3.screen.print('Distance: ', dist*2, 'mm')
    ev3.screen.print('Angle: ', angle, 'deg')

ev3.speaker.say('last try, use channel 1')

while True:
Пример #16
0
class Ev3rstorm(EV3Brick):
    WHEEL_DIAMETER = 26  # milimeters
    AXLE_TRACK = 102  # milimeters

    def __init__(self,
                 left_foot_motor_port: Port = Port.B,
                 right_foot_motor_port: Port = Port.C,
                 bazooka_blast_motor_port: Port = Port.A,
                 touch_sensor_port: Port = Port.S1,
                 color_sensor_port: Port = Port.S3,
                 ir_sensor_port: Port = Port.S4,
                 ir_beacon_channel: int = 1):
        self.drive_base = DriveBase(
            left_motor=Motor(port=left_foot_motor_port,
                             positive_direction=Direction.CLOCKWISE),
            right_motor=Motor(port=right_foot_motor_port,
                              positive_direction=Direction.CLOCKWISE),
            wheel_diameter=self.WHEEL_DIAMETER,
            axle_track=self.AXLE_TRACK)

        self.bazooka_blast_motor = Motor(
            port=bazooka_blast_motor_port,
            positive_direction=Direction.CLOCKWISE)

        self.touch_sensor = TouchSensor(port=touch_sensor_port)
        self.color_sensor = ColorSensor(port=color_sensor_port)

        self.ir_sensor = InfraredSensor(port=ir_sensor_port)
        self.ir_beacon_channel = ir_beacon_channel

    def drive_once_by_ir_beacon(
            self,
            speed: float = 1000,  # mm/s
            turn_rate: float = 90  # rotational speed deg/s
    ):
        ir_beacon_button_pressed = set(
            self.ir_sensor.buttons(channel=self.ir_beacon_channel))

        # forward
        if ir_beacon_button_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
            self.drive_base.drive(speed=speed, turn_rate=0)

        # backward
        elif ir_beacon_button_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
            self.drive_base.drive(speed=-speed, turn_rate=0)

        # turn left on the spot
        elif ir_beacon_button_pressed == {Button.LEFT_UP, Button.RIGHT_DOWN}:
            self.drive_base.drive(speed=0, turn_rate=-turn_rate)

        # turn right on the spot
        elif ir_beacon_button_pressed == {Button.RIGHT_UP, Button.LEFT_DOWN}:
            self.drive_base.drive(speed=0, turn_rate=turn_rate)

        # turn left forward
        elif ir_beacon_button_pressed == {Button.LEFT_UP}:
            self.drive_base.drive(speed=speed, turn_rate=-turn_rate)

        # turn right forward
        elif ir_beacon_button_pressed == {Button.RIGHT_UP}:
            self.drive_base.drive(speed=speed, turn_rate=turn_rate)

        # turn left backward
        elif ir_beacon_button_pressed == {Button.LEFT_DOWN}:
            self.drive_base.drive(speed=-speed, turn_rate=turn_rate)

        # turn right backward
        elif ir_beacon_button_pressed == {Button.RIGHT_DOWN}:
            self.drive_base.drive(speed=-speed, turn_rate=-turn_rate)

        # otherwise stop
        else:
            self.drive_base.stop()

    def keep_driving_by_ir_beacon(
            self,
            speed: float = 1000,  # mm/s
            turn_rate: float = 90  # rotational speed deg/s
    ):
        while True:
            self.drive_once_by_ir_beacon(speed=speed, turn_rate=turn_rate)

    def dance_whenever_ir_beacon_pressed(self):
        while True:
            while Button.BEACON in self.ir_sensor.buttons(
                    channel=self.ir_beacon_channel):
                self.drive_base.turn(angle=randint(-360, 360))

    def keep_detecting_objects_by_ir_sensor(self):
        while True:
            if self.ir_sensor.distance() < 25:
                self.light.on(color=Color.RED)
                self.speaker.play_file(file=SoundFile.OBJECT)
                self.speaker.play_file(file=SoundFile.DETECTED)
                self.speaker.play_file(file=SoundFile.ERROR_ALARM)

            else:
                self.light.off()

    def blast_bazooka_whenever_touched(self):
        MEDIUM_MOTOR_N_ROTATIONS_PER_BLAST = 3
        MEDIUM_MOTOR_ROTATIONAL_DEGREES_PER_BLAST = MEDIUM_MOTOR_N_ROTATIONS_PER_BLAST * 360

        while True:
            if self.touch_sensor.pressed():
                if self.color_sensor.ambient() < 5:  # 15 not dark enough
                    self.speaker.play_file(file=SoundFile.UP)

                    self.bazooka_blast_motor.run_angle(
                        speed=2 *
                        MEDIUM_MOTOR_ROTATIONAL_DEGREES_PER_BLAST,  # shoot quickly in half a second
                        rotation_angle=
                        -MEDIUM_MOTOR_ROTATIONAL_DEGREES_PER_BLAST,
                        then=Stop.HOLD,
                        wait=True)

                    self.speaker.play_file(file=SoundFile.LAUGHING_1)

                else:
                    self.speaker.play_file(file=SoundFile.DOWN)

                    self.bazooka_blast_motor.run_angle(
                        speed=2 *
                        MEDIUM_MOTOR_ROTATIONAL_DEGREES_PER_BLAST,  # shoot quickly in half a second
                        rotation_angle=
                        MEDIUM_MOTOR_ROTATIONAL_DEGREES_PER_BLAST,
                        then=Stop.HOLD,
                        wait=True)

                    self.speaker.play_file(file=SoundFile.LAUGHING_2)

    def main(
            self,
            driving_speed: float = 1000  # mm/s
    ):
        self.screen.load_image(ImageFile.TARGET)

        # FIXME: following thread seems to fail to run
        Thread(target=self.dance_whenever_ir_beacon_pressed).start()

        # DON'T use IR Sensor in 2 different modes in the same program / loop
        # - https://github.com/pybricks/support/issues/62
        # - https://github.com/ev3dev/ev3dev/issues/1401
        # Thread(target=self.keep_detecting_objects_by_ir_sensor).start()

        Thread(target=self.blast_bazooka_whenever_touched).start()

        self.keep_driving_by_ir_beacon(speed=driving_speed)
Пример #17
0
right_motor = Motor(Port.C)
# The wheel diameter of the Robot Educator is 56 millimeters. # The distance between wheels (axle_track) is 114 millimeters.
wheel_diameter = 43.2
axle_track = 145
# Create a DriveBase object. The wheel_diameter and axle_track values are needed to move robot correct speed/distance when you give drive commands.
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)

# This drives at 100 mm/sec straight

import random
a = random.randint(-90, 90)

while True:
    robot.drive(200, 0)

    while irSensor.distance() < 50:
        a = random.randint(0, 100)
        b = random.randint(45, 180)
        c = random.randint(-180, -45)
        robot.straight(-50)
        if a <= 50:
            robot.turn(b)
        else:
            robot.turn(c)

    while touchsensor.pressed():
        robot.straight(-50)
        a = random.randint(0, 100)
        b = random.randint(45, 180)
        c = random.randint(-180, -45)
        if a <= 50:
Пример #18
0
# Initialize sensors used to detect obstacles as the robot drives around.
obstacle_sensor = InfraredSensor(Port.S4)
touch_sensor = TouchSensor(Port.S1)
color_sensor = ColorSensor(Port.S3)

# Initialize left and right motors of the drive base with default settings on Port B and Port C.
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)

wheel_diameter = 43  # The wheel diameter of the Robot Educator in millimeters.
axle_track = 114  # The axle track is the distance between the centers of each of the wheels.

# The DriveBase is composed of 2 motors with a wheel on each. The wheel_diameter and axle_track
#  values are used to make the motors move at the correct speed when you give a motor command.
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)

# Make the robot drive forward until it detects an obstacle. Then back up and turn around.
while True:
    robot.drive(200, 0)  # Begin driving forward at 200 millimeters per second.
    # Wait until an obstacle is detected. This is done by repeatedly doing nothing (waiting for
    #  10 milliseconds) while the measured distance is still greater than 30 mm.
    while obstacle_sensor.distance() > 30 and touch_sensor.pressed() == False:
        print("Distance     : {0}".format(obstacle_sensor.distance()))
        print("Color        : {0}".format(color_sensor.color()))
        print("Ambient      : {0}".format(color_sensor.ambient()))
        print("Reflection   : {0}".format(color_sensor.reflection()))
        wait(10)
    # Drive backward at 100 millimeters per second. Keep going for 2 seconds.
    robot.drive_time(-100, 0, 2000)
    # Turn around at 60 degrees/second around the midpoint between the wheels for 2 seconds.
    robot.drive_time(0, 60, 2000)
Пример #19
0
        ev3.light.on(Color.YELLOW)

        ''' setting manual mode for sound '''
        if Button.RIGHT_UP in infraredSensor.buttons(2):
            manualSound = True

        manualLightControl()
        
        manualLightControlButtons()
        manualSoundControlButtons()

        soundControl()

        ''' controlling the room state '''
        if watch.time() > 7000:
            if sensor.distance() < 35:
                watch.reset()
                if active == True:
                    ev3.speaker.play_file(SoundFile.GOODBYE)
                    active = False
                else:
                    switchSound()
                    ev3.speaker.play_file(SoundFile.HELLO)
                    active = True


    elif manualLight == False and manualSound == True:

        ev3.light.off()

        ''' setting manual mode for light '''
RIGHT_MOTOR = Motor(port=Port.C, positive_direction=Direction.CLOCKWISE)
DRIVE_BASE = DriveBase(left_motor=LEFT_MOTOR,
                       right_motor=RIGHT_MOTOR,
                       wheel_diameter=26,
                       axle_track=115)
DRIVE_BASE.settings(
    straight_speed=750,  # milimeters per second
    straight_acceleration=750,
    turn_rate=90,  # degrees per second
    turn_acceleration=90)

IR_SENSOR = InfraredSensor(port=Port.S4)

MEDIUM_MOTOR.run_time(speed=-500, time=1000, then=Stop.COAST, wait=True)

while IR_SENSOR.distance() >= 25:
    DRIVE_BASE.drive(speed=750, turn_rate=0)

DRIVE_BASE.stop()

EV3_BRICK.speaker.play_file(file=SoundFile.AIRBRAKE)

MEDIUM_MOTOR.run_time(speed=500, time=1000, then=Stop.COAST, wait=True)

DRIVE_BASE.turn(angle=180)

while IR_SENSOR.distance() >= 25:
    DRIVE_BASE.drive(speed=750, turn_rate=0)

DRIVE_BASE.stop()