diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 9b51e78..cb26fb3 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -7,8 +7,8 @@ "iz": 0 }, "angle": { - "p": 0.004, - "i": 0, + "p": 0.035, + "i": 0.0001, "d": 1.5, "f": 0, "iz": 0 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1863b9a..86c36c2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,8 +1,11 @@ package frc.robot; import java.io.IOException; +import java.util.function.IntSupplier; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; @@ -34,10 +37,16 @@ public RobotContainer () { private void configureCommands () { + IntSupplier allianceOrientated = () -> { + + if (!DriverStation.getAlliance().isPresent()) { return -1; } + return DriverStation.getAlliance().get() == Alliance.Red ? 1 : -1; + }; + this.swerve.setDefaultCommand(new Drive( this.swerve, - () -> MathUtil.applyDeadband(-this.driverOne.getHID().getLeftY(), Constants.SwerveConstants.TRANSLATION_DEADBAND), - () -> MathUtil.applyDeadband(-this.driverOne.getHID().getLeftX(), Constants.SwerveConstants.TRANSLATION_DEADBAND), + () -> MathUtil.applyDeadband(this.driverOne.getHID().getLeftY() * allianceOrientated.getAsInt(), Constants.SwerveConstants.TRANSLATION_DEADBAND), + () -> MathUtil.applyDeadband(this.driverOne.getHID().getLeftX() * allianceOrientated.getAsInt(), Constants.SwerveConstants.TRANSLATION_DEADBAND), () -> MathUtil.applyDeadband(this.driverOne.getHID().getRightX(), Constants.SwerveConstants.OMEGA_DEADBAND), () -> this.driverOne.getHID().getPOV() )); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f53a0f8..6d2e350 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -16,6 +16,7 @@ import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -145,7 +146,20 @@ 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 zeroGyro () { + + Translation2d translation = this.getPose().getTranslation(); + Rotation2d rotation = new Rotation2d(); + + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { + + Rotation2d allianceOriented = Rotation2d.fromDegrees(180); + rotation = rotation.plus(allianceOriented); + } + + this.resetOdometry(new Pose2d(translation, rotation)); + } + public void resetOdometry (Pose2d pose) { this.swerveDrive.resetOdometry(pose); } public void setChassisSpeeds (ChassisSpeeds chassisSpeeds) { this.swerveDrive.drive(chassisSpeeds); } public void addVisionMeasurement (Pose2d pose2d, double timestamp) { this.swerveDrive.addVisionMeasurement(pose2d, timestamp); } diff --git a/src/main/java/frc/robot/util/BuildConstants.java b/src/main/java/frc/robot/util/BuildConstants.java index 452624e..90d988b 100644 --- a/src/main/java/frc/robot/util/BuildConstants.java +++ b/src/main/java/frc/robot/util/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "YAGSL"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 15; - public static final String GIT_SHA = "4a3bd63fb04d90379c01b57778a0605f20fdf2ab"; - public static final String GIT_DATE = "2024-02-15 17:54:09 CST"; - public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2024-02-17 14:12:06 CST"; - public static final long BUILD_UNIX_TIME = 1708200726277L; + public static final int GIT_REVISION = 12; + public static final String GIT_SHA = "558683cd1b04454321b19ea8ca81bdfeb3cb7d73"; + public static final String GIT_DATE = "2024-02-08 11:03:54 CST"; + public static final String GIT_BRANCH = "allianceOriented"; + public static final String BUILD_DATE = "2024-02-26 18:22:01 CST"; + public static final long BUILD_UNIX_TIME = 1708993321112L; public static final int DIRTY = 1; private BuildConstants(){}