From 71df44aedc24c3ab5ba40c4e02372a9d3e2e3788 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 20 Jul 2024 22:03:38 -0400 Subject: [PATCH 1/2] add amp alignment command --- .../swerve/SwerveDriveDriveAmpAligned.java | 25 +++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 2 ++ 2 files changed, 27 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java new file mode 100644 index 0000000..a86f2f8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAmpAligned.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.input.Gamepad; +import edu.wpi.first.math.geometry.Rotation2d; + + +public class SwerveDriveDriveAmpAligned extends SwerveDriveDriveAligned { + + public SwerveDriveDriveAmpAligned(Gamepad driver) { + super(driver); + } + + + @Override + protected Rotation2d getTargetAngle() { + return Rotation2d.fromDegrees(Robot.isBlue() ? 90 : 270); + } + + @Override + protected double getDistanceToTarget() { + return Settings.Swerve.Assist.AMP_WALL_SCORE_DISTANCE; + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8c78b38..e5152e0 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -172,6 +172,8 @@ public interface Swerve { public interface Assist { SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); + double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); + // angle PID SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); From 30cd0a93d42334b261f5e0f1d626d71c8ea5e6fb Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 20 Jul 2024 22:15:24 -0400 Subject: [PATCH 2/2] auto alignment command based on arm state --- .../com/stuypulse/robot/RobotContainer.java | 28 ++--------- .../swerve/SwerveDriveAutoAlignment.java | 46 +++++++++++++++++++ 2 files changed, 50 insertions(+), 24 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c73ef3a..a8ed4bb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ 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.swerve.SwerveDriveAutoAlignment; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh; @@ -91,33 +92,15 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - driver.getRightMenuButton().whileTrue(new SwerveDriveXMode()); - driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative()); + driver.getRightBumper().whileTrue(new SwerveDriveXMode()); + + driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver)); driver.getLeftTriggerButton() .whileTrue(new IntakeAcquire() .andThen(new BuzzController(driver)) ); - // driver.getLeftBumper() - // .whileTrue(new WaitUntilCommand(() -> !shooter.hasNote()) - // .andThen(new ArmToFeed() - // .andThen(new WaitUntilCommand(intake::hasNote).alongWith(new ArmWaitUntilAtTarget())) - // .andThen(new ShooterAcquireFromIntake()) - // .andThen(new BuzzController(driver)) - // ) - // ); - - // driver.getLeftTriggerButton() - // .whileTrue(new ArmToFeed().onlyIf(() -> !shooter.hasNote())) - // .whileTrue(new IntakeAcquire() - // .andThen(new BuzzController(driver)) - // .andThen(new WaitUntilCommand(() -> Arm.getInstance().getState() == Arm.State.FEED)) - // .andThen(new ArmWaitUntilAtTarget()) - // .andThen(new ShooterAcquireFromIntake().onlyIf(() -> !shooter.hasNote())) - // .andThen(new BuzzController(driver))) - // .onFalse(new IntakeStop()); - driver.getLeftBumper() .whileTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); @@ -127,9 +110,6 @@ private void configureButtonBindings() { .andThen(new ShooterAutoShoot()) ); - driver.getRightBumper() - .onTrue(new SwerveDriveDriveAlignedSpeakerLow(driver)); - driver.getTopButton() .onTrue(new ConditionalCommand( new ArmToSpeakerHigh(), diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java new file mode 100644 index 0000000..218f440 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoAlignment.java @@ -0,0 +1,46 @@ +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().getState()) { + 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; + } + } +}