diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7fe5e15..ca7460b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -104,7 +104,7 @@ public static final class Intake { public static final double OFF = 0.0; // In DEGREES (Pivot setpoints) - public static final double DOWN = -233; + public static final double DOWN = -228; public static final double IN = 0; // In RPS (Spinner Setpoints) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bfed3cc..6b93fa8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -54,6 +54,7 @@ public class Drive extends Subsystem { private PIDController headingCorrectionController; private boolean headingCorrectionEnabled; private boolean fieldRelativeEnabled = true; + // private Timer demoTimer = true; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -103,6 +104,15 @@ public Drive( addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SLOW_MODE, () -> Constants.controller.getLeftBumperPressed() ); + + addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.DEMO_SPIN, () -> + Constants.controller.getXButtonPressed() + ); + + addTrigger(DriveStates.DEMO_SPIN, DriveStates.REGULAR_DRIVE, () -> + Constants.controller.getXButtonPressed() + ); + // addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SPEED_MAXXING, // () -> Constants.controller.getLeftBumperPressed()); @@ -121,12 +131,14 @@ public void runState() { // good) // Logger.recordOutput("driveState", getState()); // TODO: TURN ON HEADING CORRECTION LATER + + if (DriverStation.isTeleop() && getState() != DriveStates.AUTO_ALIGN) { drive( this, () -> Constants.controller.getLeftY(), () -> Constants.controller.getLeftX(), - () -> Constants.controller.getRightX(), + () -> Constants.controller.getRightX() + (getState() == DriveStates.DEMO_SPIN ? 0.15 : 0), getState().getRotationModifier(), getState().getTranslationModifier(), headingCorrectionEnabled, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveStates.java b/src/main/java/frc/robot/subsystems/drive/DriveStates.java index 5f07146..c1431b4 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveStates.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveStates.java @@ -5,6 +5,7 @@ public enum DriveStates implements SubsystemStates { REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), + DEMO_SPIN("Spinning for Demo", 0.5, 0.2), SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 017d1f7..14c9930 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -32,8 +32,8 @@ public class IntakeIOSparkMax implements IntakeIO { boolean usingInPID; - private DigitalInput beamBreak; - Debouncer beamBreakDebouncer; + // private DigitalInput beamBreak; + // Debouncer beamBreakDebouncer; public IntakeIOSparkMax() { intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); @@ -48,11 +48,11 @@ public IntakeIOSparkMax() { pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - beamBreak = new DigitalInput(Constants.Intake.BEAM_BREAK_PORT); - beamBreakDebouncer = new Debouncer( - Constants.Intake.DEBOUNCE_TIME, - Debouncer.DebounceType.kBoth - ); + // beamBreak = new DigitalInput(Constants.Intake.BEAM_BREAK_PORT); + // beamBreakDebouncer = new Debouncer( + // Constants.Intake.DEBOUNCE_TIME, + // Debouncer.DebounceType.kBoth + // ); } public void setSetpoints( @@ -124,8 +124,8 @@ public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); } - @Override - public boolean noteDetected() { - return !beamBreakDebouncer.calculate(beamBreak.get()); - } + // @Override + // public boolean noteDetected() { + // return !beamBreakDebouncer.calculate(beamBreak.get()); + // } } diff --git a/src/main/java/frc/robot/subsystems/manager/Manager.java b/src/main/java/frc/robot/subsystems/manager/Manager.java index 570dbf3..a1d2c9d 100644 --- a/src/main/java/frc/robot/subsystems/manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/manager/Manager.java @@ -159,15 +159,18 @@ public Manager() { addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Constants.controller.getBButtonPressed() ); - addTrigger( - ManagerStates.INTAKING, - ManagerStates.IDLE, - () -> - (intakeSubsystem.noteDetected() && - intakeSubsystem.nearSetPoint() && - useBeamBreaksVal) || - Constants.controller.getBButtonPressed() + addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> + Constants.controller.getBButtonPressed() ); + // addTrigger( + // ManagerStates.INTAKING, + // ManagerStates.IDLE, + // () -> + // (intakeSubsystem.noteDetected() && + // intakeSubsystem.nearSetPoint() && + // useBeamBreaksVal) || + // Constants.controller.getBButtonPressed() + // ); // Amping (Y) addTrigger(ManagerStates.IDLE, ManagerStates.STAGING_AMP, () ->