001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.util;
007
008import static edu.wpi.first.units.Units.*;
009import static edu.wpi.first.units.Units.Second;
010import static edu.wpi.first.units.Units.Volts;
011
012import com.ctre.phoenix6.SignalLogger;
013import edu.wpi.first.units.Units;
014import edu.wpi.first.units.VoltageUnit;
015import edu.wpi.first.units.measure.*;
016import edu.wpi.first.wpilibj2.command.Subsystem;
017import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
018import java.util.function.Consumer;
019import java.util.function.Supplier;
020
021/**
022 *
023 *
024 * <h2>A class that handles the Mechanism logging and unit conversions required
025 * to create a SysId
026 * routine</h2>
027 */
028public class SysId {
029
030        /**
031         *
032         *
033         * <h4>Creates a SysId routine from the parameters</h4>
034         *
035         * @param rampRate          - Measured in volts per second, the voltage ramp
036         *                          rate used for quasistatic test
037         *                          routines.
038         * @param stepVoltage       - Measured in volts, the step voltage output used
039         *                          for dynamic test routines.
040         * @param subsystemName     - Name of the subsystem used for logging.
041         * @param voltageSetter     - Double consumer that sets the voltage of the
042         *                          subsystem.
043         * @param positionSupplier  - In rotations, a supplier for the position of the
044         *                          subsystem.
045         * @param voltageSupplier   - A supplier for the voltage of the subsystem's
046         *                          motors.
047         * @param subsystemInstance - The subsystem containing the motor(s) that is (or
048         *                          are) being
049         *                          characterized.
050         */
051        public static SysIdRoutine getRoutine(
052                        double rampRate,
053                        double stepVoltage,
054                        String subsystemName,
055                        Consumer<Double> voltageSetter,
056                        Supplier<Double> positionSupplier,
057                        Supplier<Double> velocitySupplier,
058                        Supplier<Double> voltageSupplier,
059                        Subsystem subsystemInstance) {
060                return new SysIdRoutine(
061                                new SysIdRoutine.Config(
062                                                Units.Volts.of(rampRate).per(Second),
063                                                Units.Volts.of(stepVoltage),
064                                                null,
065                                                state -> SignalLogger.writeString(subsystemName + " SysId-State",
066                                                                state.toString())),
067                                new SysIdRoutine.Mechanism(
068                                                output -> voltageSetter.accept(output.in(Volts)),
069                                                state -> {
070                                                        SignalLogger.writeDouble(subsystemName + " Position",
071                                                                        positionSupplier.get());
072                                                        SignalLogger.writeDouble(subsystemName + " Velocity",
073                                                                        velocitySupplier.get());
074                                                        SignalLogger.writeDouble(subsystemName + " Voltage",
075                                                                        voltageSupplier.get());
076                                                },
077                                                subsystemInstance));
078        }
079
080        /**
081         *
082         *
083         * <h4>Creates a SysId routine from the parameters</h4>
084         *
085         * <p>
086         * Uses WPILib unit classes so inputs can be in any unit.
087         *
088         * @param rampRate          - The voltage ramp rate used for quasistatic test
089         *                          routines.
090         * @param stepVoltage       - The step voltage output used for dynamic test
091         *                          routines.
092         * @param subsystemName     - Name of the subsystem used for logging.
093         * @param voltageSetter     - Double consumer that sets the voltage of the
094         *                          subsystem.
095         * @param positionSupplier  - A supplier for the position of the subsystem.
096         * @param voltageSupplier   - A supplier for the voltage of the subsystem's
097         *                          motors.
098         * @param subsystemInstance - The subsystem containing the motor(s) that is (or
099         *                          are) being
100         *                          characterized.
101         */
102        public static SysIdRoutine getRoutine(
103                        Velocity<VoltageUnit> rampRate,
104                        Voltage stepVoltage,
105                        String subsystemName,
106                        Consumer<Voltage> voltageSetter,
107                        Supplier<Angle> positionSupplier,
108                        Supplier<AngularVelocity> velocitySupplier,
109                        Supplier<Voltage> voltageSupplier,
110                        Subsystem subsystemInstance) {
111                return new SysIdRoutine(
112                                new SysIdRoutine.Config(
113                                                rampRate,
114                                                stepVoltage,
115                                                null,
116                                                state -> SignalLogger.writeString(subsystemName + " SysId-State",
117                                                                state.toString())),
118                                new SysIdRoutine.Mechanism(
119                                                output -> voltageSetter.accept(output),
120                                                state -> {
121                                                        SignalLogger.writeDouble(
122                                                                        subsystemName + " Position",
123                                                                        positionSupplier.get().in(Rotations));
124                                                        SignalLogger.writeDouble(
125                                                                        subsystemName + " Velocity",
126                                                                        velocitySupplier.get().in(RotationsPerSecond));
127                                                        SignalLogger.writeDouble(subsystemName + " Voltage",
128                                                                        voltageSupplier.get().in(Volts));
129                                                },
130                                                subsystemInstance));
131        }
132}