Draft demonstration of RFCOMM support

sets/mindstorms-ev3/education-expansion/remote_control/main.py

@@ -0,0 +1,142 @@
+from pybricks.hubs import EV3Brick
+from pybricks.parameters import Color, Port, Stop, Direction
+from pybricks.ev3devices import Motor
+from pybricks.tools import wait, StopWatch, run_task
+from pybricks.messaging import rfcomm_connect
+from micropython import const
+from umath import sin, pi
+import ustruct
+
+hub = EV3Brick()
+
+# The large wheel connected to port A is the steering wheel.
+axis1_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
+
+# The small wheel connected to port D is the throttle trigger.
+axis2_motor = Motor(Port.D)
+
+# These variables are used to compute a duty cycle that resists the user's
+# attempt to turn the motor in a direction with a low duty cycle, while still
+# allowing us to tell when the user stops turning the motor in that direction.
+
+OSCILLATE_PERIOD = const(22)  # ms
+ANGULAR_FREQ_COEFF = 2 * pi / OSCILLATE_PERIOD
+
+def resist_duty(base_duty, t, static_friction_duty) -> int:
+    """Computes a duty cycle that oscillates around a base duty cycle."""
+    added_duty = sin(t * ANGULAR_FREQ_COEFF) * static_friction_duty
+    return int(base_duty + added_duty)
+
+# The fraction of the motor's power that is required to overcome the
+# motor's static friction. For large motors, it's a greater fraction
+# of the motor's maximum power than for small motors.
+LARGE_MOTOR_STATIC_FRICTION_DUTY = const(20)
+SMALL_MOTOR_STATIC_FRICTION_DUTY = const(10)
+
+# The bluetooth address of the remotely controlled device. Change this
+# to the address printed by your remote controlled device program.
+TARGET_BLUETOOTH_ADDRESS = 'F0:45:DA:13:1C:8A'
+
+def steering_wheel(t: int) -> int:
+    """Drives the steering wheel process.
+
+    1. Provides force-feedback for the steering wheel depending on how
+       far it is from neutral.
+    2. Returns an integer between -127 and 127 that expresses how far
+       the steering wheel is from neutral.
+
+    Args:
+        t (int): The time in milliseconds since the process started.
+    Returns:
+        int: An integer between -127 and 127 representing the steering
+                wheel position.
+    """
+    STEERING_RESIST_MAX_DUTY = const(30)
+    STEERING_DEAD_ZONE = const(10)  # degrees
+    RESIST_START_ANGLE = const(50)  # degrees - angle where resistance begins
+    RESIST_RANGE = const(130)  # degrees - range over which resistance ramps to max
+    MAX_ANGLE = const(180)  # degrees - maximum steering wheel angle
+
+    angle = axis1_motor.angle()
+    speed = axis1_motor.speed()
+    abs_angle = abs(angle)
+
+    if abs_angle < STEERING_DEAD_ZONE:
+        hub.light.on(Color.BLUE)
+        axis1_motor.brake()
+        if abs_angle < 1:
+            axis1_motor.stop()
+    elif (angle > 0) != (speed > 0) and speed != 0:
+        # If the motor is currently turned in one direction and turning back
+        # toward zero, encourage that motion by running the motor toward zero.
+        axis1_motor.dc(-30 if angle > 0 else 30)
+    else:
+        # If the user is currently turning the motor away from zero, resist
+        # that movement with a duty cycle that increases the further we get
+        # from zero. Duty cycle of resistance maxes out at 15%.
+        base_duty = STEERING_RESIST_MAX_DUTY * max(0, min(1, (abs_angle - RESIST_START_ANGLE) / RESIST_RANGE))
+        duty = resist_duty(base_duty, t, LARGE_MOTOR_STATIC_FRICTION_DUTY)
+        duty = duty if angle < 0 else -duty
+        axis1_motor.dc(duty)
+
+    # Scale the return value: 0 in dead zone, ±127 at max resistance angle
+    if abs_angle < STEERING_DEAD_ZONE:
+        return 0
+
+    scaled = int(127 * (abs_angle - STEERING_DEAD_ZONE) / (MAX_ANGLE - STEERING_DEAD_ZONE))
+    scaled = min(127, scaled)  # Cap at 127
+    return scaled if angle > 0 else -scaled
+
+
+AXIS2_MAX_THROW = const(29)  # degrees - maximum throw for throttle
+
+
+def throttle(t: int) -> int:
+    """Returns an integer between 0 and 127 representing the throttle position."""
+    angle = axis2_motor.angle()
+
+    # This extra duty we apply helps keep the motor unstuck, which makes
+    # it easier for the user to adjust it with small increments.
+    axis2_motor.dc(resist_duty(0, t, SMALL_MOTOR_STATIC_FRICTION_DUTY))
+
+    scaled = int(127 * angle / AXIS2_MAX_THROW)
+    return min(127, max(0, scaled))
+
+
+# How often we send update messages to the remote controlled device.
+UPDATE_PERIOD = const(16)  # ms
+
+async def main():
+    """Main remote control process."""
+    while True:
+        hub.light.on(Color.RED)
+        conn = None
+        try: 
+            conn = await rfcomm_connect(TARGET_BLUETOOTH_ADDRESS)
+            hub.light.on(Color.GREEN)
+            stopwatch = StopWatch()
+            update = StopWatch()
+            msg_buf = bytearray(2)
+            while True:
+                t = stopwatch.time()
+                wheel = steering_wheel(t)
+                throttle_value = throttle(t)
+
+                if update.time() < UPDATE_PERIOD:
+                    # We drive the steering wheel and throttle at a
+                    # higher rate than we send out updates, because
+                    # their motors need to be adjusted more frequently
+                    # than our RC device needs to receive updates.
+                    continue
+
+                ustruct.pack_into('>bb', msg_buf, 0, wheel, throttle_value)
+                conn.write(msg_buf)
+                update.reset()
+
+                print(f"AXIS1:{wheel} AXIS2:{throttle_value}")
+                await wait(1)
+        finally:
+            if conn:
+                conn.close()
+
+run_task(main())

