diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 1f1b6116..0971a6e4 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -37,7 +37,7 @@ public void robotInit() { @Override public void robotPeriodic() { - if (Arm.getInstance().getActualState() == Arm.State.FEED + if (Arm.getInstance().getState() == Arm.State.FEED && Arm.getInstance().atTarget() && !Shooter.getInstance().hasNote() && Intake.getInstance().hasNote() diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 96453de6..cab92f2c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -2,35 +2,41 @@ import com.ctre.phoenix6.Utils; import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.commands.arm.ArmDisableOverride; -import com.stuypulse.robot.commands.arm.ArmEnableOverride; +import com.stuypulse.robot.commands.arm.ArmSetShootHeightToHigh; +import com.stuypulse.robot.commands.arm.ArmSetShootHeightToLow; import com.stuypulse.robot.commands.arm.ArmToAmp; +import com.stuypulse.robot.commands.arm.ArmToClimbing; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.arm.ArmToFerry; -import com.stuypulse.robot.commands.arm.ArmToLobFerry; -import com.stuypulse.robot.commands.arm.ArmToLowFerry; import com.stuypulse.robot.commands.arm.ArmToPreClimb; import com.stuypulse.robot.commands.arm.ArmToSpeaker; -import com.stuypulse.robot.commands.arm.ArmToSpeakerHigh; -import com.stuypulse.robot.commands.arm.ArmToSpeaker; -import com.stuypulse.robot.commands.arm.ArmToSpeakerLow; import com.stuypulse.robot.commands.arm.ArmToStow; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterAutoShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.commands.shooter.ShooterScoreAmp; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment; +import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb; import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerLow; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerHigh; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerLow; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerry; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerryManual; +import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndScoreSpeaker; +import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -101,59 +107,82 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - driver.getRightBumper().whileTrue(new SwerveDriveXMode()); + configureOperatorBindings(); + configureDriverBindings(); + } - driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); - driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver)); + private void configureDriverBindings() { + driver.getRightMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); - driver.getLeftTriggerButton() + // intake field relative + driver.getRightTriggerButton() + .onTrue(new ArmToFeed()) + .whileTrue(new SwerveDriveDriveToNote(driver)) .whileTrue(new IntakeAcquire() .andThen(new BuzzController(driver)) ); - driver.getLeftBumper() + // intake robot relative + driver.getLeftTriggerButton() + .onTrue(new ArmToFeed()) + .whileTrue(new IntakeAcquire() + .andThen(new BuzzController(driver)) + ) + .whileTrue(new SwerveDriveDriveRobotRelative(driver)); + + // deacquire + driver.getDPadLeft() .whileTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); - - driver.getRightTriggerButton() - .whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterAutoShoot()) - ) - .onFalse(new ShooterFeederStop()) - .onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - driver.getTopButton() - .onTrue(new ArmToSpeaker()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); + driver.getDPadUp().onTrue(new ArmSetShootHeightToHigh()); + driver.getDPadDown().onTrue(new ArmSetShootHeightToLow()); - driver.getTopButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); + // speaker align and score + driver.getRightBumper() + .whileTrue(new SwerveDriveDriveAndScoreSpeaker(driver)); - driver.getLeftButton() - .onTrue(new ArmToAmp()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - - driver.getLeftButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); + // ferry align and shoot + // move to back of controller + driver.getRightStickButton() + .whileTrue(new SwerveDriveDriveAndFerry(driver)); + // arm to amp and alignment + driver.getLeftBumper() + .onTrue(new ArmToAmp()) + .onTrue(new SwerveDriveDriveAlignedAmp(driver)); + + // manual speaker at subwoofer + // score amp + // rebind to a button on the back later + driver.getLeftStickButton() + .whileTrue(new ConditionalCommand( + new ShooterScoreAmp(), + new ArmToSubwooferShot() + .andThen(new ShooterScoreSpeaker()), + () -> Arm.getInstance().getState() == Arm.State.AMP)) + .onFalse(new ConditionalCommand( + new ShooterFeederStop(), + new ShooterStop(), + () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + + // manual ferry + driver.getTopButton() + .whileTrue(new SwerveDriveDriveAndFerryManual(driver)); + driver.getRightButton() - .whileTrue(new ArmToFerry()) - .onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote())); - + .onTrue(new ArmToPreClimb()); driver.getRightButton() - .debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME) - .onTrue(new ArmEnableOverride()) - .onFalse(new ArmDisableOverride()); - - driver.getBottomButton().onTrue(new ArmToFeed()); + .debounce(Settings.Driver.TIME_UNTIL_HOLD) + .whileTrue(new SwerveDriveDriveToChain()); - driver.getDPadUp().whileTrue(new ArmToPreClimb()); - driver.getDPadDown().whileTrue(new ArmToStow()); + driver.getBottomButton().whileTrue(new SwerveDriveDriveToChain()); + driver.getLeftMenuButton().onTrue(new ArmToClimbing()); + } + private void configureOperatorBindings() { + operator.getLeftTriggerButton().whileTrue(new IntakeDeacquire()); + operator.getRightTriggerButton().whileTrue(new IntakeAcquire()); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java index 9c89f6d4..408e153d 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmEnableOverride.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToHigh.java @@ -1,19 +1,19 @@ package com.stuypulse.robot.commands.arm; import com.stuypulse.robot.subsystems.arm.Arm; - import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ArmEnableOverride extends InstantCommand{ +public class ArmSetShootHeightToHigh extends InstantCommand{ private final Arm arm; - public ArmEnableOverride() { - this.arm = Arm.getInstance(); + public ArmSetShootHeightToHigh() { + arm = Arm.getInstance(); + addRequirements(arm); } @Override public void initialize() { - arm.setOverriding(true); + arm.setShootHeightHigh(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java similarity index 55% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java index 0babc58f..975ab487 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmDisableOverride.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetShootHeightToLow.java @@ -1,19 +1,19 @@ package com.stuypulse.robot.commands.arm; import com.stuypulse.robot.subsystems.arm.Arm; - import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ArmDisableOverride extends InstantCommand{ +public class ArmSetShootHeightToLow extends InstantCommand{ private final Arm arm; - public ArmDisableOverride() { - this.arm = Arm.getInstance(); + public ArmSetShootHeightToLow() { + arm = Arm.getInstance(); + addRequirements(arm); } @Override public void initialize() { - arm.setOverriding(false); + arm.setShootHeightLow(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java index 3f94c534..af42b187 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmSetState.java @@ -16,6 +16,6 @@ public ArmSetState(Arm.State state) { @Override public void initialize() { - arm.setRequestedState(state); + arm.setState(state); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java new file mode 100644 index 00000000..62eef26b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToClimbing extends ArmSetState{ + + public ArmToClimbing(){ + super(Arm.State.CLIMBING); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java index d39a34f3..4b64cffb 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java @@ -1,33 +1,10 @@ package com.stuypulse.robot.commands.arm; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.util.StopWatch; -import edu.wpi.first.wpilibj2.command.InstantCommand; +public class ArmToFerry extends ArmSetState{ -public class ArmToFerry extends InstantCommand{ - - private final Arm arm; - private final StopWatch stopWatch; - private double lastClick; - - public ArmToFerry() { - arm = Arm.getInstance(); - stopWatch = new StopWatch(); - lastClick = 0; - addRequirements(arm); - } - - @Override - public void initialize() { - if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setRequestedState(Arm.State.LOB_FERRY); - } - else { - arm.setRequestedState(Arm.State.LOW_FERRY); - } - lastClick = stopWatch.getTime(); + public ArmToFerry(){ + super(Arm.State.SPEAKER); } - -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java deleted file mode 100644 index d4ff5c97..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToLobFerry extends ArmSetState{ - - public ArmToLobFerry(){ - super(Arm.State.LOB_FERRY); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java deleted file mode 100644 index 902563b9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToLowFerry extends ArmSetState{ - - public ArmToLowFerry(){ - super(Arm.State.LOW_FERRY); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java index eafa55aa..2358b8e7 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java @@ -1,33 +1,10 @@ package com.stuypulse.robot.commands.arm; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.util.StopWatch; -import edu.wpi.first.wpilibj2.command.InstantCommand; +public class ArmToSpeaker extends ArmSetState{ -public class ArmToSpeaker extends InstantCommand{ - - private final Arm arm; - private final StopWatch stopWatch; - private double lastClick; - - public ArmToSpeaker() { - arm = Arm.getInstance(); - stopWatch = new StopWatch(); - lastClick = 0; - addRequirements(arm); - } - - @Override - public void initialize() { - if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) { - arm.setRequestedState(Arm.State.SPEAKER_HIGH); - } - else { - arm.setRequestedState(Arm.State.SPEAKER_LOW); - } - lastClick = stopWatch.getTime(); + public ArmToSpeaker(){ + super(Arm.State.SPEAKER); } - -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java deleted file mode 100644 index bf5c1f2b..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToSpeakerHigh extends ArmSetState{ - - public ArmToSpeakerHigh(){ - super(Arm.State.SPEAKER_HIGH); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java deleted file mode 100644 index 6ce31e50..00000000 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.arm; - -import com.stuypulse.robot.subsystems.arm.Arm; - -public class ArmToSpeakerLow extends ArmSetState{ - - public ArmToSpeakerLow(){ - super(Arm.State.SPEAKER_LOW); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java new file mode 100644 index 00000000..a30531a2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToSubwooferShot.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToSubwooferShot extends ArmSetState{ + + public ArmToSubwooferShot(){ + super(Arm.State.SUBWOOFER_SHOT); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java deleted file mode 100644 index 1377464c..00000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.subsystems.arm.Arm; - -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -/** - * automatically determines how to shoot depending on the state of the arm - */ -public class ShooterAutoShoot extends InstantCommand{ - - @Override - public void initialize() { - switch (Arm.getInstance().getActualState()) { - case AMP: - CommandScheduler.getInstance().schedule(new ShooterScoreAmp()); - break; - case SPEAKER_HIGH: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - case SPEAKER_LOW: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - case LOW_FERRY: - CommandScheduler.getInstance().schedule(new ShooterFerry()); - break; - case LOB_FERRY: - CommandScheduler.getInstance().schedule(new ShooterFerry()); - break; - default: - CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); - break; - } - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java new file mode 100644 index 00000000..c7bd69b7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerryManual.java @@ -0,0 +1,16 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ShooterFerryManual extends SequentialCommandGroup { + + public ShooterFerryManual() { + addCommands( + new ShooterSetRPM(Settings.Shooter.MANUAL_FERRY), + new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)), + new ShooterFeederShoot() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java index b64e5773..82ee480c 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java @@ -1,19 +1,20 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ShooterScoreAmp extends SequentialCommandGroup { +public class ShooterScoreAmp extends InstantCommand { + + private final Shooter shooter; public ShooterScoreAmp() { - addCommands( - new ShooterFeederDeacquire(), - new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), - new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME), - new ShooterFeederStop() - ); + shooter = Shooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.feederDeacquire(); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java deleted file mode 100644 index c2feab30..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java +++ /dev/null @@ -1,46 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.stuylib.input.Gamepad; - -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -/** - * automatically determines how to align depending on the state of the arm - * - *

