Description: Verifies motor readings such as angle, absolute angle, and speed.
"""
from pybricks.pupdevices import Motor, UltrasonicSensor
from pybricks.parameters import Port, Direction
from pybricks.tools import wait, StopWatch
# Initialize devices.
motor = Motor(Port.A)
motor = Motor(port=Port.A, positive_direction=Direction.CLOCKWISE, gears=[])
ultrasonic_sensor = UltrasonicSensor(Port.C)
# Assert initial values.
assert motor.speed() == 0, "Unexpected initial motor speed."
assert -180 <= motor.angle() <= 179, "Unexpected initial motor angle."
# Verify driving to zero.
for target in (-360, 0, 360):
motor.run_target(500, target)
assert ultrasonic_sensor.distance() < 100, "Unexpected distance result."
# Test angle reset.
for reset_value in (-98304, -360, -180, -1.234, 0, 0.1, 1, 178, 180, 13245687):
motor.reset_angle(reset_value)
assert motor.angle() == int(reset_value), "Incorrect angle reset"
# Test motion after reset. Also test kwarg.
motor.reset_angle(angle=180)
for target in (-180, 180):
motor.run_target(500, target)
[1, 2, 3],
[1, 28],
[12, 36],
[36, 12],
[[1, 24], [12, 36]],
[(1, 24), (12, 36)],
[(1, 24), [12, 36]],
[[57, 24], [12, 123456789, 1]],
]
# Initialize device to known position.
motor = Motor(Port.A, gears=None)
motor.run_target(500, 170)
wait(500)
motor.dc(0)
real_angle = motor.angle()
# Test expected result.
for gears in test_args:
# Initialize motors with new settings.
motor = Motor(Port.A, gears=gears)
# Get expected and measured value.
expected = real_angle / get_gear_ratio(gears)
measured = motor.angle()
# Allow at most one degree of error. We could be a bit more precise
# here and also test the expected rounding direction, by investigating
# how this is currently defined. For now, 1 degree both ways will do.
assert abs(motor.angle() - expected) <= 1, "{0} != {1}".format(
expected, measured)
from pybricks.pupdevices import Motor
from pybricks.parameters import Port
from pybricks.tools import wait
# Initialize a motor on port A.
example_motor = Motor(Port.A)
# Start moving at 300 degrees per second.
example_motor.run(300)
# Display the angle and speed 50 times.
for i in range(100):
# Read the angle (degrees) and speed (degrees per second).
angle = example_motor.angle()
speed = example_motor.speed()
# Print the values.
print(angle, speed)
# Wait some time so we can read what is displayed.
wait(200)
mDrive = Motor(Port.D)
mSteer = Motor(Port.B)
mSteer.reset_angle()
SPEED_DRIVE = 100
TIME_DRIVE = 30
STEP_STEER = 15
SPEED_STEER = 720
MAX_STEER = 75
mSteer.run_target(SPEED_STEER, 0, then=Stop.BRAKE)
while True:
c = getchar()
if c == ord('q'):
mDrive.dc(SPEED_DRIVE)
wait(TIME_DRIVE)
elif c == ord('a'):
mDrive.dc(-SPEED_DRIVE)
wait(TIME_DRIVE)
elif c == ord('o'):
if mSteer.angle() > -MAX_STEER:
mSteer.run_angle(SPEED_STEER, -STEP_STEER, then=Stop.BRAKE)
elif c == ord('p'):
if mSteer.angle() < MAX_STEER:
mSteer.run_angle(SPEED_STEER, STEP_STEER, then=Stop.BRAKE)
elif c == ord('r') or c == ord('0'):
mSteer.run_target(SPEED_STEER, 0, then=Stop.BRAKE)
else:
mDrive.stop()
buf = [0] * window
idx = 0
angle = 0
DRIVE_SPEED = 300
PAUSE = 5000
# Timer to generate reference position.
watch = StopWatch()
while True:
# Get angular rate and estimated angle.
rate = hub.imu.angular_velocity(Axis.Y)
angle += rate * DT / 1000
# Get motor position.
position = (left.angle() + right.angle()) / 2
# Calculate motor speed.
speed = (position - buf[idx]) / (window * DT) * 1000
buf[idx] = position
idx = (idx + 1) % window
# Calculate reference position, which just grows linearly with
# time.
reference = -max(watch.time() - PAUSE, 0) / 1000 * DRIVE_SPEED
# Calculate duty cycle.
diff = position - reference
duty = 0.018 * rate + 19 * angle + 0.45 * diff + 0.16 * speed
# Account for battery level and type.
drive = Motor(Port.D)
steer = Motor(Port.B)
SPEED_DRIVE = 100
TIME_DRIVE = 30
STEP_STEER = 15
SPEED_STEER = 720
MAX_STEER = 75
steer.reset_angle()
steer.run_target(SPEED_STEER, 0, then=Stop.BRAKE)
while True:
c = getchar()
if c == ord('q'):
drive.dc(SPEED_DRIVE)
wait(TIME_DRIVE)
elif c == ord('a'):
drive.dc(-SPEED_DRIVE)
wait(TIME_DRIVE)
elif c == ord('o'):
if steer.angle() > -MAX_STEER:
steer.run_angle(SPEED_STEER, -STEP_STEER, then=Stop.BRAKE)
elif c == ord('p'):
if steer.angle() < MAX_STEER:
steer.run_angle(SPEED_STEER, STEP_STEER, then=Stop.BRAKE)
elif c == ord('r') or c == ord('0'):
steer.run_target(SPEED_STEER, 0, then=Stop.BRAKE)
else:
drive.stop()
# Detect object.
motor.run_target(500, 0)
distance = ultrasonic_sensor.distance()
assert distance < 100, "Expected < 100 mm, got {0}.".format(distance)
# Move object away.
motor.run_target(500, 180)
distance = ultrasonic_sensor.distance()
assert distance > 100, "Expected > 100 mm, got {0}.".format(distance)
# Prepare fast detection.
motor.reset_angle(0)
motor.run(700)
DETECTIONS = 5
# Wait for given number of detections.
for i in range(DETECTIONS):
# Wait for object to be detected.
while ultrasonic_sensor.distance() > 100:
pass
# Wait for object to move away.
angle_detected = motor.angle()
while motor.angle() < angle_detected + 180:
pass
# Assert that we have made as many turns.
rotations = round(motor.angle() / 360)
assert rotations == DETECTIONS, "Expected {0} turns, got {1}.".format(
DETECTIONS, rotations)
