diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java b/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java index 2d4e6c7..e82b625 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +@Deprecated public final class IndexerFeedCommand extends Command { private final IndexerSubsystem indexer; private final Timer timer; diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java index 8d7f62c..1697f0b 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java @@ -1,8 +1,5 @@ package com.swrobotics.robot.commands; -import java.util.ArrayList; -import java.util.List; - import com.swrobotics.robot.RobotContainer; import com.swrobotics.robot.config.NTData; import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem; @@ -10,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; public final class RobotCommands { public static Command justShoot(RobotContainer robot) { @@ -19,7 +15,7 @@ public static Command justShoot(RobotContainer robot) { Commands.waitUntil(robot.shooter::isReadyToShoot) .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()), Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()), - new IndexerFeedCommand(robot.indexer) + robot.indexer.getFeedCommand() ); } @@ -57,7 +53,8 @@ public static Command aimAndShoot(RobotContainer robot, boolean waitForNote, boo // cmd = cmd.finallyDo(() -> robot.drive.setEstimatorIgnoreVision(true)); // } - cmd = cmd.andThen(new IndexerFeedCommand(robot.indexer)); + // cmd = cmd.andThen(new IndexerFeedCommand(robot.indexer)); + cmd = cmd.andThen(robot.indexer.getFeedCommand()); return cmd; } @@ -75,7 +72,7 @@ public static Command ejectHard(RobotContainer robot) { Commands.waitUntil(() -> robot.shooter.isReadyToShoot()) .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()), // Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()), - new IndexerFeedCommand(robot.indexer) + robot.indexer.getFeedCommand() ); } diff --git a/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java b/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java index 2e33059..90a3ec5 100644 --- a/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java +++ b/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java @@ -90,7 +90,6 @@ public static void updateFlywheels(double flywheelVelocity) { } public static void setShooting(boolean shooting) { - System.out.println("Shooting: " + shooting); if (shooting) { shooterPivot.setColor(new Color8Bit(Color.kBlueViolet)); view.setBackgroundColor(new Color8Bit(Color.kYellow)); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java index baccb11..7b2d1f8 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java @@ -8,9 +8,13 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; public final class IndexerSubsystem extends SubsystemBase { private final VictorSP topMotor = new VictorSP(IOAllocation.RIO.PWM_INDEXER_TOP_MOTOR); @@ -33,10 +37,15 @@ public IndexerSubsystem(IntakeSubsystem intake) { } public void setFeedToShooter(boolean feedToShooter) { + System.out.println("yah"); this.feedToShooter = feedToShooter; } public boolean hasPiece() { + if (RobotBase.isSimulation()) { + return false; + } + // Beam break is true when not blocked return !beamBreak.get(); // return !beamBreakDebounce.calculate(beamBreak.get()); @@ -174,4 +183,13 @@ public IndexerSubsystem setAutoReindexEnable(boolean autoReindexEnable) { this.autoReindexEnable = autoReindexEnable; return this; } + + public Command getFeedCommand() { + return Commands.run(() -> setFeedToShooter(true)) + .until(() -> !hasPiece()) + .andThen(new WaitCommand(NTData.INDEXER_FEED_ADDITIONAL_TIME.get())) + .andThen(Commands.runOnce(() -> setFeedToShooter(false))) + .withTimeout(1.0); // Emergency timeout for if the note does not exit the robot + + } } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java index 22c9d44..3f9cf73 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java @@ -171,6 +171,8 @@ public void periodic() { } else { activeMode.set("none"); } + + afterShootDelay.calculate(indexer.hasPiece()); } private void handleSpeaker() { @@ -180,7 +182,7 @@ private void handleSpeaker() { Translation2d robotVelocity = velocityTo(target); AimCalculator.Aim aim = TableAimCalculator.INSTANCE.calculateAim(distToSpeaker, robotVelocity.getX()); - if (/* afterShootDelay.calculate(indexer.hasPiece()) && */(aim != null)) { // Fixme + if (afterShootDelay.calculate(indexer.hasPiece()) && (aim != null)) { // Fixme pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU); flywheel.setTargetVelocity(aim.flywheelVelocity()); targetAim = aim; @@ -302,7 +304,6 @@ private Rotation2d getAimCorrection(Translation2d target, Supplier flyti Translation2d robotVelocity = velocityTo(target); double missAmount = flytime.get() * robotVelocity.getY(); - System.out.println(flytime.get()); double correctionRad = -Math.atan2(missAmount, distanceTo(target)) + Math.toRadians(otherCorrection.get()); return new Rotation2d(correctionRad); } @@ -320,7 +321,7 @@ public Command getSpeakerAimCommand() { } public Command getSpeakerSnapCommand() { - return Commands.defer(() -> getDynamicSnapCommand(getSpeakerPosition(), () -> distanceTo(getSpeakerPosition()) / 10.0, () -> 0.0), Set.of()); + return Commands.defer(() -> getDynamicSnapCommand(getSpeakerPosition(), () -> distanceTo(getSpeakerPosition()) / 10.0, () -> 0.0), Set.of()).alongWith(new PrintCommand("Shooting")); } public Command getLobAimCommand() {