diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 75ece46..5560250 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -145,7 +145,7 @@ public final class Constants { public static final double INDEXER_LOADING_WAIT = 0.5; public static final double INDEXER_HUMAN_LOADING_WAIT = 1; - public static final double INDEXER_SHOOTER_REJECTION_TIME = 0.5; + public static final double INDEXER_SHOOTER_REJECTION_TIME = 2; // put back to .5 when using full battery public static final double INDEXER_INTAKE_REJECTION_TIME = 1; public static final I2C.Port INDEXER_COLOR = I2C.Port.kMXP; @@ -171,7 +171,7 @@ public final class Constants { public static final double SHOOTER_P = 0.0002; public static final double SHOOTER_FEEDFORWARD = 0.00218; - public static final double SHOOTER_TOLERANCE = 100; + public static final double SHOOTER_TOLERANCE = 0.20; public static final InterpolatingTreeMap SHOOTER_SPEED_MAP = new InterpolatingTreeMap(); static { diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 5d3efd9..7d3b0e7 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -91,6 +91,7 @@ public void periodic() { Logger.getInstance().recordOutput("Indexer/InstantShooterBall", ballCurrentlyAtShooter); getAllianceColorFMS(); Logger.getInstance().recordOutput("Indexer/AllianceColor", allianceColor.toString()); + Logger.getInstance().recordOutput("Indexer/RejectiomTimerState", rejectionTimerStarted); switch (state) { case FIELD2: stomachMotorOff(); @@ -336,12 +337,13 @@ else if (rejectionTimerStarted && rejectionTimer.hasElapsed(Constants.INDEXER_SH shootBall(); intakeBall(); rejectionTimerStarted = false; - state = State.INTAKE1; + state = State.FIELD2; } + /* This shouldn't happen, it would be at a state with 2 balls not 1 else if (ballCurrentlyAtIntake) { intakeBall(); state = State.REJECT1INTAKE1; - } + } */ else if (rejectionTimerStarted && rejectionTimer.hasElapsed(Constants.INDEXER_SHOOTER_REJECTION_TIME)) { shootBall(); rejectionTimerStarted = false; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c1992d2..bbb99f3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -67,7 +67,8 @@ public void setSetpoint(double setpoint, boolean forced) { this.setpoint = setpoint; } else { - forcedSetpoint = setpoint; + this.setpoint = setpoint; + // forcedSetpoint = setpoint; } } @@ -76,7 +77,7 @@ public double getVelocity() { } public boolean isAtSetpoint() { - return BetterMath.epsilonEquals(inputs.velocityRotPerMin, Constants.SHOOTER_TOLERANCE) || (Constants.ROBOT_DEMO_MODE && RobotBase.isSimulation()); + return BetterMath.epsilonEquals(inputs.velocityRotPerMin, this.setpoint, Constants.SHOOTER_TOLERANCE*this.setpoint) || (Constants.ROBOT_DEMO_MODE && RobotBase.isSimulation()); } }