Demooo stuff

FRC-7525 · Oct 20, 2024 · 925bb93 · 925bb93
1 parent 81d3361
commit 925bb93
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 21 deletions.
diff --git 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
@@ -54,6 +54,7 @@ public class Drive extends Subsystem<DriveStates> {
 	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
@@ -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
@@ -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
@@ -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, () ->