Tune commands to be faster for auto

src/main/java/frc/robot/Constants.java

@@ -88,6 +88,7 @@ public static class NamedCommands {
     public static final String FEEDTHROUGH_COMMAND_NAME = "feed through";
     public static final String AUTO_SHOOT_COMMAND_NAME = "auto shoot";
     public static final String AUTO_SHOOT_LONG_COMMAND_NAME = "auto shoot long";
+    public static final String AUTO_INTAKE_COMMAND_NAME = "auto intake";
   }
 
 

src/main/java/frc/robot/RobotContainer.java

@@ -83,15 +83,15 @@ public RobotContainer() {
     // Configure auto builder
     DRIVE_SUBSYSTEM.configureAutoBuilder();
 
-    // Register Named Commands
+    // Register named commands
     NamedCommands.registerCommand(Constants.NamedCommands.INTAKE_COMMAND_NAME, autoIntakeCommand().withTimeout(7));
     NamedCommands.registerCommand(Constants.NamedCommands.PRELOAD_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(1.1));
     NamedCommands.registerCommand(Constants.NamedCommands.SHOOT_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(0.7));
     NamedCommands.registerCommand(Constants.NamedCommands.SPINUP_COMMAND_NAME, SHOOTER_SUBSYSTEM.spinupCommand());
     NamedCommands.registerCommand(Constants.NamedCommands.FEEDTHROUGH_COMMAND_NAME, feedThroughCommand().withTimeout(2));
     NamedCommands.registerCommand(Constants.NamedCommands.AUTO_SHOOT_COMMAND_NAME, shootCommand().withTimeout(0.9));
     NamedCommands.registerCommand(Constants.NamedCommands.AUTO_SHOOT_LONG_COMMAND_NAME, shootCommand().withTimeout(2.0));
-    NamedCommands.registerCommand("auto intake", aimAndIntakeObjectCommand());
+    NamedCommands.registerCommand(Constants.NamedCommands.AUTO_INTAKE_COMMAND_NAME, aimAndIntakeObjectCommand());
 
     VISION_SUBSYSTEM.setPoseSupplier(() -> DRIVE_SUBSYSTEM.getPose());
 
@@ -107,11 +107,11 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.start().onTrue(DRIVE_SUBSYSTEM.toggleTractionControlCommand());
 
     // Back button - toggles centricity between robot and field centric
-    PRIMARY_CONTROLLER.back().onTrue(DRIVE_SUBSYSTEM.toggleCentriciyCommand());
+    PRIMARY_CONTROLLER.back().onTrue(DRIVE_SUBSYSTEM.toggleCentricityCommand());
 
     // Right trigger button - aim and shoot at speaker, shooting only if speaker tag is visible and robot is in range
     // Click DPAD down to override and shoot now
-    PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.povDown().getAsBoolean()));
+    PRIMARY_CONTROLLER.a().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.povDown().getAsBoolean()));
 
     // Right bumper button - amp score, also use for outtake
     PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand());

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -94,6 +94,7 @@ public Hardware(NavX2 navx,
   public static final Measure<Current> DRIVE_CURRENT_LIMIT = Units.Amps.of(60.0);
   public static final Measure<Velocity<Angle>> NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60);
   public static final Measure<Velocity<Angle>> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI);
+  public static final Measure<Velocity<Angle>> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(5.0);
   public static final Measure<Velocity<Velocity<Angle>>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second);
   public static final Translation2d AIM_OFFSET = new Translation2d(0.0, -0.15);
   public final Measure<Velocity<Distance>> DRIVE_MAX_LINEAR_SPEED;
