Pitcheck/intake

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

@@ -183,6 +183,10 @@ public void placeGamePiecesOnField() {}
     SimulatedArena.overrideInstance(new EvergreenArena());
   }
 
+  Indexer indexer = null;
+  Intake intake = null;
+  Shooter shooter = null;
+
   // this is here because it doesn't like that the power distribution logger is never closed
   @SuppressWarnings("resource")
   public Robot() {
@@ -259,11 +263,11 @@ public Robot() {
       case COMP:
         indexer = new SpindexerSubsystem();
         intake = new LintakeSubsystem();
-        shooter = 
-        new TurretSubsystem(
-          ROBOT_MODE == RobotMode.REAL
-          ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore)
-          : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(),canivore));
+        shooter =
+            new TurretSubsystem(
+                ROBOT_MODE == RobotMode.REAL
+                    ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore)
+                    : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore));
 
         climber = new ClimberSubsystem(); // TODO climber
         break;
@@ -466,6 +470,12 @@ private void addAutos() {
     // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId());
     // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid());
     // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
+    autoChooser.addOption(
+        "Pitcheck/Intake ",
+        Commands.sequence(
+            intake.intake().withTimeout(1),
+            intake.rest().withTimeout(1),
+            intake.outtake().withTimeout(1)));
     haveAutosGenerated = true;
     System.out.println("Done generating autos");
   }

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

@@ -40,6 +40,10 @@ public void periodic() {
     Logger.processInputs("Intake", inputs);
   }
 
+  public double getRollerVoltage() {
+    return inputs.appliedVoltage;
+  }
+
   @Override
   public Command intake() {
     return this.run(() -> io.setRollerVoltage(10));
@@ -81,4 +85,9 @@ public static TalonFXConfiguration getIntakeConfig() {
 
     return config;
   }
+
+  @Override
+  public Command extend() {
+    return Commands.none();
+  }
 }

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

@@ -16,4 +16,6 @@ public interface Intake {
 
   /** Not running (set to 0) */
   public Command rest();
+
+  public Command extend();
 }

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

@@ -34,4 +34,15 @@ public Command rest() {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'rest'");
   }
+
+  @Override
+  public Command extend() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'extend'");
+  }
+
+  public static double getRollerVoltage() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getRollerVoltage'");
+  }
 }

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

@@ -36,7 +36,6 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {
 
   public TurretSubsystem(FlywheelIO flywheelIO) {
     this.flywheelIO = flywheelIO;
-
   }
 
   private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0);

src/main/java/frc/robot/utils/pitcheck/Pitcheck.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils.pitcheck;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.subsystems.intake.LintakeSubsystem;
+import java.util.function.BooleanSupplier;
+
+/** Add your docs here. */
+public class Pitcheck {
+
+  LintakeSubsystem intake = new LintakeSubsystem();
+  BooleanSupplier intakeRunning =
+      () -> LintakeSubsystem.getRollerVoltage() > 9.0 && LintakeSubsystem.getRollerVoltage() < 11.0;
+
+  public void pitcheck(Subsystem subsystem, Command command, BooleanSupplier endState) {
+    SmartDashboard.putData("intake", pitCheck(intake, intake.intake(), intakeRunning));
+  }
+
+  private Command pitCheck(Subsystem subsystem, Command command, BooleanSupplier endstate) {
+
+    return command.until(endstate);
+  }
+}