diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6080f156..853fa2b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,15 +62,17 @@ private void configureBindings() { drive.setDefaultCommand( new DriveCommand( drive, - () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01), - () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01), - () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01))); + () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05), + () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.05), + () -> MathUtil.applyDeadband(-controller.getRightX(), 0.05))); // TODO: Move deadband to constants file controller .start() .onTrue( new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive()))); + + controller.back().onTrue(new InstantCommand(() -> drive.resetOdometry())); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java index 5f902615..dc1fe07b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java @@ -1,19 +1,21 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import java.io.File; +import org.littletonrobotics.junction.AutoLogOutput; import swervelib.SwerveDrive; import swervelib.parser.SwerveParser; public class DriveSwerveYAGSL extends DriveBase { private final double maximumSpeed = - Units.feetToMeters(2); // * TODO: Calculate actual max speed */ + Units.feetToMeters(4.5); // * TODO: Calculate actual max speed */ private final File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swervePracticeBot"); private SwerveDrive swerveDrive; - private boolean fieldOrientedDrive = false; + @AutoLogOutput private boolean fieldOrientedDrive = false; public DriveSwerveYAGSL() { try { @@ -49,4 +51,8 @@ public void setFieldOrientedDrive(boolean enable) { public boolean isFieldOrientedDrive() { return fieldOrientedDrive; } + + public void resetOdometry() { + swerveDrive.resetOdometry(new Pose2d()); + } }