[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/constants.py

@@ -0,0 +1,89 @@
+#
+# 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.
+#
+
+import math
+
+import wpimath.units
+
+
+class DriveConstants:
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    kEncoderCPR = 1024
+    kWheelDiameter = wpimath.units.inchesToMeters(6)
+    kEncoderDistancePerPulse = (kWheelDiameter * math.pi) / kEncoderCPR
+
+    # These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    # These values MUST be determined either experimentally or theoretically for *your* robot's
+    # drive. The SysId tool provides a convenient method for obtaining feedback and feedforward
+    # values for your robot.
+    kTurnP = 1.0
+    kTurnI = 0.0
+    kTurnD = 0.0
+
+    kTurnToleranceDeg = 5.0
+    kTurnRateToleranceDegPerS = 10.0  # degrees per second
+
+    kMaxTurnRateDegPerS = 100
+    kMaxTurnAccelerationDegPerSSquared = 300
+
+    kS = 1.0  # V
+    kV = 0.8  # V/(deg/s)
+    kA = 0.15  # V/(deg/s^2)
+
+
+class ShooterConstants:
+    kEncoderPorts = (4, 5)
+    kEncoderReversed = False
+    kEncoderCPR = 1024
+    # Distance units will be rotations
+    kEncoderDistancePerPulse = 1.0 / kEncoderCPR
+
+    kShooterMotorPort = 4
+    kFeederMotorPort = 5
+
+    kShooterFreeRPS = 5300.0
+    kShooterTargetRPS = 4000.0
+    kShooterToleranceRPS = 50.0
+
+    # These are not real PID gains, and will have to be tuned for your specific robot.
+    kP = 1.0
+
+    # On a real robot the feedforward constants should be empirically determined; these are
+    # reasonable guesses.
+    kS = 0.05  # V
+    # Should have value 12V at free velocity
+    kV = 12.0 / kShooterFreeRPS  # V/(rot/s)
+
+    kFeederVelocity = 0.5
+
+
+class IntakeConstants:
+    kMotorPort = 6
+    kSolenoidPorts = (2, 3)
+
+
+class StorageConstants:
+    kMotorPort = 7
+    kBallSensorPort = 6
+
+
+class AutoConstants:
+    kTimeout = 3
+    kDriveDistance = 2.0  # m
+    kDriveVelocity = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

robotpyExamples/examples/RapidReactCommandBot/rapidreactcommandbot.py

@@ -0,0 +1,88 @@
+#
+# 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.
+#
+
+import commands2
+from commands2.button import CommandNiDsXboxController, Trigger
+
+from constants import AutoConstants, OIConstants, ShooterConstants
+from subsystems.drive import Drive
+from subsystems.intake import Intake
+from subsystems.pneumatics import Pneumatics
+from subsystems.shooter import Shooter
+from subsystems.storage import Storage
+
+
+class RapidReactCommandBot:
+    """This class is where the bulk of the robot should be declared. Since Command-based is a
+    "declarative" paradigm, very little robot logic should actually be handled in the
+    :class:`.Robot` periodic methods (other than the scheduler calls). Instead, the structure of
+    the robot (including subsystems, commands, and button mappings) should be declared here.
+    """
+
+    def __init__(self) -> None:
+        # The robot's subsystems
+        self.drive = Drive()
+        self.intake = Intake()
+        self.storage = Storage()
+        self.shooter = Shooter()
+        self.pneumatics = Pneumatics()
+
+        # The driver's controller
+        self.driverController = CommandNiDsXboxController(
+            OIConstants.kDriverControllerPort
+        )
+
+    def configureBindings(self) -> None:
+        """Use this method to define bindings between conditions and commands. These are useful for
+        automating robot behaviors based on button and sensor input.
+
+        Should be called in the robot class constructor.
+
+        Event binding methods are available on the :class:`.Trigger` class.
+        """
+        # Automatically run the storage motor whenever the ball storage is not full,
+        # and turn it off whenever it fills. Uses subsystem-hosted trigger to
+        # improve readability and make inter-subsystem communication easier.
+        self.storage.hasCargo.whileFalse(self.storage.runCommand())
+
+        # Automatically disable and retract the intake whenever the ball storage is full.
+        self.storage.hasCargo.onTrue(self.intake.retractCommand())
+
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                lambda: -self.driverController.getLeftY(),
+                lambda: -self.driverController.getRightX(),
+            )
+        )
+
+        # Deploy the intake with the X button
+        self.driverController.x().onTrue(self.intake.intakeCommand())
+        # Retract the intake with the Y button
+        self.driverController.y().onTrue(self.intake.retractCommand())
+
+        # Fire the shooter with the A button
+        self.driverController.a().onTrue(
+            commands2.cmd.parallel(
+                self.shooter.shootCommand(ShooterConstants.kShooterTargetRPS),
+                self.storage.runCommand(),
+            ).withName("Shoot")
+        )
+
+        # Toggle compressor with the Start button
+        self.driverController.start().toggleOnTrue(
+            self.pneumatics.disableCompressorCommand()
+        )
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+        # Drive forward for 2 meters at half velocity with a 3 second timeout
+        return self.drive.driveDistanceCommand(
+            AutoConstants.kDriveDistance, AutoConstants.kDriveVelocity
+        ).withTimeout(AutoConstants.kTimeout)

robotpyExamples/examples/RapidReactCommandBot/robot.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+#
+# 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.
+#
+
+import commands2
+import wpilib
+
+from rapidreactcommandbot import RapidReactCommandBot
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """The methods in this class are called automatically corresponding to each mode, as
+    described in the TimedRobot documentation. If you change the name of this class or the
+    package after creating this project, you must also update the Main.java file in the
+    project.
+    """
+
+    def __init__(self) -> None:
+        """This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        super().__init__()
+        self.autonomousCommand = None
+        self.robot = RapidReactCommandBot()
+
+        # Configure default commands and condition bindings on robot startup
+        self.robot.configureBindings()
+
+        # Initialize data logging.
+        wpilib.DataLogManager.start()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+        pass
+
+    def disabledPeriodic(self) -> None:
+        pass
+
+    def autonomousInit(self) -> None:
+        self.autonomousCommand = self.robot.getAutonomousCommand()
+
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous."""
+        pass
+
+    def teleopInit(self) -> None:
+        # This makes sure that the autonomous stops running when
+        # teleop starts running. If you want the autonomous to
+        # continue until interrupted by another command, remove
+        # this line or comment it out.
+        if self.autonomousCommand is not None:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control."""
+        pass
+
+    def utilityInit(self) -> None:
+        # Cancels all running commands at the start of utility mode.
+        commands2.CommandScheduler.getInstance().cancelAll()
+
+    def utilityPeriodic(self) -> None:
+        """This function is called periodically during utility mode."""
+        pass

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")