diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9136311..a9a600b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -233,20 +233,20 @@ public class RobotContainer { //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .whileTrue(new RunCommand(() -> { - // m_robotSwerveDrive.defenseXPosition(); - // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.stopDefenseXPosition(); - - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + + // .onTrue(new InstantCommand(() -> { + // currentPose = m_robotSwerveDrive.getCurrentPose(); + // })) + // .whileTrue(new RunCommand(() -> { + // m_stayInPosition.goToTargetPose(currentPose); + // }, m_robotSwerveDrive)) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.softStop(); })); @@ -352,6 +352,14 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotShooter.indexerStalled(); + })) + .onFalse(new InstantCommand(() -> { + m_robotShooter.spinUpIdle(); + })); + } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e8d2e92..b8c41f5 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 = 166; - public static final String GIT_SHA = "86b36a687e0338ed3ae5c6d035fb0f5b7404c27b"; - public static final String GIT_DATE = "2026-03-16 16:13:53 MDT"; + public static final int GIT_REVISION = 167; + public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094"; + public static final String GIT_DATE = "2026-03-18 13:39:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-18 13:27:40 MDT"; - public static final long BUILD_UNIX_TIME = 1773862060483L; + public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT"; + public static final long BUILD_UNIX_TIME = 1773871353359L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 5c2a083..3c54d38 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -117,6 +117,11 @@ public final class Constants { // Operator ready to shoot, but the driver conditions are bad public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; + // LED color for when the indexer was stuck on ball and going in reverse + public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET; + + // LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled + public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED; public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE; // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 8425c54..d293bd8 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -45,6 +45,13 @@ public class Intake extends SubsystemBase { io.setRollerOutput(state, 0); } + public double getRollerTarget() { + return state.rollerTargetOutput; + } + + public double getRollerSpeed() { + return state.rollerOutput; + } // public enum FieldZone { // // The robot should aim at the hub diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 0319706..e0d32a8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -59,7 +59,8 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { Shooting, ManualShoot, - Idle + Idle, + IndexerStall } private ShooterMode mode = ShooterMode.Idle; @@ -83,6 +84,10 @@ public class Shooter extends SubsystemBase { public void allowShooting() { shooterButtonReady = true; + } + + public void indexerStalled() { + mode = ShooterMode.IndexerStall; } public void denyShooting() { @@ -169,7 +174,7 @@ public class Shooter extends SubsystemBase { switch (bitmask) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; @@ -180,19 +185,19 @@ public class Shooter extends SubsystemBase { case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + 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 - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); break; case 0b110: // Driver error, bad shooter vel, button is not pressed case 0b111: // Driver error, bad shooter vel, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); break; } @@ -207,7 +212,7 @@ public class Shooter extends SubsystemBase { switch (bitmask2) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); break; @@ -218,7 +223,7 @@ public class Shooter extends SubsystemBase { case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; @@ -244,12 +249,16 @@ public class Shooter extends SubsystemBase { // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) ); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - - + case IndexerStall: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get())); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE); + break; } - + + io.motorStalled(state, m_Intake, m_robotLED); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index d486de3..4ef195a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -15,7 +15,7 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - public static final double SHOOTERMOTOR_GEAR_RATIO = 1.29; // TODO: supposed to be 9 rotations in to 7 out + public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out -- 0.77 or 1.29 public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 public static final double T_CONSTANT = 2; public static final double SHOOTER_RADIUS = 2/39.37; @@ -31,8 +31,8 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); - public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); - public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.4); + public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); @@ -56,8 +56,9 @@ public class ShooterConstants { // 0.318464*hubDistMeters + // 30.6293; double speed = - 5.6939*hubDistMeters + - 22.76545 + MODEL_TRIM.get(); + 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + MODEL_TRIM.get(); // double speed = // 0.00610938*hubDistMeters*hubDistMeters @@ -81,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.08) - .withKI(0.15) + .withKP(0.05) + .withKI(0.20) .withKD(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 444aeb7..b30bd2d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public interface ShooterIO { @AutoLog @@ -49,6 +51,7 @@ public interface ShooterIO { public default void updateInputs(ShooterState state) {} + public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {} 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 6ca3ce4..3f46ae0 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -8,6 +8,10 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Timer; +import frc4388.robot.constants.Constants; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public class ShooterReal implements ShooterIO { @@ -19,6 +23,12 @@ public class ShooterReal implements ShooterIO { VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); + private final Timer m_stallTimerShooter = new Timer(); + private final Timer m_stallTimerIndexer = new Timer(); + private final Timer m_stallTimerRoller = new Timer(); + private boolean m_shooterStalling = false; + private boolean m_indexerStalling = false; + private boolean m_rollerStalling = false; public ShooterReal( TalonFX shooter1Motor, @@ -41,6 +51,50 @@ public class ShooterReal implements ShooterIO { // m_indexerVelocity.Slot = 0; } + @Override + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + + // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 20) { + // if (!m_shooterStalling) { + // m_shooterStalling = true; + // m_stallTimerShooter.restart(); + // } + // if (m_stallTimerShooter.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_shooterStalling = false; + // m_stallTimerShooter.stop(); + // } + + // if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) { + // if (!m_indexerStalling) { + // m_indexerStalling = true; + // m_stallTimerIndexer.restart(); + // } + // if (m_stallTimerIndexer.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_indexerStalling = false; + // m_stallTimerIndexer.stop(); + // } + + // if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) { + // if (!m_rollerStalling) { + // m_rollerStalling = true; + // m_stallTimerRoller.restart(); + // } + // if (m_stallTimerRoller.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_rollerStalling = false; + // m_stallTimerRoller.stop(); + // } + + } + @Override public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index c471924..7e3ce17 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -410,12 +410,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); - double aimOffsetComplext = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); - + double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Offset Simple", aimOffset); - Logger.recordOutput("Offset Complex", aimOffsetComplext); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 0034c0d..5bd6369 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -73,6 +73,8 @@ public final class SwerveDriveConstants { public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; + public static ConfigurableDouble distanceTolerence = new ConfigurableDouble("Distance Tolerence", 0.5); + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); public static final class ModuleSpecificConstants { //2026