diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f19ffee..636e1361 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -59,6 +59,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.pitcheck.Pitcheck; import java.util.Optional; import java.util.Set; import org.ironmaple.simulation.SimulatedArena; @@ -211,6 +212,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() { @@ -374,6 +379,7 @@ public Robot() { // now that we've assigned the correct subsystems based on robot edition, we can pass them into // the superstructure superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); + autoAimReq = driver .leftBumper() @@ -504,6 +510,21 @@ public Robot() { .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); + SmartDashboard.putData( + "intakeRoller", + Commands.sequence( + Pitcheck.pitCheck( + intake.intake(), () -> MathUtil.isNear(7.0, this.intake.getRollerVoltage(), 1.0)), + Pitcheck.pitCheck( + intake.outtake(), + () -> MathUtil.isNear(-11.0, this.intake.getRollerVoltage(), 1.0)), + Pitcheck.pitCheck( + intake.rest(), () -> MathUtil.isNear(0.0, this.intake.getRollerVoltage(), 0.5)))); + SmartDashboard.putData( + "spindexer", + Commands.sequence( + Pitcheck.pitCheck( + indexer.kick(), () -> MathUtil.isNear(7.0, this.indexer.getRollerVoltage(), 1.0)))); // SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); // SmartDashboard.putData("Test Shot", shooter.testShoot()); diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index b27c08fa..2a304b7e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -20,4 +20,6 @@ public interface Indexer { /** Not running (set to 0) */ public Command rest(); + + public double getRollerVoltage(); } diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index c17d8d62..049bce24 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -156,4 +156,8 @@ public Command runRollerSysId() { indexRollerSysid.dynamic(Direction.kForward), indexRollerSysid.dynamic(Direction.kReverse)); } + + public double getRollerVoltage() { + return rollerInputs.appliedVoltage; + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 9ab3e2e4..90a2b260 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -153,4 +153,8 @@ public Command runRollerSysId() { indexRollerSysid.dynamic(Direction.kForward), indexRollerSysid.dynamic(Direction.kReverse)); } + + public double getRollerVoltage() { + return rollerInputs.appliedVoltage; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 3d3256c3..566c6104 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -22,7 +22,7 @@ public class FintakeSubsystem extends SubsystemBase implements Intake { public static final double GEAR_RATIO = 2.0; private RollerIO io; - private RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); + private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged(); CANrangeIOInputsAutoLogged canrangeInputs = new CANrangeIOInputsAutoLogged(); @@ -45,8 +45,8 @@ public FintakeSubsystem(RollerIO io, CANBus canbus) { @Override public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); + io.updateInputs(rollerIOInputs); + Logger.processInputs("Intake", rollerIOInputs); canrangeIO.updateInputs(canrangeInputs); Logger.processInputs("Indexer/First Beambreak", canrangeInputs); @@ -98,4 +98,8 @@ public static TalonFXConfiguration getIntakeConfig() { public boolean beambreak() { return canrangeInputs.isDetected; } + + public double getRollerVoltage() { + return rollerIOInputs.appliedVoltage; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e193211b..2f91a7fb 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -19,4 +19,6 @@ public interface Intake { /** for controller rumble */ public boolean beambreak(); + + public double getRollerVoltage(); } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 0eb9a618..260076f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -159,4 +159,13 @@ public static TalonFXConfiguration getRollerMotorConfig() { public boolean beambreak() { return canRangeIOInputs.isDetected; } + + public Command extend() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'extend'"); + } + + public double getRollerVoltage() { + return rollerIOInputs.appliedVoltage; + } } 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 00000000..9f34c1a6 --- /dev/null +++ b/src/main/java/frc/robot/utils/pitcheck/Pitcheck.java @@ -0,0 +1,48 @@ +// 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.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; + +/** Add your docs here. */ +public class Pitcheck { + + // LintakeSubsystem intake = new LintakeSubsystem(null, null, null); + // SpindexerSubsystem spindexer = new SpindexerSubsystem(null, null, null); + // BooleanSupplier intakeRunningForward = () -> MathUtil.isNear(7.0, intake.getRollerVoltage(), + // 1.0); + // BooleanSupplier intakeRunningBackward = + // () -> MathUtil.isNear(-11.0, intake.getRollerVoltage(), 1.0); + // BooleanSupplier intakeRest = () -> MathUtil.isNear(0.0, intake.getRollerVoltage(), 0.5); + // BooleanSupplier spindexerRunningForward = + // () -> MathUtil.isNear(7.0, spindexer.getRollerVoltage(), 1.0); + + // public void pitcheck() { + // SmartDashboard.putData( + // "intakeRoller", + // Commands.sequence( + // pitCheck(intake.intake(), intakeRunningForward), + // pitCheck(intake.outtake(), intakeRunningBackward), + // pitCheck(intake.rest(), intakeRest))); + // SmartDashboard.putData( + // "spindexer", Commands.sequence(pitCheck(spindexer.kick(), spindexerRunningForward))); + // } + + public static Command pitCheck(Command command, BooleanSupplier endstate) { + + return command + .until( + () -> { + if (endstate.getAsBoolean()) { + + return true; + } + return false; + }) + .withTimeout(2.0) + .andThen(() -> System.out.println("Pitcheck success: " + endstate.getAsBoolean())); + } +}