# Choose the drive speed based on the right controls.
drive_speed = 0
if Button.RIGHT_PLUS in pressed:
drive_speed += 1000
if Button.RIGHT_MINUS in pressed:
drive_speed -= 1000
# Apply the selected speed.
drive_motor1.run(drive_speed)
drive_motor2.run(drive_speed)
# Button for differential lock
if Button.CENTER in pressed:
# Stop the drive motors
drive_motor1.stop()
drive_motor2.stop()
# Run lock motor for half a second.
remote.light.on(Color.RED)
lock_motor.dc(LOCK_POWER if locked else -LOCK_POWER)
wait(500)
lock_motor.stop()
# Update lock state.
locked = not locked
remote.light.on(Color.BLUE if locked else Color.GREEN)
# Wait.
wait(10)
# Basic trajectories starting from zero. It would be nice to compute them
# here in the loop so we can also include random tests, vary acceleration, etc.
trajectories = [
((500, 90), (0, 244, 244, 488, 0, 45, 45, 90, 0, 367, 1500, -1500)),
((1000, 720), (0, 666, 720, 1386, 0, 333, 387, 720, 0, 1000, 1500, -1500)),
((2000, 720), (0, 666, 720, 1386, 0, 333, 387, 720, 0, 1000, 1500, -1500)),
((10, 720), (0, 6, 72006, 72012, 0, 0, 720, 720, 0, 10, 1500, -1500)),
((-500, 360), (0, 333, 721, 1054, 0, -83, -277, -360, 0, -500, -1500,
1500)),
((500, -360), (0, 333, 721, 1054, 0, -83, -277, -360, 0, -500, -1500,
1500)),
((-500, -360), (0, 333, 721, 1054, 0, 83, 277, 360, 0, 500, 1500, -1500)),
((500, 1), (0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1500, -1500)),
((500, 0), (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)),
]
# Assert that all trajectories are correct.
for (speed, angle), trajectory in trajectories:
# Start from 0.
motor.reset_angle(0)
# Initiate run_target command, stopping immediately.
motor.run_angle(speed, angle, wait=False)
result = motor.control.trajectory()
motor.stop()
# Compare generated trajectory to saved trajectory.
assert trajectory == result, "Bad trajectory: {0} for {1}".format(
result, (speed, angle))
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) # Run at 500 deg/s and then stop by coasting. example_motor.run(500) wait(1500) example_motor.stop() wait(1500) # Run at 500 deg/s and then stop by braking. example_motor.run(500) wait(1500) example_motor.brake() wait(1500) # Run at 500 deg/s and then stop by holding. example_motor.run(500) wait(1500) example_motor.hold() wait(1500) # Run at 500 deg/s and then stop by running at 0 speed. example_motor.run(500) wait(1500) example_motor.run(0) wait(1500)
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()
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()
class ExplorationRover():
"""Control the Mars Exploration Rover."""
# In this robot, we want to detect red and cyan/teal "mars rocks".
ROCK_COLORS = (Color.RED, Color.CYAN)
# These are the gears between the motor and the rear wheels.
REAR_GEARS = (8, 24, 12, 20)
# An animation of heart icons of varying brightness, giving a heart beat.
HEART_BEAT = [
Icon.HEART * i / 100
for i in list(range(0, 100, 4)) + list(range(100, 0, -4))
]
def __init__(self):
# Initialize each motor with the correct direction and gearing.
self.left_rear_wheels = Motor(Port.E, Direction.CLOCKWISE,
self.REAR_GEARS)
self.right_rear_wheels = Motor(Port.F, Direction.COUNTERCLOCKWISE,
self.REAR_GEARS)
self.left_front_wheel = Motor(Port.C, Direction.COUNTERCLOCKWISE, None)
self.right_front_wheel = Motor(Port.D, Direction.CLOCKWISE, None)
# Initialize sensors, named after the Perseverance Mars Rover instruments.
self.mast_cam = UltrasonicSensor(Port.B)
self.sherloc = ColorSensor(Port.A)
# Initialize the hub and start the animation
self.hub = InventorHub()
self.hub.display.animate(self.HEART_BEAT, 30)
def calibrate(self):
# First, measure the color of the floor. This makes it easy to explicitly
# ignore the floor later. This is useful if your floor has a wood color,
# which can appear red or yellow to the sensor.
self.floor_color = self.sherloc.hsv()
# Set up all the colors we want to distinguish, including no color at all.
self.sherloc.detectable_colors(self.ROCK_COLORS +
(self.floor_color, None))
def drive(self, speed, steering, time=None):
# Drive the robot at a given speed and steering for a given amount of time.
# Speed and steering is expressed as degrees per second of the wheels.
self.left_rear_wheels.run(speed + steering)
self.left_front_wheel.run(speed + steering)
self.right_rear_wheels.run(speed - steering)
self.right_front_wheel.run(speed - steering)
# If the user specified a time, wait for this duration and then stop.
if time is not None:
wait(time)
self.stop()
def stop(self):
# Stops all the wheels.
self.left_rear_wheels.stop()
self.left_front_wheel.stop()
self.right_rear_wheels.stop()
self.right_front_wheel.stop()
def scan_rock(self, time):
# Stops the robot and moves the scan arm.
self.stop()
self.left_front_wheel.run(100)
# During the given duration, scan for rocks.
watch = StopWatch()
while watch.time() < time:
# If a rock color is detected, display it and make a sound.
if self.sherloc.color() in self.ROCK_COLORS:
self.hub.display.image(Icon.CIRCLE)
self.hub.speaker.beep()
else:
self.hub.display.off()
wait(10)
# Turn the arm motor and restore the heartbeat animation again.
self.hub.display.animate(self.HEART_BEAT, 30)
self.left_front_wheel.stop()
