Skip to content
This repository has been archived by the owner on Nov 24, 2024. It is now read-only.

Commit

Permalink
flatten
Browse files Browse the repository at this point in the history
  • Loading branch information
@memori034
memori034 committed Oct 11, 2024
1 parent 64647b2 commit 7cd877a
Showing 1 changed file with 169 additions and 130 deletions.
299 changes: 169 additions & 130 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -504,141 +504,180 @@ public void configureSwerve() {
// Azimuth angle bindings. isRed == true for red alliance presets. isRed != true
// for blue.
if (isRed) {
// azi source red
{
driver
.rightBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSourceRed,
Timer.getFPGATimestamp()))));
}

// azi amp red
{
driver
.a()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziAmpRed,
Timer.getFPGATimestamp()))));
}

// azi feeder red
{
driver
.povRight()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziFeederRed,
Timer.getFPGATimestamp()))));
}
}

else {
// azi source blue
{
driver
.rightBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSourceBlue,
Timer.getFPGATimestamp()))));
}

// azi amp blue
{
driver
.a()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziAmpBlue,
Timer.getFPGATimestamp()))));
}

// azi feeder blue
{
driver
.povRight()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziFeederBlue,
Timer.getFPGATimestamp()))));
}
}

/* Universal Azimuth Bindings */

// azi subwoofer front
{
driver
.rightBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSourceRed,
Timer.getFPGATimestamp()))));
driver
.a()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziAmpRed,
Timer.getFPGATimestamp()))));
driver
.povRight()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziFeederRed,
Timer.getFPGATimestamp()))));
} else {
driver
.rightBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSourceBlue,
Timer.getFPGATimestamp()))));
driver
.a()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziAmpBlue,
Timer.getFPGATimestamp()))));
.leftBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferFront,
Timer.getFPGATimestamp()))));
}

// azi subwoofer left
{
driver
.povRight()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziFeederBlue,
Timer.getFPGATimestamp()))));
.x()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferLeft,
Timer.getFPGATimestamp()))));
}

// Universal azimuth bindings
// azi subwoofer right
{
driver
.b()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferRight,
Timer.getFPGATimestamp()))));
}

driver
.leftBumper()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferFront,
Timer.getFPGATimestamp()))));
driver
.x()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferLeft,
Timer.getFPGATimestamp()))));
driver
.b()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziSubwooferRight,
Timer.getFPGATimestamp()))));
driver
.povDown()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziCleanUp,
Timer.getFPGATimestamp()))));
// azi cleanup
{
driver
.povDown()
.whileTrue(
drivetrain.applyRequest(
() ->
drive
.withVelocityX(driver.getLeftY() * SlowMaxSpeed)
.withVelocityY(driver.getLeftX() * SlowMaxSpeed)
.withRotationalRate(
SwerveConstants.azimuthController.calculate(
drivetrain.getPigeon2().getAngle(),
aziCleanUp,
Timer.getFPGATimestamp()))));
}

if (Utils.isSimulation()) {
drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
Expand Down

0 comments on commit 7cd877a

Please sign in to comment.