Add Python sysid example

python/PhoenixSysId/.gitignore

@@ -0,0 +1,5 @@
+ctre_sim
+__pycache__
+.pytest_cache
+.deploy_cfg
+.installer_config

python/PhoenixSysId/constants.py

@@ -0,0 +1,3 @@
+class Constants:
+    kCANbus = "canivore"
+    kTalonFxId = 18

python/PhoenixSysId/robot.py

@@ -0,0 +1,57 @@
+#!/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 robotcontainer import RobotContainer
+
+
+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.
+        """
+        self.container = RobotContainer()
+
+    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.autonomousCommand = self.container.getAutonomousCommand()
+
+        if self.autonomousCommand:
+            self.autonomousCommand.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.
+        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.
+        CommandScheduler.getInstance().cancelAll()
+
+    def testPeriodic(self) -> None:
+        """This function is called periodically during test mode."""
+        pass

python/PhoenixSysId/robotcontainer.py

@@ -0,0 +1,49 @@
+from commands2 import Command, cmd
+from commands2.button import CommandXboxController
+from commands2.sysid import SysIdRoutine
+
+from phoenix6 import SignalLogger
+
+from subsystems.flywheel import FlywheelMechanism
+
+class SysIdRoutineBot:
+    def __init__(self) -> None:
+        self.joystick = CommandXboxController(0)
+        self.mechanism = FlywheelMechanism()
+
+        self.configureBindings()
+
+    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.
+        """
+
+        # Default command is duty cycle control with the left up/down stick
+        self.mechanism.setDefaultCommand(self.mechanism.joystick_drive_command(self.joystick.getLeftY))
+
+        # Manually start logging with left bumper before running any tests,
+        # and stop logging with right bumper after we're done with ALL tests.
+        # This isn't necessary but is convenient to reduce the size of the hoot file.
+        self.joystick.leftBumper().onTrue(cmd.runOnce(SignalLogger.start))
+        self.joystick.rightBumper().onTrue(cmd.runOnce(SignalLogger.stop))
+
+        # Joystick Y = quasistatic forward
+        # Joystick A = quasistatic reverse
+        # Joystick B = dynamic forward
+        # Joystick X = dynamic reverse
+        self.joystick.y().whileTrue(self.mechanism.sys_id_quasistatic(SysIdRoutine.Direction.kForward))
+        self.joystick.a().whileTrue(self.mechanism.sys_id_quasistatic(SysIdRoutine.Direction.kReverse))
+        self.joystick.b().whileTrue(self.mechanism.sys_id_dynamic(SysIdRoutine.Direction.kForward))
+        self.joystick.x().whileTrue(self.mechanism.sys_id_dynamic(SysIdRoutine.Direction.kReverse))
+
+    def getAutonomousCommand(self) -> Command:
+        """Use this to define the command that runs during autonomous.
+
+        Scheduled during :meth:`.Robot.autonomousInit`.
+        """
+
+        return cmd.print("No autonomous command configured")

python/PhoenixSysId/subsystems/flywheel.py

@@ -0,0 +1,58 @@
+from commands2 import Command, Subsystem
+from commands2.sysid import SysIdRoutine
+from wpilib.sysid import SysIdRoutineLog
+from wpimath.units import volts
+
+from phoenix6 import configs, controls, hardware, BaseStatusSignal, SignalLogger
+
+from constants import Constants
+
+from typing import Callable
+
+class FlywheelMechanism:
+    def __init__(self) -> None:
+        self.motor_to_test = hardware.TalonFX(Constants.kTalonFxId, Constants.kCANbus)
+        self.joystick_control = controls.DutyCycleOut(0)
+        self.sys_id_control = controls.VoltageOut(0)
+
+        self.sys_id_routine = SysIdRoutine(
+            SysIdRoutine.Config(
+                # Use default ramp rate (1 V/s) and timeout (10 s)
+                # Reduce dynamic voltage to 4 to prevent brownout
+                stepVoltage = 4.0,
+                # Log state with Phoenix SignalLogger class
+                recordState = lambda state: SignalLogger.write_string("state", SysIdRoutineLog.stateEnumToString(state))
+            ),
+            SysIdRoutine.Mechanism(
+                lambda volts: self.motor.set_control(self.voltage_req.with_output(volts)),
+                lambda log: None,
+                self
+            )
+        )
+
+        self.setName("Flywheel")
+
+        cfg = configs.TalonFXConfiguration()
+        # Set any necessary configs in the Feedback group here
+        self.motor_to_test.configurator.apply(cfg)
+
+        # Speed up signals for better characterization data
+        BaseStatusSignal.set_update_frequency_for_all(250,
+            self.motor_to_test.get_position(),
+            self.motor_to_test.get_velocity(),
+            self.motor_to_test.get_motor_voltage())
+
+        # Optimize out the other signals, since they're not useful for SysId
+        self.motor_to_test.optimize_bus_utilization()
+
+        # Star the signal logger
+        SignalLogger.start()
+
+    def joystick_drive_command(self, output: Callable[[], float]) -> Command:
+        return self.run(lambda: self.motor_to_test.set_control(self.joystick_control.with_output(output())))
+
+    def sys_id_quasistatic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.quasistatic(direction)
+
+    def sys_id_dynamic(self, direction: SysIdRoutine.Direction) -> Command:
+        return self.sys_id_routine.dynamic(direction)

python/PhoenixSysId/tests/pyfrc_test.py

@@ -0,0 +1,38 @@
+'''
+    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 *
+from wpilib.simulation import XboxControllerSim
+import time
+from subsystems.flywheel import FlywheelMechanism
+
+FIRST_SET = 0
+SECOND_SET = 4.8
+
+def assert_almost_equal(a: float, b: float, range_val: float):
+    """
+    Assert that a is within range of b
+    """
+    assert a >= (b - range_val) and a <= (b + range_val)
+
+def test_sysid_quasistatic(control, robot):
+    joysim = XboxControllerSim(robot.container.joystick.getHID())
+
+    joysim.setYButton(True)
+    control.step_timing(seconds=0.1, autonomous=False, enabled=True)
+
+    assert robot.container.mechanism.sys_id_control.output > 0.1
+
+    joysim.setYButton(False)
+
+def test_sysid_dynamic(control, robot):
+    joysim = XboxControllerSim(robot.container.joystick.getHID())
+
+    joysim.setXButton(True)
+    control.step_timing(seconds=0.1, autonomous=False, enabled=True)
+
+    assert robot.container.mechanism.sys_id_control.output > 0.1
+
+    joysim.setXButton(False)