# 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()