Skip to content

Commit

Permalink
Zero Gyro Trigger
Browse files Browse the repository at this point in the history
  • Loading branch information
1939DriverStation committed Feb 3, 2024
1 parent 7cf8713 commit 2fc8947
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 4 deletions.
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RepeatCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.lib.Controller;
import frc.robot.commands.swerve.BrakeMode;
import frc.robot.commands.swerve.Drive;
Expand All @@ -20,7 +19,7 @@ public class RobotContainer {

private Swerve swerve;
private Limelight limelight;
private CommandXboxController driverOne;
private Controller driverOne;

public RobotContainer () {

Expand All @@ -35,8 +34,6 @@ public RobotContainer () {

private void configureCommands () {

this.driverOne.leftBumper().whileTrue(new RepeatCommand(new InstantCommand(this.swerve::lock, this.swerve)));

this.swerve.setDefaultCommand(new Drive(
this.swerve,
() -> MathUtil.applyDeadband(-this.driverOne.getHID().getLeftY(), Constants.SwerveConstants.TRANSLATION_DEADBAND),
Expand All @@ -46,6 +43,9 @@ private void configureCommands () {
));

this.limelight.setDefaultCommand(new TrackAprilTags(this.swerve, this.limelight));

this.driverOne.x().onTrue(new InstantCommand(this.swerve::zeroGyro, this.swerve));
this.driverOne.leftBumper().whileTrue(new RepeatCommand(new InstantCommand(this.swerve::lock, this.swerve)));
}

public Command getAutonomousCommand () { return this.swerve.getAutonomousCommand(); }
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/subsystems/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,7 @@ public void driveHeadingLock (Translation2d translation, Rotation2d rotation) {
public ChassisSpeeds getRobotVelocity () { return this.swerveDrive.getRobotVelocity(); }
public ChassisSpeeds getFieldVelocity () { return this.swerveDrive.getFieldVelocity(); }

public void zeroGyro () { this.swerveDrive.zeroGyro(); }
public void resetOdometry (Pose2d pose) { this.swerveDrive.resetOdometry(pose); }
public void setChassisSpeeds (ChassisSpeeds chassisSpeeds) { this.swerveDrive.drive(chassisSpeeds); }

Expand Down

0 comments on commit 2fc8947

Please sign in to comment.