Testing/apr4

src/main/java/frc/robot/Robot.java

@@ -129,6 +129,7 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    m_robotContainer.configureTeleopBindings();
   }
 
   /** This function is called periodically during operator control. */
@@ -141,8 +142,7 @@ public void testInit() {
     CommandScheduler.getInstance().cancelAll();
 
     SignalLogger.stop();
-    // m_robotContainer.removeSubsystemDefaultCommands();
-    // m_robotContainer.configureTestBindings();
+    m_robotContainer.configureTestBindings();
   }
 
   /** This function is called periodically during test mode. */

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

@@ -63,7 +63,6 @@ public class RobotContainer {
   private final DriveState driveState = DriveState.getInstance();
   private final LaunchState launchState = LaunchState.getInstance();
 
-  /*
   private final Command driveAndLaunchCommand =
       drivetrain
           .applyRequest(() -> getDriveAndLaunchRequest())
@@ -83,14 +82,13 @@ public class RobotContainer {
           .stopFullIndexingNoPID()
           .andThen(shooter.stopFlywheelCommand())
           .andThen(shooter.stowHood());
-   */
 
   private final SendableChooser<Command> autoChooser;
 
   public RobotContainer() {
     NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric));
-    // NamedCommands.registerCommand("AutonShoot", autonShootCommand);
-    // NamedCommands.registerCommand("StopShot", stopShotCommand);
+    NamedCommands.registerCommand("AutonShoot", autonShootCommand);
+    NamedCommands.registerCommand("StopShot", stopShotCommand);
     NamedCommands.registerCommand("StopRoller", intake.stopRollerNoPID());
     NamedCommands.registerCommand(
         "Collect Intake",
@@ -126,7 +124,6 @@ public RobotContainer() {
                 .withName("Rumble & Set Pose"));
 
     configureSubsystemDefaultCommands();
-    configureTeleopBindings();
   }
 
   public void configureSubsystemDefaultCommands() {
@@ -163,11 +160,6 @@ public void configureSubsystemDefaultCommands() {
             .withName("Teleop Drive"));
   }
 
-  public void removeSubsystemDefaultCommands() {
-    drivetrain.removeDefaultCommand();
-  }
-
-  /*
   public void configureTestBindings() {
     // Idle while the robot is disabled. This ensures the configured
     // neutral mode is applied to the drive motors while disabled.
@@ -194,14 +186,13 @@ public void configureTestBindings() {
 
     driverJoystick
         .a()
-        .whileTrue(driveAndLaunchCommand)
+        .whileTrue(driveAndLaunchCommand.alongWith(indexer.startFullIndexingNoPID()))
         .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood()));
 
     driverJoystick.b().onTrue(intake.testRollerNoPID()).onFalse(intake.stopRollerNoPID());
 
     driverJoystick.x().onTrue(intake.spinRollerCommand()).onFalse(intake.stopRollerCommand());
   }
-  */
 
   public void configureTeleopBindings() {
     driverJoystick
@@ -213,14 +204,13 @@ public void configureTeleopBindings() {
         .leftBumper()
         .onTrue(
             intake
-                .stopRollerNoPID()
+                .stopRollerCommand()
                 .andThen(intake.setIntakeExtensionCommand(IntakeConstants.INTAKE_REVERSE_LIMIT))
                 .withName("Stow Intake"));
 
     // stop the roller without retracting.
     driverJoystick.x().onTrue(intake.stopRollerNoPID());
 
-    /*
     // outtake fuel.  don't retract when done.
     driverJoystick
         .b()
@@ -233,14 +223,15 @@ public void configureTeleopBindings() {
         .onFalse(
             intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID()).withName("Stop Roller"));
 
+    driverJoystick.rightTrigger().onTrue(shooter.spinFlywheelCommand());
+
     operatorJoystick
         .rightTrigger()
         .whileTrue(
             drivetrain
                 .setXCommand()
-                .alongWith(indexer.pulsingIndexCommand())
-                .withName("Lock Wheels and Index"))
-        .onFalse(indexer.stopFullIndexingNoPID());
+                .alongWith(indexer.startFullIndexingNoPID())
+                .withName("Lock Wheels and Index"));
 
     operatorJoystick
         .leftTrigger()
@@ -256,7 +247,6 @@ public void configureTeleopBindings() {
         .rightBumper()
         .whileTrue(shooter.spinFlywheelPostCommand())
         .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood()));
-    */
 
     // Reset the field-centric heading on start button press.
     driverJoystick
@@ -302,11 +292,6 @@ public void configureTeleopBindings() {
                 .manualRumbleCommand(driverJoystick.getHID())
                 .withName("Rumble Driver Controller"));
 
-    operatorJoystick.rightTrigger().whileTrue(indexer.startFullIndexingNoPID());
-    operatorJoystick.a().onTrue(shooter.setHoodTargetCommand());
-    operatorJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand());
-    operatorJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand());
-
     drivetrain.registerTelemetry(logger::telemeterize);
 
     SmartDashboard.putData(

src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java

@@ -9,25 +9,15 @@ private IndexerPreferences() {}
   protected static DoublePreference indexerPercent =
       new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent
 
-  protected static DoublePreference acceleratorPercent =
-      new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent
-
-  /*
-  protected static DoublePreference indexSpeed =
-      new DoublePreference("Indexer/Index Speed (RotationsPS)", 0.1); // in rotations per second
-
   protected static DoublePreference indexerReversePercent =
       new DoublePreference("Indexer/Reverse Index Percent (for without PID)", -0.1); // in percent
 
-  protected static DoublePreference accelerateSpeed =
-      new DoublePreference(
-          "Indexer/Accelerate Speed (RotationsPS)", 0.1); // in rotations per second
-
+  protected static DoublePreference acceleratorPercent =
+      new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent
 
   protected static DoublePreference indexerRunTime =
       new DoublePreference("Indexer/Pulsing Run Time", 2.0); // in seconds
 
   protected static DoublePreference indexerPauseTime =
       new DoublePreference("Indexer/Pulsing Pause Time", 0.1); // in seconds
-  */
 }

