From 58593d11f20cb717fbe209aec568234957bd2eaf Mon Sep 17 00:00:00 2001 From: Momentum Robotics Date: Fri, 29 Mar 2024 18:18:49 -0700 Subject: [PATCH] Bunch of changes from OCR Day 2 not enough time to sort through them all --- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../frc/robot/command/TuneShooterCommand.java | 4 ++-- .../command/shooter/IdleShooterCommand.java | 2 +- .../command/shooter/ShootSpeakerCommand.java | 8 +++++-- .../command/shooter/SpinupShooterCommand.java | 4 ++-- .../input/JoystickDualControllerInput.java | 4 ++-- .../robot/subsystem/AutoBuilderSubsystem.java | 23 +++++++++++++------ .../frc/robot/subsystem/ShooterSubsystem.java | 13 ++++++++--- src/main/java/frc/robot/util/MoPrefs.java | 4 ++-- 9 files changed, 44 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a148f27..9a9a417 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -72,15 +72,18 @@ public class RobotContainer { new WaitForArmSetpointCommand(arm, ArmSetpoint.HANDOFF).andThen(new HandoffCommand(intake, shooter)); private Command shootSpeakerCommand = new WaitForArmSetpointCommand(arm, ArmSetpoint.SPEAKER) + .withTimeout(1.5) .deadlineWith(new SpinupShooterCommand(shooter, () -> MoPrefs.flywheelSpeakerSetpoint.get())) .andThen(new ShootSpeakerCommand(shooter)) .withName("ShootSpeakerCommand"); private Command shootAmpCommand = new WaitForArmSetpointCommand(arm, ArmSetpoint.AMP) + .withTimeout(1.5) .andThen(new ShootAmpCommand(shooter)) .withName("ShootAmpCommand"); private Command shootShuttleCommand = new WaitForArmSetpointCommand(arm, ArmSetpoint.SHUTTLE) + .withTimeout(1.5) .deadlineWith(new SpinupShooterCommand(shooter, () -> MoPrefs.flywheelSpeakerSetpoint.get())) .andThen(new ShootSpeakerCommand(shooter)) .withName("ShootShuttleCommand"); diff --git a/src/main/java/frc/robot/command/TuneShooterCommand.java b/src/main/java/frc/robot/command/TuneShooterCommand.java index 317d10a..e416b17 100644 --- a/src/main/java/frc/robot/command/TuneShooterCommand.java +++ b/src/main/java/frc/robot/command/TuneShooterCommand.java @@ -45,7 +45,7 @@ public TuneShooterCommand(ShooterSubsystem shooter, ArmSubsystem arm, Positionin @Override public void execute() { - double thresh = MoPrefs.shooterSetpointVarianceThreshold.get().in(Units.Value); + // double thresh = MoPrefs.shooterSetpointVarianceThreshold.get().in(Units.Value); ArmPosition position = ArmSetpointManager.getInstance().getSetpoint(ArmSetpoint.SPEAKER); position = new ArmPosition( @@ -64,7 +64,7 @@ public void execute() { var flywheelSetpoint = MoPrefs.flywheelSpeakerSetpoint.get(); shooter.setFlywheelSpeed(flywheelSetpoint); - if (shooter.getAvgFlywheelVelocity().isNear(flywheelSetpoint, thresh)) { + if (shooter.getAvgFlywheelVelocity().gte(flywheelSetpoint.minus(MoPrefs.shooterSetpointThreshold.get()))) { shooter.setRollerVelocity(flywheelSetpoint); } else { shooter.setRollerVelocity(MoUnits.CentimetersPerSec.zero()); diff --git a/src/main/java/frc/robot/command/shooter/IdleShooterCommand.java b/src/main/java/frc/robot/command/shooter/IdleShooterCommand.java index 53eb5e9..9ce27c7 100644 --- a/src/main/java/frc/robot/command/shooter/IdleShooterCommand.java +++ b/src/main/java/frc/robot/command/shooter/IdleShooterCommand.java @@ -13,7 +13,7 @@ import java.util.function.Supplier; public class IdleShooterCommand extends Command { - private static final Measure> IDLE_SPEED = MoUnits.CentimetersPerSec.zero(); + public static final Measure> IDLE_SPEED = MoUnits.CentimetersPerSec.zero(); private final ShooterSubsystem shooter; private final Supplier inputSupplier; diff --git a/src/main/java/frc/robot/command/shooter/ShootSpeakerCommand.java b/src/main/java/frc/robot/command/shooter/ShootSpeakerCommand.java index 17e7b0e..34f0894 100644 --- a/src/main/java/frc/robot/command/shooter/ShootSpeakerCommand.java +++ b/src/main/java/frc/robot/command/shooter/ShootSpeakerCommand.java @@ -36,8 +36,7 @@ public void execute() { var flywheelSpeed = MoPrefs.flywheelSpeakerSetpoint.get(); shooter.setFlywheelSpeed(flywheelSpeed); - double thresh = MoPrefs.shooterSetpointVarianceThreshold.get().in(Units.Value); - if (shooter.getAvgFlywheelVelocity().isNear(flywheelSpeed, thresh)) { + if (shooter.getAvgFlywheelVelocity().gte(flywheelSpeed.minus(MoPrefs.shooterSetpointThreshold.get()))) { upToSpeed = true; } @@ -49,6 +48,11 @@ public void execute() { } } + @Override + public void end(boolean interrupted) { + shooter.setFlywheelSpeed(IdleShooterCommand.IDLE_SPEED); + } + @Override public boolean isFinished() { var rollerFeedTime = MoPrefs.shooterRollerRunTime.get(); diff --git a/src/main/java/frc/robot/command/shooter/SpinupShooterCommand.java b/src/main/java/frc/robot/command/shooter/SpinupShooterCommand.java index 4906ca0..861f01f 100644 --- a/src/main/java/frc/robot/command/shooter/SpinupShooterCommand.java +++ b/src/main/java/frc/robot/command/shooter/SpinupShooterCommand.java @@ -36,8 +36,8 @@ public void execute() { } public boolean onTarget() { - double thresh = MoPrefs.shooterSetpointVarianceThreshold.get().in(Units.Value); + var flywheelSpeed = flywheelSpeedSupplier.get(); - return shooter.getAvgFlywheelVelocity().isNear(flywheelSpeedSupplier.get(), thresh); + return shooter.getAvgFlywheelVelocity().gte(flywheelSpeed.minus(MoPrefs.shooterSetpointThreshold.get())); } } diff --git a/src/main/java/frc/robot/input/JoystickDualControllerInput.java b/src/main/java/frc/robot/input/JoystickDualControllerInput.java index a8060c3..8e97b1e 100644 --- a/src/main/java/frc/robot/input/JoystickDualControllerInput.java +++ b/src/main/java/frc/robot/input/JoystickDualControllerInput.java @@ -48,7 +48,7 @@ public double getTurnRequest() { @Override public boolean driveRobotOriented() { - return joystick.getRawButton(2); + return joystick.getRawButton(3); } @Override @@ -120,7 +120,7 @@ public boolean getReverseShooter() { } public boolean getReverseIntake() { - return joystick.getRawButton(3); + return joystick.getRawButton(2); } @Override diff --git a/src/main/java/frc/robot/subsystem/AutoBuilderSubsystem.java b/src/main/java/frc/robot/subsystem/AutoBuilderSubsystem.java index 6f09f03..50e3a42 100644 --- a/src/main/java/frc/robot/subsystem/AutoBuilderSubsystem.java +++ b/src/main/java/frc/robot/subsystem/AutoBuilderSubsystem.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.command.arm.MoveArmCommand; import frc.robot.command.arm.WaitForArmSetpointCommand; +import frc.robot.command.shooter.IdleShooterCommand; import frc.robot.command.shooter.ShootSpeakerCommand; import frc.robot.component.ArmSetpointManager.ArmSetpoint; import frc.robot.util.MoShuffleboard; @@ -40,14 +41,22 @@ private enum StartPos { private enum TaskType { LEAVE((s, c) -> MoveArmCommand.forSetpoint(s.arm, ArmSetpoint.STOW).raceWith(c)), - SHOOT((s, c) -> new WaitForArmSetpointCommand(s.arm, ArmSetpoint.SPEAKER) - .withTimeout(2) + SHOOT((s, c) -> MoveArmCommand.goToSetpointAndEnd(s.arm, ArmSetpoint.STOW) + .withTimeout(1.65) .andThen( - new ShootSpeakerCommand(s.shooter), - Commands.runOnce(() -> s.shooter.setFlywheelSpeed(Units.MetersPerSecond.zero()))) - .deadlineWith(MoveArmCommand.forSetpoint(s.arm, ArmSetpoint.SPEAKER)) - .withTimeout(7.5) - .andThen(c, Commands.runOnce(() -> s.positioning.resetFieldOrientedFwd()))); + new WaitForArmSetpointCommand(s.arm, ArmSetpoint.SPEAKER) + .withTimeout(1) + .andThen(new ShootSpeakerCommand(s.shooter).withTimeout(3)) + .deadlineWith(MoveArmCommand.forSetpoint(s.arm, ArmSetpoint.SPEAKER)), + c.deadlineWith( + MoveArmCommand.forSetpoint(s.arm, ArmSetpoint.STOW), + Commands.run( + () -> { + s.shooter.setFlywheelSpeed(IdleShooterCommand.IDLE_SPEED); + s.shooter.setRollerVelocity(Units.MetersPerSecond.zero()); + }, s.shooter)))); + // .andThen(c, Commands.runOnce(() -> s.positioning.resetFieldOrientedFwd()))); + // new SpinupShooterCommand(s.shooter, () -> MoPrefs.flywheelSpeakerSetpoint.get()) BiFunction commandModifier; diff --git a/src/main/java/frc/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/robot/subsystem/ShooterSubsystem.java index 279f137..b927a95 100644 --- a/src/main/java/frc/robot/subsystem/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystem/ShooterSubsystem.java @@ -126,6 +126,9 @@ public ShooterSubsystem() { MoShuffleboard.getInstance().shooterTab.add(this); } + private void addFlexDebugWidget() { + } + public Measure getRollerPosition() { return rollerEncoder.getPosition(); } @@ -221,9 +224,13 @@ public void burnFlash() { System.out.println("SHOULD NOT BURN FLASH THIS FREQUENLTY"); return; } - flywheelLower.burnFlash(); - flywheelUpper.burnFlash(); - roller.burnFlash(); + var rFlyLow = flywheelLower.burnFlash(); + var rFlyUp = flywheelUpper.burnFlash(); + var rRoll = roller.burnFlash(); + + System.out.printf("flywL:%s, flywU:%s, roll%s", rFlyLow, rFlyUp, rRoll); + System.out.println(); + burnFlashDebounce.restart(); } diff --git a/src/main/java/frc/robot/util/MoPrefs.java b/src/main/java/frc/robot/util/MoPrefs.java index 4f78e31..16ccb3e 100644 --- a/src/main/java/frc/robot/util/MoPrefs.java +++ b/src/main/java/frc/robot/util/MoPrefs.java @@ -106,8 +106,8 @@ public class MoPrefs { encoderTicksPerCentimeterPref( "Shooter Flywheel Scale", MoUnits.EncoderTicksPerCentimeter.of(0.04699456981059901)); - public static final UnitPref shooterSetpointVarianceThreshold = getInstance() - .new UnitPref("Shooter Setpoint Variance Threshold", Units.Percent, Units.Percent.of(5)); + public static final UnitPref> shooterSetpointThreshold = + metersPerSecPref("Shooter Setpoint Threshold", Units.MetersPerSecond.of(0.75)); public static final UnitPref armSetpointVarianceThreshold = getInstance() .new UnitPref("Arm Setpoint Variance Threshold", Units.Percent, Units.Percent.of(3));