Add arm routine for intake

arm/.wpilib/wpilib_preferences.json

@@ -0,0 +1 @@
+{"teamNumber": 4774}

arm/constants.py

@@ -0,0 +1,11 @@
+import enum
+
+
+@enum.unique
+class SparkId(enum.IntEnum):
+    intake_deploy_leader = 4
+    intake_deploy_follower = 5
+
+
+class OIConstants:
+    kDriverControllerPort = 0

arm/robot.py

@@ -0,0 +1,48 @@
+from commands2 import CommandScheduler, TimedCommandRobot
+from urcl import URCL
+
+from sysidroutinebot import SysIdRoutineBot
+
+
+class MyRobot(TimedCommandRobot):
+    """The VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
+    project.
+    """
+
+    def robotInit(self) -> None:
+        """This function is run when the robot is first started up and should be used for any
+        initialization code.
+        """
+        URCL.start()
+
+        self.robot = SysIdRoutineBot()
+        self.robot.configureBindings()
+
+        self.autonomous_command = self.robot.getAutonomousCommand()
+
+    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.schedule()
+
+    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.
+        self.autonomous_command.cancel()
+
+    def testInit(self) -> None:
+        # Cancels all running commands at the start of test mode.
+        CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self) -> None:
+        """This function is called periodically during test mode."""
+        pass

arm/subsystems/arm.py

@@ -0,0 +1,76 @@
+import math
+
+import rev
+
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+from wpilib import sysid
+
+
+class Arm(Subsystem):
+    def __init__(
+        self,
+        motor: rev.CANSparkBase,
+        follower: rev.CANSparkBase,
+        *,
+        oppose_leader: bool,
+        gearing: float,
+        name: str | None = None,
+    ) -> None:
+        self.motor = motor
+        self.encoder = self._init_encoder(self.motor, gearing)
+        if name is not None:
+            self.setName(name)
+
+        self.motor.setIdleMode(rev.CANSparkBase.IdleMode.kBrake)
+        if True:
+            self.follower = follower
+            self.follower_encoder = self._init_encoder(follower, gearing)
+            follower.setIdleMode(rev.CANSparkBase.IdleMode.kBrake)
+            # follower.setInverted(oppose_leader)
+            follower.follow(self.motor, invert=oppose_leader)
+
+        # Tell SysId to make generated commands require this subsystem, suffix test state in
+        # WPILog with this subsystem's name ("drive")
+        self.sys_id_routine = SysIdRoutine(
+            SysIdRoutine.Config(),
+            SysIdRoutine.Mechanism(self.motor.setVoltage, self.log, self),
+        )
+
+    def _init_encoder(
+        self, motor: rev.CANSparkBase, gearing: float
+    ) -> rev.SparkRelativeEncoder:
+        encoder = motor.getEncoder()
+
+        # Measure position in rad and velocity in rad/s.
+        output_rad_per_motor_rev = gearing * math.tau
+        encoder.setPositionConversionFactor(output_rad_per_motor_rev)
+        encoder.setVelocityConversionFactor(output_rad_per_motor_rev / 60)
+
+        return encoder
+
+    # Tell SysId how to record a frame of data for each motor on the mechanism being
+    # characterized.
+    def log(self, sys_id_routine: sysid.SysIdRoutineLog) -> None:
+        (
+            sys_id_routine.motor("leader")
+            .voltage(self.motor.get() * self.motor.getBusVoltage())
+            .position(self.encoder.getPosition())
+            .velocity(self.encoder.getVelocity())
+        )
+        if True:
+            (
+                sys_id_routine.motor("follower")
+                .voltage(self.follower.get() * self.follower.getBusVoltage())
+                .position(self.follower_encoder.getPosition())
+                .velocity(self.follower_encoder.getVelocity())
+            )
+
+    def defaultCommand(self) -> Command:
+        return self.run(lambda: None)
+
+    def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.dynamic(direction)

arm/sysidroutinebot.py

@@ -0,0 +1,69 @@
+import rev
+from commands2 import Command
+from commands2.button import CommandXboxController
+from commands2.sysid import SysIdRoutine
+
+from subsystems.arm import Arm
+
+from constants import OIConstants, SparkId
+
+
+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.arm = Arm(
+            self._create_neo(SparkId.intake_deploy_leader),
+            self._create_neo(SparkId.intake_deploy_follower),
+            oppose_leader=True,
+            gearing=(1 / 5) * (1 / 3) * (24 / 72),
+        )
+
+        # The driver's controller
+        self.controller = CommandXboxController(OIConstants.kDriverControllerPort)
+
+    @staticmethod
+    def _create_neo(id: int) -> rev.CANSparkMax:
+        return rev.CANSparkMax(id, rev.CANSparkMax.MotorType.kBrushless)
+
+    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 during :meth:`.Robot.robotInit`.
+
+        Event binding methods are available on the :class:`.Trigger` class.
+        """
+
+        # Control the drive with split-stick arcade controls
+        self.arm.setDefaultCommand(self.arm.defaultCommand())
+
+        # Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+        # once.
+
+        self.controller.a().whileTrue(
+            self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.b().whileTrue(
+            self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)
+        )
+        self.controller.x().whileTrue(
+            self.arm.sysIdDynamic(SysIdRoutine.Direction.kForward)
+        )
+        self.controller.y().whileTrue(
+            self.arm.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.arm.run(lambda: None)

arm/tests/pyfrc_test.py

@@ -0,0 +1,6 @@
+"""
+This test module imports tests that come with pyfrc, and can be used
+to test basic functionality of just about any robot.
+"""
+
+from pyfrc.tests import *  # noqa: F403