src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java

@@ -3,11 +3,9 @@
 import static edu.wpi.first.units.Units.Volts;
 
 import com.ctre.phoenix6.SignalLogger;
-import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.epilogue.Logged;
-import edu.wpi.first.epilogue.Logged.Importance;
-import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -17,15 +15,6 @@ public class IndexerSubsystem extends SubsystemBase {
   private final TalonFX indexerMotor;
   private final TalonFX acceleratorMotor;
 
-  @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL)
-  private AngularVelocity indexerVelocityTarget; // RotationsPerSecond
-
-  @Logged(name = "Accelerator Velocity Target", importance = Importance.CRITICAL)
-  private AngularVelocity acceleratorVelocityTarget;
-
-  private VelocityVoltage indexerControl;
-  private VelocityVoltage acceleratorControl;
-
   final SysIdRoutine m_sysIdRoutineIndexer =
       new SysIdRoutine(
           new SysIdRoutine.Config(
@@ -86,22 +75,6 @@ private void setAcceleratorVoltage(double magnitude) {
     acceleratorMotor.setVoltage(magnitude);
   }
 
-  // Indexer Commands
-  public Command startIndexerNoPID() {
-    return runEnd(
-            () -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue()),
-            () -> indexerMotor.set(0))
-        .withName("Set Indexer Percent");
-  }
-
-  // Accelerator Commands
-  public Command startAcceleratorNoPID() {
-    return runEnd(
-            () -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()),
-            () -> acceleratorMotor.set(0))
-        .withName("Set Acceleration Percent");
-  }
-
   public Command startFullIndexingNoPID() {
     return runEnd(
         () -> {
@@ -123,16 +96,17 @@ public Command stopFullIndexingNoPID() {
         .withName("Stop Full Indexing No PID");
   }
 
-  /*
   public Command startIndexerReverseNoPID() {
-    return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue()))
+    return runOnce(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue()))
         .withName("Set Indexer Reverse Percent");
   }
 
   public Command stopIndexerNoPID() {
     return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent");
   }
 
+  /*
+
   public Command stopAcceleratorNoPID() {
     return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent");
   }
@@ -172,6 +146,8 @@ public Command stopCommand() {
         .withName("Stop Indexing Lemons");
   }
 
+  */
+
   public Command pulsingIndexCommand() {
     Timer timer = new Timer();
     return runEnd(
@@ -192,5 +168,4 @@ public Command pulsingIndexCommand() {
         .beforeStarting(() -> timer.restart())
         .withName("Pulsing Index");
   }
-  */
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -16,12 +16,10 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
-import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 @Logged
@@ -99,12 +97,14 @@ public IntakeSubsystem() {
   public void periodic() {
 
     // rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond)));
+    /*
     if (extensionMotor.getStatorCurrent().getValueAsDouble()
             > IntakePreferences.resistanceCurrentLimit.getValue()
         && isCompliantMode
         && RobotModeTriggers.teleop().getAsBoolean())
       CommandScheduler.getInstance()
           .schedule(stopRollerNoPID().andThen(setIntakeExtensionCommand(0)));
+    */
 
     extensionMotor.setControl(
         extensionControl
@@ -161,10 +161,14 @@ public boolean atExtensionSetpoint() {
   }
 
   public Command setIntakeExtensionCommand(double rotations) {
+    /*
     return runOnce(() -> isCompliantMode = false)
         .andThen(runOnce(() -> extensionTarget = Rotations.of(rotations)))
         .andThen(Commands.waitUntil(() -> atExtensionSetpoint()))
         .finallyDo(() -> isCompliantMode = true);
+    */
+    return runOnce(() -> extensionTarget = Rotations.of(rotations))
+        .andThen(Commands.waitUntil(() -> atExtensionSetpoint()));
   }
 
   public Command setExtendNoPID() {

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

@@ -16,35 +16,48 @@
 /** Add your docs here. */
 public class MappedLaunchRequestBuilder implements LaunchRequestBuilder {
 
-  private static final double minDistance;
-  private static final double maxDistance;
-
   // Launching Maps
   //   private static final InterpolatingTreeMap<Double, Rotation2d> hoodAngleMap =
   //   new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate);
   private static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap flywheelSpeedMap =
       new InterpolatingDoubleTreeMap();
 
-  // TODO: All of this is made up.  Need real numbers.
   static {
-    minDistance = 0.9;
-    maxDistance = 4.0;
+    hoodAngleMap.put(1.3, 0.0);
+    hoodAngleMap.put(2.0, 0.6);
+    hoodAngleMap.put(2.6, 1.4);
+    hoodAngleMap.put(3.1, 1.8);
+    hoodAngleMap.put(3.21, 2.2);
+    hoodAngleMap.put(3.7, 2.6);
+    hoodAngleMap.put(4.2, 2.6);
+    hoodAngleMap.put(5.4, 3.0);
+
+    flywheelSpeedMap.put(1.3, 51.9);
+    flywheelSpeedMap.put(2.0, 54.5);
+    flywheelSpeedMap.put(2.6, 57.1);
+    flywheelSpeedMap.put(3.1, 62.4);
+    flywheelSpeedMap.put(3.21, 62.2);
+    flywheelSpeedMap.put(3.7, 65.0);
+    flywheelSpeedMap.put(4.2, 67.6);
+    flywheelSpeedMap.put(5.4, 78.1);
 
+    /* Old shooter settings
     hoodAngleMap.put(0.99, 0.0);
     hoodAngleMap.put(1.62, 1.0);
     hoodAngleMap.put(1.94, 2.1);
     hoodAngleMap.put(2.53, 3.3);
     hoodAngleMap.put(3.00, 4.0);
     hoodAngleMap.put(3.51, 4.0);
-    hoodAngleMap.put(6.00, 6.1); // put in a value to max out the hood
+    hoodAngleMap.put(6.00, 6.1);
 
     flywheelSpeedMap.put(0.99, 57.7);
     flywheelSpeedMap.put(1.62, 64.3);
     flywheelSpeedMap.put(1.94, 64.7);
     flywheelSpeedMap.put(3.00, 74.5);
     flywheelSpeedMap.put(3.51, 80.0);
-    flywheelSpeedMap.put(6.00, 108.0); // put in a value to max out the hood
+    flywheelSpeedMap.put(6.00, 108.0);
+    */
   }
 
   public LaunchRequest createLaunchRequest(