Merge pull request #107 from midtownrobotics/96-odometry-switch-to-single-origin-coordinate-system

96 odometry switch to single origin coordinate system

Author: G3-Robotics

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

src/main/java/frc/robot/Robot.java

@@ -87,17 +87,6 @@ public void autonomousInit() {
     if (autonCommand != null) {
       autonCommand.schedule();
     }
-    String currentAuton = m_robotContainer.getAutonFactory().getAutonCommandString();
-    double desiredHeading = 0;
-    if (currentAuton.toLowerCase().contains("amp")) {
-      desiredHeading = 60;
-    } else if (currentAuton.toLowerCase().contains("source")) {
-      desiredHeading = -60;
-    }
-    if (Alliance.Red.equals(DriverStation.getAlliance().get())) {
-      desiredHeading *= -1;
-    }
-    m_robotContainer.getDrivetrain().resetHeading(desiredHeading);
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -116,6 +116,7 @@ private void configureBindings() {
               if (drivingMode != null && drivingMode.get() != null && drivingMode.get().equals("robot")) {
                 pigeonValue = 0;
               }
+
               drivetrain.setDriverDesired(
                   ChassisSpeeds.fromFieldRelativeSpeeds(
                       driverX, driverY, driverRot, Rotation2d.fromDegrees(pigeonValue)));

src/main/java/frc/robot/subsystems/Drivetrain/Drivetrain.java

@@ -14,6 +14,7 @@
 import frc.robot.subsystems.Drivetrain.SwerveDrivetrainIO.SwerveDrivetrainIO;
 import frc.robot.subsystems.Drivetrain.SwerveDrivetrainIO.SwerveIOInputsAutoLogged;
 import frc.robot.subsystems.Limelight.Limelight;
+import frc.robot.utils.AllianceFlipUtil;
 import frc.robot.utils.LoggedTunableNumber;
 import frc.robot.utils.ApriltagHelper.Tags;
 
@@ -183,11 +184,18 @@ public void setBoost(boolean boost) {
 
   public void setDriverDesired(ChassisSpeeds speeds) {
     boolean discretizing = false;
+
+    if (AllianceFlipUtil.shouldFlip()) {
+      speeds.vxMetersPerSecond = -speeds.vxMetersPerSecond;
+      speeds.vyMetersPerSecond = -speeds.vyMetersPerSecond;
+    }
+
     if ((Math.abs(speeds.vxMetersPerSecond) + Math.abs(speeds.vyMetersPerSecond)) > 1
         && Math.abs(speeds.omegaRadiansPerSecond) > 0.25) {
       speeds = ChassisSpeeds.discretize(speeds, 0.02);
       discretizing = true;
     }
+
     Logger.recordOutput("Drive/Discretizing", discretizing);
     this.driverChassisSpeeds = speeds;
   }

src/main/java/frc/robot/subsystems/Drivetrain/SwerveDrivetrainIO/SwerveDrivetrainIONeo.java

@@ -19,6 +19,7 @@
 import frc.robot.subsystems.Drivetrain.SwerveModuleIO.SwerveModuleIOInputsAutoLogged;
 import frc.robot.subsystems.Drivetrain.SwerveModuleIO.SwerveModuleIONeo;
 import frc.robot.subsystems.Limelight.Limelight;
+import frc.robot.utils.AllianceFlipUtil;
 import frc.robot.utils.LoggedTunableNumber;
 import frc.robot.utils.NeoSwerveUtils;
 import org.littletonrobotics.junction.Logger;
@@ -209,7 +210,11 @@ public void resetHeading(double heading) {
   }
 
   public void resetHeading() {
-    resetHeading(0);
+    int heading = 0;
+    if (AllianceFlipUtil.shouldFlip()) {
+      heading += 180;
+    }
+    resetHeading(heading);
   }
 
   @Override
@@ -341,8 +346,8 @@ public Pose2d getPose() {
   }
 
   public void resetOdometry(Pose2d pose) {
-    // resetHeading(-pose.getRotation().getDegrees());
-    m_poseEstimator.resetPosition(Rotation2d.fromDegrees(0), getSwerveModulePositions(), pose);
+    resetHeading(pose.getRotation().getDegrees());
+    m_poseEstimator.resetPosition(pose.getRotation(), getSwerveModulePositions(), pose);
   }
 
   public void updateOdometry() {