bricks/ev3: Add Motor Control on apps tab.

Author: laurensvalk

bricks/_common/micropython.c

@@ -306,6 +306,8 @@ pbio_error_t pbsys_main_program_validate(pbsys_main_program_t *program) {
                 return PBIO_SUCCESS;
             #endif
             #if PBSYS_CONFIG_FEATURE_BUILTIN_USER_PROGRAM_EV3_APPS
+            case PBIO_PYBRICKS_USER_PROGRAM_ID_EV3_MOTOR_CONTROL:
+                return PBIO_SUCCESS;
             #endif
             default:
                 return PBIO_ERROR_NOT_SUPPORTED;
@@ -398,6 +400,10 @@ void pbsys_main_run_program(pbsys_main_program_t *program) {
         #endif
 
         #if PBSYS_CONFIG_FEATURE_BUILTIN_USER_PROGRAM_EV3_APPS
+        case PBIO_PYBRICKS_USER_PROGRAM_ID_EV3_MOTOR_CONTROL:
+            pb_package_pybricks_init(false);
+            pyexec_frozen_module("_ev3_motor_control.py", false);
+            break;
         #endif
 
         default:

bricks/ev3/manifest.py

@@ -1 +1,2 @@
 include("../_common/manifest.py")
+freeze_as_mpy("../ev3/modules", "_ev3_motor_control.py")

bricks/ev3/modules/_ev3_motor_control.py

@@ -0,0 +1,113 @@
+from pybricks.tools import wait
+from pybricks.parameters import ImageFile, Button, Port, Color
+from pybricks.hubs import EV3Brick
+from pybricks.ev3devices import Motor
+
+ev3 = EV3Brick()
+
+# True means A&D pair is active. False means B&C pair is active.
+pair_ad_active = False
+
+# Re-index motors by port for re-initialization on the fly.
+motors = {
+    Port.A: None,
+    Port.B: None,
+    Port.C: None,
+    Port.D: None,
+}
+
+POWER = 100
+
+# Preload to avoid repeated allocation.
+IMG_POS = ImageFile._ROTATE_CW18
+IMG_NEG = ImageFile._ROTATE_CCW18
+IMG_AD = ImageFile._APP_MOTOR_CONTROL_AD
+IMG_BC = ImageFile._APP_MOTOR_CONTROL_BC
+
+
+# Wrapper to set DC and reinitialize motor on the fly.
+def set_dc(port, dc):
+    # If there was a motor, close if it got unplugged.
+    if motors[port] is not None:
+        try:
+            motors[port].angle()
+        except OSError:
+            motors[port].close()
+            motors[port] = None
+            pass
+
+    # If there isn't a motor, try to initialize it now.
+    if motors[port] is None:
+        try:
+            motors[port] = Motor(port)
+        except OSError:
+            pass
+
+    # Drive the motor if there is one.
+    if motors[port]:
+        try:
+            motors[port].dc(dc)
+            # On success, indicate the active direction on the display.
+            image = IMG_POS if dc > 0 else IMG_NEG if dc < 0 else None
+            if image:
+                index = ord(repr(port)[5]) - ord("A")
+                ev3.screen.draw_image(20 + index * 40, 2, image)
+        except OSError:
+            motors[port].close()
+            motors[port] = None
+
+
+def draw_ui():
+    ev3.screen.clear()
+    file = IMG_AD if pair_ad_active else IMG_BC
+    ev3.screen.draw_image(0, 20, file)
+    ev3.screen.draw_box(0, 0, 177, 16, fill=True, color=Color.WHITE)
+
+
+while True:
+    draw_ui()
+    ev3.light.on(Color.GREEN)
+
+    # Wait for anything to happen
+    while not (pressed := ev3.buttons.pressed()):
+        wait(10)
+
+    # Center button toggles motor pair
+    if Button.CENTER in pressed:
+        pair_ad_active = not pair_ad_active
+        draw_ui()
+        while any(ev3.buttons.pressed()):
+            wait(10)
+        continue
+
+    # Determine motor power for vertical buttons.
+    vertical_dc = 0
+    if Button.UP in pressed:
+        vertical_dc += POWER
+    if Button.DOWN in pressed:
+        vertical_dc -= POWER
+
+    # Determine motor power for horizontal buttons.
+    horizontal_dc = 0
+    if Button.RIGHT in pressed:
+        horizontal_dc += POWER
+    if Button.LEFT in pressed:
+        horizontal_dc -= POWER
+
+    # Activity heartbeat
+    ev3.light.blink(Color.GREEN, [100, 100, 800, 100])
+
+    # Drive one or two motors.
+    set_dc(Port.A if pair_ad_active else Port.B, vertical_dc)
+    set_dc(Port.D if pair_ad_active else Port.C, horizontal_dc)
+
+    # Keep going until button state changes.
+    while ev3.buttons.pressed() == pressed:
+        wait(10)
+
+    # Stop all motors.
+    for motor in motors.values():
+        try:
+            motor.stop()
+        except (OSError, AttributeError):
+            pass

bricks/virtualhub/manifest.py

@@ -1 +1,2 @@
 include("../_common/manifest.py")
+freeze_as_mpy("../ev3/modules", "_ev3_motor_control.py")

lib/pbio/include/pbio/protocol.h

@@ -69,6 +69,10 @@ typedef enum {
      * data persistently on the hub.
      */
     PBIO_PYBRICKS_USER_PROGRAM_ID_IMU_CALIBRATION = 130,
+    /**
+     * Program to control EV3 motors with the buttons.
+     */
+    PBIO_PYBRICKS_USER_PROGRAM_ID_EV3_MOTOR_CONTROL = 133,
 } pbio_pybricks_user_program_id_t;
 
 /**