Add armbot example (#70)

Following #49

In `constants.py` file, the constants are grouped in different classes
(I don't know if this style will respect the [RobotPy's porting
guide](https://github.com/robotpy/examples/blob/main/CONTRIBUTING.md)).

In `robotcontainer.py`, I created a function named `moveArm`, to adapt
in a single line (two calls in `configureButtonBindings`).

Author: megarubber

commands-v2/armbot/constants.py

@@ -0,0 +1,61 @@
+# 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.
+
+"""
+The constants module is a convenience place for teams to hold robot-wide
+numerical or boolean constants. Don't use this for any other purpose!
+"""
+
+import math
+
+
+class DriveConstants:
+    # The PWM IDs for the drivetrain motor controllers.
+    kLeftMotor1Port = 0
+    kLeftMotor2Port = 1
+    kRightMotor1Port = 2
+    kRightMotor2Port = 3
+
+    # Encoders and their respective motor controllers.
+    kLeftEncoderPorts = (0, 1)
+    kRightEncoderPorts = (2, 3)
+    kLeftEncoderReversed = False
+    kRightEncoderReversed = True
+
+    # Encoder counts per revolution/rotation.
+    kEncoderCPR = 1024
+    kWheelDiameterInches = 6
+
+    # Assumes the encoders are directly mounted on the wheel shafts
+    kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR
+
+
+class ArmConstants:
+    # NOTE: Please do NOT use these values on your robot.
+    kMotorPort = 4
+    kP = 1
+    kSVolts = 1
+    kGVolts = 1
+    kVVoltSecondPerRad = 0.5
+    kAVoltSecondSquaredPerRad = 0.1
+
+    kMaxVelocityRadPerSecond = 3
+    kMaxAccelerationRadPerSecSquared = 10
+
+    kEncoderPorts = (4, 5)
+    kEncoderPPR = 256
+    kEncoderDistancePerPulse = 2.0 * math.pi / kEncoderPPR
+
+    # The offset of the arm from the horizontal in its neutral position,
+    # measured from the horizontal
+    kArmOffsetRads = 0.5
+
+
+class AutoConstants:
+    kAutoTimeoutSeconds = 12
+    kAutoShootTimeSeconds = 7
+
+
+class OIConstants:
+    kDriverControllerPort = 0

commands-v2/armbot/robot.py

@@ -0,0 +1,83 @@
+#!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 typing
+
+from robotcontainer import RobotContainer
+
+
+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.
+        """
+
+        # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+        # autonomous chooser on the dashboard.
+        self.container = RobotContainer()
+
+    def robotPeriodic(self) -> None:
+        """This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+        that you want ran during disabled, autonomous, teleoperated and test.
+
+        This runs after the mode specific periodic functions, but before LiveWindow and
+        SmartDashboard integrated updating."""
+
+        # Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+        # commands, running already-scheduled commands, removing finished or interrupted commands,
+        # and running subsystem periodic() methods.  This must be called from the robot's periodic
+        # block in order for anything in the Command-based framework to work.
+        commands2.CommandScheduler.getInstance().run()
+
+    def disabledInit(self) -> None:
+        """This function is called once each time the robot enters Disabled mode."""
+        self.container.disablePIDSubsystems()
+
+    def disabledPeriodic(self) -> None:
+        """This function is called periodically when disabled"""
+        pass
+
+    def autonomousInit(self) -> None:
+        """This autonomous runs the autonomous command selected by your RobotContainer class."""
+        self.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            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:
+            self.autonomousCommand.cancel()
+
+    def teleopPeriodic(self) -> None:
+        """This function is called periodically during operator control"""
+        pass
+
+    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/armbot/robotcontainer.py

@@ -0,0 +1,96 @@
+# 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 constants
+
+from subsystems.armsubsystem import ArmSubsystem
+from subsystems.drivesubsystem import DriveSubsystem
+
+
+class RobotContainer:
+    """
+    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.robot_drive = DriveSubsystem()
+        self.robot_arm = ArmSubsystem()
+
+        # The driver's controller
+        self.driver_controller = commands2.button.CommandXboxController(
+            constants.OIConstants.kDriverControllerPort
+        )
+
+        # Configure the button bindings
+        self.configureButtonBindings()
+
+        # Set the default drive command
+        # Set the default drive command to split-stick arcade drive
+        self.robot_drive.setDefaultCommand(
+            commands2.cmd.run(
+                # A split-stick arcade command, with forward/backward controlled by the left
+                # hand, and turning controlled by the right.
+                lambda: self.robot_drive.arcadeDrive(
+                    -self.driver_controller.getLeftY(),
+                    -self.driver_controller.getRightX(),
+                ),
+                [self.robot_drive],
+            )
+        )
+
+    def configureButtonBindings(self) -> None:
+        """
+        Use this method to define your button->command mappings. Buttons can be created by
+        instantiating a :GenericHID or one of its subclasses (Joystick or XboxController),
+        and then passing it to a JoystickButton.
+        """
+
+        # Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+        self.driver_controller.A().onTrue(
+            commands2.cmd.run(self.moveArm(2), [self.robot_arm])
+        )
+
+        # Move the arm to neutral position when the 'B' button is pressed
+        self.driver_controller.B().onTrue(
+            commands2.cmd.run(
+                self.moveArm(constants.ArmConstants.kArmOffsetRads), [self.robot_arm]
+            )
+        )
+
+        # Disable the arm controller when Y is pressed
+        self.driver_controller.Y().onTrue(
+            commands2.cmd.runOnce(lambda: self.robot_arm.disable())
+        )
+
+        # Drive at half speed when the bumper is held
+        self.driver_controller.rightTrigger().onTrue(
+            commands2.cmd.runOnce(lambda: self.robot_drive.setMaxOutput(0.5))
+        )
+        self.driver_controller.rightTrigger().onFalse(
+            commands2.cmd.runOnce(lambda: self.robot_drive.setMaxOutput(1.0))
+        )
+
+    def disablePIDSubsystems(self) -> None:
+        """Disables all ProfiledPIDSubsystem and PIDSubsystem instances.
+        This should be called on robot disable to prevent integral windup."""
+        self.robot_arm.disable()
+
+    def getAutonomousCommand(self) -> commands2.Command:
+        """Use this to pass the autonomous command to the main {@link Robot} class.
+
+        :returns: the command to run in autonomous
+        """
+        return commands2.cmd.nothing()
+
+    def moveArm(self, radians: int) -> None:
+        self.robot_arm.setGoal(radians)
+        self.robot_arm.enable()

commands-v2/armbot/subsystems/armsubsystem.py

@@ -0,0 +1,60 @@
+# 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 wpimath.controller
+import wpimath.trajectory
+
+import constants
+
+
+class ArmSubsystem(commands2.ProfiledPIDSubsystem):
+    """A robot arm subsystem that moves with a motion profile."""
+
+    # Create a new ArmSubsystem
+    def __init__(self) -> None:
+        super().__init__(
+            wpimath.controller.ProfiledPIDController(
+                constants.ArmConstants.kP,
+                0,
+                0,
+                wpimath.trajectory.TrapezoidProfile.Constraints(
+                    constants.ArmConstants.kMaxVelocityRadPerSecond,
+                    constants.ArmConstants.kMaxAccelerationRadPerSecSquared,
+                ),
+            ),
+            0,
+        )
+
+        self.motor = wpilib.PWMSparkMax(constants.ArmConstants.kMotorPort)
+        self.encoder = wpilib.Encoder(
+            constants.ArmConstants.kEncoderPorts[0],
+            constants.ArmConstants.kEncoderPorts[1],
+        )
+        self.feedforward = wpimath.controller.ArmFeedforward(
+            constants.ArmConstants.kSVolts,
+            constants.ArmConstants.kGVolts,
+            constants.ArmConstants.kVVoltSecondPerRad,
+            constants.ArmConstants.kAVoltSecondSquaredPerRad,
+        )
+
+        self.encoder.setDistancePerPulse(
+            constants.ArmConstants.kEncoderDistancePerPulse
+        )
+
+        # Start arm at rest in neutral position
+        self.setGoal(constants.ArmConstants.kArmOffsetRads)
+
+    def _useOutput(
+        self, output: float, setpoint: wpimath.trajectory.TrapezoidProfile.State
+    ) -> None:
+        # Calculate the feedforward from the setpoint
+        feedforward = self.feedforward.calculate(setpoint.position, setpoint.velocity)
+
+        # Add the feedforward to the PID output to get the motor output
+        self.motor.setVoltage(output + feedforward)
+
+    def _getMeasurement(self) -> float:
+        return self.encoder.getDistance() + constants.ArmConstants.kArmOffsetRads