From a15a6d08bf31c3ec80ed3e4327cbf214cc9781f5 Mon Sep 17 00:00:00 2001 From: Mira Date: Fri, 17 Apr 2026 18:42:57 -0600 Subject: [PATCH] demo --- .../java/frc4388/robot/RobotContainer.java | 289 +----------------- .../robot/constants/BuildConstants.java | 12 +- .../robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/shooter/Shooter.java | 42 +-- .../subsystems/shooter/ShooterConstants.java | 7 +- .../robot/subsystems/shooter/ShooterIO.java | 7 + .../robot/subsystems/shooter/ShooterReal.java | 9 + 7 files changed, 49 insertions(+), 319 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a04c1f8..ad95e0c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -263,94 +263,9 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - - - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.io.fixEncoder(); - m_robotShooter.io.updateGains(); - })); - - - // TEST-> the driver is holding the left trigger, drive slow and rotation up - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotSwerveDrive.setPercentOutput(0.10); - m_robotSwerveDrive.shiftUpRot(); - })) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.setToFast(); - // m_robotSwerveDrive.shiftDownRot(); - })); - - //TEST - > X positino on wheels - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.defenseXPosition(); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.stopDefenseXPosition(); - })); - - //TEST - > PID positinon - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); - - })); - - - - // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotSwerveDrive.setToSlow(); - })) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - // D-PAD fine alignment - new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) - .whileTrue(new RunCommand( - () -> m_robotSwerveDrive.driveFine( - new Translation2d( - 1, - Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) - ), - getDeadbandedDriverController().getRight(), 0.15 - ), m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - //Operator Controls - // new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Extended); - // })); - - - //allow shooting with right trigger - new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotShooter.allowShooting(); - })).onFalse(new InstantCommand(() -> { - m_robotShooter.denyShooting(); - })); -// manually shoot from climb post/ feed balls + // manually shoot from climb post/ feed balls new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.spinUpFeeding(); @@ -361,228 +276,56 @@ public class RobotContainer { })); - //set shooter ready (rev) with left trigger hold - new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - m_robotShooter.spinUpShooting(); - })) - .onFalse(new InstantCommand(() -> { - m_robotShooter.spinUpIdle(); - })); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.RetractedREMOVEME); - // })); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME); - // })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.ExtendingRolling); })); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.EncoderFix); - })); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.LabubuGrowl); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ExtendingRolling); - })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracting); - })) - .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot); })); - - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ExtendingIdle); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracting); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); - - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.RectractTorque); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.ExpelBalls); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); - - - // .onFalse(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Idle); - // })); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - // .whileTrue( - // new PathPlannerAuto("Right_AutoClimb") - // ) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - // .whileTrue( - // new PathPlannerAuto("Left_AutoClimb") - // ) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> { - // m_robotClimber.toggleDeployed(); - // })); - } private void configureSINGLEBindings() { + //Driver controls new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED))); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - - - - // TEST-> the driver is holding the left trigger, drive slow and rotation up - // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - // .onTrue(new InstantCommand(() -> { - // m_robotSwerveDrive.setToSlow(); - // m_robotSwerveDrive.shiftUpRot(); - // })) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.setToFast(); - // m_robotSwerveDrive.shiftDownRot(); - // })); - - //TEST - > X positino on wheels - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.defenseXPosition(); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.stopDefenseXPosition(); - })); - - //TEST - > PID positinon - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); - - })); - - - - // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotSwerveDrive.setToSlow(); - })) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ); - }, m_robotSwerveDrive) - ); - - // D-PAD fine alignment - new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) - .whileTrue(new RunCommand( - () -> m_robotSwerveDrive.driveFine( - new Translation2d( - 1, - Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) - ), - getDeadbandedDriverController().getRight(), 0.15 - ), m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - - //allow shooting with right trigger - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .onTrue(new InstantCommand(() -> { - m_robotShooter.allowShooting(); - })).onFalse(new InstantCommand(() -> { - m_robotShooter.denyShooting(); - })); - - //set shooter ready (rev) with left trigger hold - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + // manually shoot from climb post/ feed balls + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); + m_robotShooter.spinUpFeeding(); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); })); - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.ExtendedIdle); - // })) - // .onFalse(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Idle); - // })); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Retracting); - // })) - // .onFalse(new InstantCommand(() -> { - // m_robotIntake.setMode(IntakeMode.Idle); - // })); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.ExtendingRolling); + })); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot); + })); } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 07903b8..946d103 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 239; - public static final String GIT_SHA = "4d732970da558d90c4be1787635dc5786674bd27"; - public static final String GIT_DATE = "2026-04-10 13:59:21 MDT"; - public static final String GIT_BRANCH = "Encoder-Fix"; - public static final String BUILD_DATE = "2026-04-10 14:33:17 MDT"; - public static final long BUILD_UNIX_TIME = 1775853197175L; + public static final int GIT_REVISION = 241; + public static final String GIT_SHA = "6316daedf7fea6d03ec620bc433364a6881e3316"; + public static final String GIT_DATE = "2026-04-17 17:27:23 MDT"; + public static final String GIT_BRANCH = "demo"; + public static final String BUILD_DATE = "2026-04-17 18:31:24 MDT"; + public static final long BUILD_UNIX_TIME = 1776472284468L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index fce9bcd..62ea042 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -146,7 +146,7 @@ public class Intake extends SubsystemBase { break; case ArmIdleRollingNot: // io.armOutput(0); - io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, 0); break; case Bouncing: // io.setRollerOutput(state, 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 07ec15c..b31e451 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -210,43 +210,13 @@ public class Shooter extends SubsystemBase { } break; case ManualShoot: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_OVERRIDE_VELOCITY.get())); - - int bitmask2 = ( - (shooterButtonReady ? 1 : 0) + - (badShooterVelocity ? 2 : 0) - ); - - switch (bitmask2) { - case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, 0); - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); - break; - - case 0b001: // No errors and shoot button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); - break; - - case 0b010: // Bad shooter velocity, button is not pressed - case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, 0); - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); - break; - - // case 0b100: // Driver error, button is not pressed - // case 0b101: // Driver error, button is pressed - // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); - // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - // break; - - // case 0b110: // Driver error, bad shooter vel, button is not pressed - // case 0b111: // Driver error, bad shooter vel, button is pressed - // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); - // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - // break; + if(io.demoSpeed(state)){ + io.setIndexerOutput(state, -0.5); + }else{ + io.setIndexerOutput(state, 0.2); } - + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_OVERRIDE_VELOCITY.get())); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); break; case Idle: diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 35819cb..44538c2 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -21,13 +21,13 @@ public class ShooterConstants { public static final double SHOOTER_RADIUS = 2/39.37; public static final double INDEXER_RADIUS = 0.625/39.37; public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); - public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42); + public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -28); // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); - public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); - public static final ConfigurableDouble DEMO_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.0); + public static final ConfigurableDouble DEMO_PERCENT_OUTPUT = new ConfigurableDouble("Demo % output", 0.15); // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); @@ -42,6 +42,7 @@ public class ShooterConstants { public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1); + // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 96cbd19..5aeaab5 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -52,6 +52,13 @@ public interface ShooterIO { public default void updateInputs(ShooterState state) {} public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {} + public default boolean demoSpeed(ShooterState state) { + boolean demo = false; + if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor2TargetVelocity.in(RotationsPerSecond))) < 3)){ + demo = true; + } + return demo; + } public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index f92a760..0bac943 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -156,6 +156,15 @@ public class ShooterReal implements ShooterIO { state.indexerTargetOutput = percentOutput; m_indexerMotor.set(percentOutput); } + + // @Override + // public boolean demoSpeed(ShooterState state) { + // boolean demo = false; + // if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor2TargetVelocity.in(RotationsPerSecond))) < 3)){ + // demo = true; + // } + // return demo; + // } @Override