Add arm routine for intake

arm/.wpilib/wpilib_preferences.json

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

arm/constants.py

@@ -0,0 +1,10 @@
+import enum
+
+
+@enum.unique
+class SparkId(enum.IntEnum):
+    wrist_motor = 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,103 @@
+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.SparkMax,
+        follower: rev.SparkMax | None,
+        *,
+        oppose_leader: bool,
+        gearing: float,
+        lower_limit: float,
+        upper_limit: float,
+        motor_inverted: bool,
+        name: str | None = None,
+    ) -> None:
+        self.motor = motor
+        config = self._create_smax_config(gearing, upper_limit, lower_limit, motor_inverted)
+        self._configure_ephemeral(motor, config)
+        self.encoder = motor.getEncoder()
+        self.upper_limit = upper_limit
+        self.lower_limit = lower_limit
+        if name is not None:
+            self.setName(name)
+
+        self.follower = follower
+
+        if self.follower is not None:
+
+            self.follower_encoder = self.follower.getEncoder()
+            follower_config = self._create_smax_config(gearing, upper_limit, lower_limit, motor_inverted)
+            follower_config.follow(motor, invert=oppose_leader)
+            self._configure_ephemeral(self.follower, follower_config)
+
+        # 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(rampRate=3, stepVoltage=7),
+            SysIdRoutine.Mechanism(self.motor.setVoltage, self.log, self),
+        )
+
+    @classmethod
+    def _create_smax_config(cls, gearing: float, upper_limit: float, lower_limit: float, inverted: bool) -> rev.SparkMaxConfig:
+        config = rev.SparkMaxConfig()
+        config.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
+        config.softLimit.reverseSoftLimitEnabled(True)
+        config.softLimit.reverseSoftLimit(lower_limit)
+        config.softLimit.forwardSoftLimitEnabled(True)
+        config.softLimit.forwardSoftLimit(upper_limit)
+        config.inverted(inverted)
+
+        # Measure position in rad and velocity in rad/s.
+        output_rad_per_motor_rev = (1/gearing) * math.tau
+        config.encoder.positionConversionFactor(output_rad_per_motor_rev)
+        config.encoder.velocityConversionFactor(output_rad_per_motor_rev / 60)
+
+        return config
+
+    @classmethod
+    def _configure_ephemeral(cls, motor: rev.SparkBase, config: rev.SparkBaseConfig):
+        motor.configure(
+            config,
+            rev.SparkBase.ResetMode.kNoResetSafeParameters,
+            rev.SparkBase.PersistMode.kNoPersistParameters,
+        )
+
+    # 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 self.follower is not None:
+            (
+                sys_id_routine.motor("follower")
+                .voltage(self.follower.get() * self.follower.getBusVoltage())
+                .position(self.follower_encoder.getPosition())
+                .velocity(self.follower_encoder.getVelocity())
+            )
+
+
+    def below_upper_limit(self) -> bool:
+        return self.encoder.getPosition() < self.upper_limit
+
+    def above_lower_limit(self) -> bool:
+        return self.encoder.getPosition() > self.lower_limit
+
+    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,72 @@
+import rev
+import math
+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.wrist_motor),
+            None,
+            oppose_leader=True,
+            gearing=432,
+            upper_limit=math.radians(-10.0),
+            lower_limit=math.radians(-113.0),
+            motor_inverted=False
+        )
+
+        # The driver's controller
+        self.controller = CommandXboxController(OIConstants.kDriverControllerPort)
+
+    @staticmethod
+    def _create_neo(id: int) -> rev.SparkMax:
+        return rev.SparkMax(id, rev.SparkMax.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).onlyWhile(self.arm.below_upper_limit)
+        )
+        self.controller.b().whileTrue(
+            self.arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse).onlyWhile(self.arm.above_lower_limit)
+        )
+        self.controller.x().whileTrue(
+            self.arm.sysIdDynamic(SysIdRoutine.Direction.kForward).onlyWhile(self.arm.below_upper_limit)
+        )
+        self.controller.y().whileTrue(
+            self.arm.sysIdDynamic(SysIdRoutine.Direction.kReverse).onlyWhile(self.arm.above_lower_limit)
+        )
+
+    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