def walk_timed():
"""Makes the puppy walk a certain time in ms.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change walk_time to adjust the time the puppy walks in ms."""
#walk_time equals desired walk time in ms
walk_time = 6000
elapsed_time = StopWatch()
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
wait(500)
#puppy walks for specified time in ms
while elapsed_time.time() < walk_time:
left_leg_motor.run_target(-100, 25, wait=False)
right_leg_motor.run_target(-100, 25)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(100, 65, wait=False)
right_leg_motor.run_target(100, 65)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
wait(100)
elapsed_time.reset()
def drive_time(self, dist=100, time=1, ang=0):
stopwatch = StopWatch()
stopwatch.reset()
while True:
self.drive.drive(dist, ang)
if stopwatch.time() > time:
self.drive.drive.stop()
break
def andarTempo(self, velocEsquerda, velocDireita, tempo):
cronometro = StopWatch() # Definimos um cronometro
cronometro.reset() # Resetamos o tempo marcado no cronometro
while cronometro.time() < tempo:
self.motorDireito.run(velocEsquerda)
self.motorEsquerdo.run(velocDireita)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def get_double_press(
self
): # used to indicate a user has finished their time with CHORE bot
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed() and self.touch_right.pressed():
return True
if stopwatch.time() > 3000:
self.ev3.light.off()
return False
class Pid:
def __init__(self, kp, ki, kd):
"""Class that can be used to do PID calculations.
:param kp: Proportional multiplier for the error
:type kp: Number
:param ki: Multiplier for the intergral of the error
:type ki: Number
:param kd: Multiplier for the derivative of the error
:type kd: Number
"""
self.clock = StopWatch()
self.kp = kp
self.ki = ki
self.kd = kd
self.last_error = 0
self.total_error = 0
self.last_time = 0
def reset(self):
"""Reset parameters before start of PID loop
"""
self.clock.reset()
self.last_time = self.clock.time()
self.last_error = 0
self.total_error = 0
def compute_steering(self, error):
"""Computes and returns the corrections.
:param error: Difference between target and actual angles
:type error: Number
:return: Returns correction value
:rtype: Number
"""
current_time = self.clock.time()
elapsed_time = current_time - self.last_time
self.last_time = current_time
error_change = 0
if elapsed_time > 0:
error_change = (error - self.last_error) / elapsed_time
self.total_error = self.last_error * elapsed_time + self.total_error
self.last_error = error
steering = error * self.kp
steering += error_change * self.kd
steering += self.total_error * self.ki / 100
return steering
def get_feedback(self):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed():
self.ev3.light.off()
return -1
elif self.touch_right.pressed():
self.ev3.light.off()
return 1
if stopwatch.time() > 10000:
self.ev3.light.off()
return 0
def wait_until_bumped(self, wait_timer=500):
"""Wait until the TouchSensor is bumped
:param wait_timer: Time to wait (milliseconds) to consider a press and release a bump,
defaults to 500
:type wait_timer: int, float, optional
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_pressed()
my_watch.reset()
my_watch.resume()
self.wait_until_released()
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
def act_playful():
"""Makes the puppy act playful."""
playful_timer = StopWatch()
playful_bark_interval = None
ev3.screen.show_image(ImageFile.NEUTRAL)
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
playful_bark_interval = 0
if playful_timer.time() > playful_bark_interval:
ev3.speaker.play_file(SoundFile.DOG_BARK_2)
playful_timer.reset()
playful_bark_interval = randint(4, 8) * 1000
def get_color(self, time=1000):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.color_sensor.color() == Color.BLUE:
self.ev3.light.off()
return Color.BLUE
if self.color_sensor.color() == Color.GREEN:
self.ev3.light.off()
return Color.GREEN
if self.color_sensor.color() == Color.YELLOW:
self.ev3.light.off()
return Color.YELLOW
if self.color_sensor.color() == Color.RED:
self.ev3.light.off()
return Color.RED
if stopwatch.time() > time:
self.ev3.light.off()
return None
def wait_until_button_bumped(self, button, channel, wait_timer=500):
"""Waits until a specified button has been bumped
''NOTE: using a list, tuple or dict for buttons will make this function act the same as
wait_until_button_pressed if all of the buttons listed are pressed at once''
:param button: Button or Buttons to wait for
:type button: Button, list, tuple, dict
:param channel: Channel number of the remote
:type channel: int
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_button_pressed(button, channel)
my_watch.reset()
my_watch.resume()
self.wait_until_button_released(button, channel)
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
def motoresDianteiros(motorBotton, motorTop):
ev3.speaker.say("Acionando motores")
cronometro = StopWatch()
cronometro.reset()
botoes = []
botoesAnt = botoes
while(True):
botoes = ev3.buttons.pressed()
if(len(botoes) == 0 or botoes != botoesAnt):
cronometro.reset()
motorBotton.brake()
motorTop.brake()
elif([Button.CENTER] == botoes and cronometro.time()>=TKEYPRESS):
break
elif([Button.UP] == botoes and cronometro.time()>=TKEYPRESS):
motorBotton.dc(100)
elif([Button.DOWN] == botoes and cronometro.time()>=TKEYPRESS):
motorBotton.dc(-100)
elif([Button.LEFT] == botoes and cronometro.time()>=TKEYPRESS):
motorTop.dc(-50)
elif([Button.RIGHT] == botoes and cronometro.time()>=TKEYPRESS):
motorTop.dc(50)
botoesAnt = botoes
class FUNCTION_LIBRARY:
def __init__(self, robot, ev3, left_motor, right_motor, medium_motor,
color_sensor_1, color_sensor_2):
#self, DriveBase, Hub
self.driveBase = robot
self.hub = ev3
self.left_motor = left_motor
self.right_motor = right_motor
self.medium_motor = medium_motor
self.left_motor.reset_angle(0)
self.left_motor.reset_angle(0)
self.stopWatch = StopWatch()
self.color_sensor_1 = color_sensor_1
self.color_sensor_2 = color_sensor_2
def shutdown(self):
self.hub.speaker.say(
"Logic error, error error error error error error error error error error errorrr Non halting program detected, shutting down"
)
#self.hub.speaker.say("Shutting down...")
self.hub.speaker.play_notes(['C4/4', 'F3/4', 'F2/4'])
def line_follow_until_black(self,
p=1.2,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
sensor_stop=-1,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_1
if (sensor_stop == -1):
sensor_stop = self.color_sensor_2
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if sensor_stop.reflection() <= BLACK: #
self.driveBase.stop()
break #thrillitup
def line_follow_until_white(self,
p=1.2,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
sensor_stop=-1,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_1
if (sensor_stop == -1):
sensor_stop = self.color_sensor_2 # I NEED TO CREATE NEW CONSTANt
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if sensor_stop.reflection() <= WHITE: #
self.driveBase.stop()
break #thrillitup
def line_follow_for_time(self,
p=1,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
time=10000,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_2
self.stopWatch.reset()
self.stopWatch.resume()
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if self.stopWatch.time() > time: #
break #STOP THIEF
self.driveBase.stop()
self.hub.speaker.say("I line followed for" + str(floor(time / 1000)) +
"seconds")
def line_follow_for_distance(self,
p=1,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
distance=10000,
debug=False):
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
if (sensor_lf == -1):
sensor_lf = self.color_sensor_2
elif (sensor_lf == -2):
sensor_lf = self.color_sensor_1
PROPORTIONAL_GAIN = p
threshold = (BLACK + WHITE) / 2 #the center of black+white
startingDistance = self.driveBase.distance()
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if self.driveBase.distance() - startingDistance > distance: #
break #STOP THIEF
self.driveBase.stop()
##self.hub.speaker.say("I have reached " + str(floor(distance/25.4)) + "inches") removed this, pauses robot too long
def mm_to_inch(self, mm):
return mm / 25.4
def inch_to_mm(self, inch):
return inch * 25.4
def ms_to_second(self, ms):
return ms / 1000
def second_to_ms(self, s):
return s * 1000
class Puppy:
# Constants for leg angle
HALF_UP_ANGLE = 25
STAND_UP_ANGLE = 65
STRETCH_ANGLE = 125
# Loads the different eyes
HEART_EYES = Image(ImageFile.LOVE)
SQUINTY_EYES = Image(ImageFile.TEAR)
def __init__(self):
# Sets up the brick
self.ev3 = EV3Brick()
# Identifies which motor is connected to which ports
self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
# Sets up the gear ratio (automatically translates it to the correct angle)
self.head_motor = Motor(Port.C,
Direction.COUNTERCLOCKWISE,
gears=[[1, 24], [12, 36]])
# Sets up touch sensor
self.touch_sensor = TouchSensor(Port.S1)
# Sets up constants for the eye
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
def movements(self):
self.ev3.screen.load_image(ImageFile.EV3_ICON)
self.ev3.light.on(Color.ORANGE)
dog_pat = 0
# Movement interactions
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)
elif self.touch_sensor.pressed():
self.ev3.speaker.play_file(SoundFile.DOG_BARK_1)
self.eyes = self.HEART_EYES
self.sit_down()
dog_pat += 1
print(dog_pat)
elif dog_pat == 3:
self.go_to_bathroom()
dog_pat -= 3
print(dog_pat)
else:
self.head_motor.stop()
wait(100)
self.head_motor.stop()
self.head_motor.reset_angle(0)
self.ev3.light.on(Color.GREEN)
# Below this line I honestly have no clue how it works. It came from the boiler plate of functions
def move_head(self, target):
self.head_motor.run_target(20, target)
@property
def eyes(self):
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 stand_up(self):
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 sit_down(self):
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)
def go_to_bathroom(self):
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)
def run(self):
self.movements()
self.eyes = self.SLEEPING_EYES
cronometro = StopWatch()
ev3.speaker.say("Iniciando programa de controle")
botoes = []
botoesAnt = botoes
(a1, a0) = carregaCalibracao()
contador = 0
while(True):
# Vamos implementar o menu principal
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
cronometro.reset()
elif(botoes != botoesAnt):
cronometro.reset()
elif([Button.UP] == botoes and cronometro.time()>=TKEYPRESS):
(a1, a0, contador) = executaCalibracao(corLeft, corRight, contador, udp)
salvaCalibracao(a1, a0)
elif([Button.DOWN] == botoes and cronometro.time()>=TKEYPRESS):
motoresDianteiros(motorBotton, motorTop)
elif([Button.RIGHT] == botoes and cronometro.time()>=TKEYPRESS):
potRef = 80
K = 2
contador = executaProva(potRef, K, corLeft, corRight, a1, a0, topSensor, bottonSensor, motorLeft, motorRight, udp, dest, contador)
elif([Button.CENTER] == botoes and cronometro.time()>=TKEYPRESS):
opcoesMenuPrincipal()
left = corLeft.rgb()
# Set up the Timer. It is used to move for a random time.
timer = StopWatch()
# This is the main part of the program. It is a loop that repeats
# endlessly.
while True:
checking = True
move = 0
# This loop moves Znap around while checking for objects. The loop
# repeats until an object is closer than 400 mm.
while checking:
# Reset the Timer and generate a random time to move for.
timer.reset()
random_time = 600 * randint(1, 3)
# Znap moves in three different ways.
if move <= 1:
# Turn clockwise.
robot.drive(0, 250)
# Wait a bit the first time.
if move == 0:
wait(500)
move = 2
elif move == 2:
# Turn counterclockwise.
robot.drive(0, -250)
move = 3
else:
num_moves = []
data = DataLog('Episode #', 'Reward', "Time", "Num_feedbacks", "agent_pos", "is_done", \
"username", name="ep_log", timestamp=True, extension='csv', append=False)
q_table_data = DataLog(name="q_table", extension='csv', append=False)
child_fdbktable_data = DataLog(name="child_table",
extension='csv',
append=False)
parent_fdbktable_data = DataLog(name="parent_table",
extension='csv',
append=False)
episode_stopwatch = StopWatch()
episode_stopwatch.reset()
user_color = None
user = None
fam.say("Please enter user")
user_color = fam.get_color(3000)
if user_color is Color.RED:
fam.say("Hello child")
user = child
elif user_color is Color.GREEN:
fam.say("Hello parent")
user = parent
elif user_color is Color.BLUE or user_color is None:
user = no_user
first_session = True
rightmotor = Motor(Port.D, Direction.CLOCKWISE)
robot = DriveBase(leftmotor, rightmotor, 40, 200)
color = ColorSensor(Port.S1)
uart = UARTDevice(Port.S4, 9600, timeout=1000)
direction = 1
#initial waiting phase:
holder = b'a'
motorspeed = 0
minispeed = 0
filename = 'letters.csv'
fobj = open(filename, 'w')
for up in range(20):
print(up)
watch.reset()
watch.resume()
while watch.time() < 200:
robot.drive(20, 0)
robot.drive(0, 0)
for side in range(80):
watchside.reset()
watchside.resume()
while watchside.time() < 150:
minimotor.run(direction * -50)
colorcheck(side, up, direction)
direction *= -1
while True:
colorcheck()
direction = 1
if uart.waiting() != 0:
# the speaker could be beeping, so we need to stop both of those.
def stop_action():
ev3.speaker.beep(0, -1)
arm_motor.run_target(ARM_MOTOR_SPEED, 0)
while True:
# Sleeping eyes and light off let us know that the robot is waiting for
# any movement to stop before the program can continue.
ev3.screen.load_image(ImageFile.SLEEPING)
ev3.light.off()
# Reset the sensors and variables.
left_motor.reset_angle(0)
right_motor.reset_angle(0)
fall_timer.reset()
motor_position_sum = 0
wheel_angle = 0
motor_position_change = [0, 0, 0, 0]
drive_speed, steering = 0, 0
control_loop_count = 0
robot_body_angle = -0.25
# Since update_action() is a generator (it uses "yield" instead of
# "return") this doesn't actually run update_action() right now but
# rather prepares it for use later.
action_task = update_action()
# Calibrate the gyro offset. This makes sure that the robot is perfectly
# still by making sure that the measured rate does not fluctuate more than
# 3. ToDo: Setzen der Parameter des PID-Reglers
# Die passenden Parameter müsst ihr selbst durch Tests heraus finden!
# Tipp: Zu Beginn setzt nur kP = 1 und den Rest auf Null.
# Anschließend könnt ihr andere Parametereinstellungen durchführen.
# kP: Parameter des P-Anteils
# kI: Parameter des I-Anteils
# kD: Parameter des D-Anteils
# Initialisieung einer Liste, um vergangene Abstandsmessungen zu speichern.
# Diese ist notwendig damit am Ende, der durchschnittliche Abstand zum Vordermann bestimmt werden kann
dist = []
# Initialisierung eines Timers, um kontinuierlich den Abstand zum Vordermann zu messen
# und ihn in der Liste zu speichern.
clockDist = StopWatch()
clockDist.reset()
clockDist.resume()
durationDist = 0
programRunning = True
while programRunning:
# 4. ToDo: Implementierung der Linien-Verfolgung
# Sobald eines der beiden Farbsensoren die Linie detektiert, soll der passende Motor stoppen.
# Ansonsten sollen beide Motoren mit der aktuellen Geschwindigkeit laufen.
# Abstandsmessung zum Vordermann (distance) und speichern in der Liste
if clockDist.time() > durationDist:
distance = ultraSensor.distance()
dist.append(distance)
durationDist = 500
def executaCalibracao(sensorLeft, sensorRight, contador, udp):
ev3.speaker.say("Calibração dos sensores")
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
ev3.speaker.say("Para cima, branco")
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
ev3.speaker.say("Para baixo, preto")
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
ev3.speaker.say("Para esquerda, verde")
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
ev3.speaker.say("Para direita, cinza")
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
ev3.speaker.say("Centro, retorna ao menu principal")
botoes = ev3.buttons.pressed()
cronometro = StopWatch()
cronometro.reset()
botoes = []
botoesAnt = botoes
while(True):
# Vamos implementar o menu principal
botoes = ev3.buttons.pressed()
if(len(botoes) == 0):
cronometro.reset()
elif(botoes != botoesAnt):
cronometro.reset()
elif([Button.UP] == botoes and cronometro.time()>=TKEYPRESS):
ev3.speaker.say("Calibrando branco")
branco = leCorMedia(sensorLeft, sensorRight)
erroBranco = encontraErroRGB(branco)
ev3.speaker.say("Feito")
elif([Button.DOWN] == botoes and cronometro.time()>=TKEYPRESS):
ev3.speaker.say("Calibrando preto")
preto = leCorMedia(sensorLeft, sensorRight)
erroPreto = encontraErroRGB(preto)
ev3.speaker.say("Feito")
elif([Button.LEFT] == botoes and cronometro.time()>=TKEYPRESS):
ev3.speaker.say("Calibrando verde")
verde = leCorMedia(sensorLeft, sensorRight)
erroVerde = encontraErroRGB(verde)
ev3.speaker.say("Feito")
elif([Button.RIGHT] == botoes and cronometro.time()>=TKEYPRESS):
ev3.speaker.say("Calibrando cinza")
cinza = leCorMedia(sensorLeft, sensorRight)
erroCinza = encontraErroRGB(cinza)
ev3.speaker.say("Feito")
elif([Button.CENTER] == botoes and cronometro.time()>=TKEYPRESS):
break
left = corLeft.rgb()
right= corRight.rgb()
lr = "{0:.2f}".format(left[0])
lg = "{0:.2f}".format(left[1])
lb = "{0:.2f}".format(left[2])
rr = "{0:.2f}".format(right[0])
rg = "{0:.2f}".format(right[1])
rb = "{0:.2f}".format(right[2])
contador += 1
msg = str(contador)+" "+lr+" "+lg+" "+lb+" "+rr+" "+rg+" "+rb
udp.sendto (msg, dest)
botoesAnt = botoes
X = [(branco[3], branco[4], branco[5]), (preto[3], preto[4], preto[5]), (verde[3], verde[4], verde[5]), (cinza[3], cinza[4], cinza[5])]
Y = [(erroBranco[0], erroBranco[1], erroBranco[2]), (erroPreto[0], erroPreto[1], erroPreto[2]), (erroVerde[0], erroVerde[1], erroVerde[2]), (erroCinza[0], erroCinza[1], erroCinza[2])]
(a1, a0) = minimosQuadradosRGB(Y, X)
return (a1, a0, contador)
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.
#replaced HURT, HEART, SQUINTY with AWAKE,DIZZY,PINCHED_MIDDLE respectively
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.AWAKE)
ANGRY_EYES = Image(ImageFile.ANGRY)
HEART_EYES = Image(ImageFile.DIZZY)
SQUINTY_EYES = Image(ImageFile.PINCHED_MIDDLE)
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.
self.color_sensor = ColorSensor(Port.S4)
# Initialize the touch sensor.
self.touch_sensor = TouchSensor(Port.S1)
# This attribute is used by properties.
self._eyes = None
# These attributes are used by the playful behavior.
self.playful_timer = StopWatch()
self.playful_bark_interval = 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.show_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)
self.behavior = self.idle
# The next 10 methods define the 10 behaviors of the puppy.
def idle(self):
"""The puppy is idle."""
self.stand_up()
self.eyes = self.NEUTRAL_EYES
def go_to_sleep(self):
"""Makes the puppy go to sleep.
Press the center button and touch sensor at the same time to awaken the puppy."""
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.behavior = self.wake_up
def wake_up(self):
"""Makes the puppy 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."""
self.eyes = self.NEUTRAL_EYES
self.stand_up()
self.playful_bark_interval = 0
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 = randint(4, 8) * 1000
def act_angry(self):
"""Makes the puppy 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)
def act_hungry(self):
"""Makes the puppy act hungry."""
self.eyes = self.HURT_EYES
self.sit_down()
self.ev3.speaker.play_file(SoundFile.DOG_WHINE)
def go_to_bathroom(self):
"""Makes the puppy go to the 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.behavior = self.idle
def act_happy(self):
"""Makes the puppy 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)
def walk_steps(self):
"""Makes the puppy walk a certain number of steps.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change steps to adjuct the number of steps."""
#steps equals number of steps pup takes
steps = 10
self.stand_up()
for value in range(1, steps + 1):
self.left_leg_motor.run_target(-100, 25, wait=False)
self.right_leg_motor.run_target(-100, 25)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(100, 65, wait=False)
self.right_leg_motor.run_target(100, 65)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(50, 65, wait=False)
self.right_leg_motor.run_target(50, 65)
wait(100)
def walk_timed(self):
"""Makes the puppy walk a certain time in ms.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change walk_time to adjust the time the puppy walks in ms."""
#walk_time equals desired walk time in ms
walk_time = 6000
elapsed_time = StopWatch()
while elapsed_time.time() < walk_time:
self.left_leg_motor.run_target(-100, 25, wait=False)
self.right_leg_motor.run_target(-100, 25)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(100, 65, wait=False)
self.right_leg_motor.run_target(100, 65)
while not self.left_leg_motor.control.target_tolerances():
wait(200)
self.left_leg_motor.run_target(50, 65, wait=False)
self.right_leg_motor.run_target(50, 65)
wait(100)
elapsed_time.reset()
# 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.target_tolerances():
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.target_tolerances():
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)
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)
wait(500)
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.stop()
self.right_leg_motor.stop()
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 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.show_image(value)
def run(self):
"""This is the main program run loop."""
self.sit_down()
self.adjust_head()
self.eyes = self.SLEEPING_EYES
self.reset()
#self.eyes = self.SLEEPING_EYES
"""The following code cycles through all of the behaviors, separated by beeps."""
self.act_playful()
wait(1000)
self.ev3.speaker.beep()
self.act_happy()
wait(1000)
self.ev3.speaker.beep()
self.act_hungry()
wait(1000)
self.ev3.speaker.beep()
self.act_angry()
wait(1000)
self.ev3.speaker.beep()
self.go_to_bathroom()
wait(1000)
self.ev3.speaker.beep()
self.go_to_sleep()
wait(1000)
self.ev3.speaker.beep()
self.wake_up()
wait(1000)
self.ev3.speaker.beep()
self.walk_steps()
wait(1000)
self.ev3.speaker.beep()
self.walk_timed()
wait(1000)
self.ev3.speaker.beep()
self.idle()
wait(1000)
self.ev3.speaker.beep()
# Sensors
#ultraSensor = UltrasonicSensor(Port.S3)
leftColorSensor = ColorSensor(Port.S1)
rightColorSensor = ColorSensor(Port.S4)
# Button pressed
#up_pressed = Button.Up in brick.buttons()
programRunning = True
# Speed
minSpeed = 100
maxSpeed = 400
# Clock
clockLine = StopWatch()
clockLine.reset()
clockLine.resume()
durationLine = 0
clockDist = StopWatch()
clockDist.reset()
clockDist.resume()
durationDist = 0
# Distance array
dist = []
while programRunning:
print("GO")
# Timer
if clockLine.time() > durationLine:
if Button.DOWN in brick.buttons():
print("It's time for the DOWN BUTTON")
CurrentMission += 1
if len(missions) == CurrentMission:
CurrentMission = 0
elif Button.UP in brick.buttons():
print("It's time for the UP BUTTON")
CurrentMission -= 1
if -1 == CurrentMission:
CurrentMission = len(missions) - 1
elif Button.CENTER in brick.buttons():
if Start == False:
time.reset()
wait(100)
robot.gyro.reset_angle(0)
print("It's time to RUN")
print("Mission Started. Your time was", time.time() // 1000)
Start = True
if CurrentMission == 4:
alpha.Testie()
elif CurrentMission == 0:
alpha.Run1()
elif CurrentMission == 1:
alpha.Run2()
def test_max_speed(self):
log.info('--- test_max_speed ---')
SPEED_STEP, MIN_TEST_TIME, CHECK_INTERVAL = 50, 16000, 250
max_speed, speed_left= 0, SPEED_STEP
motor_right, motor_left = Motor(ROBOT['drive_motor_port_left']), Motor(ROBOT['drive_motor_port_right'])
motor_left.reset_angle(0)
motor_right.reset_angle(0)
robot = DriveBase(motor_left, motor_right, ROBOT['wheel_diameter'], ROBOT['wheel_axle_track'])
watch = StopWatch()
robot.drive(speed_left, 0)
angle_left, angle_right = 0,0
while True:
run_time = watch.time()
old_speed = speed_left
speed_left = motor_left.speed()
# timeout
if run_time >= MIN_TEST_TIME:
log.info('reach max test time, speed_left=%d, max_speed=%d' % (speed_left, max_speed))
break
# found higher speed reached
if speed_left > max_speed:
print('motor angle: left=%d, right=%d' % (motor_left.angle(), motor_right.angle()))
print('motor speed: left=%d, right=%d' % (motor_left.speed(), motor_right.speed()))
# stop and run more time with higher speed
robot.stop()
watch.reset()
motor_left.reset_angle(0)
motor_right.reset_angle(0)
angle_left, angle_right = 0,0
max_speed = speed_left
speed_left += SPEED_STEP
print('drive one more time, speed_left=%d, max_speed=%d' % (speed_left, max_speed ))
robot.drive(speed_left, 0)
continue
# continue
a_l = motor_left.angle()
a_r = motor_right.angle()
angle_left += a_l
angle_right += a_r
#print('angle/total angle: %d/%d - %d/%d' % (a_l, angle_left, a_r, angle_right))
steering = (abs(angle_left-angle_right) // 10) * 10
if steering > 30:
steering = 30
if angle_left > angle_right:
steering = 0 - steering
if abs(angle_left - angle_right) > 10:
print('delta/total/steering: %3d/%3d/%3d' % (a_l - a_r,angle_left-angle_right, steering))
motor_left.reset_angle(0)
motor_right.reset_angle(0)
robot.drive((motor_left.speed() + motor_right.speed()) / 2, steering)
else:
print('.')
wait(CHECK_INTERVAL) # wait one second to let motor reach higher speed
print('motor speed: left=%d, right=%d' % (motor_left.speed(), motor_right.speed()))
print('total motor angle: left=%d, right=%d' % (angle_left, angle_right))
log.info('test_max_speed: battery=%d, max_speed=%d' % (brick.battery.voltage(), max_speed))
robot.stop()
if Button.LEFT_UP in infraredSensor.buttons(2):
switchSound()
else:
if Button.LEFT_DOWN in infraredSensor.buttons(2):
switchSound()
if Button.RIGHT_DOWN in infraredSensor.buttons(2):
manualSound = False
# system setup
ev3.speaker.set_volume(100, which='_all_')
motor.reset_angle(20)
soundMotor.reset_angle(0)
motor.run_target(500, 75, wait=True)
ev3.light.on(Color.GREEN)
watch.reset()
# main project loop
while shutdown == False:
''' finishing the progamm '''
if Button.LEFT_UP in infraredSensor.buttons(4):
shutdown = True
''' checking touch sensor '''
if touchSensor.pressed() == True:
if manualLight == False and manualSound == False:
manualLight = True
manualSound = True
else:
manualLight = False
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
running = True
energy_used = 0
energy_watch = StopWatch()
motorR = Motor(Port.A)
motorL = Motor(Port.B)
while running:
#Manuelle Steuerung der Motoren. Kann entfernt werden
if Button.DOWN in brick.buttons():
motorR.run(400)
else:
motorR.stop()
if Button.UP in brick.buttons():
motorL.run(400)
else:
motorL.stop()
wait(100)
#Messung der verbrauchten Arbeit
energy_used += brick.battery.current() * brick.battery.voltage(
) * energy_watch.time() / math.pow(10, 9)
energy_watch.reset()
brick.display.clear()
r = "Arbeit: " + str('%.2f' % round(energy_used, 2)) + "J"
brick.display.text(r, (40, 60))
class Robot:
def __init__(self):
# micro controller
ev3 = EV3Brick()
ev3.speaker.beep()
# sensors
# low value = registers black tape
# high value = aluminum
self.sensorYellow = ColorSensor(Port.S1)
self.sensorRed = ColorSensor(Port.S3)
self.sensorBlue = ColorSensor(Port.S2)
# motor
left_motor = Motor(Port.A, gears=[40, 24])
right_motor = Motor(Port.B, gears=[40, 24])
axle_track = 205
wheel_diameter = 35
self.motors = DriveBase(left_motor, right_motor,
wheel_diameter, axle_track)
# constants
# intersection detection of side sensors
self.thresholdBlueSensor = 30
# value for making turns
self.thresholdSideSensors = 15
# timer
self.watch = StopWatch()
self.timer = 0
def drive(self, directions):
i = 0
while i < len(directions):
self.timer = self.watch.time()
if self.timer % 100 == 0:
print(self.timer)
self.correctPath()
if self.senseIntersection() == True and self.timer >= 500:
print('intersection')
self.motors.drive_time(0, 0, 1000) # reduce this when done
self.executeCommand(directions[i])
i += 1
self.motors.drive(-125, 0)
def executeCommand(self, cmd):
if cmd == 0:
print('straight')
self.driveStraight()
if cmd == 1:
print('right')
self.turnRight()
if cmd == 2:
print('left')
self.turnLeft()
if cmd == 3:
print('reverse')
self.reverse()
if cmd == 4:
print('stop')
# turning behaviours at intersection
def turnLeft(self):
self.motors.drive_time(-30, 44, 2000)
self.watch.reset()
def turnRight(self):
self.motors.drive_time(-30, -46, 2000)
self.watch.reset()
def driveStraight(self):
self.motors.drive_time(-60, 0, 1800)
self.watch.reset()
def reverse(self):
self.motors.drive_time(60, 0, 1800)
self.motors.drive_time(0, 94, 2000)
self.motors.drive_time(-60, 0, 800)
# intersection detection
def senseIntersection(self):
if self.sensorRed.reflection() < 2 or self.sensorYellow.reflection() < 2:
return True
# path correction
# completely aluminum = 23
# completely black tape = 1
def correctPath(self):
if self.sensorBlue.reflection() < 8:
self.adjustLeft()
if self.sensorBlue.reflection() > 16:
self.adjustRight()
# default: -125, angle
def adjustLeft(self):
angle = 12 - min(self.sensorBlue.reflection(), 10)
step = 125 + (12 - min(self.sensorBlue.reflection(), 10))
self.motors.drive(-step, angle)
def adjustRight(self):
angle = max(self.sensorBlue.reflection(), 14) -12
step = 125 + (max(self.sensorBlue.reflection(), 14) -12)
self.motors.drive(-step, -angle)
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)