@@ -108,12 +109,12 @@ public Hardware(NavX2 navx,
   private static final Matrix<N3, N1> ODOMETRY_STDDEV = VecBuilder.fill(0.03, 0.03, Math.toRadians(1.0));
   private static final Matrix<N3, N1> VISION_STDDEV = VecBuilder.fill(1.0, 1.0, Math.toRadians(3.0));
   private static final PIDConstants AUTO_AIM_PID = new PIDConstants(10.0, 0.0, 0.5, 0.0, 0.0, GlobalConstants.ROBOT_LOOP_PERIOD);
-  private static final TrapezoidProfile.Constraints AIM_PID_CONSTRAINT = new TrapezoidProfile.Constraints(2160.0, 2160.0);
+  private static final TrapezoidProfile.Constraints AIM_PID_CONSTRAINT = new TrapezoidProfile.Constraints(2160.0, 4320.0);
 
-  private static final Measure<Angle> BLUE_AMP_DIRECTION = Units.Radians.of(-Math.PI/2);
-  private static final Measure<Angle> BLUE_SOURCE_DIRECTION = Units.Radians.of(-1.060);
+  private static final Measure<Angle> BLUE_AMP_DIRECTION = Units.Radians.of(-Math.PI / 2);
+  private static final Measure<Angle> BLUE_SOURCE_DIRECTION = Units.Radians.of(-1.060 + Math.PI);
 
-  private static final Measure<Angle> RED_AMP_DIRECTION = Units.Radians.of(-Math.PI/2);
+  private static final Measure<Angle> RED_AMP_DIRECTION = Units.Radians.of(-Math.PI / 2);
   private static final Measure<Angle> RED_SOURCE_DIRECTION = Units.Radians.of(-2.106 + Math.PI);
 
   private static Measure<Angle> m_selectedAmpDirection = BLUE_AMP_DIRECTION;
@@ -211,7 +212,7 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     while (m_navx.isCalibrating()) stop();
     m_navx.reset();
 
-    // Setup turn PID
+    // Setup rotate PID
     m_rotatePIDController.setTolerance(TOLERANCE);
     m_rotatePIDController.setSetpoint(getAngle().in(Units.Degrees));
 
@@ -1034,7 +1035,7 @@ public Command toggleTractionControlCommand() {
    * Toggles between field and robot oriented drive control
    * @return Command to toggle control centricity between robot and field centric drive control
    */
-  public Command toggleCentriciyCommand() {
+  public Command toggleCentricityCommand() {
     return runOnce(() -> toggleControlCentricity());
   }
 
@@ -1188,8 +1189,12 @@ public boolean isBalanced() {
            Math.abs(getRoll().in(Units.Degrees)) < BALANCED_THRESHOLD;
   }
 
+  /**
+   * Get if robot is aimed at desired target
+   * @return True if aimed
+   */
   public boolean isAimed() {
-    return m_autoAimPIDControllerFront.atGoal() || m_autoAimPIDControllerBack.atGoal();
+    return (m_autoAimPIDControllerFront.atGoal() || m_autoAimPIDControllerBack.atGoal()) && getRotateRate().lt(AIM_VELOCITY_THRESHOLD);
   }
 
   /**
@@ -1230,7 +1235,7 @@ public Measure<Angle> getAngle() {
 
   /**
    * Get rotate rate of robot
-   * @return Current rotate rate of robot (degrees/s)
+   * @return Current rotate rate of robot
    */
   public Measure<Velocity<Angle>> getRotateRate() {
     return m_navx.getInputs().yawRate;

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -111,9 +111,9 @@ public State(Measure<Velocity<Distance>> speed, Measure<Angle> angle) {
 
   private final Measure<Current> NOTE_SHOT_CURRENT_THRESHOLD = Units.Amps.of(10.0);
   private final Measure<Time> NOTE_SHOT_TIME_THRESHOLD = Units.Seconds.of(0.1);
-  private final Measure<Time> READY_TIME_THRESHOLD = Units.Seconds.of(0.05);
+  private final Measure<Time> READY_TIME_THRESHOLD = Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD * 2);
   private final Debouncer NOTE_SHOT_DETECTOR = new Debouncer(NOTE_SHOT_TIME_THRESHOLD.in(Units.Seconds), DebounceType.kRising);
-  private final Debouncer READY_DEBOUNCER = new Debouncer(READY_TIME_THRESHOLD.in(Units.Seconds), DebounceType.kRising);
+  private final Debouncer READY_DEBOUNCER = new Debouncer(READY_TIME_THRESHOLD.in(Units.Seconds), DebounceType.kBoth);
 
   private Spark m_topFlywheelMotor;
   private Spark m_bottomFlywheelMotor;
@@ -223,7 +223,6 @@ public ShooterSubsystem(Hardware shooterHardware, SparkPIDConfig flywheelConfig,
       var state = getAutomaticState();
       state = new State(SPINUP_SPEED, state.angle);
       setState(state);
-      feedStop();
     }));
 
     // Initialize sim variables
@@ -444,17 +443,16 @@ public void simulationPeriodic() {
    * @return Command to intake to the shooter
    */
   public Command intakeCommand() {
-    return startEnd(
-      () -> {
-        setState(new State(SPINUP_SPEED, m_desiredShooterState.angle));
-        m_indexerMotor.enableForwardLimitSwitch();
-        feedStart(true);
-      },
+    return runEnd(
+      () -> getDefaultCommand().execute(),
       () -> {
         m_indexerMotor.disableForwardLimitSwitch();
         feedStop();
       }
-    ).until(() -> isObjectPresent());
+    ).beforeStarting(() -> {
+        m_indexerMotor.enableForwardLimitSwitch();
+        feedStart(true);
+    }).until(() -> isObjectPresent());
   }
 
   /**
@@ -543,7 +541,7 @@ public Command shootCommand(BooleanSupplier isAimed, BooleanSupplier override) {
     return runEnd(
       () -> {
         setState(getAutomaticState());
-        if (RobotBase.isSimulation() | isReady() && isAimed.getAsBoolean())
+        if ((RobotBase.isSimulation() | isReady() && isAimed.getAsBoolean()) || override.getAsBoolean())
           feedStart(false);
         else feedStop();
       },

src/test/java/frc/robot/subsystems/ShooterSubsystemTest.java

@@ -31,6 +31,7 @@
 import edu.wpi.first.units.Dimensionless;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 
@@ -186,6 +187,7 @@ public void runIndexer() {
 
     // Execute the command when the state is ready
     command.execute();
+    Timer.delay(0.1);
     command.execute();
 
     // Verify that motors are being driven with expected values