class RoboDoz3r(RemoteControlledTank):
WHEEL_DIAMETER = 24 # milimeters
AXLE_TRACK = 100 # milimeters
def __init__(
self,
left_motor_port: Port = Port.C, right_motor_port: Port = Port.B,
shovel_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
tank_drive_ir_beacon_channel: int = 1,
shovel_control_ir_beacon_channel: int = 4):
super().__init__(
wheel_diameter=self.WHEEL_DIAMETER, axle_track=self.AXLE_TRACK,
left_motor_port=left_motor_port, right_motor_port=right_motor_port,
polarity='inversed',
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=tank_drive_ir_beacon_channel)
self.ev3_brick = EV3Brick()
self.shovel_motor = Motor(port=shovel_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.shovel_control_ir_beacon_channel = \
shovel_control_ir_beacon_channel
def raise_or_lower_shovel_by_ir_beacon(self):
"""
If the channel 4 is selected on the IR remote
then you can control raising and lowering the shovel on the RoboDoz3r.
- Raise the shovel by either Up button
- Raise the shovel by either Down button
"""
ir_beacon_button_pressed = \
set(self.ir_sensor.buttons(
channel=self.shovel_control_ir_beacon_channel))
# raise the shovel
if ir_beacon_button_pressed.intersection(
{Button.LEFT_UP, Button.RIGHT_UP}):
self.shovel_motor.run(speed=100)
# lower the shovel
elif ir_beacon_button_pressed.intersection(
{Button.LEFT_DOWN, Button.RIGHT_DOWN}):
self.shovel_motor.run(speed=-100)
else:
self.shovel_motor.hold()
class R3ptar:
"""
R3ptar can be driven around by the IR Remote Control,
strikes when the Beacon button is pressed,
and hisses when the Touch Sensor is pressed
(inspiration from LEGO Mindstorms EV3 Home Edition: R3ptar: Tutorial #4)
"""
def __init__(self,
steering_motor_port: Port = Port.A,
driving_motor_port: Port = Port.B,
striking_motor_port: Port = Port.D,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.ev3_brick = EV3Brick()
self.steering_motor = Motor(port=steering_motor_port,
positive_direction=Direction.CLOCKWISE)
self.driving_motor = Motor(port=driving_motor_port,
positive_direction=Direction.CLOCKWISE)
self.striking_motor = Motor(port=striking_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 drive_by_ir_beacon(
self,
speed: float = 1000, # mm/s
):
ir_beacons_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.driving_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.LEFT_UP}:
self.steering_motor.run(speed=-500)
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.RIGHT_UP}:
self.steering_motor.run(speed=500)
self.driving_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN}:
self.steering_motor.run(speed=-500)
self.driving_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.RIGHT_DOWN}:
self.steering_motor.run(speed=500)
self.driving_motor.run(speed=-speed)
else:
self.steering_motor.hold()
self.driving_motor.stop()
def strike_by_ir_beacon(self, speed: float = 1000):
if Button.BEACON in \
self.ir_sensor.buttons(channel=self.ir_beacon_channel):
self.striking_motor.run_time(speed=speed,
time=1000,
then=Stop.COAST,
wait=True)
self.striking_motor.run_time(speed=-speed,
time=1000,
then=Stop.COAST,
wait=True)
while Button.BEACON in \
self.ir_sensor.buttons(channel=self.ir_beacon_channel):
pass
def hiss_if_touched(self):
if self.touch_sensor.pressed():
self.ev3_brick.speaker.play_file(file=SoundFile.SNAKE_HISS)
def main(self, speed: float = 1000):
"""
R3ptar's main program performing various capabilities
"""
while True:
self.drive_by_ir_beacon(speed=speed)
self.strike_by_ir_beacon(speed=speed)
self.hiss_if_touched()
wait(1)
class Rac3Truck:
WHEEL_DIAMETER = 30 # milimeters
AXLE_TRACK = 120 # milimeters
def __init__(
self,
left_motor_port: str = Port.B, right_motor_port: str = Port.C,
polarity: str = 'inversed',
steer_motor_port: str = Port.A,
ir_sensor_port: str = Port.S4, ir_beacon_channel: int = 1):
if polarity == 'normal':
self.left_motor = Motor(port=left_motor_port,
positive_direction=Direction.CLOCKWISE)
self.right_motor = Motor(port=right_motor_port,
positive_direction=Direction.CLOCKWISE)
else:
self.left_motor = \
Motor(port=left_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.right_motor = \
Motor(port=right_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.driver = DriveBase(left_motor=self.left_motor,
right_motor=self.right_motor,
wheel_diameter=self.WHEEL_DIAMETER,
axle_track=self.AXLE_TRACK)
self.steer_motor = Motor(port=steer_motor_port,
positive_direction=Direction.CLOCKWISE)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def reset(self):
self.steer_motor.run_time(
speed=300,
time=1500,
then=Stop.COAST,
wait=True)
self.steer_motor.run_angle(
speed=-500,
rotation_angle=120,
then=Stop.HOLD,
wait=True)
self.steer_motor.reset_angle(angle=0)
def steer_left(self):
if self.steer_motor.angle() > -65:
self.steer_motor.run_target(
speed=-200,
target_angle=-65,
then=Stop.HOLD,
wait=True)
else:
self.steer_motor.hold()
def steer_right(self):
if self.steer_motor.angle() < 65:
self.steer_motor.run_target(
speed=200,
target_angle=65,
then=Stop.HOLD,
wait=True)
else:
self.steer_motor.hold()
def steer_center(self):
if self.steer_motor.angle() < -7:
self.steer_motor.run_target(
speed=200,
target_angle=4,
then=Stop.HOLD,
wait=True)
elif self.steer_motor.angle() > 7:
self.steer_motor.run_target(
speed=-200,
target_angle=-4,
then=Stop.HOLD,
wait=True)
self.steer_motor.hold()
wait(100)
def drive_by_ir_beacon(self):
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.driver.drive(
speed=800,
turn_rate=0)
self.steer_center()
# backward
elif ir_beacon_button_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.driver.drive(
speed=-800,
turn_rate=0)
self.steer_center()
# turn left forward
elif ir_beacon_button_pressed == {Button.LEFT_UP}:
self.left_motor.run(speed=600)
self.right_motor.run(speed=1000)
self.steer_left()
# turn right forward
elif ir_beacon_button_pressed == {Button.RIGHT_UP}:
self.left_motor.run(speed=1000)
self.right_motor.run(speed=600)
self.steer_right()
# turn left backward
elif ir_beacon_button_pressed == {Button.LEFT_DOWN}:
self.left_motor.run(speed=-600)
self.right_motor.run(speed=-1000)
self.steer_left()
# turn right backward
elif ir_beacon_button_pressed == {Button.RIGHT_DOWN}:
self.left_motor.run(speed=-1000)
self.right_motor.run(speed=-600)
self.steer_right()
# otherwise stop
else:
self.driver.stop()
self.steer_center()
robot.stop()
while Button.CENTER not in ev3.buttons.pressed():
pass
#treadmill
accangle = 0
gyroSensor.reset_angle(0)
straight(270, 200)
line_follow(rightcolor, 175, 1.6, 0.04, 0.1, 0.01,
1200) # arives at treadmill
#turntoangle(0.01)
robot.drive(95, -1.5) #80, -2
mediummotor2.run_time(300, 1000, wait=True) #rolls up platform
wait(900) #1500
robot.stop()
leftmotor.hold()
rightmotor.hold()
mediummotor2.run_time(1000, 3500) #runs treadmill
accangle = 1
oldstraight(2000, -500) # goes back
#health units, minifigure on tire, weight machine, drop bricks
elif Button.LEFT in ev3.buttons.pressed():
# set point 1 for gyro (parallel to south wall)
accangle = 0
gyroSensor.reset_angle(0)
#health units
straight(250, 200) #move to first line follow
line_follow(rightcolor, 200, 1.6, 0.02, 0.1, 0.01,
420) #linefollow to health unit spot
# Set up the Color Sensor. This sensor detects when the elbow
# is in the starting position. This is when the sensor sees the
# white beam up close.
elbow_sensor = ColorSensor(Port.S3)
# Initialize the elbow. First make it go down for one second.
# Then make it go upwards slowly (15 degrees per second) until
# the Color Sensor detects the white beam. Then reset the motor
# angle to make this the zero point. Finally, hold the motor
# in place so it does not move.
elbow_motor.run_time(-30, 1000)
elbow_motor.run(15)
while elbow_sensor.reflection() < 32:
wait(10)
elbow_motor.reset_angle(0)
elbow_motor.hold()
# Initialize the base. First rotate it until the Touch Sensor
# in the base is pressed. Reset the motor angle to make this
# the zero point. Then hold the motor in place so it does not move.
base_motor.run(-60)
while not base_switch.pressed():
wait(10)
base_motor.reset_angle(0)
base_motor.hold()
# Initialize the gripper. First rotate the motor until it stalls.
# Stalling means that it cannot move any further. This position
# corresponds to the closed position. Then rotate the motor
# by 90 degrees such that the gripper is open.
gripper_motor.run_until_stalled(200, then=Stop.COAST, duty_limit=50)
class Bobb3e:
WHEEL_DIAMETER = 24 # milimeters
AXLE_TRACK = 100 # milimeters
def __init__(self,
left_motor_port: str = Port.B,
right_motor_port: str = Port.C,
lift_motor_port: str = Port.A,
ir_sensor_port: str = Port.S4,
ir_beacon_channel: int = 1):
self.ev3_brick = EV3Brick()
left_motor = Motor(port=left_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
right_motor = Motor(port=right_motor_port,
positive_direction=Direction.COUNTERCLOCKWISE)
self.drive_base = DriveBase(left_motor=left_motor,
right_motor=right_motor,
wheel_diameter=self.WHEEL_DIAMETER,
axle_track=self.AXLE_TRACK)
self.lift_motor = Motor(port=lift_motor_port,
positive_direction=Direction.CLOCKWISE)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.reversing = False
def drive_or_operate_forks_by_ir_beacon(
self,
driving_speed: float = 1000, # mm/s
turn_rate: float = 90 # rotational speed deg/s
):
"""
Read the commands from the remote control and convert them into actions
such as go forward, lift and turn.
"""
while True:
ir_beacon_button_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
# lower the forks
if ir_beacon_button_pressed == {Button.LEFT_UP, Button.LEFT_DOWN}:
self.reversing = False
self.drive_base.stop()
self.lift_motor.run(speed=100)
# raise the forks
elif ir_beacon_button_pressed == \
{Button.RIGHT_UP, Button.RIGHT_DOWN}:
self.reversing = False
self.drive_base.stop()
self.lift_motor.run(speed=-100)
# forward
elif ir_beacon_button_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.reversing = False
self.drive_base.drive(speed=driving_speed, turn_rate=0)
self.lift_motor.hold()
# backward
elif ir_beacon_button_pressed == \
{Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.reversing = True
self.drive_base.drive(speed=-driving_speed, turn_rate=0)
self.lift_motor.hold()
# turn left on the spot
elif ir_beacon_button_pressed == \
{Button.LEFT_UP, Button.RIGHT_DOWN}:
self.reversing = False
self.drive_base.drive(speed=0, turn_rate=-turn_rate)
self.lift_motor.hold()
# turn right on the spot
elif ir_beacon_button_pressed == \
{Button.RIGHT_UP, Button.LEFT_DOWN}:
self.reversing = False
self.drive_base.drive(speed=0, turn_rate=turn_rate)
self.lift_motor.hold()
# turn left forward
elif ir_beacon_button_pressed == {Button.LEFT_UP}:
self.reversing = False
self.drive_base.drive(speed=driving_speed,
turn_rate=-turn_rate)
self.lift_motor.hold()
# turn right forward
elif ir_beacon_button_pressed == {Button.RIGHT_UP}:
self.reversing = False
self.drive_base.drive(speed=driving_speed, turn_rate=turn_rate)
self.lift_motor.hold()
# turn left backward
elif ir_beacon_button_pressed == {Button.LEFT_DOWN}:
self.reversing = True
self.drive_base.drive(speed=-driving_speed,
turn_rate=turn_rate)
self.lift_motor.hold()
# turn right backward
elif ir_beacon_button_pressed == {Button.RIGHT_DOWN}:
self.reversing = True
self.drive_base.drive(speed=-driving_speed,
turn_rate=-turn_rate)
self.lift_motor.hold()
# otherwise stop
else:
self.reversing = False
self.drive_base.stop()
self.lift_motor.hold()
wait(10)
def sound_alarm_whenever_reversing(self):
while True:
if self.reversing:
self.ev3_brick.speaker.play_file(file=SoundFile.BACKING_ALERT)
wait(10)
class Robot:
def __init__(self):
"""Class that represents the robot
"""
try:
self.state = "Port 1: Right Color"
self.right_color = ColorSensor(Port.S1)
self.state = "Port 2: Center Color"
self.center_color = ColorSensor(Port.S2)
self.state = "Port 3: Left Color"
self.left_color = ColorSensor(Port.S3)
self.state = "Port 4: Gyro"
self.gyro = GyroSensor(Port.S4, Direction.COUNTERCLOCKWISE)
self.state = "Port A: Left Motor"
self.left_motor = Motor(Port.A)
self.state = "Port B: Right Motor"
self.right_motor = Motor(Port.B)
self.state = "Port C: Linear Gear"
self.linear_attachment_motor = Motor(Port.C)
self.state = "Port D: Block Dropper"
self.dropper_attachment_motor = Motor(Port.D)
self.wheel_diameter = 55
self.axle_track = 123
self.drive_base = DriveBase(self.left_motor, self.right_motor,
self.wheel_diameter, self.axle_track)
self.state = "OK"
self.clock = StopWatch()
self.dance_clock = 0
self.sign = 1
except:
brick.screen.clear()
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.draw_text(0, 20, "Error!")
brick.screen.draw_text(0, 40, self.state)
def display_sensor_values(self):
"""Displays sensor values
"""
gyro_value = "Gyro : {}".format(self.gyro.angle())
left_color_value = "Left Color : {}".format(
self.left_color.reflection())
right_color_value = "Right Color : {}".format(
self.right_color.reflection())
center_color_value = "Center Color : {}".format(
self.center_color.reflection())
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.clear()
brick.screen.draw_text(0, 20, gyro_value)
brick.screen.draw_text(0, 40, left_color_value)
brick.screen.draw_text(0, 60, right_color_value)
brick.screen.draw_text(0, 80, center_color_value)
def is_ok(self):
"""Tells if all sensors are plugged in
:return: Checks the state of the sensors
:rtype: Boolean
"""
return self.state == "OK"
def beep(self, is_down=False):
"""Plays a series of beeps
:param is_down: Tells if to play series downwards, defaults to False
:type is_down: bool, optional
"""
beep_counts = range(1, 7) if not is_down else range(6, 0, -1)
for beep_count in beep_counts:
brick.speaker.beep(400 * beep_count, 100)
wait(20)
def drift_check(self):
brick.speaker.beep(1200, 500)
wait(100)
brick.speaker.beep(1200, 500)
drift = False
start_gyro = self.gyro.angle()
brick.screen.clear()
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.draw_text(0, 20, "Checking Gyro drift...")
wait(2000)
if start_gyro != self.gyro.angle():
brick.screen.draw_text(0, 60, "Error!")
brick.screen.draw_text(0, 80, "Gyro drift")
drift = True
return drift
def print_sensor_values(self):
"""Display robot sensor values. For debugging only
"""
print("Gyro: ", self.gyro.angle())
print("Left Color: ", self.left_color.reflection())
print("Right Color: ", self.right_color.reflection())
print("Center Color: ", self.center_color.reflection())
def stop_motors(self):
""" Stops all motors
"""
self.left_motor.stop(Stop.BRAKE)
self.right_motor.stop(Stop.BRAKE)
self.linear_attachment_motor.stop(Stop.BRAKE)
def drive(self, pid, speed, target_angle, duration):
"""Drives the robot using a gyro to a specific angle
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Class
:param speed: Speed of the robot
:type speed: Number
:param target_angle: Angle to drive the robot at
:type target_angle: Number
:param duration: Duration the function is run
:type duration: Number
"""
# Inititialize values
pid.reset()
while pid.clock.time() < duration:
# Calculatr error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def drive_dead_reckon(self, speed, duration, steering=0):
self.drive_base.drive(speed, steering)
wait(duration)
self.drive_base.stop(Stop.BRAKE)
def turn(self, pid, target_angle, tolerance=1):
"""Turns the robot to a specific angle.
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Number
:param target_angle: Angle the robot turns to
:type target_angle: Number
:param tolerance: How close to the target angle you want the robot to be
:type tolerance: Number
"""
# Inititialize values
pid.reset()
error = tolerance + 1
min_speed = 50
while abs(error) > tolerance:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Call Pid compute_steering
steering = pid.compute_steering(error) * -1
# Set speed using a min_speed
right_speed = max(min_speed, abs(steering))
if steering < 0:
right_speed = -1 * right_speed
left_speed = -1 * right_speed
# Run motors
self.left_motor.run(left_speed)
self.right_motor.run(right_speed)
# Stop motors
self.left_motor.stop()
self.right_motor.stop()
def follow_line(self, pid, speed, duration, which_sensor, which_edge):
"""Follows the line using a color sensor.
:param pid: Uses Pid instance with parameters set beforehand
:type pid: Pid
:param speed: Speed of the Robot
:type speed: Number
:param duration: Duration of the function
:type duration: Number
:param which_sensor: The sensor the robot uses to follow the line
:type which_sensor: LineSensor
:param which_edge: Which side the white is on relative to the robot
:type which_edge: LineEdge
"""
# Inititialize values
pid.reset()
while pid.clock.time() < duration:
# Selecting which sensor to use using an Enum
if which_sensor == LineSensor.RIGHT:
error = 50 - self.right_color.reflection()
if which_sensor == LineSensor.LEFT:
error = 50 - self.left_color.reflection()
if which_sensor == LineSensor.CENTER:
error = 50 - self.center_color.reflection()
# Selecting which edge of the line to use
if which_edge == LineEdge.RIGHT:
pass
else:
error = error * -1
# Calculate steering
steering = pid.compute_steering(error)
# Run motors
self.drive_base.drive(speed, steering)
self.drive_base.stop(Stop.BRAKE)
def stop_on_white(self,
pid,
speed,
target_angle,
which_sensor,
color_value=80):
""" Gyro drives until given color sensor is on white
:param pid: PID setting of drive
:type pid: Number
:param speed: The speed the robot moves at
:type speed: Number
:param target_angle: the angle the gyro drives at
:type target_angle:
:param which_sensor: Chooses which color sensor to use
:type which_sensor: Enum
:param color_value: The value of color that the robot stops at
:type color_value: Number
"""
# Inititialize values
sensor = 0
pid.reset()
target_angle = target_angle % 360
if which_sensor == LineSensor.LEFT:
sensor = self.left_color
elif which_sensor == LineSensor.RIGHT:
sensor = self.right_color
else:
sensor = self.center_color
while sensor.reflection() < color_value:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def stop_on_black(self,
pid,
speed,
target_angle,
which_sensor,
color_value=15):
""" Gyro drives until given color sensor is on black
:param pid: PID setting of drive
:type pid: Number
:param speed: The speed the robot moves at
:type speed: Number
:param target_angle: the angle the gyro drives at
:type target_angle:
:param which_sensor: Chooses which color sensor to use
:type which_sensor: Enum
:param color_value: The value of color that the robot stops at
:type color_value: Number
"""
# Inititialize values
sensor = 0
pid.reset()
target_angle = target_angle % 360
if which_sensor == LineSensor.LEFT:
sensor = self.left_color
elif which_sensor == LineSensor.RIGHT:
sensor = self.right_color
else:
sensor = self.center_color
while sensor.reflection() > color_value:
# Calculate error
actual_angle = self.gyro.angle()
error = (target_angle - actual_angle) % 360
error = error - (360 * int(error / 180))
# Calculate steering output
steering = pid.compute_steering(error)
# Drive the motors
self.drive_base.drive(speed, steering)
self.print_sensor_values
# Stop motors
self.drive_base.stop(Stop.BRAKE)
def align(self, speed, sensor1, sensor2):
"""Aligns using color sensors on black line
:param speed: The speed the robot moves at
:type speed: Number
:param sensor1: The first sensor the robot uses to align
:type sensor1: Enum
:param sensor2: The second sensor the robot uses to align
:type sensor2: Enum
"""
self.left_motor.run(speed)
self.right_motor.run(speed)
first_sensor = 0
second_sensor = 0
if sensor1 == LineSensor.LEFT:
first_sensor = self.left_color
elif sensor1 == LineSensor.RIGHT:
first_sensor = self.right_color
else:
first_sensor = self.center_color
if sensor2 == LineSensor.LEFT:
second_sensor = self.left_color
elif sensor2 == LineSensor.RIGHT:
second_sensor = self.right_color
else:
second_sensor = self.center_color
while True:
first = False
second = False
if first_sensor.reflection() <= 10:
self.left_motor.hold()
first = True
if second_sensor.reflection() <= 10:
self.right_motor.hold()
second = True
if first and second == True:
break
def reset_sensors(self, reset_angle=0):
"""Reset the robot's sensor values
:param reset_angle: inital angle for the gyro, defaults to 0
:type reset_angle: int, optional
"""
# Resets the gyro
self.gyro.reset_angle(reset_angle)
def run_linear(self, speed, time, wait=True):
"""Runs linear gear
:param speed: The speed the linear gear moves at
:type speed: Number
:param time: How long the linear gear runs for
:type time: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.linear_attachment_motor.run_time(speed, time, Stop.BRAKE, wait)
def move_linear(self, speed, rotations, wait=True):
"""Will calculate the ratio of the gears and then move the linear gear
to a specific angle
:param speed: The speed the linear gear moves at
:type speed: Number
:param rotations: How much the linear gear moves by in rotations
:type rotations: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
target_angle = rotations * 360
self.linear_attachment_motor.run_angle(speed, target_angle, Stop.BRAKE,
wait)
def run_dropper(self, speed, time, wait=True):
"""Runs block dropper
:param speed: The speed the yeeter moves at
:type speed: Number
:param time: How long the yeeter runs for
:type time: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.dropper_attachment_motor.run_time(speed, time, Stop.BRAKE, wait)
def move_dropper(self, speed, degrees, wait=True):
"""Will calculate the ratio of the gears and then move the block dropper
to a specific angle
:param speed: The speed the yeeter moves at
:type speed: Number
:param degrees: How much the yeeter moves by in degrees
:type degrees: Number
:param wait: Wait for action to complete before next step
:type wait: Boolean
"""
self.stop_motors()
self.dropper_attachment_motor.run_angle(speed, degrees, Stop.BRAKE,
wait)
def dance(self, speed, duration):
if self.dance_clock == 0:
self.dance_clock = self.clock.time()
self.left_motor.run(speed * self.sign * -1)
self.right_motor.run(speed * self.sign)
if self.clock.time() - self.dance_clock > duration:
self.sign *= -1
self.left_motor.run(speed * self.sign * -1)
self.right_motor.run(speed * self.sign)
self.dance_clock = self.clock.time()
return True
return False
class MbMotor():
"""
Control a motor, besides the fact that you can detect if a motor got stalled
the main reason for this class is to solve a bug for pybricks.ev3devices.Motor. The bug is that when you set the motor
to move in a Direction.COUNTERCLOCKWISE sometimes it failes to detect it.
This class is made on top of pybricks.ev3devices.Motor
Args:
port (Port): The port of the device
clockwise_direction (bool): Sets the defualt movement of the motor clockwise or counterclockwise, True for clockwise else counterclockwise
exit_exec (Function): Function that returns True or False, the motor will stop if returns True
"""
def __init__(self,
port,
clockwise_direction=True,
exit_exec=lambda: False):
self.core = Motor(port)
self.port = port
self.direction = 1 if clockwise_direction else -1
self.exit_exec = exit_exec
def angle(self):
"""
Get the distance the robot has moved in degrees
Returns:
angle (int): The distance the robot has moved in degrees
"""
return self.core.angle() * self.direction
def speed(self):
"""
Get the speed of the motor
Returns:
speed (int): The current speed of the motor
"""
return self.core.speed() * self.direction
def stalled(self, min_speed=0):
if abs(self.speed()) <= abs(min_speed):
return True
return False
def run_angle(self, speed, angle, wait=True, detect_stall=False):
"""
Run the motor to a specific angle
Args:
speed (int): The speed of the motor
angle (int): Degrees to run the motor at
wait (bool): Sets if the robot is going to stop for the motor to complete this method or not
"""
def exec(self, speed, angle):
moved_enough = False
self.reset_angle()
self.run(speed)
while True:
if abs(self.angle()) > 50:
moved_enough = True
if moved_enough and detect_stall:
if self.stalled():
break
if abs(self.angle()) > abs(angle) or self.exit_exec():
break
self.hold()
if wait:
exec(self, speed, angle)
else:
threading.Thread(target=exec, args=[self, speed, angle]).start()
def run_time(self, speed, msec, wait=True):
"""
Run the motor to a amount of time
Args:
speed (int): The speed of the motor
msec (int): Time to move the robot
wait (bool): Sets if the robot is going to stop for the motor to complete this method or not
"""
def exec(self, speed, msec):
self.reset_angle()
self.run(speed)
s = time()
while True:
if round(time() - s,
2) * 1000 >= abs(msec) or self.exit_exec():
break
self.hold()
if wait:
exec(self, speed, msec)
else:
threading.Thread(target=exec, args=[self, speed, msec]).start()
def run(self, speed):
"""
Run the motor to a constant speed
Args:
speed (int): Speed to run at
Note:
speed parameter should be between -800 and 800
"""
self.core.run(speed * self.direction)
def dc(self, speed):
"""
Run the motor to a constant speed
Args:
speed (int): Speed to run at
Note:
speed parameter should be between -100 and 100
"""
self.core.dc(speed * self.direction)
def hold(self):
"""
Stop the motor and hold its position
"""
self.core.hold()
def brake(self):
"""
Passively stop the motor
"""
self.core.brake()
def stop(self):
"""
No current is being aplied to the robot, so its gonna stop due to friction
"""
self.core.stop()
def reset_angle(self, angle=0):
"""
Set the motor angle
Args:
angle (int): Angle to set the motor at
"""
self.core.reset_angle(angle)
def is_stalled(self, min_speed=0):
"""
Check if the motor got stalled
Args:
min_speed (int): The minim speed the motor should be moving at
"""
if abs(self.speed()) <= abs(min_speed):
return True
return False
def __repr__(self):
return "Motor Properties:\nPort: " + str(
self.port) + "\nDefault Direction: " + str(self.direction)
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):
...
# motor moves the robot backward while the lift motor moves the rear
# structure up until the Touch Sensor is pressed. Second, the rear
# motor moves the robot forward while the lift motor moves the rear
# structure down for a set amount of degrees to move to its starting
# position. Finally, the lift motor resets the angle to "0." This
# means that when it moves to "0" later on, it returns to this starting
# position.
rear_motor.dc(-20)
lift_motor.dc(100)
while not touch_sensor.pressed():
wait(10)
lift_motor.dc(-100)
rear_motor.dc(40)
wait(50)
lift_motor.run_angle(-145, 510)
rear_motor.hold()
lift_motor.run_angle(-30, 44)
lift_motor.reset_angle(0)
gyro_sensor.reset_angle(0)
# Initialize the steps variable to 0.
steps = 0
# This loop checks the Brick Buttons to update and display the steps
# variable. It repeats until the Center Button is pressed.
while True:
# Display the steps variable on the screen.
ev3.screen.clear()
ev3.screen.draw_text(70, 50, steps)
wait(200)
speed=1000 # degrees per second
)
DRIVE_BASE.straight(
distance=100 # milimeters
)
MEDIUM_MOTOR.run(
speed=-1000 # degrees per second
)
DRIVE_BASE.straight(
distance=-100 # milimeters
)
MEDIUM_MOTOR.hold()
DRIVE_BASE.turn(
angle=135 # degrees
)
DRIVE_BASE.turn(
angle=-30 # degrees
)
else:
BRICK.light.on(color=Color.GREEN)
if time() % 3 < 1.5:
DRIVE_BASE.drive(
speed=360, # degrees per second
class R3ptar(EV3Brick):
def __init__(self,
turn_motor_port: Port = Port.A,
move_motor_port: Port = Port.B,
scare_motor_port: Port = Port.D,
touch_sensor_port: Port = Port.S1,
color_sensor_port: Port = Port.S3,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.turn_motor = Motor(port=turn_motor_port,
positive_direction=Direction.CLOCKWISE)
self.move_motor = Motor(port=move_motor_port,
positive_direction=Direction.CLOCKWISE)
self.scare_motor = Motor(port=scare_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):
ir_beacons_pressed = set(
self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}:
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}:
self.move_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.LEFT_UP}:
self.turn_motor.run(speed=-500)
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.RIGHT_UP}:
self.turn_motor.run(speed=500)
self.move_motor.run(speed=speed)
elif ir_beacons_pressed == {Button.LEFT_DOWN}:
self.turn_motor.run(speed=-500)
self.move_motor.run(speed=-speed)
elif ir_beacons_pressed == {Button.RIGHT_DOWN}:
self.turn_motor.run(speed=500)
self.move_motor.run(speed=-speed)
else:
self.turn_motor.hold()
self.move_motor.stop()
def bite_by_ir_beacon(self, speed: float = 1000):
if Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
self.speaker.play_file(file=SoundFile.SNAKE_HISS)
self.scare_motor.run_time(speed=speed,
time=1000,
then=Stop.HOLD,
wait=True)
self.scare_motor.run_time(speed=-300,
time=1000,
then=Stop.COAST,
wait=True)
while Button.BEACON in self.ir_sensor.buttons(
channel=self.ir_beacon_channel):
pass
def run_away_if_chased(self):
if self.color_sensor.reflection() > 30:
self.move_motor.run_time(speed=500,
time=4000,
then=Stop.HOLD,
wait=True)
for i in range(2):
self.turn_motor.run_time(speed=500,
time=1000,
then=Stop.HOLD,
wait=True)
self.turn_motor.run_time(speed=-500,
time=1000,
then=Stop.HOLD,
wait=True)
def bite_if_touched(self):
if self.touch_sensor.pressed():
self.speaker.play_file(file=SoundFile.SNAKE_HISS)
self.scare_motor.run_time(speed=1000,
time=1000,
then=Stop.HOLD,
wait=True)
self.scare_motor.run_time(speed=-300,
time=1000,
then=Stop.COAST,
wait=True)
def main(self, speed: float = 1000):
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.bite_by_ir_beacon(speed=speed)
self.bite_if_touched()
self.run_away_if_chased()
class Charlie():
'''
Charlie is the class responsible for driving,
Robot-Movement and general Real-world interaction of the robot with Sensors and motors.
Args:
config (dict): The parsed config
brick (EV3Brick): EV3Brick for getting button input
logger (Logger): Logger for logging
'''
def __init__(self, config, brick, logger):
logger.info(self, 'Starting initialisation of Charlie')
self.__config = config
self.brick = brick
self.logger = logger
self.__conf2port = {
1: Port.S1,
2: Port.S2,
3: Port.S3,
4: Port.S4,
'A': Port.A,
'B': Port.B,
'C': Port.C,
'D': Port.D
}
self.__initSensors()
self.__initMotors()
self.min_speed = 35 # lage motor 20, medium motor 30
self.__gyro.reset_angle(0)
self.__screenRoutine = False
self.showDetails()
self.logger.info(self, 'Driving for Charlie initialized')
##TODO
def __repr__(self):
outputString = "(TODO)\n Config: " + self.__config + "\n Brick: " + self.brick + "\n Logger: " + self.logger
outputString += "\n--Debug--\n Minimum Speed: " + str(
self.min_speed) + "\n "
return "TODO"
def __str__(self):
return "Charlie"
def __initSensors(self):
'''Sub-method for initializing Sensors.'''
self.logger.debug(self, "Starting sensor initialisation...")
try:
self.__gyro = GyroSensor(
self.__conf2port[self.__config['gyroSensorPort']],
Direction.CLOCKWISE if not self.__config['gyroInverted'] else
Direction.COUNTERCLOCKWISE
) if self.__config['gyroSensorPort'] != 0 else 0
self.logger.debug(
self, 'Gyrosensor initialized sucessfully on port %s' %
self.__config['gyroSensorPort'])
except Exception as exception:
self.__gyro = 0
self.logger.error(
self,
"Failed to initialize the Gyro-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__rLight = ColorSensor(
self.__conf2port[self.__config['rightLightSensorPort']]
) if self.__config['rightLightSensorPort'] != 0 else 0
self.logger.debug(
self, 'Colorsensor initialized sucessfully on port %s' %
self.__config['rightLightSensorPort'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the right Color-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__lLight = ColorSensor(
self.__conf2port[self.__config['leftLightSensorPort']]
) if self.__config['leftLightSensorPort'] != 0 else 0
self.logger.debug(
self, 'Colorsensor initialized sucessfully on port %s' %
self.__config['leftLightSensorPort'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the left Color-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
try:
self.__touch = TouchSensor(
self.__conf2port[self.__config['backTouchSensor']]
) if self.__config['backTouchSensor'] != 0 else 0
self.logger.debug(
self, 'Touchsensor initialized sucessfully on port %s' %
self.__config['backTouchSensor'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize the Touch-Sensor - Are u sure it's connected to Port %s?"
% exception, exception)
self.logger.debug(self, "Sensor initialisation done")
def __initMotors(self):
'''Sub-method for initializing Motors.'''
self.logger.debug(self, "Starting motor initialisation...")
if self.__config['robotType'] == 'NORMAL':
try:
self.__lMotor = Motor(
self.__conf2port[self.__config['leftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['leftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['leftMotorGears'])
self.__rMotor = Motor(
self.__conf2port[self.__config['rightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['rightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['rightMotorGears'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize movement motors for robot type NORMAL - Are u sure they\'re all connected?",
exception)
if self.__config['useGearing']:
try:
self.__gearingPortMotor = Motor(
self.__conf2port[
self.__config['gearingSelectMotorPort']],
Direction.CLOCKWISE if
(not self.__config['gearingSelectMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['gearingSelectMotorGears'])
self.__gearingTurnMotor = Motor(
self.__conf2port[
self.__config['gearingTurnMotorPort']],
Direction.CLOCKWISE if
(not self.__config['gearingTurnMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['gearingTurnMotorGears'])
except Exception as exception:
self.logger.error(
self,
"Failed to initialize action motors for the gearing - Are u sure they\'re all connected?",
exception)
else:
try:
self.__aMotor1 = Motor(
self.__conf2port[
self.__config['firstActionMotorPort']],
Direction.CLOCKWISE if
(not self.__config['firstActionMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['firstActionMotorGears']) if (
self.__config['firstActionMotorPort'] != 0) else 0
self.__aMotor2 = Motor(
self.__conf2port[
self.__config['secondActionMotorPort']],
Direction.CLOCKWISE if
(not self.__config['secondActionMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['secondActionMotorGears']) if (
self.__config['secondActionMotorPort'] != 0) else 0
except Exception as exception:
self.logger.error(
self,
"Failed to initialize action motors - Are u sure they\'re all connected?",
exception)
else:
try:
self.__fRMotor = Motor(
self.__conf2port[self.__config['frontRightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['frontRightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['frontRightMotorGears']) if (
self.__config['frontRightMotorPort'] != 0) else 0
self.__bRMotor = Motor(
self.__conf2port[self.__config['backRightMotorPort']],
Direction.CLOCKWISE if
(not self.__config['backRightMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['backRightMotorGears']) if (
self.__config['backRightMotorPort'] != 0) else 0
self.__fLMotor = Motor(
self.__conf2port[self.__config['frontLeftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['frontLeftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['frontLeftMotorGears']) if (
self.__config['frontLeftMotorPort'] != 0) else 0
self.__bLMotor = Motor(
self.__conf2port[self.__config['backLeftMotorPort']],
Direction.CLOCKWISE if
(not self.__config['backLeftMotorInverted']) else
Direction.COUNTERCLOCKWISE,
gears=self.__config['backLeftMotorGears']) if (
self.__config['backLeftMotorPort'] != 0) else 0
except Exception as exception:
self.logger.error(
self,
"Failed to initialize movement motors for robot type %s - Are u sure they\'re all connected? Errored at Port"
% self.__config['robotType'], exception)
self.logger.debug(self, "Motor initialisation done")
self.logger.info(self, 'Charlie initialized')
def showDetails(self):
'''
Processes sensor data in a separate thread and shows
'''
threadLock = _thread.allocate_lock()
def __screenPrintRoutine():
while True:
if self.__gyro.angle() > 360:
ang = self.__gyro.angle() - 360
else:
ang = self.__gyro.angle()
speedRight = self.__rMotor.speed() if self.__config[
'robotType'] == 'NORMAL' else self.__fRMotor.speed()
speedRight = speedRight / 360 # from deg/s to revs/sec
speedRight = speedRight * (self.__config['wheelDiameter'] *
math.pi) # from revs/sec to cm/sec
speedLeft = self.__lMotor.speed() if self.__config[
'robotType'] == 'NORMAL' else self.__fLMotor.speed()
speedLeft = speedLeft / 360 # from deg/s to revs/sec
speedLeft = speedLeft * (self.__config['wheelDiameter'] *
math.pi) # from revs/sec to cm/sec
#self.brick.screen.set_font(Font(family = 'arial', size = 16))
if self.__screenRoutine:
print(self.__gyro.angle())
self.brick.screen.draw_text(5,
10,
'Robot-Angle: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
self.brick.screen.draw_text(5,
40,
'Right Motor Speed: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
self.brick.screen.draw_text(5,
70,
'Left Motor Speed: %s' % ang,
text_color=Color.BLACK,
background_color=Color.WHITE)
time.sleep(0.1)
with threadLock:
_thread.start_new_thread(__screenPrintRoutine, ())
def execute(self, params):
'''
This function interprets the number codes from the given array and executes the driving methods accordingly
Args:
params (array): The array of number code arrays to be executed
'''
if self.brick.battery.voltage() <= 7600:
if (self.__config["ignoreBatteryWarning"] == True):
self.logger.warn(
"Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results. ignoreBatteryWarning IS SET TO True, THIS WILL BE IGNORED!!!"
% self.brick.battery.voltage() * 0.001)
else:
self.logger.warn(
"Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results."
% self.brick.battery.voltage() * 0.001)
return
if self.__gyro == 0:
self.logger.error(self, "Cannot drive without gyro", '')
return
methods = {
3: self.absTurn,
4: self.turn,
5: self.action,
7: self.straight,
9: self.intervall,
11: self.curve,
12: self.toColor,
15: self.toWall
}
self.__gyro.reset_angle(0)
self.__gyro.reset_angle(0)
time.sleep(0.1)
self.__screenRoutine = True
while params != [] and not any(self.brick.buttons.pressed()):
pparams = params.pop(0)
mode, arg1, arg2, arg3 = pparams.pop(0), pparams.pop(
0), pparams.pop(0), pparams.pop(0)
methods[mode](arg1, arg2, arg3)
self.breakMotors()
if self.__config['useGearing']:
self.__gearingPortMotor.run_target(300, 0, Stop.HOLD,
True) # reset gearing
time.sleep(0.3)
self.__screenRoutine = False
def turn(self, speed, deg, port):
'''
Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM)
Args:
speed (int): the speed to drive at
deg (int): the angle to turn
port (int): the motor(s) to turn with
'''
startValue = self.__gyro.angle()
speed = self.min_speed if speed < self.min_speed else speed
# turn only with left motor
if port == 2:
# right motor off
self.__rMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnLeftMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnLeftMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn only with right motor
elif port == 3:
# left motor off
self.__lMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnRightMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnRightMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.1
) if speed > self.min_speed else self.min_speed + 5
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn with both motors
elif port == 23:
dualMotorbonus = 19
speed = speed * 2
# turn the angle
if deg > 0:
while self.__gyro.angle() - startValue < deg:
self.turnLeftMotor(speed / 2)
self.turnRightMotor(-speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() - startValue < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() - startValue > deg:
self.turnLeftMotor(-speed / 2)
self.turnRightMotor(speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() - startValue > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
def absTurn(self, speed, deg, port):
'''
Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM)
This method turns in contrast to the normal turn() method to an absolute ange (compared to starting point)
Args:
speed (int): the speed to drive at
deg (int): the angle to turn to
port (int): the motor(s) to turn with
'''
speed = self.min_speed if speed < self.min_speed else speed
# turn only with left motor
if port == 2:
# right motor off
self.__rMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnLeftMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnLeftMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn only with right motor
elif port == 3:
# left motor off
self.__lMotor.dc(0)
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnRightMotor(-speed)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10,
0.1) if speed > self.min_speed else self.min_speed
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnRightMotor(speed)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.1
) if speed > self.min_speed else self.min_speed + 5
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
# turn with both motors
elif port == 23:
dualMotorbonus = 19
speed = speed * 2
# turn the angle
if deg > 0:
while self.__gyro.angle() < deg:
self.turnLeftMotor(speed / 2)
self.turnRightMotor(-speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() < deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while self.__gyro.angle() > deg:
self.turnLeftMotor(-speed / 2)
self.turnRightMotor(speed / 2)
# slow down to not overshoot
if not self.__gyro.angle() > deg * 0.6:
speed = speed - self._map(
deg, 1, 360, 10, 0.01
) if speed - self._map(
deg, 1, 360, 10, 0.01
) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
def straight(self, speed, dist, ang):
'''
Drives the Robot in a straight line.
Also it self-corrects while driving with the help of a gyro-sensor. This is used to make the Robot more accurate
Args:
speed (int): the speed to drive at
dist (int): the distance in cm to drive
'''
if self.__config['robotType'] != 'MECANUM':
correctionStrength = 2.5 # how strongly the self will correct. 2 = default, 0 = nothing
startValue = self.__gyro.angle()
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi)
motor = self.__rMotor if self.__config[
'robotType'] == 'NORMAL' else self.__fRMotor
# drive
motor.reset_angle(0)
if revs > 0:
while revs > (motor.angle() / 360):
# if not driving staright correct it
if self.__gyro.angle() - startValue > 0:
lSpeed = speed - abs(self.__gyro.angle() -
startValue) * correctionStrength
rSpeed = speed
elif self.__gyro.angle() - startValue < 0:
rSpeed = speed - abs(self.__gyro.angle() -
startValue) * correctionStrength
lSpeed = speed
else:
lSpeed = speed
rSpeed = speed
self.turnLeftMotor(lSpeed)
self.turnRightMotor(rSpeed)
#cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
while revs < motor.angle() / 360:
# if not driving staright correct it
if self.__gyro.angle() - startValue < 0:
rSpeed = speed + abs(self.__gyro.angle() -
startValue) * correctionStrength
lSpeed = speed
elif self.__gyro.angle() - startValue > 0:
lSpeed = speed + abs(self.__gyro.angle() -
startValue) * correctionStrength
rSpeed = speed
else:
lSpeed = speed
rSpeed = speed
self.turnLeftMotor(-lSpeed)
self.turnRightMotor(-rSpeed)
# cancel if button pressed
if any(self.brick.buttons.pressed()):
return
else:
self.__fRMotor.reset_angle(0)
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi)
speed = speed * 1.7 * 6 # convert speed form % to deg/min
# driving the robot into the desired direction
if ang >= 0 and ang <= 45:
multiplier = _map(ang, 0, 45, 1, 0)
self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang >= -45 and ang < 0:
multiplier = _map(ang, -45, 0, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang > 45 and ang <= 90:
multiplier = _map(ang, 45, 90, 0, 1)
self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang < -45 and ang >= -90:
multiplier = _map(ang, -45, -90, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True)
elif ang > 90 and ang <= 135:
multiplier = _map(ang, 90, 135, 1, 0)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang < -90 and ang >= -135:
multiplier = _map(ang, -90, -135, 1, 0)
self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * 360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang > 135 and ang <= 180:
multiplier = _map(ang, 135, 180, 0, 1)
self.__fRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
elif ang < -135 and ang >= -180:
multiplier = _map(ang, -135, -180, 0, 1)
self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False)
self.__bRMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__fLMotor.run_angle(speed * multiplier + 1,
revs * -360 * multiplier, Stop.COAST,
False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True)
def intervall(self, speed, dist, count):
'''
Drives forwads and backwards x times.
Args:
speed (int): the speed to drive at
revs (int): the distance (in cm) to drive
count (int): how many times it should repeat the driving
'''
# convert the input (cm) to revs
revs = dist / (self.__config['wheelDiameter'] * math.pi) / 2
speed = speed * 1.7 * 6 # speed in deg/s to %
# move count times forwards and backwards
for i in range(count + 1):
if self.__config['robotType'] == 'NORMAL':
ang = self.__lMotor.angle()
# drive backwards
self.__rMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__lMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__lMotor.angle() > revs * -360:
if any(self.brick.buttons.pressed()):
return
# drive forwards
self.__lMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__rMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__rMotor.angle() <= ang:
if any(self.brick.buttons.pressed()):
return
elif self.__config['robotType'] == 'ALLWHEEL' or self.__config[
'robotType'] == 'MECANUM':
ang = self.__lMotor.angle()
# drive backwards
self.__fRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__bRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__fLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
self.__bLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__lMotor.angle() > revs * -360:
if any(self.brick.buttons.pressed()):
return
# drive forwards
self.__fRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__bRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__fLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
self.__bLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False)
# return to cancel if any button is pressed
while self.__rMotor.angle() <= ang:
if any(self.brick.buttons.pressed()):
return
def curve(self, speed, dist, deg):
'''
Drives forwads and backwards x times.
Args:
speed (int): the speed to drive at
revs1 (int): the distance (in motor revolutions) for the outer wheel to drive
deg (int): how much of a circle it should drive
'''
speed = speed * 1.7 * 6 # speed to deg/s from %
# gyro starting point
startValue = self.__gyro.angle()
revs1 = dist / (self.__config['wheelDiameter'] * math.pi)
# claculate revs for the second wheel
pathOutside = self.__config['wheelDiameter'] * math.pi * revs1
rad1 = pathOutside / (math.pi * (deg / 180))
rad2 = rad1 - self.__config['wheelDistance']
pathInside = rad2 * math.pi * (deg / 180)
revs2 = pathInside / (self.__config['wheelDiameter'] * math.pi)
# claculate the speed for the second wheel
relation = revs1 / revs2
speedSlow = speed / relation
if deg > 0:
# asign higher speed to outer wheel
lSpeed = speed
rSpeed = speedSlow
self.__rMotor.run_angle(rSpeed, revs2 * 360, Stop.COAST, False)
self.__lMotor.run_angle(lSpeed, revs1 * 360 + 5, Stop.COAST, False)
#turn
while self.__gyro.angle() - startValue < deg and not any(
self.brick.buttons.pressed()):
pass
else:
# asign higher speed to outer wheel
lSpeed = speed
rSpeed = speedSlow
self.__rMotor.run_angle(rSpeed, revs2 * 360 + 15, Stop.COAST,
False)
self.__lMotor.run_angle(lSpeed, revs1 * 360, Stop.COAST, False)
#turn
while self.__gyro.angle() - startValue > deg and not any(
self.brick.buttons.pressed()):
pass
def toColor(self, speed, color, side):
'''
Drives forward until the given colorSensor sees a given color.
Args:
speed (int): the speed to drive at
color (int): the color to look for (0 = Black, 1 = White)
side (int): which side's color sensor should be used
'''
# sets color to a value that the colorSensor can work with
if color == 0:
color = Color.BLACK
else:
color = Color.WHITE
# Refactor code
# only drive till left colorSensor
if side == 2:
# if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
while lLight.color() != Color.WHITE and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
while lLight.color() != color and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
# only drive till right colorSensor
elif side == 3:
# if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
while rLight.color() != Color.WHITE and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
while rLight.color() != color and not any(
self.brick.buttons.pressed()):
self.turnBothMotors(speed)
# drive untill both colorSensors
elif side == 23:
rSpeed = speed
lSpeed = speed
rWhite = False
lWhite = False
while (rLight.color() != color or lLight.color() != color
) and not any(self.brick.buttons.pressed()):
#if drive to color black drive until back after white to not recognize colors on the field as lines
if color == Color.BLACK:
if rLight.color() == Color.WHITE:
rWhite = True
if lLight.color() == Color.WHITE:
lWhite = True
self.__rMotor.dc(rSpeed)
self.__lMotor.dc(lSpeed)
# if right at color stop right Motor
if rLight.color() == color and rWhite:
rSpeed = 0
# if left at color stop left Motor
if lLight.color() == color and lWhite:
lSpeed = 0
def toWall(self, speed, *args):
'''
Drives until a pressure sensor is pressed
Args:
speed (int): the speed to drive at
'''
while not touch.pressed():
self.turnBothMotors(-abs(speed))
if any(self.brick.buttons()):
break
self.turnBothMotors(0)
def action(self, speed, revs, port):
'''
Doesn't drive the robot, but drives the action motors
Args:
speed (int): the speed to turn the motor at
revs (int): how long to turn the motor for
port (int): which one of the motors should be used
'''
speed = abs(speed) * 1.7 * 6 # speed to deg/s from %
if self.__config['useGearing']:
self.__gearingPortMotor.run_target(300, port * 90, Stop.HOLD,
True) # select gearing Port
ang = self.__gearingTurnMotor.angle()
self.__gearingTurnMotor.run_angle(speed, revs * 360, Stop.BRAKE,
False) # start turning the port
# cancel, if any brick button is pressed
if revs > 0:
while self.__gearingTurnMotor.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__gearingTurnMotor.dc(0)
return
else:
while self.__gearingTurnMotor.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__gearingTurnMotor.dc(0)
return
else:
# turn motor 1
if port == 1:
ang = self.__aMotor1.angle()
self.__aMotor1.run_angle(speed, revs * 360, Stop.HOLD, False)
if revs > 0:
while self.__aMotor1.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__aMotor1.dc(0)
return
else:
while self.__aMotor1.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__aMotor1.dc(0)
return
# turm motor 2
elif port == 2:
ang = self.__aMotor2.angle()
self.__aMotor2.run_angle(speed, revs * 360, Stop.HOLD, False)
if revs > 0:
while self.__aMotor2.angle() < revs * 360 - ang:
if any(self.brick.buttons.pressed()):
self.__aMotor2.dc(0)
return
else:
while self.__aMotor2.angle() > revs * 360 + ang:
if any(self.brick.buttons.pressed()):
self.__aMotor2.dc(0)
return
def turnLeftMotor(self, speed):
'''
Sub-method for driving the left Motor(s)
Args:
speed (int): the speed to drive the motor at
'''
if self.__config['robotType'] == 'NORMAL':
self.__lMotor.dc(speed)
else:
self.__fLMotor.dc(speed)
self.__bLMotor.dc(speed)
def turnRightMotor(self, speed):
'''
Sub-method for driving the right Motor(s)
Args:
speed (int): the speed to drive the motor at
'''
if self.__config['robotType'] == 'NORMAL':
self.__rMotor.dc(speed)
else:
self.__fRMotor.dc(speed)
self.__bRMotor.dc(speed)
def turnBothMotors(self, speed):
'''
Submethod for setting a motor.dc() to all motors
Args:
speed (int): the speed (in percent) to set the motors to
'''
if self.__config['robotType'] == 'NORMAL':
self.__rMotor.dc(speed)
self.__lMotor.dc(speed)
else:
self.__fRMotor.dc(speed)
self.__bRMotor.dc(speed)
self.__fLMotor.dc(speed)
self.__bLMotor.dc(speed)
def breakMotors(self):
'''Sub-method for breaking all the motors'''
if self.__config['robotType'] == 'NORMAL':
self.__lMotor.hold()
self.__rMotor.hold()
else:
self.__fRMotor.hold()
self.__bRMotor.hold()
self.__fLMotor.hold()
self.__bLMotor.hold()
time.sleep(0.2)
def _map(self, x, in_min, in_max, out_min, out_max):
'''
Converts a given number in the range of two numbers to a number in the range of two other numbers
Args:
x (int): the input number that should be converted
in_min (int): The minimal point of the range of input number
in_max (int): The maximal point of the range of input number
out_min (int): The minimal point of the range of output number
out_max (int): The maximal point of the range of output number
Returns:
int: a number between out_min and out_max, de
'''
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def getGyroAngle(self):
return self.__gyro.angle()
def setRemoteValues(self, data):
x = data['x']
y = data['y']
if x == 0:
x = 0.0001
if data['y'] == 0 and data['x'] == 0:
self.breakMotors()
else:
radius = int(math.sqrt(x**2 + y**2)) # distance from center
ang = math.atan(y / x) # angle in radians
a = int(self._map(radius, 0, 180, 0, 100))
b = int(-1 * math.cos(2 * ang) * self._map(radius, 0, 180, 0, 100))
if x < 0:
temp = a
a = b
b = temp
if y < 0:
temp = a
a = -b
b = -temp
self.turnLeftMotor(int(self._map(a, 0, 100, 0, data['maxSpeed'])))
self.turnRightMotor(int(self._map(b, 0, 100, 0, data['maxSpeed'])))
if data['a1'] == 0:
self.__aMotor1.hold()
else:
a1Speed = data['a1']
self.__aMotor1.dc(a1Speed)
class Puppy:
# These constants are used for positioning the legs.
HALF_UP_ANGLE = 25
STAND_UP_ANGLE = 65
STRETCH_ANGLE = 125
# These constants are for positioning the head.
HEAD_UP_ANGLE = 0
HEAD_DOWN_ANGLE = -40
# These constants are for the eyes.
NEUTRAL_EYES = Image(ImageFile.NEUTRAL)
TIRED_EYES = Image(ImageFile.TIRED_MIDDLE)
TIRED_LEFT_EYES = Image(ImageFile.TIRED_LEFT)
TIRED_RIGHT_EYES = Image(ImageFile.TIRED_RIGHT)
SLEEPING_EYES = Image(ImageFile.SLEEPING)
HURT_EYES = Image(ImageFile.HURT)
ANGRY_EYES = Image(ImageFile.ANGRY)
HEART_EYES = Image(ImageFile.LOVE)
SQUINTY_EYES = Image(ImageFile.TEAR) # the tear is erased later
def __init__(self):
# Initialize the EV3 brick.
self.ev3 = EV3Brick()
# Initialize the motors connected to the back legs.
self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
# Initialize the motor connected to the head.
# Worm gear moves 1 tooth per rotation. It is interfaced to a 24-tooth
# gear. The 24-tooth gear is connected to parallel 12-tooth gears via
# an axle. The 12-tooth gears interface with 36-tooth gears.
self.head_motor = Motor(Port.C,
Direction.COUNTERCLOCKWISE,
gears=[[1, 24], [12, 36]])
# Initialize the Color Sensor. It is used to detect the colors when
# feeding the puppy.
self.color_sensor = ColorSensor(Port.S4)
# Initialize the touch sensor. It is used to detect when someone pets
# the puppy.
self.touch_sensor = TouchSensor(Port.S1)
self.pet_count_timer = StopWatch()
self.feed_count_timer = StopWatch()
self.count_changed_timer = StopWatch()
# These attributes are initialized later in the reset() method.
self.pet_target = None
self.feed_target = None
self.pet_count = None
self.feed_count = None
# These attributes are used by properties.
self._behavior = None
self._behavior_changed = None
self._eyes = None
self._eyes_changed = None
# These attributes are used in the eyes update
self.eyes_timer_1 = StopWatch()
self.eyes_timer_1_end = 0
self.eyes_timer_2 = StopWatch()
self.eyes_timer_2_end = 0
self.eyes_closed = False
# These attributes are used by the playful behavior.
self.playful_timer = StopWatch()
self.playful_bark_interval = None
# These attributes are used in the update methods.
self.prev_petted = None
self.prev_color = None
def adjust_head(self):
"""Use the up and down buttons on the EV3 brick to adjust the puppy's
head up or down.
"""
self.ev3.screen.load_image(ImageFile.EV3_ICON)
self.ev3.light.on(Color.ORANGE)
while True:
buttons = self.ev3.buttons.pressed()
if Button.CENTER in buttons:
break
elif Button.UP in buttons:
self.head_motor.run(20)
elif Button.DOWN in buttons:
self.head_motor.run(-20)
else:
self.head_motor.stop()
wait(100)
self.head_motor.stop()
self.head_motor.reset_angle(0)
self.ev3.light.on(Color.GREEN)
def move_head(self, target):
"""Move the head to the target angle.
Arguments:
target (int):
The target angle in degrees. 0 is the starting position,
negative values are below this point and positive values
are above this point.
"""
self.head_motor.run_target(20, target)
def reset(self):
# must be called when puppy is sitting down.
self.left_leg_motor.reset_angle(0)
self.right_leg_motor.reset_angle(0)
# Pick a random number of time to pet the puppy.
self.pet_target = urandom.randint(3, 6)
# Pick a random number of time to feed the puppy.
self.feed_target = urandom.randint(2, 4)
# Pet count and feed count both start at 1
self.pet_count, self.feed_count = 1, 1
# Reset timers.
self.pet_count_timer.reset()
self.feed_count_timer.reset()
self.count_changed_timer.reset()
# Set initial behavior.
self.behavior = self.idle
# The next 8 methods define the 8 behaviors of the puppy.
def idle(self):
"""The puppy is idle and waiting for someone to pet it or feed it."""
if self.did_behavior_change:
print('idle')
self.stand_up()
self.update_eyes()
self.update_behavior()
self.update_pet_count()
self.update_feed_count()
def go_to_sleep(self):
"""Makes the puppy go to sleep."""
if self.did_behavior_change:
print('go_to_sleep')
self.eyes = self.TIRED_EYES
self.sit_down()
self.move_head(self.HEAD_DOWN_ANGLE)
self.eyes = self.SLEEPING_EYES
self.ev3.speaker.play_file(SoundFile.SNORING)
if self.touch_sensor.pressed(
) and Button.CENTER in self.ev3.buttons.pressed():
self.count_changed_timer.reset()
self.behavior = self.wake_up
def wake_up(self):
"""Makes the puppy wake up."""
if self.did_behavior_change:
print('wake_up')
self.eyes = self.TIRED_EYES
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
self.move_head(self.HEAD_UP_ANGLE)
self.sit_down()
self.stretch()
wait(1000)
self.stand_up()
self.behavior = self.idle
def act_playful(self):
"""Makes the puppy act playful."""
if self.did_behavior_change:
print('act_playful')
self.eyes = self.NEUTRAL_EYES
self.stand_up()
self.playful_bark_interval = 0
if self.update_pet_count():
# If the puppy was petted, then we are done being playful
self.behavior = self.idle
if self.playful_timer.time() > self.playful_bark_interval:
self.ev3.speaker.play_file(SoundFile.DOG_BARK_2)
self.playful_timer.reset()
self.playful_bark_interval = urandom.randint(4, 8) * 1000
def act_angry(self):
"""Makes the puppy act angry."""
if self.did_behavior_change:
print('act_angry')
self.eyes = self.ANGRY_EYES
self.ev3.speaker.play_file(SoundFile.DOG_GROWL)
self.stand_up()
wait(1500)
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
self.pet_count -= 1
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
self.behavior = self.idle
def act_hungry(self):
if self.did_behavior_change:
print('act_hungry')
self.eyes = self.HURT_EYES
self.sit_down()
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
if self.update_feed_count():
# If we got food, then we are not longer hungry.
self.behavior = self.idle
if self.update_pet_count():
# If we got a pet instead of food, then we are angry.
self.behavior = self.act_angry
def go_to_bathroom(self):
if self.did_behavior_change:
print('go_to_bathroom')
self.eyes = self.SQUINTY_EYES
self.stand_up()
wait(100)
self.right_leg_motor.run_target(100, self.STRETCH_ANGLE)
wait(800)
self.ev3.speaker.play_file(SoundFile.HORN_1)
wait(1000)
for _ in range(3):
self.right_leg_motor.run_angle(100, 20)
self.right_leg_motor.run_angle(100, -20)
self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE)
self.feed_count = 1
self.behavior = self.idle
def act_happy(self):
if self.did_behavior_change:
print('act_happy')
self.eyes = self.HEART_EYES
# self.move_head(self.?)
self.sit_down()
for _ in range(3):
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
self.hop()
wait(500)
self.sit_down()
self.reset()
def sit_down(self):
"""Makes the puppy sit down."""
self.left_leg_motor.run(-50)
self.right_leg_motor.run(-50)
wait(1000)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
wait(100)
# The next 4 methods define actions that are used to make some parts of
# the behaviors above.
def stand_up(self):
"""Makes the puppy stand up."""
self.left_leg_motor.run_target(100, self.HALF_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.HALF_UP_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
self.left_leg_motor.run_target(50, self.STAND_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(50, self.STAND_UP_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
wait(500)
def stretch(self):
"""Makes the puppy stretch its legs backwards."""
self.stand_up()
self.left_leg_motor.run_target(100, self.STRETCH_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.STRETCH_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
self.left_leg_motor.run_target(100, self.STAND_UP_ANGLE, wait=False)
self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE)
while not self.left_leg_motor.control.done():
wait(100)
def hop(self):
"""Makes the puppy hop."""
self.left_leg_motor.run(500)
self.right_leg_motor.run(500)
wait(275)
self.left_leg_motor.hold()
self.right_leg_motor.hold()
wait(275)
self.left_leg_motor.run(-50)
self.right_leg_motor.run(-50)
wait(275)
self.left_leg_motor.stop()
self.right_leg_motor.stop()
@property
def behavior(self):
"""Gets and sets the current behavior."""
return self._behavior
@behavior.setter
def behavior(self, value):
if self._behavior != value:
self._behavior = value
self._behavior_changed = True
@property
def did_behavior_change(self):
"""bool: Tests if the behavior changed since the last time this
property was read.
"""
if self._behavior_changed:
self._behavior_changed = False
return True
return False
def update_behavior(self):
"""Updates the :prop:`behavior` property based on the current state
of petting and feeding.
"""
if self.pet_count == self.pet_target and self.feed_count == self.feed_target:
# If we have the exact right amount of pets and feeds, act happy.
self.behavior = self.act_happy
elif self.pet_count > self.pet_target and self.feed_count < self.feed_target:
# If we have too many pets and not enough food, act angry.
self.behavior = self.act_angry
elif self.pet_count < self.pet_target and self.feed_count > self.feed_target:
# If we have not enough pets and too much food, go to the bathroom.
self.behavior = self.go_to_bathroom
elif self.pet_count == 0 and self.feed_count > 0:
# If we have no pets and some food, act playful.
self.behavior = self.act_playful
elif self.feed_count == 0:
# If we have no food, act hungry.
self.behavior = self.act_hungry
@property
def eyes(self):
"""Gets and sets the eyes."""
return self._eyes
@eyes.setter
def eyes(self, value):
if value != self._eyes:
self._eyes = value
self.ev3.screen.load_image(value)
def update_eyes(self):
if self.eyes_timer_1.time() > self.eyes_timer_1_end:
self.eyes_timer_1.reset()
if self.eyes == self.SLEEPING_EYES:
self.eyes_timer_1_end = urandom.randint(1, 5) * 1000
self.eyes = self.TIRED_RIGHT_EYES
else:
self.eyes_timer_1_end = 250
self.eyes = self.SLEEPING_EYES
if self.eyes_timer_2.time() > self.eyes_timer_2_end:
self.eyes_timer_2.reset()
if self.eyes != self.SLEEPING_EYES:
self.eyes_timer_2_end = urandom.randint(1, 10) * 1000
if self.eyes != self.TIRED_LEFT_EYES:
self.eyes = self.TIRED_LEFT_EYES
else:
self.eyes = self.TIRED_RIGHT_EYES
def update_pet_count(self):
"""Updates the :attr:`pet_count` attribute if the puppy is currently
being petted (touch sensor pressed).
Returns:
bool:
``True`` if the puppy was petted since the last time this method
was called, otherwise ``False``.
"""
changed = False
petted = self.touch_sensor.pressed()
if petted and petted != self.prev_petted:
self.pet_count += 1
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
self.count_changed_timer.reset()
if not self.behavior == self.act_hungry:
self.eyes = self.SQUINTY_EYES
self.ev3.speaker.play_file(SoundFile.DOG_SNIFF)
changed = True
self.prev_petted = petted
return changed
def update_feed_count(self):
"""Updates the :attr:`feed_count` attribute if the puppy is currently
being fed (color sensor detects a color).
Returns:
bool:
``True`` if the puppy was fed since the last time this method
was called, otherwise ``False``.
"""
color = self.color_sensor.color()
changed = False
if color is not None and color != Color.BLACK and color != self.prev_color:
self.feed_count += 1
print('feed_count:', self.feed_count, 'feed_target:',
self.feed_target)
changed = True
self.count_changed_timer.reset()
self.prev_color = color
self.eyes = self.SQUINTY_EYES
self.ev3.speaker.play_file(SoundFile.CRUNCHING)
return changed
def monitor_counts(self):
"""Monitors pet and feed counts and decreases them over time."""
if self.pet_count_timer.time() > 15000:
self.pet_count_timer.reset()
self.pet_count = max(0, self.pet_count - 1)
print('pet_count:', self.pet_count, 'pet_target:', self.pet_target)
if self.feed_count_timer.time() > 15000:
self.feed_count_timer.reset()
self.feed_count = max(0, self.feed_count - 1)
print('feed_count:', self.feed_count, 'feed_target:',
self.feed_target)
if self.count_changed_timer.time() > 30000:
# If nothing has happened for 30 seconds, go to sleep
self.count_changed_timer.reset()
self.behavior = self.go_to_sleep
def run(self):
"""This is the main program run loop."""
self.sit_down()
self.adjust_head()
self.eyes = self.SLEEPING_EYES
self.reset()
while True:
self.monitor_counts()
self.behavior()
wait(100)
right_motor = Motor(Port.C)
gyro = GyroSensor(Port.S3)
# Resetando valores
left_motor.reset_angle(0)
right_motor.reset_angle(0)
gyro.reset_angle(0)
# Variáveis utilizadas no processo
porcentagem = 60
distancia = cm_to_degree(100)
target = 45
kp, ki, kd = 1, 0.01, 2
integral, derivate, error, last_error = 0, 0, 0, 0
while distancia > media_wheels(left_motor, right_motor):
error = target - gyro.angle()
integral += error
derivate = error - last_error
correcao = kp * (error + ki * integral + kd * derivate)
if (correcao >= 0):
left_motor.dc(porcentagem + correcao)
right_motor.dc(porcentagem)
else:
right_motor.dc(porcentagem + correcao * -1)
left_motor.dc(porcentagem)
left_motor.hold()
right_motor.hold()
class MrB3am:
def __init__(
self,
gear_motor_port: Port = Port.A,
color_sensor_port: Port = Port.S3):
self.ev3_brick = EV3Brick()
self.gear_motor = Motor(port=gear_motor_port,
positive_direction=Direction.CLOCKWISE)
self.color_sensor = ColorSensor(port=color_sensor_port)
def header_text(self):
self.ev3_brick.screen.clear()
self.ev3_brick.screen.draw_text(
x=0, y=0,
text='MR. B3AM',
text_color=Color.BLACK,
background_color=None)
def insert_b3am(self):
"""
This waits for a B3am to be inserted.
A B3am is detected when the ambient light level is
above or equal to the value 3.
When a B3am is inserted the motor stops
and the EV3 Brick says "Thank you".
"""
self.header_text()
self.ev3_brick.screen.draw_text(
x=0, y=15,
text='Insert B3am!',
text_color=Color.BLACK,
background_color=None)
self.gear_motor.run(speed=-150)
while self.color_sensor.reflection() < 3:
pass
self.gear_motor.hold()
self.ev3_brick.speaker.play_file(SoundFile.THANK_YOU)
def measure_b3am(self):
"""
This measures the length of the B3am,
by first resetting the motor counter and then moving the B3am
until the other end is found.
This is detected when the ambient light level is below the value 1.
Note that the length measured is the number of degrees
that the wheels has turned.
This value will later be converted to the actual B3am length.
After having found the length of the B3am, the length is saved
in a variable, named "current_b3am_length_in_degrees".
"""
self.header_text()
self.ev3_brick.screen.draw_text(
x=0, y=15,
text='Measuring B3am...',
text_color=Color.BLACK,
background_color=None)
self.gear_motor.reset_angle(angle=0)
self.gear_motor.run(speed=-150)
while self.color_sensor.reflection() > 1:
wait(10)
self.gear_motor.hold()
self.current_b3am_length_in_degrees = abs(self.gear_motor.angle())
def detect_color(self):
"""
Afterwards the B3am is moved half way thorugh the machine
so its color can be measured.
When the color is found it is saved in a variable,
named "current_b3am_color_code" and the EV3 Brick says "Detected".
Note the saved value is the color ID and this will later be converted
to the actual color name.
"""
self.header_text()
self.ev3_brick.screen.draw_text(
x=0, y=15,
text='Detecting Color...',
text_color=Color.BLACK,
background_color=None)
self.gear_motor.run_angle(
speed=150,
rotation_angle=self.current_b3am_length_in_degrees / 2,
then=Stop.HOLD,
wait=True)
self.current_b3am_color_code = self.color_sensor.color()
self.ev3_brick.speaker.play_file(SoundFile.DETECTED)
def eject_b3am(self):
"""
After the color is found, the EV3 calculates the number of degrees
required to move the wheels,
such that the B3am is ejected from the machine.
"""
self.header_text()
self.ev3_brick.screen.draw_text(
x=0, y=15,
text='Ejecting B3am...',
text_color=Color.BLACK,
background_color=None)
self.gear_motor.run_angle(
speed=150,
rotation_angle=self.current_b3am_length_in_degrees / 2 + 700,
then=Stop.HOLD,
wait=True)
def process_b3am(self):
self.insert_b3am()
self.measure_b3am()
self.detect_color()
self.eject_b3am()
def report_result(self, debug=False):
"""
Report the result of the measurement.
The switch to the right has a case for each color
the Color Sensor is able to detect.
MR-B3AM converts from the number of rotation degrees to B3am lengths.
"""
self.header_text()
if self.current_b3am_color_code == Color.BLACK:
self.current_b3am_color = 'black'
if 400 <= self.current_b3am_length_in_degrees <= 600:
self.current_b3am_length = 5
elif 601 <= self.current_b3am_length_in_degrees <= 800:
self.current_b3am_length = 7
elif 801 <= self.current_b3am_length_in_degrees <= 1000:
self.current_b3am_length = 9
elif 1001 <= self.current_b3am_length_in_degrees <= 1300:
self.current_b3am_length = 11
elif 1301 <= self.current_b3am_length_in_degrees <= 1500:
self.current_b3am_length = 13
elif 1501 <= self.current_b3am_length_in_degrees <= 1700:
self.current_b3am_length = 15
elif self.current_b3am_color_code == Color.RED:
self.current_b3am_color = 'red'
if 400 <= self.current_b3am_length_in_degrees <= 800:
self.current_b3am_length = 5
elif 801 <= self.current_b3am_length_in_degrees <= 1050:
self.current_b3am_length = 7
elif 1051 <= self.current_b3am_length_in_degrees <= 1300:
self.current_b3am_length = 9
elif 1301 <= self.current_b3am_length_in_degrees <= 1500:
self.current_b3am_length = 11
elif 1501 <= self.current_b3am_length_in_degrees <= 1700:
self.current_b3am_length = 13
elif 1701 <= self.current_b3am_length_in_degrees <= 1900:
self.current_b3am_length = 15
else:
self.current_b3am_color = 'UNKNOWN'
self.current_b3am_length = 'UNKNOWN'
self.ev3_brick.screen.draw_text(
x=0, y=15,
text='Color: {}'.format(self.current_b3am_color.upper()),
text_color=Color.BLACK,
background_color=None)
self.ev3_brick.screen.draw_text(
x=0, y=30,
text='Length: {}'.format(self.current_b3am_length),
text_color=Color.BLACK,
background_color=None)
if debug:
self.ev3_brick.screen.draw_text(
x=0, y=60,
text='{}'.format(self.current_b3am_color_code),
text_color=Color.BLACK,
background_color=None)
self.ev3_brick.screen.draw_text(
x=0, y=75,
text='Degrees: {:,}'.format(
self.current_b3am_length_in_degrees),
text_color=Color.BLACK,
background_color=None)
self.ev3_brick.speaker.say(
text='{color} {length}{n_degrees}'.format(
color=self.current_b3am_color,
length=self.current_b3am_length,
n_degrees=' ({} Degrees)'.format(
self.current_b3am_length_in_degrees)
if debug
else ''))