diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 54cd82c..3383376 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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"); } diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index b4dcfd1..e0693a6 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 9a6196f..1aa57a2 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/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(); } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index a400db1..52ccb3e 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/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'"); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index f0b3051..a159762 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/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); diff --git a/src/main/java/frc/robot/utils/pitcheck/Pitcheck.java b/src/main/java/frc/robot/utils/pitcheck/Pitcheck.java new file mode 100644 index 0000000..d5057cc --- /dev/null +++ b/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); + } +}