Merge pull request #2 from FIRST1939/allianceOriented

Field Orientation Post-Autonomous
FIRST1939 · Feb 29, 2024 · 4017fc9 · 4017fc9
2 parents c72179d + 681efce
commit 4017fc9
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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(){}