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