add rapid-react-command-bot example

commands-v2/armbot/robotcontainer.py

@@ -62,7 +62,8 @@ def configureButtonBindings(self) -> None:
         # Move the arm to neutral position when the 'B' button is pressed
         self.driver_controller.B().onTrue(
             commands2.cmd.run(
-                lambda: self.moveArm(constants.ArmConstants.kArmOffsetRads), [self.robot_arm]
+                lambda: self.moveArm(constants.ArmConstants.kArmOffsetRads),
+                [self.robot_arm],
             )
         )
 

commands-v2/rapid-react-command-bot/constants.py

@@ -0,0 +1,80 @@
+#!/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 wpimath.units
+import math
+
+"""The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+constants. This class should not be used for any other purpose. All constants should be declared
+globally (i.e. public static). Do not put anything functional in this class.
+
+It is advised to statically import this class (or one of its inner classes) wherever the
+constants are needed, to reduce verbosity.
+"""
+
+
+class DriveConstants:
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    kEncoderCPR = 1024
+    kWheelDiameterMeters = wpimath.units.inchesToMeters(6)
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR
+
+
+class ShooterConstants:
+    kEncoderPorts = (4, 5)
+    kEncoderReversed = False
+    kEncoderCPR = 1024
+    # Distance units will be rotations
+    kEncoderDistancePerPulse = 1 / kEncoderCPR
+
+    kShooterMotorPort = 4
+    kFeederMotorPort = 5
+
+    kShooterFreeRPS = 5300
+    kShooterTargetRPS = 4000
+    kShooterToleranceRPS = 50
+
+    # These are not real PID gains, and will have to be tuned for your specific robot.
+    kP = 1
+
+    # On a real robot the feedforward constants should be empirically determined; these are
+    # reasonable guesses.
+    kSVolts = 0.05
+    # Should have value 12V at free speed...
+    kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS
+
+    kFeederSpeed = 0.5
+
+
+class IntakeConstants:
+    kMotorPort = 6
+    kSolenoidPorts = (0, 1)
+
+
+class StorageConstants:
+    kMotorPort = 7
+    kBallSensorPort = 6
+
+
+class AutoConstants:
+    kTimeoutSeconds = 3
+    kDriveDistanceMeters = 2
+    kDriveSpeed = 0.5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

commands-v2/rapid-react-command-bot/rapidreactcommandbot.py

@@ -0,0 +1,92 @@
+#!/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 commands2.button
+import commands2.cmd
+
+import subsystems.intake
+import subsystems.drive
+import subsystems.shooter
+import subsystems.storage
+
+import constants
+
+
+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 = subsystems.drive.Drive()
+        self.intake = subsystems.intake.Intake()
+        self.storage = subsystems.storage.Storage()
+        self.shooter = subsystems.shooter.Shooter()
+
+        # The driver's controller
+        self.driverController = commands2.button.CommandXboxController(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        self.configureButtonBindings()
+
+    def configureButtonBindings(self):
+        """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 during :meth:`.Robot.robotInit`.
+
+        Event binding methods are available on the {@link Trigger} class.
+        """
+        # Automatically run the storage motor whenever the ball storage is not full,
+        # and turn it off whenever it fills.
+        commands2.Trigger(lambda: self.storage.isFull()).whileFalse(
+            self.storage.runCommand()
+        )
+
+        # Automatically disable and retract the intake whenever the ball storage is full.
+        commands2.Trigger(lambda: self.storage.isFull()).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())
+
+        # Compose master shoot command
+        self.masterShootCommand = commands2.cmd.parallel(
+            self.shooter.shootCommand(constants.ShooterConstants.kShooterTargetRPS),
+            self.storage.runCommand(),
+        )
+        # Since we composed this inline we should give it a name
+        self.masterShootCommand.setName("Shoot")
+        # Fire the shooter with the A button
+        self.driverController.A().onTrue(self.masterShootCommand)
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+        return self.drive.driveDistanceCommand(
+            constants.AutoConstants.kDriveDistanceMeters,
+            constants.AutoConstants.kDriveSpeed,
+        ).withTimeout(constants.AutoConstants.kTimeoutSeconds)

