From e524577c90b58767635f5f7e68ddfe1b1bd3d9ed Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 12:07:34 -0800 Subject: [PATCH 1/7] fix: driver flipping needs to be done in execute for some reason (at least in sim) --- .../commands/SwerveDriveCommand.java | 41 ++++--------------- .../subsystems/drive/Drivetrain.java | 22 ++++++++++ 2 files changed, 30 insertions(+), 33 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java index de3995cc..88c7b89d 100644 --- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -2,11 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -20,8 +15,6 @@ public class SwerveDriveCommand extends Command { private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); - private boolean isFlipped; - public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; @@ -33,38 +26,20 @@ public void initialize() { xLimiter.reset(0); yLimiter.reset(0); rotLimiter.reset(0); - isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } @Override public void execute() { - double xPercent = xLimiter.calculate(-controller.getLeftY()); - double yPercent = yLimiter.calculate(-controller.getLeftX()); - double rotPercent = rotLimiter.calculate(-controller.getRightX()); - - // Apply deadband - double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1); - Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); - double omega = MathUtil.applyDeadband(rotPercent, 0.1); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - // Calculate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); + double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); + double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1); - // Convert to field relative speeds & send command - drivetrain.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(), - omega * drivetrain.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180)) - : drivetrain.getRotation() - ) - ); + drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 373d06fa..e8e15ec1 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -125,6 +126,27 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } + public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean isFlipped) { + Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); + double linearMagnitude = Math.hypot(xPercent, yPercent); + + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); + + // Convert to field relative + runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), + rotPercent * getMaxAngularSpeedRadPerSec(), + isFlipped + ? getRotation().plus(Rotation2d.fromDegrees(180)) + : getRotation() + ) + ); + } + /** * Stops the drive. */ From a9355eb6125e9182e98a05feec1dfd6497952626 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 12:41:00 -0800 Subject: [PATCH 2/7] feat: drivetrain targets speaker --- .../org/team1540/robot2024/Constants.java | 9 +++ .../team1540/robot2024/RobotContainer.java | 4 +- .../DriveWithSpeakerTargetingCommand.java | 73 +++++++++++++++++++ 3 files changed, 84 insertions(+), 2 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index b13796e1..61c6c3a3 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -232,6 +233,14 @@ public static class Tramp { public static final int MOTOR_ID = -1; //TODO: Configure this later } + public static class Targeting { + public static final double ROT_KP = 1.18; + public static final double ROT_KI = 0.0; + public static final double ROT_KD = 0.0; + + public static final Pose2d SPEAKER_POSE = new Pose2d(0.0, 5.12445, new Rotation2d()); + } + public static boolean isTuningMode() { return tuningMode && !DriverStation.isFMSAttached(); } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6f7840a7..e5801371 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -9,15 +9,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.robot2024.Constants.Elevator.ElevatorState; import org.team1540.robot2024.commands.FeedForwardCharacterization; +import org.team1540.robot2024.commands.DriveWithSpeakerTargetingCommand; import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.shooter.ShootSequence; -import org.team1540.robot2024.commands.shooter.TuneShooterCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.elevator.ElevatorIO; @@ -176,6 +175,7 @@ private void configureButtonBindings() { indexer.setDefaultCommand(new IntakeCommand(indexer, tramp)); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); + driver.y().toggleOnFalse(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.b().onTrue( Commands.runOnce( () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())), diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java new file mode 100644 index 00000000..c712c407 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -0,0 +1,73 @@ +package org.team1540.robot2024.commands; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.util.LoggedTunableNumber; + +import static org.team1540.robot2024.Constants.Targeting.*; + +public class DriveWithSpeakerTargetingCommand extends Command { + private final Drivetrain drivetrain; + private final CommandXboxController controller; + + private final SlewRateLimiter xLimiter = new SlewRateLimiter(2); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); + private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); + + private boolean isFlipped; + private Pose2d speakerPose; + + private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); + + public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) { + this.drivetrain = drivetrain; + this.controller = controller; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + xLimiter.reset(0); + yLimiter.reset(0); + rotController.reset(); + + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; + } + + @Override + public void execute() { + if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + rotController.setPID(kP.get(), kI.get(), kD.get()); + } + + Rotation2d targetRot = + drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() + .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180)); + Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); + + double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); + double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); + drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); + + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} From cf41c8fab15ca8dc614abbcd2290ca0b9a0f5a4c Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 14:47:02 -0800 Subject: [PATCH 3/7] fix: correct speaker pose --- src/main/java/org/team1540/robot2024/Constants.java | 3 ++- .../commands/DriveWithSpeakerTargetingCommand.java | 13 +++++-------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 61c6c3a3..4036fec2 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -238,7 +238,8 @@ public static class Targeting { public static final double ROT_KI = 0.0; public static final double ROT_KD = 0.0; - public static final Pose2d SPEAKER_POSE = new Pose2d(0.0, 5.12445, new Rotation2d()); + public static final Pose2d SPEAKER_POSE = + new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d()); } public static boolean isTuningMode() { diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java index c712c407..6e6efd99 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -23,9 +23,6 @@ public class DriveWithSpeakerTargetingCommand extends Command { private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); - private boolean isFlipped; - private Pose2d speakerPose; - private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP); private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); @@ -41,11 +38,6 @@ public void initialize() { xLimiter.reset(0); yLimiter.reset(0); rotController.reset(); - - isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; } @Override @@ -54,10 +46,15 @@ public void execute() { rotController.setPID(kP.get(), kI.get(), kD.get()); } + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + Pose2d speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; + Rotation2d targetRot = drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180)); Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); + Logger.recordOutput("Targeting/speakerPose", speakerPose); double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); From 198e5614d46a7828a6d075d7b92e706158447f88 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 14:50:09 -0800 Subject: [PATCH 4/7] fix: use continuous output on the pid * i'm really stupid --- .../robot2024/commands/DriveWithSpeakerTargetingCommand.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java index 6e6efd99..5c78d916 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -30,6 +30,7 @@ public class DriveWithSpeakerTargetingCommand extends Command { public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; + rotController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drivetrain); } From 46b36311dc44be5fb7baf60042ba06c0987f71b9 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 15:19:44 -0800 Subject: [PATCH 5/7] fix: stop resetting rate limiters on init (smoother transition between targeting and normal) --- .../commands/DriveWithSpeakerTargetingCommand.java | 2 -- .../team1540/robot2024/commands/SwerveDriveCommand.java | 8 -------- 2 files changed, 10 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java index 5c78d916..6bfd550d 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -36,8 +36,6 @@ public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxContro @Override public void initialize() { - xLimiter.reset(0); - yLimiter.reset(0); rotController.reset(); } diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java index 88c7b89d..e040f576 100644 --- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -21,14 +21,6 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle addRequirements(drivetrain); } - @Override - public void initialize() { - xLimiter.reset(0); - yLimiter.reset(0); - rotLimiter.reset(0); - - } - @Override public void execute() { boolean isFlipped = From 973ed09b0c514ee8e33d4234c8dfafc7dc8c694c Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 15:28:27 -0800 Subject: [PATCH 6/7] fix: flipping actually works in init you just have to go disconnected -> disabled -> teleop --- src/main/java/org/team1540/robot2024/Constants.java | 1 + .../commands/DriveWithSpeakerTargetingCommand.java | 13 ++++++++----- .../robot2024/commands/SwerveDriveCommand.java | 13 +++++++++---- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 4036fec2..65d39bd8 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -234,6 +234,7 @@ public static class Tramp { } public static class Targeting { + // TODO: tune these public static final double ROT_KP = 1.18; public static final double ROT_KI = 0.0; public static final double ROT_KD = 0.0; diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java index 6bfd550d..0bf10fe7 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -27,6 +27,9 @@ public class DriveWithSpeakerTargetingCommand extends Command { private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); + private boolean isFlipped; + private Pose2d speakerPose; + public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; @@ -37,6 +40,11 @@ public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxContro @Override public void initialize() { rotController.reset(); + + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; } @Override @@ -45,10 +53,6 @@ public void execute() { rotController.setPID(kP.get(), kI.get(), kD.get()); } - boolean isFlipped = DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - Pose2d speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; - Rotation2d targetRot = drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180)); @@ -59,7 +63,6 @@ public void execute() { double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); - } @Override diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java index e040f576..60f2b29e 100644 --- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -15,6 +15,8 @@ public class SwerveDriveCommand extends Command { private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); + private boolean isFlipped; + public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) { this.drivetrain = drivetrain; this.controller = controller; @@ -22,11 +24,14 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle } @Override - public void execute() { - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + public void initialize() { + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + @Override + public void execute() { double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1); From eaf26c34e72a10c8f8df2923e809b1706b880677 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 10 Feb 2024 16:13:02 -0800 Subject: [PATCH 7/7] fix: toggle speaker targeting on true --- src/main/java/org/team1540/robot2024/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index e5801371..1840f5df 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -175,7 +175,7 @@ private void configureButtonBindings() { indexer.setDefaultCommand(new IntakeCommand(indexer, tramp)); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); - driver.y().toggleOnFalse(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); + driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.b().onTrue( Commands.runOnce( () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())),