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

@@ -0,0 +1,73 @@
+#
+# 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)
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameter * math.pi) / kEncoderCPR
+
+
+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)
+    kA = 0.0  # V/(rot/s^2)
+
+    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/SysId/robot.py

@@ -0,0 +1,66 @@
+#!/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.
+#
+
+from commands2 import CommandScheduler, TimedCommandRobot
+
+from sysidroutinebot import SysIdRoutineBot
+
+
+class MyRobot(TimedCommandRobot):
+    """
+    The methods in this class are called automatically corresponding to each mode,
+    as described in the TimedRobot documentation.
+    """
+
+    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.robot = SysIdRoutineBot()
+
+        # Configure default commands and condition bindings on robot startup
+        self.robot.configureBindings()
+
+        self.autonomous_command = None
+
+    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.autonomous_command = self.robot.getAutonomousCommand()
+
+        if self.autonomous_command is not None:
+            CommandScheduler.getInstance().schedule(self.autonomous_command)
+
+    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.autonomous_command is not None:
+            self.autonomous_command.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.
+        CommandScheduler.getInstance().cancelAll()
+
+    def utilityPeriodic(self) -> None:
+        """This function is called periodically during utility mode."""
+        pass

robotpyExamples/examples/SysId/subsystems/drive.py

@@ -0,0 +1,125 @@
+#
+# 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
+from commands2.sysid import SysIdRoutine
+from wpilib import DifferentialDrive, Encoder, PWMSparkMax, RobotController
+from wpilib.sysid import SysIdRoutineLog
+
+from constants import DriveConstants
+
+
+class Drive(Subsystem):
+    """Represents the drive subsystem."""
+
+    def __init__(self) -> None:
+        """Creates a new Drive subsystem."""
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.left_motor = PWMSparkMax(DriveConstants.kLeftMotor1Port)
+
+        # The motors on the right side of the drive.
+        self.right_motor = PWMSparkMax(DriveConstants.kRightMotor1Port)
+
+        # The robot's drive
+        self.drive = DifferentialDrive(self.left_motor, self.right_motor)
+
+        # The left-side drive encoder
+        self.left_encoder = Encoder(
+            DriveConstants.kLeftEncoderPorts[0],
+            DriveConstants.kLeftEncoderPorts[1],
+            DriveConstants.kLeftEncoderReversed,
+        )
+
+        # The right-side drive encoder
+        self.right_encoder = Encoder(
+            DriveConstants.kRightEncoderPorts[0],
+            DriveConstants.kRightEncoderPorts[1],
+            DriveConstants.kRightEncoderReversed,
+        )
+
+        # Create a new SysId routine for characterizing the drive.
+        self.sys_id_routine = SysIdRoutine(
+            # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(
+                # Tell SysId how to plumb the driving voltage to the motors.
+                self._driveVoltage,
+                # Tell SysId how to record a frame of data for each motor on the mechanism being
+                # characterized.
+                self._log,
+                # Tell SysId to make generated commands require this subsystem, suffix test state in
+                # WPILog with this subsystem's name ("drive")
+                self,
+            ),
+        )
+
+        # Add the second motors on each side of the drivetrain
+        self.left_motor.addFollower(PWMSparkMax(DriveConstants.kLeftMotor2Port))
+        self.right_motor.addFollower(PWMSparkMax(DriveConstants.kRightMotor2Port))
+
+        # 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.right_motor.setInverted(True)
+
+        # Sets the distance per pulse for the encoders
+        self.left_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+        self.right_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse)
+
+    def _driveVoltage(self, voltage: float) -> None:
+        self.left_motor.setVoltage(voltage)
+        self.right_motor.setVoltage(voltage)
+
+    def _log(self, sys_id_routine: SysIdRoutineLog) -> None:
+        # Record a frame for the left motors. Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-left").voltage(
+            self.left_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.left_encoder.getDistance()).velocity(
+            self.left_encoder.getRate()
+        )
+        # Record a frame for the right motors. Since these share an encoder, we consider
+        # the entire group to be one motor.
+        sys_id_routine.motor("drive-right").voltage(
+            self.right_motor.get() * RobotController.getBatteryVoltage()
+        ).position(self.right_encoder.getDistance()).velocity(
+            self.right_encoder.getRate()
+        )
+
+    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 sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a quasistatic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a dynamic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.dynamic(direction)

robotpyExamples/examples/SysId/subsystems/shooter.py

