Resolver test bench

main.py

#!/usr/bin/env pybricks-micropython
# SPDX-License-Identifier: MIT
# Copyright (c) 2024 David Lechner <[email protected]>

import math

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import Motor, InfraredSensor
from pybricks.parameters import Port, Direction, Button
from pybricks.tools import wait


# This program requires LEGO EV3 MicroPython v2.x.
# Click "Open user guide" on the EV3 extension tab for more information.


ev3 = EV3Brick()
ir = InfraredSensor(Port.S4)
motor = Motor(Port.A, Direction.COUNTERCLOCKWISE, [(36, 12)] * 3)

# startup sound
ev3.speaker.beep(100)
ev3.speaker.beep(200)

while True:
    # angle isn't accurate because of high gearing
    # ev3.screen.print(
    #     "angle:", "{:.2f}".format(math.radians(motor.angle()) % (math.pi * 2)), "rad"
    # )
    ev3.screen.print("spd:", "{:.2f}".format(math.radians(motor.speed())), "rad/s")

    pressed = ir.buttons(channel=1)

    if pressed:
        motor.dc(100)
        # speed control doesn't work with default tuning
        # motor.run(DEFAULT_SPEED)
    else:
        motor.stop()

    wait(100)