commands-v2/rapid-react-command-bot/robot.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+
+import typing
+import wpilib
+import commands2
+
+from rapidreactcommandbot import RapidReactCommandBot
+
+
+class MyRobot(commands2.TimedCommandRobot):
+    """
+    Our default robot class, pass it to wpilib.run
+
+    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
+    has an implementation of robotPeriodic which runs the scheduler for you
+    """
+
+    autonomousCommand: typing.Optional[commands2.Command] = None
+
+    def robotInit(self) -> None:
+        """
+        This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+
+        self.robot = RapidReactCommandBot()
+
+        # Configure default commands and condition bindings on robot startup
+        self.robot.configureButtonBindings()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.robot.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.schedule()
+
+    def autonomousPeriodic(self) -> None:
+        """This function is called periodically during autonomous"""
+
+    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:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode
+        commands2.CommandScheduler.getInstance().cancelAll()
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

commands-v2/rapid-react-command-bot/subsystems/drive.py

@@ -0,0 +1,92 @@
+#!/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 wpilib
+import wpilib.drive
+import commands2
+import commands2.cmd
+
+import constants
+
+
+class Drive(commands2.SubsystemBase):
+    def __init__(self) -> None:
+        """Creates a new Drive subsystem."""
+
+        super().__init__()
+
+        # The motors on the left side of the drive.
+        self.leftMotors = wpilib.MotorControllerGroup(
+            wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port),
+            wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port),
+        )
+
+        # The motors on the right side of the drive.
+        self.rightMotors = wpilib.MotorControllerGroup(
+            wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port),
+            wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port),
+        )
+
+        # The robot's drive
+        self.drive = wpilib.drive.DifferentialDrive(self.leftMotors, self.rightMotors)
+
+        # The left-side drive encoder
+        self.leftEncoder = wpilib.Encoder(
+            *constants.DriveConstants.kLeftEncoderPorts,
+            constants.DriveConstants.kLeftEncoderReversed
+        )
+
+        # The right-side drive encoder
+        self.rightEncoder = wpilib.Encoder(
+            *constants.DriveConstants.kRightEncoderPorts,
+            constants.DriveConstants.kRightEncoderReversed
+        )
+
+        # 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.rightMotors.setInverted(True)
+
+        # Sets the distance per pulse for the encoders
+        self.leftEncoder.setDistancePerPulse(
+            constants.DriveConstants.kEncoderDistancePerPulse
+        )
+        self.rightEncoder.setDistancePerPulse(
+            constants.DriveConstants.kEncoderDistancePerPulse
+        )
+
+    def arcadeDriveCommand(self, fwd, rot) -> commands2.Command:
+        # A split-stick arcade command, with forward/backward controlled by the left
+        # hand, and turning controlled by the right.
+        command = self.run(lambda: self.drive.arcadeDrive(fwd(), rot()))
+        command.setName("arcadeDrive")
+        return command
+
+    def driveDistanceCommand(
+        self, distanceMeters: float, speed: float
+    ) -> commands2.SequentialCommandGroup:
+        return (
+            self.runOnce(
+                # Reset encoders at the start of the command
+                lambda: [self.leftEncoder.reset(), self.rightEncoder.reset()]
+            )
+            .andThen(
+                # Drive forward at specified speed
+                self.run(lambda: self.drive.arcadeDrive(speed, 0))
+            )
+            .until(
+                # End command when we've traveled the specified distance
+                lambda: max(
+                    self.leftEncoder.getDistance(), self.rightEncoder.getDistance()
+                )
+                >= distanceMeters
+            )
+            .andThen(
+                # Stop the drive when the command ends
+                commands2.cmd.runOnce(lambda: self.drive.stopMotor(), [self])
+            )
+        )

commands-v2/rapid-react-command-bot/subsystems/intake.py

@@ -0,0 +1,41 @@
+#!/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 wpilib
+import commands2
+import commands2.cmd
+
+import constants
+
+
+class Intake(commands2.SubsystemBase):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.motor = wpilib.PWMSparkMax(constants.IntakeConstants.kMotorPort)
+        self.pistons = wpilib.DoubleSolenoid(
+            wpilib.PneumaticsModuleType.REVPH, *constants.IntakeConstants.kSolenoidPorts
+        )
+
+    # Returns a command that deploys the intake, and then runs the intake motor indefinitely.
+    def intakeCommand(self) -> commands2.Command:
+        self.motorSetCommand = self.run(lambda: self.motor.set(1))
+        self.motorSetCommand.setName("Intake")
+        return self.runOnce(
+            lambda: self.pistons.set(wpilib.DoubleSolenoid.Value.kForward)
+        ).andThen(self.motorSetCommand)
+
+    # Returns a command that turns off and retracts the intake.
+    def retractCommand(self) -> commands2.Command:
+        command = self.runOnce(
+            lambda: [
+                self.motor.disable(),
+                self.pistons.set(wpilib.DoubleSolenoid.Value.kReverse),
+            ]
+        )
+        command.setName("Retract")
+        return command