buzzes controller if arm is not in one of the scoring/ferrying positions - */ -public class SwerveDriveAutoAlignment extends InstantCommand{ - - private final Gamepad driver; - - public SwerveDriveAutoAlignment(Gamepad driver) { - this.driver = driver; - } - - @Override - public void initialize() { - switch (Arm.getInstance().getActualState()) { - case AMP: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAmpAligned(driver)); - break; - case SPEAKER_HIGH: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerHigh(driver)); - break; - case SPEAKER_LOW: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedSpeakerLow(driver)); - break; - case LOW_FERRY: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLowFerry(driver)); - break; - case LOB_FERRY: - CommandScheduler.getInstance().schedule(new SwerveDriveDriveAlignedLobFerry(driver)); - break; - default: - CommandScheduler.getInstance().schedule(new BuzzController(driver)); - break; - } - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java new file mode 100644 index 00000000..fd9b91eb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java @@ -0,0 +1,67 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveRobotRelative extends Command { + + private final SwerveDrive swerve; + + private final Gamepad driver; + + private final SwerveRequest.RobotCentric drive; + + private final VStream speed; + private final IStream turn; + + public SwerveDriveDriveRobotRelative(Gamepad driver) { + swerve = SwerveDrive.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND.get()), + x -> SLMath.spow(x, Turn.POWER.get()), + x -> x * Turn.MAX_TELEOP_TURNING.get(), + new LowPassFilter(Turn.RC.get())); + + this.driver = driver; + + drive = new SwerveRequest.RobotCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + addRequirements(swerve); + } + + @Override + public void execute() { + swerve.setControl(drive.withVelocityX(speed.get().y) + .withVelocityY(-speed.get().x) + .withRotationalRate(turn.get()) + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java new file mode 100644 index 00000000..62f07b38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToChain extends Command { + + private final SwerveDrive swerve; + + private final SwerveRequest.FieldCentric drive; + + private Pose2d trapPose; + + public SwerveDriveDriveToChain() { + swerve = SwerveDrive.getInstance(); + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + addRequirements(swerve); + } + + @Override + public void initialize() { + trapPose = Field.getClosestAllianceTrapPose(swerve.getPose()); + } + + @Override + public void execute() { + Rotation2d translationAngle = trapPose.getTranslation().minus(swerve.getPose().getTranslation()).getAngle(); + Translation2d translation = new Translation2d(Alignment.INTO_CHAIN_SPEED.get(), translationAngle); + + swerve.setControl(drive.withVelocityX(translation.getX()) + .withVelocityY(translation.getY()) + .withRotationalRate(0) + ); + } + + private double getDistanceToTrap() { + return swerve.getPose().getTranslation().minus(trapPose.getTranslation()).getNorm(); + } + + @Override + public boolean isFinished() { + return false; + // return getDistanceToTrap() <= Alignment.TRAP_CLIMB_DISTANCE.get(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java new file mode 100644 index 00000000..b4ce4e79 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java @@ -0,0 +1,114 @@ +package com.stuypulse.robot.commands.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToClimb extends Command { + + private final SwerveDrive swerve; + + private final SwerveRequest.FieldCentric drive; + + private final HolonomicController controller; + + private final FieldObject2d targetPose2d; + + private Pose2d targetPose; + + private final double distance; + + public SwerveDriveDriveToClimb() { + this(Alignment.CLIMB_SETUP_DISTANCE.get()); + } + + public SwerveDriveDriveToClimb(double distance) { + swerve = SwerveDrive.getInstance(); + + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + this.distance = distance; + + targetPose2d = swerve.getField().getObject("Target Pose"); + + controller = new HolonomicController( + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new AnglePIDController(Rotation.kP, Rotation.kI, Rotation.kD)); + } + + private Pose2d getTargetPose() { + Pose2d closestTrap = Field.getClosestAllianceTrapPose(swerve.getPose()); + Translation2d offsetTranslation = new Translation2d(distance, closestTrap.getRotation()); + + return new Pose2d(closestTrap.getTranslation().plus(offsetTranslation), closestTrap.getRotation()); + } + + private boolean shouldSlow() { + double toTarget = getTargetPose() + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm(); + + return toTarget < Units.inchesToMeters(14.0); + } + + @Override + public void initialize() { + targetPose = getTargetPose(); + } + + @Override + public void execute() { + targetPose2d.setPose(targetPose); + controller.update(targetPose, swerve.getPose()); + + double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); + if (Math.abs(rotation) < Alignment.Rotation.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) + .clamp(2.0); + + if (shouldSlow()) + speed = speed.clamp(0.3); + + SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); + + swerve.setControl(drive.withVelocityX(speed.x) + .withVelocityY(speed.y) + .withRotationalRate(rotation) + ); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + Field.clearFieldObject(targetPose2d); + } +} + + diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java similarity index 95% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index 584cd721..7f340daa 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; @@ -50,7 +50,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kP) + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); @@ -73,6 +73,11 @@ protected double getAngleError() { return controller.getError().getRotation2d().getDegrees(); } + @Override + public boolean isFinished() { + return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble(); + } + @Override public void execute() { swerve.setControl( @@ -86,9 +91,4 @@ public void execute() { ) ); } - - @Override - public boolean isFinished() { - return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DEADBAND.getAsDouble(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java similarity index 72% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java index a86f2f84..d6a93797 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; @@ -6,9 +6,9 @@ import edu.wpi.first.math.geometry.Rotation2d; -public class SwerveDriveDriveAmpAligned extends SwerveDriveDriveAligned { +public class SwerveDriveDriveAlignedAmp extends SwerveDriveDriveAligned { - public SwerveDriveDriveAmpAligned(Gamepad driver) { + public SwerveDriveDriveAlignedAmp(Gamepad driver) { super(driver); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java similarity index 92% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java index 860c2c41..941187fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; @@ -14,7 +14,6 @@ public SwerveDriveDriveAlignedLobFerry(Gamepad driver) { super(driver); } - // returns pose of close amp corner private Translation2d getAmpCornerPose() { Translation2d targetPose = Robot.isBlue() ? new Translation2d(0.0, Field.WIDTH - 1.5) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java similarity index 81% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java index fc75a02f..701eacbc 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; @@ -14,8 +14,7 @@ public SwerveDriveDriveAlignedLowFerry(Gamepad driver) { super(driver); } - // returns pose of close amp corner - private Translation2d getTargetPose() { + private Translation2d getAmpCornerPose() { Translation2d targetPose = Robot.isBlue() ? new Translation2d(0.0, Field.WIDTH - 1.5) : new Translation2d(0.0, 1.5); @@ -25,11 +24,11 @@ private Translation2d getTargetPose() { @Override protected Rotation2d getTargetAngle() { - return SwerveDrive.getInstance().getPose().getTranslation().minus(getTargetPose()).getAngle(); + return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle(); } @Override protected double getDistanceToTarget() { - return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getTargetPose()); + return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java index e45037ab..7e61e70b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerHigh.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerHigh.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java similarity index 94% rename from src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java index 75b9e500..9586608d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedSpeakerLow.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeakerLow.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.swerve; +package com.stuypulse.robot.commands.swerve.driveAligned; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java new file mode 100644 index 00000000..b10c1dd0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerry.java @@ -0,0 +1,53 @@ +package com.stuypulse.robot.commands.swerve.driveAndShoot; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.commands.shooter.ShooterFerry; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class SwerveDriveDriveAndFerry extends SwerveDriveDriveAndShoot{ + + public SwerveDriveDriveAndFerry(Gamepad driver) { + super(driver, Arm.State.FERRY); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle() + : SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose()); + } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return shooter.getFerrySpeeds(); + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterFerry()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java new file mode 100644 index 00000000..456148c8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndFerryManual.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.swerve.driveAndShoot; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.commands.shooter.ShooterFerry; +import com.stuypulse.robot.commands.shooter.ShooterFerryManual; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class SwerveDriveDriveAndFerryManual extends SwerveDriveDriveAndShoot{ + + public SwerveDriveDriveAndFerryManual(Gamepad driver) { + super(driver, Arm.State.FERRY); + } + + private Translation2d getAmpCornerPose() { + Translation2d targetPose = Robot.isBlue() + ? new Translation2d(0.0, Field.WIDTH - 1.5) + : new Translation2d(0.0, 1.5); + + return targetPose; + } + + @Override + protected Rotation2d getTargetAngle() { + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle() + : Field.getManualFerryPosition().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + return Field.getManualFerryPosition().getDistance(getAmpCornerPose()); + } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return Settings.Shooter.MANUAL_FERRY; + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterFerryManual()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java new file mode 100644 index 00000000..0b0c5493 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java @@ -0,0 +1,50 @@ +package com.stuypulse.robot.commands.swerve.driveAndShoot; + +import com.stuypulse.robot.commands.shooter.ShooterFerry; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class SwerveDriveDriveAndScoreSpeaker extends SwerveDriveDriveAndShoot{ + + public SwerveDriveDriveAndScoreSpeaker(Gamepad driver) { + super(driver, Arm.State.SPEAKER); + } + + @Override + protected Rotation2d getTargetAngle() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return Arm.getInstance().getShootHeight() == Arm.ShootHeight.LOW + ? speakerPose.minus(currentPose).getAngle() + : speakerPose.minus(currentPose).getAngle().plus(Rotation2d.fromDegrees(180)); + } + + @Override + protected double getDistanceToTarget() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return currentPose.getDistance(speakerPose); + } + + @Override + protected ShooterSpeeds getTargetSpeeds() { + return Settings.Shooter.SPEAKER; + } + + @Override + public void execute() { + super.execute(); + if (isAligned.get()) { + CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java new file mode 100644 index 00000000..4eb7600d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java @@ -0,0 +1,122 @@ +package com.stuypulse.robot.commands.swerve.driveAndShoot; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.stuypulse.stuylib.util.AngleVelocity; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public abstract class SwerveDriveDriveAndShoot extends Command { + + protected final SwerveDrive swerve; + protected final Arm arm; + protected final Shooter shooter; + + protected final Arm.State armState; + + protected final Gamepad driver; + protected final VStream velocity; + protected final IStream angleVelocity; + + protected final BStream isAligned; + + protected final SwerveRequest.FieldCentric drive; + + protected final AngleController controller; + + public SwerveDriveDriveAndShoot(Gamepad driver, Arm.State armState) { + swerve = SwerveDrive.getInstance(); + shooter = Shooter.getInstance(); + arm = Arm.getInstance(); + this.driver = driver; + this.armState = armState; + + velocity = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) + .setOutputFilter(x -> -x); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + + AngleVelocity derivative = new AngleVelocity(); + + angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle()))) + .filtered(new LowPassFilter(Assist.ANGLE_DERIV_RC)) + // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST + // so that angular velocity doesn't oscillate + .filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST)) + .filtered(x -> -x); + + addRequirements(swerve); + } + + protected abstract Rotation2d getTargetAngle(); + protected abstract double getDistanceToTarget(); + + protected abstract ShooterSpeeds getTargetSpeeds(); + + private boolean isAligned() { + return controller.isDoneDegrees(Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void initialize() { + arm.setState(armState); + shooter.setTargetSpeeds(getTargetSpeeds()); + } + + @Override + public void execute() { + swerve.setControl( + drive.withVelocityX(velocity.get().y) + .withVelocityY(-velocity.get().x) + .withRotationalRate( + angleVelocity.get() + + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(swerve.getPose().getRotation())) + ) + ); + } + + @Override + public void end(boolean interrupted) { + CommandScheduler.getInstance().schedule(new ShooterStop().onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java new file mode 100644 index 00000000..79b4c7dd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java @@ -0,0 +1,116 @@ +package com.stuypulse.robot.commands.swerve.noteAlignment; + +import static com.stuypulse.robot.constants.Settings.Alignment.*; +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.HolonomicController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + private final SwerveDrive swerve; + private final NoteVision vision; + private final Intake intake; + + private final SwerveRequest.FieldCentric drive; + + private final VStream speed; + + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(Gamepad driver) { + this.swerve = SwerveDrive.getInstance(); + this.vision = NoteVision.getInstance(); + this.intake = Intake.getInstance(); + + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + controller = new HolonomicController( + new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), + new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), + new AnglePIDController(NoteDetection.Rotation.kP, NoteDetection.Rotation.kI, NoteDetection.Rotation.kD)); + + SmartDashboard.putData("Note Detection/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + Translation2d targetTranslation = swerve.getPose() + .getTranslation().plus( + new Translation2d(Settings.CENTER_TO_FRONT_OF_INTAKE, 0) + .rotateBy(swerve.getPose().getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + if (vision.hasNoteData()) { + ChassisSpeeds speeds = controller.update(targetPose, swerve.getPose()); + + if (vision.withinIntakePath()) + speeds.omegaRadiansPerSecond = Math.signum(speeds.omegaRadiansPerSecond) * Math.toRadians(5.0); + + // drive to note + swerve.setControl(drive.withVelocityX(speeds.vxMetersPerSecond) + .withVelocityY(speeds.vyMetersPerSecond) + .withRotationalRate(speeds.omegaRadiansPerSecond) + ); + } else { + swerve.setControl(drive.withVelocityX(speed.get().y) + .withVelocityY(-speed.get().x) + .withRotationalRate(0) + ); + } + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get() || intake.hasNote(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index df90c72a..275446b2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -28,7 +28,7 @@ public interface Limelight { "samera0", //tower camera new Pose3d( new Translation3d(-11.25, -3.333797, 23.929362), - new Rotation3d(0, 15, 0) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(0)) ), "29", 3000 @@ -38,7 +38,7 @@ public interface Limelight { "samera1", //electronic plate camera new Pose3d( new Translation3d(0, 4.863591, 19.216471), - new Rotation3d(0, 80, 0) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) ), "96", 3001 diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 31a4d931..ba389642 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -269,16 +269,13 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria /*** FERRYING ***/ - // MIDLINE: 8.27 - - // BEFORE: 9 - // MIDLINE + 60% of WING_TO_CENTERLINE distance - double FERRY_SHOT_MAX_X = LENGTH / 2.0 + WING_TO_CENTERLINE * 0.60; - double FERRY_SHOT_MIN_X = 6.0; - double FERRY_SHOT_MIN_FAR_X = 8.5; + public static Translation2d getManualFerryPosition() { + return Robot.isBlue() + ? new Translation2d(LENGTH / 2 + WING_TO_CENTERLINE * 0.8, 1) + : new Translation2d(LENGTH / 2 - WING_TO_CENTERLINE * 0.8, 1); + } - double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0; - double FERRY_CUTOFF = 1.8; + // MIDLINE: 8.27 /**** EMPTY FIELD POSES ****/ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6a5916ad..f0af98e9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -68,11 +68,9 @@ public interface Arm { SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17); - SmartNumber PODIUM_SHOT_ANGLE = new SmartNumber("Arm/Podium Shot Angle", -60); - - double SHOULD_RETURN_TO_FEED_TIME = 1.0; - double EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP = 2.5; + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17); + SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -60); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -99,7 +97,7 @@ public interface Encoder { } public interface Intake { - double INTAKE_ACQUIRE_SPEED = 0.8; + double INTAKE_ACQUIRE_SPEED = 0.6; double INTAKE_DEACQUIRE_SPEED = 1.0; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; @@ -113,7 +111,7 @@ public interface Intake { public interface Shooter { double FEEDER_INTAKE_SPEED = 0.25; double FEEDER_DEAQUIRE_SPEED = 0.5; - double FEEDER_SHOOT_SPEED = 0.25; + double FEEDER_SHOOT_SPEED = 0.4; boolean ALWAYS_KEEP_AT_SPEED = false; @@ -125,6 +123,11 @@ public interface Shooter { new SmartNumber("Shooter/Speaker RPM differential", 500) ); + ShooterSpeeds MANUAL_FERRY = new ShooterSpeeds( + new SmartNumber("Shooter/Manual Ferry RPM", 4800), + new SmartNumber("Shooter/Manual Ferry RPM differential", 500) + ); + // Different falling debounce is used to detect note shooting; SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.01); SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.005); @@ -183,7 +186,7 @@ public interface Assist { double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); + SmartNumber kP = new SmartNumber("SwerveAssist/kP", 5.0); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); @@ -296,7 +299,8 @@ public interface Alignment { SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); - SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); + SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); + SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); double MAX_ALIGNMENT_SPEED = 2.5; @@ -310,26 +314,13 @@ public interface Rotation { SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 6.0); SmartNumber kI = new SmartNumber("Alignment/Rotation/kI", 0.0); SmartNumber kD = new SmartNumber("Alignment/Rotation/kD", 0.4); - } - public interface Shoot { - public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); - SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); - } - - public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); - SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); - } + SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } } public interface Driver { - double HOLD_TO_OVERRIDE_TIME = 0.55; - double DOUBLE_CLICK_TIME_BETWEEN_CLICKS = 0.5; + double TIME_UNTIL_HOLD = 0.7; public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.015); @@ -342,7 +333,8 @@ public interface Drive { } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.015); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.03); + SmartNumber DISABLE_ALIGNMENT_DEADBAND = new SmartNumber("Driver Settings/Turn/Disable Alignment Deadband", 0.08); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index 842ee3a1..81b73014 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,13 +1,10 @@ package com.stuypulse.robot.subsystems.arm; -import com.stuypulse.robot.subsystems.shooter.Shooter; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Arm extends SubsystemBase { - /* SINGLETON */ private static final Arm instance; static { @@ -20,54 +17,53 @@ public static Arm getInstance(){ public enum State { AMP, - SPEAKER_HIGH, - SPEAKER_LOW, - LOW_FERRY, - LOB_FERRY, + SUBWOOFER_SHOT, + SPEAKER, + FERRY, FEED, STOW, PRE_CLIMB, + CLIMBING, RESETTING } - protected State requestedState; - protected State actualState; - protected boolean overriding; + public enum ShootHeight { + HIGH, + LOW + } + + protected State state; + protected ShootHeight shootHeight; protected Arm() { - requestedState = State.RESETTING; - actualState = State.RESETTING; - overriding = false; + state = State.RESETTING; + shootHeight = ShootHeight.LOW; } - public void setRequestedState(State state) { - this.requestedState = state; + public void setState(State state) { + this.state = state; } - public State getRequestedState() { - return this.requestedState; + public State getState() { + return this.state; } - public State getActualState() { - return this.actualState; + public void setShootHeightHigh() { + this.shootHeight = ShootHeight.HIGH; } - public void setOverriding(boolean overriding) { - this.overriding = overriding; + public void setShootHeightLow() { + this.shootHeight = ShootHeight.LOW; } - public boolean isOverriding() { - return this.overriding; + public ShootHeight getShootHeight() { + return this.shootHeight; } public abstract boolean atTarget(); - public abstract boolean shouldReturnToFeed(); - @Override public void periodic() { - SmartDashboard.putBoolean("Arm/Is Overriding", overriding); - SmartDashboard.putString("Arm/Requested State", getRequestedState().toString()); - SmartDashboard.putString("Arm/Actual State", getActualState().toString()); + SmartDashboard.putString("Arm/State", state.toString()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 26b8c0d4..f957622a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -39,9 +39,6 @@ public class ArmImpl extends Arm { private final Controller controller; private final MotionProfile motionProfile; - private final BStream shouldGoBackToFeed; - private final BStream shouldGoBackToFeedFromAmp; - protected ArmImpl() { super(); leftMotor = new CANSparkMax(Ports.Arm.LEFT_MOTOR, MotorType.kBrushless); @@ -66,14 +63,6 @@ protected ArmImpl() { .add(new ArmEncoderFeedforward(Settings.Arm.Feedforward.kG)) .add(new PIDController(Settings.Arm.PID.kP, Settings.Arm.PID.kI, Settings.Arm.PID.kD)) .setSetpointFilter(motionProfile); - - shouldGoBackToFeed = BStream.create(() -> !Shooter.getInstance().hasNote()) - .filtered(new BDebounce.Rising(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME)); - - shouldGoBackToFeedFromAmp = BStream.create(shouldGoBackToFeed) - .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP)) - .and(BStream.create(() -> !overriding) - .filtered(new BDebounce.Rising(Settings.Arm.EXTRA_TIME_BEFORE_RETURNING_TO_FEED_FOR_AMP))); } @Override @@ -81,29 +70,26 @@ public boolean atTarget() { return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.getAsDouble(); } - @Override - public boolean shouldReturnToFeed() { - return shouldGoBackToFeed.get(); - } - private double getTargetDegrees() { - switch (actualState) { + switch (state) { case AMP: return Settings.Arm.AMP_ANGLE.getAsDouble(); - case SPEAKER_LOW: - return getSpeakerAngle(true); - case SPEAKER_HIGH: - return getSpeakerAngle(false); - case LOW_FERRY: - return Settings.Arm.LOW_FERRY_ANGLE.getAsDouble(); - case LOB_FERRY: - return Settings.Arm.LOB_FERRY_ANGLE.getAsDouble(); + case SUBWOOFER_SHOT: + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + case SPEAKER: + return getSpeakerAngle(shootHeight == ShootHeight.LOW); + case FERRY: + return shootHeight == ShootHeight.LOW + ? Settings.Arm.LOW_FERRY_ANGLE.getAsDouble() + : Settings.Arm.LOB_FERRY_ANGLE.getAsDouble(); case FEED: return Settings.Arm.FEED_ANGLE.getAsDouble(); case STOW: return Settings.Arm.MIN_ANGLE.getAsDouble(); case PRE_CLIMB: return Settings.Arm.PRE_CLIMB_ANGLE.getAsDouble(); + case CLIMBING: + return Settings.Arm.POST_CLIMB_ANGLE.get(); default: return Settings.Arm.MIN_ANGLE.getAsDouble(); } @@ -150,7 +136,7 @@ private double getSpeakerAngle(boolean getLowAngle) { } catch (Exception exception) { exception.printStackTrace(); - return Settings.Arm.PODIUM_SHOT_ANGLE.get(); + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); } } @@ -169,38 +155,15 @@ public void periodic() { if (bumpSwitchTriggered.get()) { armEncoder.setPosition(Settings.Arm.MIN_ANGLE.get()/360); - if (actualState == State.RESETTING) { - actualState = State.FEED; - requestedState = State.FEED; + if (state == State.RESETTING) { + state = State.FEED; } } - if (actualState == State.RESETTING) { + if (state == State.RESETTING) { setVoltage(-1.5); } else { - if (overriding - || requestedState == State.STOW - || requestedState == State.PRE_CLIMB - || requestedState == State.FEED - || requestedState == State.RESETTING - || Shooter.getInstance().hasNote() - ) { - this.actualState = this.requestedState; - } - else if (shouldGoBackToFeed.get()) { - if (actualState == State.AMP) { - if (shouldGoBackToFeedFromAmp.get()) { - requestedState = State.FEED; - actualState = State.FEED; - } - } - else if (actualState != State.FEED) { - requestedState = State.FEED; - actualState = State.FEED; - } - } - controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); setVoltage(controller.getOutput()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 41f6b74d..cb173b5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -22,8 +22,8 @@ public static Shooter getInstance() { private final SmartNumber rightTargetRPM; public Shooter() { - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0); } public void setTargetSpeeds(ShooterSpeeds speeds) { @@ -55,6 +55,4 @@ public void stop() { public abstract void feederStop(); public abstract boolean hasNote(); - - public abstract boolean noteShot(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 59854b2e..70d8e88b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -77,7 +77,7 @@ protected ShooterImpl() { Motors.Shooter.LEFT_SHOOTER.configure(leftMotor); Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); - Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); + Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); } private double getLeftShooterRPM() { @@ -127,11 +127,6 @@ public boolean hasNote() { return hasNote.get(); } - @Override - public boolean noteShot() { - return getLeftTargetRPM() > 0 && getRightTargetRPM() > 0 && hasNote.get() == false; - } - @Override public ShooterSpeeds getFerrySpeeds() { Translation2d ferryZone = Robot.isBlue() @@ -140,7 +135,7 @@ public ShooterSpeeds getFerrySpeeds() { double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); - if (Arm.getInstance().getActualState() == Arm.State.LOB_FERRY) { + if (Arm.getInstance().getShootHeight() == Arm.ShootHeight.HIGH) { double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); return new ShooterSpeeds(targetRPM, 500); } @@ -154,20 +149,6 @@ public ShooterSpeeds getFerrySpeeds() { public void periodic () { super.periodic(); - Arm.State armState = Arm.getInstance().getActualState(); - - if (Settings.Shooter.ALWAYS_KEEP_AT_SPEED) { - if (armState == Arm.State.SPEAKER_HIGH || armState == Arm.State.SPEAKER_LOW) { - setTargetSpeeds(Settings.Shooter.SPEAKER); - } - else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { - setTargetSpeeds(getFerrySpeeds()); - } - else { - setTargetSpeeds(Settings.Shooter.SPEAKER); - } - } - if (getLeftTargetRPM() == 0) { leftMotor.set(0); } @@ -199,8 +180,6 @@ else if (armState == Arm.State.LOW_FERRY || armState == Arm.State.LOB_FERRY) { SmartDashboard.putNumber("Shooter/Left Current", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Right Current", rightMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); - - SmartDashboard.putBoolean("Shooter/Note Shot", noteShot()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java index 5b74f6c5..9920ca15 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java @@ -76,6 +76,16 @@ private boolean hasNoteDataRaw() { return false; } + /** + * Get the estimated pose of the note. + * + * @return the estimated pose of the note + */ + @Override + public Translation2d getEstimatedNotePose() { + return notePose.get().getTranslation2d(); + } + @Override public Translation2d getRobotRelativeNotePose() { Translation2d sum = new Translation2d(); @@ -83,7 +93,6 @@ public Translation2d getRobotRelativeNotePose() { for (Limelight limelight : limelights) { sum = sum.plus(new Translation2d( limelight.getDistanceToNote(), - // Angle.fromRotation2d(Rotation2d.fromDegrees(limelight.getXAngle()).plus(new Rotation2d(Math.PI))).getRotation2d())); Rotation2d.fromDegrees(limelight.getXAngle()))); } @@ -98,6 +107,8 @@ private void updateNotePose() { Translation2d sum = new Translation2d(); for (Limelight limelight : limelights) { + SwerveDrive swerve = SwerveDrive.getInstance(); + Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); Translation2d robotToNote = limelightToNote @@ -105,8 +116,8 @@ private void updateNotePose() { .rotateBy(limelight.getRobotRelativePose().getRotation().toRotation2d()); Translation2d fieldToNote = robotToNote - .rotateBy(SwerveDrive.getInstance().getPose().getRotation()) - .plus(SwerveDrive.getInstance().getPose().getTranslation()); + .rotateBy(swerve.getPose().getRotation()) + .plus(swerve.getPose().getTranslation()); sum = sum.plus(fieldToNote); } @@ -122,7 +133,7 @@ public void periodic() { limelights[i].updateData(); } - note.setPose(new Pose2d(SwerveDrive.getInstance().getPose().getTranslation().plus(getRobotRelativeNotePose()), new Rotation2d())); + note.setPose(new Pose2d(getEstimatedNotePose(), new Rotation2d())); if (hasNoteDataRaw()) updateNotePose(); updateTelemetry(); @@ -133,6 +144,8 @@ private void updateTelemetry() { SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); SmartDashboard.putNumber("Note Detection/Distance", limelights[0].getDistanceToNote()); + SmartDashboard.putNumber("Note Detection/Estimated X", getEstimatedNotePose().getX()); + SmartDashboard.putNumber("Note Detection/Estimated Y", getEstimatedNotePose().getY()); } else { SmartDashboard.putNumber("Note Detection/X Angle", 0); SmartDashboard.putNumber("Note Detection/Y Angle", 0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java index 4eeca3ed..813a2531 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java @@ -31,10 +31,11 @@ public final boolean withinIntakePath() { public abstract boolean hasNoteData(); + public abstract Translation2d getEstimatedNotePose(); + public abstract Translation2d getRobotRelativeNotePose(); public final Rotation2d getRotationToNote() { - // return Angle.fromRotation2d(getRobotRelativeNotePose().getAngle()).add(Angle.fromDegrees(180)).getRotation2d(); return getRobotRelativeNotePose().getAngle(); } @@ -42,6 +43,5 @@ public final Rotation2d getRotationToNote() { public void periodic() { SmartDashboard.putBoolean("Note Detection/Has Note Data", hasNoteData()); SmartDashboard.putBoolean("Note Detection/Is in Intake Path", withinIntakePath()); - SmartDashboard.putNumber("Note Detection/angle to note", getRotationToNote().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/util/HolonomicController.java b/src/main/java/com/stuypulse/robot/util/HolonomicController.java new file mode 100644 index 00000000..9074fe58 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/HolonomicController.java @@ -0,0 +1,79 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class HolonomicController implements Sendable { + private PIDController xController; + private PIDController yController; + private AnglePIDController angleController; + + public HolonomicController(PIDController xController, PIDController yController, AnglePIDController angleController) { + this.xController = xController; + this.yController = yController; + this.angleController = angleController; + } + + public ChassisSpeeds update(Pose2d setpoint, Pose2d measurement) { + xController.update(setpoint.getX(), measurement.getX()); + yController.update(setpoint.getY(), measurement.getY()); + angleController.update( + Angle.fromRotation2d(setpoint.getRotation()), + Angle.fromRotation2d(measurement.getRotation())); + + return getOutput(); + } + + public ChassisSpeeds getOutput() { + return ChassisSpeeds.fromFieldRelativeSpeeds( + xController.getOutput(), + yController.getOutput(), + angleController.getOutput(), + angleController.getMeasurement().getRotation2d()); + } + + public ChassisSpeeds getError() { + return ChassisSpeeds.fromFieldRelativeSpeeds( + xController.getError(), + yController.getError(), + angleController.getError().toDegrees(), + angleController.getMeasurement().getRotation2d()); + } + + public boolean isDone(double xToleranceMeters, double yToleranceMeters, double angleToleranceDegrees) { + return xController.isDone(xToleranceMeters) + && yController.isDone(yToleranceMeters) + && angleController.isDoneDegrees(angleToleranceDegrees); + } + + public HolonomicController setTranslationConstants(double p, double i, double d) { + xController.setPID(p, i, d); + yController.setPID(p, i, d); + return this; + } + + public HolonomicController setRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Holonomic Controller"); + builder.addDoubleProperty("Angle Setpoint (degrees)", () -> angleController.getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Measurement (degrees)", () -> angleController.getMeasurement().toDegrees(), null); + builder.addDoubleProperty("X Setpoint (meters)", () -> xController.getSetpoint(), null); + builder.addDoubleProperty("X Measurement (meters)", () -> xController.getMeasurement(), null); + builder.addDoubleProperty("Y Setpoint (meters)", () -> yController.getSetpoint(), null); + builder.addDoubleProperty("Y Measurement (meters)", () -> yController.getMeasurement(), null); + builder.addDoubleProperty("X Error (meters)", () -> xController.getError(), null); + builder.addDoubleProperty("Y Error (meters)", () -> yController.getError(), null); + builder.addDoubleProperty("Angle Error (degrees)", () -> angleController.getError().getRotation2d().getDegrees(), null); + } +}