sets/mindstorms-ev3/education-expansion/tank_bot_rc/main.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env pybricks-micropython
+
+"""
+Example LEGO® MINDSTORMS® EV3 Tank Bot Program
+----------------------------------------------
+
+This program requires LEGO® EV3 MicroPython v2.0.
+Download: https://education.lego.com/en-us/support/mindstorms-ev3/python-for-ev3
+
+Building instructions can be found at:
+https://education.lego.com/en-us/support/mindstorms-ev3/building-instructions#building-expansion
+"""
+
+from pybricks.hubs import EV3Brick
+from pybricks.ev3devices import Motor, GyroSensor
+from pybricks.parameters import Port, Direction, Button, Color
+from pybricks.tools import StopWatch, wait, run_task
+from pybricks.robotics import DriveBase
+from pybricks.messaging import rfcomm_listen, local_address 
+
+from micropython import const
+
+import ustruct
+
+# Initialize the EV3 brick.
+ev3 = EV3Brick()
+
+# Configure 2 motors on Ports B and C.  Set the motor directions to
+# counterclockwise, so that positive speed values make the robot move
+# forward.  These will be the left and right motors of the Tank Bot.
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
+
+# The wheel diameter of the Tank Bot is about 54 mm.
+WHEEL_DIAMETER = 54
+
+# The axle track is the distance between the centers of each of the
+# wheels.  This is about 200 mm for the Tank Bot.
+AXLE_TRACK = 200
+
+# The Driving Base is comprised of 2 motors.  There is a wheel on each
+# motor.  The wheel diameter and axle track values are used to make the
+# motors move at the correct speed when you give a drive command.
+robot = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+
+SPEED_SCALE = 6  # Scale factor for speed (768 // 127)
+TURN_SCALE = 2  # Scale factor for turn rate (320 // 127)
+
+# Storage for incoming messages from remote control.
+msg_buf = bytearray(2)    
+msg_buf_view = memoryview(msg_buf)
+
+# Tracks the next-to-be-filled index in msg_buf.
+cur_idx = 0
+
+async def main():
+    while True:
+        print('Local address: ', local_address())
+        ev3.light.on(Color.RED)
+        print('Waiting for connection...')
+        conn = await rfcomm_listen()
+        print('Connected!')
+        ev3.light.on(Color.GREEN)
+
+        timeout = StopWatch()
+        cur_idx = 0
+        while timeout.time() < 100:
+            cur_idx += conn.readinto(msg_buf_view[cur_idx:], len(msg_buf) - cur_idx)
+
+            if cur_idx != len(msg_buf):
+                # We were not able to read the entire message. Loop again.
+                await wait(1)
+                continue
+
+            timeout.reset()
+            axis1, axis2 = ustruct.unpack('>bb', msg_buf)
+            cur_idx = 0
+
+            speed = axis2 * SPEED_SCALE  # -768 to +768 mm/s
+            turn_rate = axis1 * TURN_SCALE  # -320 to +320 deg/s
+            robot.drive(speed, turn_rate)
+
+        robot.stop()
+        print('Client disconnected or timed out.')
+        conn.close()
+
+run_task(main())