From ad68058ca4ccafe66a9fb92c33cad9bc0a438069 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Sun, 17 Nov 2024 15:17:37 -0500 Subject: [PATCH] sysid boilerplate --- .../subsystems/CommandSwerveDrivetrain.java | 88 +++++++++++++++---- src/main/java/frc/robot/utils/SysId.java | 25 ++++++ 2 files changed, 95 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/utils/SysId.java diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 12192f9..5e9a522 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -26,9 +27,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.InputStream; import frc.robot.Constants; import frc.robot.Constants.SwerveConstants; +import frc.robot.utils.SysId; @Logged(strategy = Strategy.OPT_IN) public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { @@ -47,6 +50,49 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private final SysIdSwerveRotation _rotationSysIdRequest = new SysIdSwerveRotation(); // how does this work? + // SysId routine for characterizing translation. This is used to find PID gains for the drive + // motors. + private final SysIdRoutine _sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + state -> DogLog.log("SysId Translation Routine State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(_translationSysIdRequest.withVolts(volts)), null, this)); + + // SysId routine for characterizing steer. This is used to find PID gains for the steer motors. + private final SysIdRoutine _sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + state -> DogLog.log("SysId Steer Routine State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(_steerSysIdRequest.withVolts(volts)), null, this)); + + // SysId routine for characterizing rotation. + private final SysIdRoutine _sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + // This is in radians per second², but SysId only supports "volts per second" + Volts.of(Math.PI / 6).per(Second), + // This is in radians per second, but SysId only supports "volts" + Volts.of(Math.PI), + null, // Use default timeout (10 s) + state -> DogLog.log("SysId Rotation Routine State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + // output is actually radians per second, but SysId only supports "volts" + setControl(_rotationSysIdRequest.withRotationalRate(output.in(Volts))); + // also log the requested output for SysId + SignalLogger.writeDouble("Rotational Rate", output.in(Volts)); + }, + null, + this)); + private double _lastSimTime = 0; private Notifier _simNotifier; @@ -113,9 +159,33 @@ public CommandSwerveDrivetrain( _isOpenLoop = true; })); + // display all sysid routines + SysId.displayRoutine("Swerve Translation", _sysIdRoutineTranslation); + SysId.displayRoutine("Swerve Steer", _sysIdRoutineSteer); + SysId.displayRoutine("Swerve Rotation", _sysIdRoutineRotation); + if (RobotBase.isSimulation()) startSimThread(); } + private void startSimThread() { + _lastSimTime = Utils.getCurrentTimeSeconds(); + + // Run simulation at a faster rate so PID gains behave more reasonably + _simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - _lastSimTime; + _lastSimTime = currentTime; + + // use the measured time delta, get battery voltage from WPILib + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + + _simNotifier.setName("Swerve Sim Thread"); + _simNotifier.startPeriodic(1 / Constants.simUpdateFrequency.in(Hertz)); + } + /** Toggles the field oriented boolean. */ public Command toggleFieldOriented() { return brake() @@ -230,22 +300,4 @@ public ChassisSpeeds getChassisSpeeds() { public void periodic() { // This method will be called once per scheduler run } - - private void startSimThread() { - _lastSimTime = Utils.getCurrentTimeSeconds(); - - // Run simulation at a faster rate so PID gains behave more reasonably - _simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - _lastSimTime; - _lastSimTime = currentTime; - - // use the measured time delta, get battery voltage from WPILib - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - - _simNotifier.startPeriodic(1 / Constants.simUpdateFrequency.in(Hertz)); - } } diff --git a/src/main/java/frc/robot/utils/SysId.java b/src/main/java/frc/robot/utils/SysId.java new file mode 100644 index 0000000..b23f48c --- /dev/null +++ b/src/main/java/frc/robot/utils/SysId.java @@ -0,0 +1,25 @@ +// 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. + +package frc.robot.utils; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + +/** Displays SysId routines on SmartDashboard. */ +public class SysId { + /** + * Displays a single routine to SmartDashboard. + * + * @param name The base name of this routine (ex: "Swerve Translation" or "Arm Rotation") + * @param routine The SysId Routine. + */ + public static void displayRoutine(String name, SysIdRoutine routine) { + SmartDashboard.putData(name + " Forward Quasistatic", routine.quasistatic(Direction.kForward)); + SmartDashboard.putData(name + " Reverse Quasistatic", routine.quasistatic(Direction.kReverse)); + SmartDashboard.putData(name + " Forward Dynamic", routine.dynamic(Direction.kForward)); + SmartDashboard.putData(name + " Reverse Dynamic", routine.dynamic(Direction.kReverse)); + } +}