@@ -0,0 +1,118 @@
+#
+# 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
+from commands2.sysid import SysIdRoutine
+from wpilib import Encoder, PWMSparkMax, RobotController
+from wpilib.sysid import SysIdRoutineLog
+
+import wpimath
+import wpimath.units
+
+from constants import ShooterConstants
+
+
+class Shooter(Subsystem):
+    """Represents the shooter subsystem."""
+
+    def __init__(self) -> None:
+        """Creates a new Shooter subsystem."""
+        super().__init__()
+
+        # The motor on the shooter wheel .
+        self.shooter_motor = PWMSparkMax(ShooterConstants.kShooterMotorPort)
+
+        # The motor on the feeder wheels.
+        self.feeder_motor = PWMSparkMax(ShooterConstants.kFeederMotorPort)
+
+        # The shooter wheel encoder
+        self.shooter_encoder = Encoder(
+            ShooterConstants.kEncoderPorts[0],
+            ShooterConstants.kEncoderPorts[1],
+            ShooterConstants.kEncoderReversed,
+        )
+
+        # Create a new SysId routine for characterizing the shooter.
+        self.sys_id_routine = SysIdRoutine(
+            # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(
+                # Tell SysId how to plumb the driving voltage to the motor(s).
+                self.shooter_motor.setVoltage,
+                # Tell SysId how to record a frame of data for each motor on the mechanism being
+                # characterized.
+                self._log,
+                # Tell SysId to make generated commands require this subsystem, suffix test state in
+                # WPILog with this subsystem's name ("shooter")
+                self,
+            ),
+        )
+
+        # PID controller to run the shooter wheel in closed-loop, set the constants equal to those
+        # calculated by SysId
+        self.shooter_feedback = wpimath.PIDController(ShooterConstants.kP, 0, 0)
+        # Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
+        # those calculated by SysId
+        self.shooter_feedforward = wpimath.SimpleMotorFeedforwardRadians(
+            ShooterConstants.kS,
+            ShooterConstants.kV / wpimath.units.rotationsToRadians(1),
+            ShooterConstants.kA / wpimath.units.rotationsToRadians(1),
+        )
+
+        # Sets the distance per pulse for the encoders
+        self.shooter_encoder.setDistancePerPulse(
+            ShooterConstants.kEncoderDistancePerPulse
+        )
+
+    def _log(self, sys_id_routine: SysIdRoutineLog) -> None:
+        # Record a frame for the shooter motor.
+        sys_id_routine.motor("shooter-wheel").voltage(
+            self.shooter_motor.get() * RobotController.getBatteryVoltage()
+        ).angularPosition(self.shooter_encoder.getDistance()).angularVelocity(
+            self.shooter_encoder.getRate()
+        )
+
+    def runShooter(self, shooterVelocity: Callable[[], float]) -> Command:
+        """Returns a command that runs the shooter at a specifc velocity.
+
+        :param shooterVelocity: The commanded shooter wheel velocity in rotations per second
+        """
+
+        # Run shooter wheel at the desired velocity using a PID controller and feedforward.
+        def _run_shooter() -> None:
+            target_velocity = shooterVelocity()
+            target_velocity_radians = wpimath.units.rotationsToRadians(target_velocity)
+            self.shooter_motor.setVoltage(
+                self.shooter_feedback.calculate(
+                    self.shooter_encoder.getRate(), target_velocity
+                )
+                + self.shooter_feedforward.calculate(target_velocity_radians)
+            )
+            self.feeder_motor.setThrottle(ShooterConstants.kFeederVelocity)
+
+        def _stop_motors(interrupted: bool) -> None:
+            self.shooter_motor.stopMotor()
+            self.feeder_motor.stopMotor()
+
+        return self.run(_run_shooter).finallyDo(_stop_motors).withName("runShooter")
+
+    def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a quasistatic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        """Returns a command that will execute a dynamic test in the given direction.
+
+        :param direction: The direction (forward or reverse) to run the test in
+        """
+
+        return self.sys_id_routine.dynamic(direction)

robotpyExamples/examples/SysId/sysidroutinebot.py

@@ -0,0 +1,91 @@
+#
+# 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
+from commands2.button import CommandNiDsXboxController
+from commands2.sysid import SysIdRoutine
+
+from subsystems.drive import Drive
+from subsystems.shooter import Shooter
+
+from constants import OIConstants
+
+
+class SysIdRoutineBot:
+    """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.shooter = Shooter()
+
+        # The driver's controller
+        self.controller = 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.
+        """
+
+        # Control the drive with split-stick arcade controls
+        self.drive.setDefaultCommand(
+            self.drive.arcadeDriveCommand(
+                lambda: -self.controller.getLeftY(),
+                lambda: -self.controller.getRightX(),
+            )
+        )
+
+        # Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+        # once.
+        # Using bumpers as a modifier and combining it with the buttons so that we can have both sets
+        # of bindings at once
+        self.controller.a().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().and_(self.controller.rightBumper()).whileTrue(
+            self.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        )
+
+        # Control the shooter wheel with the left trigger
+        self.shooter.setDefaultCommand(
+            self.shooter.runShooter(self.controller.getLeftTriggerAxis)
+        )
+
+        self.controller.a().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().and_(self.controller.leftBumper()).whileTrue(
+            self.shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)
+        )
+
+    def getAutonomousCommand(self) -> Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+
+        # Do nothing
+        return self.drive.run(lambda: None)