diff --git a/python/PhoenixSysId/.gitignore b/python/PhoenixSysId/.gitignore new file mode 100644 index 00000000..d18c5608 --- /dev/null +++ b/python/PhoenixSysId/.gitignore @@ -0,0 +1,5 @@ +ctre_sim +__pycache__ +.pytest_cache +.deploy_cfg +.installer_config diff --git a/python/PhoenixSysId/constants.py b/python/PhoenixSysId/constants.py new file mode 100644 index 00000000..30baf07f --- /dev/null +++ b/python/PhoenixSysId/constants.py @@ -0,0 +1,3 @@ +class Constants: + kCANbus = "canivore" + kTalonFxId = 18 diff --git a/python/PhoenixSysId/robot.py b/python/PhoenixSysId/robot.py new file mode 100644 index 00000000..246ba5d4 --- /dev/null +++ b/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 diff --git a/python/PhoenixSysId/robotcontainer.py b/python/PhoenixSysId/robotcontainer.py new file mode 100644 index 00000000..a5fe5b79 --- /dev/null +++ b/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") diff --git a/python/PhoenixSysId/subsystems/flywheel.py b/python/PhoenixSysId/subsystems/flywheel.py new file mode 100644 index 00000000..ef633c7c --- /dev/null +++ b/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) diff --git a/python/PhoenixSysId/tests/pyfrc_test.py b/python/PhoenixSysId/tests/pyfrc_test.py new file mode 100644 index 00000000..bc5b4f42 --- /dev/null +++ b/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)