diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bc36dc54..1217ac0d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.LimeLightConstants; import frc.robot.commands.RHRFullRoutine; import frc.robot.commands.fullRoutines.BottomTwo; @@ -133,6 +134,8 @@ public void robotInit() { // ? new VisionIOSim(LimeLightConstants.REAR_LIMELIGHT_INFO) // : new VisionIOLimelight(LimeLightConstants.REAR_LIMELIGHT_INFO)); + DriverStation.silenceJoystickConnectionWarning(isSimulation()); + mechManager = new MechanismManager(); createDriverBindings(); @@ -359,6 +362,30 @@ public void createDriverBindings() { DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? 180 : 0)))); + + driver + .a() + .whileTrue( + Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION)) + .andThen(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward))); + + driver + .b() + .whileTrue( + Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION)) + .andThen(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse))); + + driver + .x() + .whileTrue( + Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION)) + .andThen(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward))); + + driver + .y() + .whileTrue( + Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION)) + .andThen(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))); } public void createOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java b/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java index 530011f6..54e3a41b 100644 --- a/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java @@ -1,10 +1,22 @@ package frc.robot.subsystems.shooterIO; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -124,6 +136,7 @@ public enum State { OFF(() -> 0, () -> 0, () -> 0, () -> true), OUTTAKE_BACKWARDS(() -> -4000, () -> -4000, () -> -5, () -> true), CLEANING(() -> 10, () -> 10, () -> 1, () -> true), + CHARACTERIZATION(() -> 0, () -> 0, () -> 0, () -> true), FEEDER_SHOT( feederShotRPM, feederShotRPM, @@ -141,19 +154,57 @@ public enum State { private final ShooterIO IO; public final ShooterInputsAutoLogged inputs; + private final SysIdRoutine sysIdRoutine; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutableMeasure m_angle = mutable(Rotations.of(0)); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutableMeasure> m_velocity = mutable(RotationsPerSecond.of(0)); + public Shooter(ShooterIO IO) { this.IO = IO; this.inputs = new ShooterInputsAutoLogged(); this.IO.updateInputs(inputs, state); + + sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + this.IO.setShooterVolts(volts.in(Volts), volts.in(Volts)); + }, + log -> { + log.motor("shooter") + .voltage(m_appliedVoltage.mut_replace(inputs.leftOutputVoltage, Volts)) + .angularPosition(m_angle.mut_replace(inputs.leftPosDeg, Degrees)) + .angularVelocity( + m_velocity.mut_replace(inputs.leftSpeedRPM / 60.0, RotationsPerSecond)); + }, + this)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); } @Override public void periodic() { IO.updateInputs(inputs, state); + Logger.processInputs("Shooter", inputs); boolean shouldSpinFeeder = debouncer.calculate(isAtTarget()); Logger.recordOutput("Shooter/Should spin feeder", shouldSpinFeeder); + if (state == State.CHARACTERIZATION) { + return; + } + if (state == State.INTAKING && hasGamePiece()) { state = State.HOLDING_GP; } @@ -181,7 +232,6 @@ public void periodic() { IO.setMotorSetPoint( state.leftRpm.getAsDouble() + differential, state.rightRpm.getAsDouble() - differential); } - Logger.processInputs("Shooter", inputs); } @AutoLogOutput(key = "Shooter/isAtTarget") diff --git a/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java index e74a4a7b..08eae1bf 100644 --- a/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java @@ -25,6 +25,8 @@ public class ShooterIOSim implements ShooterIO { private static final FlywheelSim feeder = new FlywheelSim(DCMotor.getKrakenX60(1), 1.0, 0.0001); + private boolean useFlywheelPID = false; + private double leftVolts, rightVolts, feederVolts; Timer fakeGamepieceTimer = new Timer(); @@ -36,19 +38,23 @@ public ShooterIOSim() { @Override public void updateInputs(ShooterInputsAutoLogged inputs, Shooter.State state) { - leftFlyWheel.update(0.02); - rightFlyWheel.update(0.02); - feeder.update(0.02); + if (useFlywheelPID) { + leftVolts = + MathUtil.clamp(leftController.calculate(leftFlyWheel.getAngularVelocityRPM()), -12, 12); + rightVolts = + MathUtil.clamp(rightController.calculate(rightFlyWheel.getAngularVelocityRPM()), -12, 12); + } - leftVolts = - MathUtil.clamp(leftController.calculate(leftFlyWheel.getAngularVelocityRPM()), -12, 12); - rightVolts = - MathUtil.clamp(rightController.calculate(rightFlyWheel.getAngularVelocityRPM()), -12, 12); + System.err.println(useFlywheelPID + " - updating with volts " + leftVolts); leftFlyWheel.setInputVoltage(leftVolts); rightFlyWheel.setInputVoltage(rightVolts); feeder.setInputVoltage(feederVolts); + leftFlyWheel.update(0.02); + rightFlyWheel.update(0.02); + feeder.update(0.02); + inputs.leftOutputVoltage = this.leftVolts; inputs.rightOutputVoltage = this.rightVolts; @@ -100,6 +106,7 @@ public void updateInputs(ShooterInputsAutoLogged inputs, Shooter.State state) { public void setMotorSetPoint(double leftRPM, double rightRPM) { leftController.setSetpoint(leftRPM); rightController.setSetpoint(rightRPM); + useFlywheelPID = true; } @Override @@ -108,5 +115,10 @@ public void setFeederVolts(double volts) { } @Override - public void setShooterVolts(double lVolts, double rVolts) {} + public void setShooterVolts(double lVolts, double rVolts) { + System.err.println("set shooter volts to " + lVolts); + useFlywheelPID = false; + leftVolts = lVolts; + rightVolts = rVolts; + } } diff --git a/src/main/java/frc/robot/util/RedHawkUtil.java b/src/main/java/frc/robot/util/RedHawkUtil.java index 14a9449d..1fc97079 100644 --- a/src/main/java/frc/robot/util/RedHawkUtil.java +++ b/src/main/java/frc/robot/util/RedHawkUtil.java @@ -93,6 +93,7 @@ public static void maybeFlipLog(ChoreoTrajectory traj) { var alliance = DriverStation.getAlliance(); if (alliance.isEmpty()) { Logger.recordOutput("SHOULD_BE_FLIPPING", false); + return; } if (alliance.get() == Alliance.Blue) {