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
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)
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
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
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)
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)
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(
# 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)
# 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()
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)
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:
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)
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:
# 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)
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()