From e07d3d05e85e3159d0b1ea4cccaa04366cec105d Mon Sep 17 00:00:00 2001
From: David Lechner <david@pybricks.com>
Date: Sun, 16 Feb 2020 22:05:03 -0600
Subject: [PATCH] ev3/gyro_boy: Add sensor input task

Functionality is still not quite the same as EV3-G/Scratch.
action_update() should remember the old speed during ultrasonic
reaction and then restore it.

Also, tuning is a bit off since loop time is ~26ms compared to ~18
in EV3-G/Scratch.
---
 .../ev3/education_core/gyro_boy/main.py       | 99 +++++++++++++++++--
 1 file changed, 90 insertions(+), 9 deletions(-)

diff --git a/official_models/ev3/education_core/gyro_boy/main.py b/official_models/ev3/education_core/gyro_boy/main.py
index 6f6ab7d..a653bc1 100755
--- a/official_models/ev3/education_core/gyro_boy/main.py
+++ b/official_models/ev3/education_core/gyro_boy/main.py
@@ -11,6 +11,8 @@ Building instructions can be found at:
 https://education.lego.com/en-us/support/mindstorms-ev3/building-instructions#building-core
 """
 
+import urandom
+
 from pybricks.hubs import EV3Brick
 from pybricks.ev3devices import Motor, UltrasonicSensor, ColorSensor, GyroSensor
 from pybricks.parameters import Port, Button, Color, ImageFile, SoundFile
@@ -21,6 +23,7 @@ ev3 = EV3Brick()
 
 left_motor = Motor(Port.D)
 right_motor = Motor(Port.A)
+arm_motor = Motor(Port.C)
 
 # Initialize the Color Sensor. It is used to detect the colors that command
 # which way the robot should move
@@ -33,19 +36,92 @@ ultrasonic_sensor = UltrasonicSensor(Port.S4)
 
 fall_timer = StopWatch()
 control_loop_timer = StopWatch()
+action_timer = StopWatch()
 
 GYRO_CALIBRATION_LOOP_COUNT = 200
 GYRO_OFFSET_FACTOR = 0.0005
-TARGET_LOOP_PERIOD = 15  # ms
+TARGET_LOOP_PERIOD = 25  # ms
+ARM_MOTOR_SPEED = 600  # deg/s
 
 # The colors on the stand are mapped to steering and drive speed values.
-# ACTION_MAP = {
-#     Color.RED: (0, 0),  # stop
-#     Color.GREEN: (0, 150),  # drive forward
-#     Color.BLUE: (-70, 0),  # turn left
-#     Color.YELLOW: (70, 0),  # turn right
-#     Color.WHITE: (0, -75),  # drive backward
-# }
+ACTION_MAP = {
+    Color.RED: (0, 0),  # stop
+    Color.GREEN: (0, 150),  # drive forward
+    Color.BLUE: (-70, 0),  # turn left
+    Color.YELLOW: (70, 0),  # turn right
+    Color.WHITE: (0, -75),  # drive backward
+}
+
+
+# This function monitors the color sensor and ultrasonic sensor. It is important
+# that no blocking calls are made in this function, otherwise it will affect the
+# control loop time in the main program.
+def update_action():
+    arm_motor.reset_angle(0)
+    action_timer.reset()
+
+    # drive forward for 4 seconds to leave stand
+    yield 0, 40
+    while action_timer.time() < 4000:
+        yield
+
+    # stop
+    yield 0, 0
+
+    # start checking sensors on arms
+    while True:
+        # Look up the detected color in the action map
+        action = ACTION_MAP.get(color_sensor.color())
+
+        # If the color was found, beep for 0.1 seconds and then yield the action
+        if action is not None:
+            action_timer.reset()
+            ev3.speaker.beep(1000, -1)
+            while action_timer.time() < 100:
+                yield
+            ev3.speaker.beep(0, -1)
+            yield action
+
+        # If the measured distance of the ultrasonic sensor is less than 250
+        # millimeters, then back up slowly.
+        if ultrasonic_sensor.distance() < 250:
+            yield 0, -10
+
+            # Wiggle the arms back and forth
+            arm_motor.run_angle(ARM_MOTOR_SPEED, 30, wait=False)
+            while not arm_motor.control.done():
+                yield
+            arm_motor.run_angle(ARM_MOTOR_SPEED, -60, wait=False)
+            while not arm_motor.control.done():
+                yield
+            arm_motor.run_angle(ARM_MOTOR_SPEED, 30, wait=False)
+            while not arm_motor.control.done():
+                yield
+
+            # Randomly turn left or right for 4 seconds
+            yield urandom.choice([70, -70]), -10
+            action_timer.reset()
+            while action_timer.time() < 4000:
+                yield
+
+            action_timer.reset()
+            ev3.speaker.beep(1000, -1)
+            while action_timer.time() < 100:
+                yield
+            ev3.speaker.beep(0, -1)
+
+            yield 0, 0
+
+        action_timer.reset()
+        while action_timer.time() < 100:
+            yield
+
+
+# If we fall over in the middle of an action, the arm motors could be moving or
+# the speaker could be beeping, so we need to stop those.
+def stop_action():
+    ev3.speaker.beep(0, -1)
+    arm_motor.run_target(ARM_MOTOR_SPEED, 0)
 
 
 while True:
@@ -64,6 +140,8 @@ while True:
     control_loop_count = 0
     robot_body_angle = -0.25
 
+    action_task = update_action()
+
     # Calibrate gyro offset
     while True:
         gyro_minimum_rate, gyro_maximum_rate = 440, -440
@@ -132,12 +210,15 @@ while True:
         elif fall_timer.time() > 1000:
             break
 
-        # TODO: add sensor checks to control drive_speed and steering
+        action = next(action_task)
+        if action is not None:
+            steering, drive_speed = action
 
         # make sure loop time is at least TARGET_LOOP_PERIOD
         wait(TARGET_LOOP_PERIOD - control_loop_period)
 
     # Handle falling over
+    stop_action()
     left_motor.stop()
     right_motor.stop()
     ev3.light.on(Color.RED)