Bunch of changes from OCR Day 2 not enough time to sort through them all

momentumfrc · Mar 30, 2024 · 58593d1 · 58593d1
1 parent a5a34e0
commit 58593d1
Show file tree

Hide file tree

Showing 9 changed files with 44 additions and 21 deletions.
diff --git 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
@@ -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
@@ -13,7 +13,7 @@
 import java.util.function.Supplier;
 
 public class IdleShooterCommand extends Command {
-    private static final Measure<Velocity<Distance>> IDLE_SPEED = MoUnits.CentimetersPerSec.zero();
+    public static final Measure<Velocity<Distance>> IDLE_SPEED = MoUnits.CentimetersPerSec.zero();
 
     private final ShooterSubsystem shooter;
     private final Supplier<MoInput> inputSupplier;

diff --git 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
@@ -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
@@ -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
@@ -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<AutoBuilderSubsystem, Command, Command> commandModifier;
 

diff --git 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<Distance> 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
@@ -106,8 +106,8 @@ public class MoPrefs {
             encoderTicksPerCentimeterPref(
                     "Shooter Flywheel Scale", MoUnits.EncoderTicksPerCentimeter.of(0.04699456981059901));
 
-    public static final UnitPref<Dimensionless> shooterSetpointVarianceThreshold = getInstance()
-    .new UnitPref<Dimensionless>("Shooter Setpoint Variance Threshold", Units.Percent, Units.Percent.of(5));
+    public static final UnitPref<Velocity<Distance>> shooterSetpointThreshold =
+            metersPerSecPref("Shooter Setpoint Threshold", Units.MetersPerSecond.of(0.75));
 
     public static final UnitPref<Dimensionless> armSetpointVarianceThreshold = getInstance()
     .new UnitPref<Dimensionless>("Arm Setpoint Variance Threshold", Units.Percent, Units.Percent.of(3));