[robotpy][examples] Split examples and snippets (#8944)

This also updates the bazel scripts to behave more like the C++ and Java
examples, and updates the copybara scripts to be able to sync up
`mostrobotpy`

robotpyExamples/examples/RapidReactCommandBot/subsystems/drive.py

@@ -0,0 +1,145 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from typing import Callable
+
+from commands2 import Command, Subsystem
+import wpilib
+import wpimath
+
+from constants import DriveConstants
+
+
+class Drive(Subsystem):
+    def __init__(self) -> None:
+        """Creates a new Drive subsystem."""
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.leftLeader = wpilib.PWMSparkMax(DriveConstants.kLeftMotor1Port)
+        self.leftFollower = wpilib.PWMSparkMax(DriveConstants.kLeftMotor2Port)
+
+        # The motors on the right side of the drive.
+        self.rightLeader = wpilib.PWMSparkMax(DriveConstants.kRightMotor1Port)
+        self.rightFollower = wpilib.PWMSparkMax(DriveConstants.kRightMotor2Port)
+
+        # The robot's drive
+        self.drive = wpilib.DifferentialDrive(self.leftLeader, self.rightLeader)
+
+        # The left-side drive encoder
+        self.leftEncoder = wpilib.Encoder(
+            DriveConstants.kLeftEncoderPorts[0],
+            DriveConstants.kLeftEncoderPorts[1],
+            DriveConstants.kLeftEncoderReversed,
+        )
+
+        # The right-side drive encoder
+        self.rightEncoder = wpilib.Encoder(
+            DriveConstants.kRightEncoderPorts[0],
+            DriveConstants.kRightEncoderPorts[1],
+            DriveConstants.kRightEncoderReversed,
+        )
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+        self.controller = wpimath.ProfiledPIDController(
+            DriveConstants.kTurnP,
+            DriveConstants.kTurnI,
+            DriveConstants.kTurnD,
+            wpimath.TrapezoidProfile.Constraints(
+                DriveConstants.kMaxTurnRateDegPerS,
+                DriveConstants.kMaxTurnAccelerationDegPerSSquared,
+            ),
+        )
+        self.feedforward = wpimath.SimpleMotorFeedforwardMeters(
+            DriveConstants.kS, DriveConstants.kV, DriveConstants.kA
+        )
+
+        self.leftLeader.addFollower(self.leftFollower)
+        self.rightLeader.addFollower(self.rightFollower)
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightLeader.setInverted(True)
+
+        # Sets the distance per pulse for the encoders
+        self.leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        self.rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+
+        # Set the controller to be continuous (because it is an angle controller)
+        self.controller.enableContinuousInput(-180, 180)
+        # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+        # setpoint before it is considered as having reached the reference
+        self.controller.setTolerance(
+            DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS
+        )
+
+    def arcadeDriveCommand(
+        self, fwd: Callable[[], float], rot: Callable[[], float]
+    ) -> Command:
+        """Returns a command that drives the robot with arcade controls.
+
+        :param fwd: the commanded forward movement
+        :param rot: the commanded rotation
+        """
+        # A split-stick arcade command, with forward/backward controlled by the left
+        # hand, and turning controlled by the right.
+        return self.run(lambda: self.drive.arcadeDrive(fwd(), rot())).withName(
+            "arcadeDrive"
+        )
+
+    def driveDistanceCommand(self, distance: float, velocity: float) -> Command:
+        """Returns a command that drives the robot forward a specified distance at a specified
+        velocity.
+
+        :param distance: The distance to drive forward in meters
+        :param velocity: The fraction of max velocity at which to drive
+        """
+        return (
+            self.runOnce(
+                lambda: (
+                    self.leftEncoder.reset(),
+                    self.rightEncoder.reset(),
+                )
+            )
+            .andThen(self.run(lambda: self.drive.arcadeDrive(velocity, 0)))
+            .until(
+                lambda: max(
+                    self.leftEncoder.getDistance(), self.rightEncoder.getDistance()
+                )
+                >= distance
+            )
+            .finallyDo(lambda interrupted: self.drive.stopMotor())
+        )
+
+    def turnToAngleCommand(self, angleDeg: float) -> Command:
+        """Returns a command that turns to robot to the specified angle using a motion profile and
+        PID controller.
+
+        :param angleDeg: The angle to turn to
+        """
+
+        def _reset() -> None:
+            self.controller.reset(self.imu.getRotation2d().degrees())
+
+        def _run() -> None:
+            rotation_output = self.controller.calculate(
+                self.imu.getRotation2d().degrees(), angleDeg
+            )
+            feedforward = self.feedforward.calculate(
+                self.controller.getSetpoint().velocity
+            )
+            self.drive.arcadeDrive(
+                0,
+                rotation_output
+                + feedforward / wpilib.RobotController.getBatteryVoltage(),
+            )
+
+        return (
+            self.startRun(_reset, _run)
+            .until(self.controller.atGoal)
+            .finallyDo(lambda interrupted: self.drive.arcadeDrive(0, 0))
+        )

robotpyExamples/examples/RapidReactCommandBot/subsystems/intake.py

@@ -0,0 +1,43 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+import wpilib
+
+from constants import IntakeConstants
+
+
+class Intake(Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+        self.motor = wpilib.PWMSparkMax(IntakeConstants.kMotorPort)
+
+        # Double solenoid connected to two channels of a PCM with the default CAN ID
+        self.pistons = wpilib.DoubleSolenoid(
+            0,
+            wpilib.PneumaticsModuleType.CTRE_PCM,
+            IntakeConstants.kSolenoidPorts[0],
+            IntakeConstants.kSolenoidPorts[1],
+        )
+
+    def intakeCommand(self) -> Command:
+        """Returns a command that deploys the intake, and then runs the intake motor
+        indefinitely.
+        """
+        return (
+            self.runOnce(lambda: self.pistons.set(wpilib.DoubleSolenoid.Value.FORWARD))
+            .andThen(self.run(lambda: self.motor.set(1.0)))
+            .withName("Intake")
+        )
+
+    def retractCommand(self) -> Command:
+        """Returns a command that turns off and retracts the intake."""
+        return self.runOnce(
+            lambda: (
+                self.motor.disable(),
+                self.pistons.set(wpilib.DoubleSolenoid.Value.REVERSE),
+            )
+        ).withName("Retract")

robotpyExamples/examples/RapidReactCommandBot/subsystems/pneumatics.py

@@ -0,0 +1,56 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+import wpilib
+
+
+class Pneumatics(Subsystem):
+    """Subsystem for managing the compressor, pressure sensor, etc."""
+
+    # External analog pressure sensor
+    # product-specific voltage->pressure conversion, see product manual
+    # in this case, 250(V/5)-25
+    # the scale parameter in the AnalogPotentiometer constructor is scaled from 1 instead of 5,
+    # so if r is the raw AnalogPotentiometer output, the pressure is 250r-25
+    kScale = 250.0
+    kOffset = -25.0
+
+    def __init__(self) -> None:
+        super().__init__()
+        self.pressureTransducer = wpilib.AnalogPotentiometer(
+            # the AnalogIn port
+            2,
+            self.kScale,
+            self.kOffset,
+        )
+
+        # Compressor connected to a PCM with a default CAN ID (0)
+        self.compressor = wpilib.Compressor(0, wpilib.PneumaticsModuleType.CTRE_PCM)
+
+    def getPressure(self) -> float:
+        """Query the analog pressure sensor.
+
+        :returns: the measured pressure, in PSI
+        """
+        # Get the pressure (in PSI) from an analog pressure sensor connected to the RIO.
+        return self.pressureTransducer.get()
+
+    def disableCompressorCommand(self) -> Command:
+        """Disable the compressor closed-loop for as long as the command runs.
+
+        Structured this way as the compressor is enabled by default.
+
+        :returns: command
+        """
+        return self.startEnd(
+            # Disable closed-loop mode on the compressor.
+            self.compressor.disable,
+            # Enable closed-loop mode based on the digital pressure switch connected to the
+            # PCM/PH.
+            # The switch is open when the pressure is over ~120 PSI.
+            self.compressor.enableDigital,
+        ).withName("Compressor Disabled")

robotpyExamples/examples/RapidReactCommandBot/subsystems/shooter.py

@@ -0,0 +1,69 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, InstantCommand, Subsystem, cmd
+import wpilib
+import wpimath
+
+from constants import ShooterConstants
+
+
+class Shooter(Subsystem):
+    def __init__(self) -> None:
+        """The shooter subsystem for the robot."""
+        super().__init__()
+        self.shooterMotor = wpilib.PWMSparkMax(ShooterConstants.kShooterMotorPort)
+        self.feederMotor = wpilib.PWMSparkMax(ShooterConstants.kFeederMotorPort)
+        self.shooterEncoder = wpilib.Encoder(
+            ShooterConstants.kEncoderPorts[0],
+            ShooterConstants.kEncoderPorts[1],
+            ShooterConstants.kEncoderReversed,
+        )
+        self.shooterFeedforward = wpimath.SimpleMotorFeedforwardMeters(
+            ShooterConstants.kS, ShooterConstants.kV
+        )
+        self.shooterFeedback = wpimath.PIDController(ShooterConstants.kP, 0.0, 0.0)
+
+        self.shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS)
+        self.shooterEncoder.setDistancePerPulse(
+            ShooterConstants.kEncoderDistancePerPulse
+        )
+
+        # Set default command to turn off both the shooter and feeder motors, and then idle
+        self.setDefaultCommand(
+            self.runOnce(
+                lambda: (
+                    self.shooterMotor.disable(),
+                    self.feederMotor.disable(),
+                )
+            )
+            .andThen(self.run(lambda: None))
+            .withName("Idle")
+        )
+
+    def shootCommand(self, setpointRotationsPerSecond: float) -> Command:
+        """Returns a command to shoot the balls currently stored in the robot. Spins the shooter
+        flywheel up to the specified setpoint, and then runs the feeder motor.
+
+        :param setpointRotationsPerSecond: The desired shooter velocity
+        """
+
+        def _run_shooter() -> None:
+            self.shooterMotor.set(
+                self.shooterFeedforward.calculate(setpointRotationsPerSecond)
+                + self.shooterFeedback.calculate(
+                    self.shooterEncoder.getRate(), setpointRotationsPerSecond
+                )
+            )
+
+        return cmd.parallel(
+            # Run the shooter flywheel at the desired setpoint using feedforward and feedback
+            self.run(_run_shooter),
+            # Wait until the shooter has reached the setpoint, and then run the feeder
+            cmd.waitUntil(self.shooterFeedback.atSetpoint).andThen(
+                InstantCommand(lambda: self.feederMotor.set(1))
+            ),
+        ).withName("Shoot")

robotpyExamples/examples/RapidReactCommandBot/subsystems/storage.py

@@ -0,0 +1,35 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+from commands2 import Command, Subsystem
+from commands2.button import Trigger
+import wpilib
+
+from constants import StorageConstants
+
+
+class Storage(Subsystem):
+    def __init__(self) -> None:
+        """Create a new Storage subsystem."""
+        super().__init__()
+        self.motor = wpilib.PWMSparkMax(StorageConstants.kMotorPort)
+        self.ballSensor = wpilib.DigitalInput(StorageConstants.kBallSensorPort)
+
+        # Expose trigger from subsystem to improve readability and ease
+        # inter-subsystem communications
+        # Whether the ball storage is full.
+        self.hasCargo = Trigger(self.ballSensor.get)
+
+        # Set default command to turn off the storage motor and then idle
+        self.setDefaultCommand(
+            self.runOnce(self.motor.disable)
+            .andThen(self.run(lambda: None))
+            .withName("Idle")
+        )
+
+    def runCommand(self) -> Command:
+        """Returns a command that runs the storage motor indefinitely."""
+        return self.run(lambda: self.motor.set(1)).withName("run")