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 8ddc8bc..b21e2cd 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java @@ -25,7 +25,6 @@ public void initialize() { @Override public void execute() { indexer.setFeedToShooter(true); - SimView.setShooting(true); if (!indexer.hasPiece() && !hasStartedTimer) { timer.start(); hasStartedTimer = true; @@ -35,7 +34,6 @@ public void execute() { @Override public void end(boolean interrupted) { indexer.setFeedToShooter(false); - SimView.setShooting(false); } @Override diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java index c3397b6..5008388 100644 --- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java +++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java @@ -15,11 +15,11 @@ import com.swrobotics.robot.subsystems.climber.ClimberArm; import com.swrobotics.robot.subsystems.speaker.IntakeSubsystem; import com.swrobotics.robot.subsystems.speaker.PivotSubsystem; -import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem; import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator; import com.swrobotics.robot.subsystems.swerve.SwerveDrive; import com.swrobotics.robot.subsystems.swerve.SwerveDrive.TurnRequest; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -101,7 +101,7 @@ public ControlBoard(RobotContainer robot) { .onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF))); - forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).debounce(0.1).whileTrue(robot.shooter.getSpeakerCommand()); + forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).whileTrue(robot.shooter.getSpeakerCommand()); new Trigger(operator.start::isPressed) .onTrue(Commands.runOnce(robot.indexer::beginReverse)) @@ -110,11 +110,12 @@ public ControlBoard(RobotContainer robot) { new Trigger(() -> driver.leftBumper.isPressed()).whileTrue(robot.shooter.getLobCommand()).whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter)); - new Trigger(() -> operator.y.isPressed()).whileTrue(robot.shooter.getAmpCommand()); + new Trigger(() -> operator.y.isPressed()).debounce(0.2, DebounceType.kFalling).whileTrue(robot.shooter.getAmpCommand()); new Trigger(() -> operator.b.isPressed()).debounce(0.075).whileTrue(Commands.runOnce(() -> robot.indexer.setFeedToShooter(true))).onFalse(Commands.runOnce(() -> robot.indexer.setFeedToShooter(false))); - new Trigger(() -> operator.b.isPressed() && !driverWantsAim()).debounce(0.075).whileTrue(robot.shooter.getPoopCommand()); + // Shooter will check that it would otherwise be idle + new Trigger(() -> operator.b.isPressed()).debounce(0.075).whileTrue(robot.shooter.getPoopCommand()); new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive)); } @@ -184,10 +185,6 @@ public void periodic() { climbState = ClimbState.IDLE; robot.indexer.endReverse(); robot.intake.setReverse(false); - robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop - - // robot.drive.setEstimatorIgnoreVision(false && DriverStation.isAutonomous()); - return; } 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 6ef732a..5ea67c9 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 @@ -4,6 +4,8 @@ import com.swrobotics.lib.net.NTString; import com.swrobotics.robot.config.IOAllocation; import com.swrobotics.robot.config.NTData; +import com.swrobotics.robot.logging.SimView; + import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -32,6 +34,7 @@ public IndexerSubsystem(IntakeSubsystem intake) { public void setFeedToShooter(boolean feedToShooter) { this.feedToShooter = feedToShooter; + SimView.setShooting(feedToShooter); } public boolean hasPiece() { 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 582417f..8308524 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 @@ -8,10 +8,7 @@ import com.swrobotics.mathlib.MathUtil; import com.swrobotics.robot.config.NTData; import com.swrobotics.robot.logging.FieldView; -import com.swrobotics.robot.logging.SimView; import com.swrobotics.robot.subsystems.speaker.aim.AimCalculator; -import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator; -import com.swrobotics.robot.subsystems.speaker.aim.LobCalculator; import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator; import com.swrobotics.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.math.filter.Debouncer; @@ -20,20 +17,12 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public final class ShooterSubsystem extends SubsystemBase { - public enum FlywheelControl { - SHOOT, - POOP, - IDLE, - REVERSE - } - private static final Pose2d blueSpeakerPose = new Pose2d(Units.inchesToMeters(6), 5.5475, new Rotation2d(0)); // Opening // extends // 18" @@ -49,10 +38,6 @@ public enum FlywheelControl { private boolean isPreparing; private AimCalculator.Aim targetAim; // Target aim is null if not currently aiming - private AimCalculator aimCalculator; - private final TableAimCalculator tableAimCalculator; - - private FlywheelControl flywheelControl; public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) { this.pivot = new PivotSubsystem(); @@ -61,15 +46,9 @@ public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) { this.drive = drive; this.indexer = indexer; - // aimCalculator = new ManualAimCalculator(); - tableAimCalculator = new TableAimCalculator(); - aimCalculator = tableAimCalculator; - NTData.SHOOTER_AFTER_DELAY .nowAndOnChange((delay) -> afterShootDelay = new Debouncer(delay, Debouncer.DebounceType.kFalling)); - flywheelControl = FlywheelControl.SHOOT; - setDefaultCommand(getIdleCommand().ignoringDisable(true)); handleIdle(); } @@ -226,7 +205,8 @@ public Command getIdleCommand() { } private void handleAmp() { - + pivot.setTargetAngle(Math.toRadians(NTData.SHOOTER_AMP_ANGLE.get()) / MathUtil.TAU); + flywheel.setTargetVelocity(NTData.SHOOTER_AMP_VELOCITY.get()); } public Command getAmpCommand() { @@ -251,8 +231,11 @@ public Command getFeedCommand() { } public Command getPoopCommand() { - return Commands.run(() -> flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()), this) - .withName("Shooter Poop"); + Command poop = Commands.run(() -> flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()), this) + .withName("Shooter Poop"); + + // Every other command takes priority other than idle + return Commands.either(poop.asProxy(), Commands.none(), () -> getCurrentCommand() == getDefaultCommand()); } NTDouble pctErr = new NTDouble("Shooter/Debug/Percent Error", 0); @@ -268,18 +251,10 @@ public boolean isReadyToShoot() { return isPreparing && flywheel.isReadyToShoot() && pivot.isAtSetpoint(); } - public void setFlywheelControl(FlywheelControl flywheelControl) { - this.flywheelControl = flywheelControl; - } - public double getFlywheelPercentOfTarget() { return flywheel.getPercentOfTarget(); } - public void setTempAimCalculator(AimCalculator calculator) { - aimCalculator = calculator; - } - public AimCalculator.Aim getTargetAim() { return targetAim; } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AmpAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AmpAimCalculator.java deleted file mode 100644 index b230739..0000000 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AmpAimCalculator.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.swrobotics.robot.subsystems.speaker.aim; - -import com.swrobotics.robot.config.NTData; - -public class AmpAimCalculator implements AimCalculator { - public static final AmpAimCalculator INSTANCE = new AmpAimCalculator(); - - @Override - public Aim calculateAim(double distanceToSpeaker) { - // Always returns the same aim regardless of distance to speaker - return new Aim( - NTData.SHOOTER_AMP_VELOCITY.get(), - Math.toRadians(NTData.SHOOTER_AMP_ANGLE.get()), - distanceToSpeaker); - } -}