Apply TalonFX settings correctly

Correctly apply the TalonFX settings to the motor controllers after
all settings been, well, set.

	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/commands/DriveCommands.java
	modified:   src/main/java/frc/robot/generated/TunerConstants.java
	modified:   src/main/java/frc/robot/subsystems/drive/Drive.java
	modified:   src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
	modified:   src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
	modified:   src/main/java/frc/robot/subsystems/drive/SwerveConstants.java

Author: tbowers7

src/main/java/frc/robot/Constants.java

@@ -69,7 +69,7 @@ public final class Constants {
   //       under strict caveat emptor -- and submit any error and bugfixes
   //       via GitHub issues.
   private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL
-  private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED
+  private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED
   private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO
   private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE
 
@@ -137,7 +137,7 @@ public static final class DrivebaseConstants {
 
     // Drive and Turn PID constants
     public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0);
-    public static final PIDConstants steerPID = new PIDConstants(5.0, 0.0, 0.4);
+    public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4);
 
     // Hold time on motor brakes when disabled
     public static final double kWheelLockTime = 10; // seconds

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

@@ -228,7 +228,7 @@ private void configureBindings() {
             m_drivebase,
             () -> -driveStickY.value(),
             () -> -driveStickX.value(),
-            () -> turnStickX.value()));
+            () -> -turnStickX.value()));
 
     // ** Example Commands -- Remap, remove, or change as desired **
     // Press B button while driving --> ROBOT-CENTRIC

src/main/java/frc/robot/commands/DriveCommands.java

@@ -179,16 +179,15 @@ public static Command fieldRelativeDriveAtAngle(
   private static Translation2d getLinearVelocity(double x, double y) {
     // Apply deadband
     double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband);
-    Rotation2d linearDirection = new Rotation2d(x, y);
+    Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x));
 
     // Square magnitude for more precise control
     // NOTE: The x & y values range from -1 to +1, so their squares are as well
     linearMagnitude = linearMagnitude * linearMagnitude;
 
     // Return new linear velocity
     return new Pose2d(new Translation2d(), linearDirection)
-        .transformBy(
-            new Transform2d(linearVelocityFilter.calculate(linearMagnitude), 0.0, new Rotation2d()))
+        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
         .getTranslation();
   }
 
@@ -198,7 +197,7 @@ private static Translation2d getLinearVelocity(double x, double y) {
    */
   private static double getOmega(double omega) {
     omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband);
-    return omegaFilter.calculate(Math.copySign(omega * omega, omega));
+    return Math.copySign(omega * omega, omega);
   }
 
   /***************************************************************************/

src/main/java/frc/robot/generated/TunerConstants.java

@@ -138,7 +138,7 @@ public class TunerConstants {
   private static final int kFrontLeftDriveMotorId = 1;
   private static final int kFrontLeftSteerMotorId = 2;
   private static final int kFrontLeftEncoderId = 3;
-  private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.472412109375);
+  private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125);
   private static final boolean kFrontLeftSteerMotorInverted = true;
   private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -149,7 +149,7 @@ public class TunerConstants {
   private static final int kFrontRightDriveMotorId = 4;
   private static final int kFrontRightSteerMotorId = 5;
   private static final int kFrontRightEncoderId = 6;
-  private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.11376953125);
+  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875);
   private static final boolean kFrontRightSteerMotorInverted = true;
   private static final boolean kFrontRightEncoderInverted = false;
 
@@ -160,7 +160,7 @@ public class TunerConstants {
   private static final int kBackLeftDriveMotorId = 7;
   private static final int kBackLeftSteerMotorId = 8;
   private static final int kBackLeftEncoderId = 9;
-  private static final Angle kBackLeftEncoderOffset = Rotations.of(0.409423828125);
+  private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125);
   private static final boolean kBackLeftSteerMotorInverted = true;
   private static final boolean kBackLeftEncoderInverted = false;
 
@@ -171,7 +171,7 @@ public class TunerConstants {
   private static final int kBackRightDriveMotorId = 10;
   private static final int kBackRightSteerMotorId = 11;
   private static final int kBackRightEncoderId = 12;
-  private static final Angle kBackRightEncoderOffset = Rotations.of(0.086669921875);
+  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125);
   private static final boolean kBackRightSteerMotorInverted = true;
   private static final boolean kBackRightEncoderInverted = false;
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -129,9 +129,12 @@ public Drive() {
         for (int i = 0; i < 4; i++) {
           switch (modType) {
             case 0b00000000: // ALL-CTRE
-              // TODO: ADD CASE FOR USING NAVX!!!
-              throw new RuntimeException(
-                  "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!");
+              if (kImuType == "navx" || kImuType == "navx_spi") {
+                modules[i] = new Module(new ModuleIOTalonFX(i), i);
+              } else {
+                throw new RuntimeException(
+                    "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!");
+              }
             case 0b00010000: // Blended Talon Drive / NEO Steer
               modules[i] = new Module(new ModuleIOBlended(i), i);
               break;

src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java

@@ -185,8 +185,6 @@ public ModuleIOBlended(int module) {
         constants.DriveMotorInverted
             ? InvertedValue.Clockwise_Positive
             : InvertedValue.CounterClockwise_Positive;
-    PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25));
-    PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25));
 
     // Configure turn motor
     var turnConfig = new SparkMaxConfig();
@@ -234,7 +232,6 @@ public ModuleIOBlended(int module) {
         constants.EncoderInverted
             ? SensorDirectionValue.Clockwise_Positive
             : SensorDirectionValue.CounterClockwise_Positive;
-    cancoder.getConfigurator().apply(cancoderConfig);
 
     // Set motor Closed Loop Output type based on Phoenix Pro status
     constants.DriveMotorClosedLoopOutput =
@@ -243,6 +240,12 @@ public ModuleIOBlended(int module) {
           case UNLICENSED -> ClosedLoopOutputType.Voltage;
         };
 
+    // Finally, apply the configs to the motor controllers
+    PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25));
+    PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25));
+    cancoder.getConfigurator().apply(cancoderConfig);
+
+
     // Create timestamp queue
     timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
 

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -128,8 +128,6 @@ public ModuleIOTalonFX(int module) {
         constants.DriveMotorInverted
             ? InvertedValue.Clockwise_Positive
             : InvertedValue.CounterClockwise_Positive;
-    tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25));
-    tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25));
 
     // Configure turn motor
     var turnConfig = new TalonFXConfiguration();
@@ -147,7 +145,6 @@ public ModuleIOTalonFX(int module) {
         constants.SteerMotorInverted
             ? InvertedValue.Clockwise_Positive
             : InvertedValue.CounterClockwise_Positive;
-    tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25));
 
     // Configure CANCoder
     CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs;
@@ -156,7 +153,6 @@ public ModuleIOTalonFX(int module) {
         constants.EncoderInverted
             ? SensorDirectionValue.Clockwise_Positive
             : SensorDirectionValue.CounterClockwise_Positive;
-    cancoder.getConfigurator().apply(cancoderConfig);
 
     // Set motor Closed Loop Output type and CANCoder feedback type based on Phoenix Pro status
     switch (Constants.getPhoenixPro()) {
@@ -170,6 +166,12 @@ public ModuleIOTalonFX(int module) {
         constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage;
     }
 
+    // Finally, apply the configs to the motor controllers
+    tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25));
+    tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25));
+    tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25));
+    cancoder.getConfigurator().apply(cancoderConfig);
+
     // Create timestamp queue
     timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
 

src/main/java/frc/robot/subsystems/drive/SwerveConstants.java

@@ -137,7 +137,7 @@ public class SwerveConstants {
             -Units.rotationsToRadians(TunerConstants.FrontLeft.EncoderOffset) + Math.PI;
         kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted;
         kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted;
-        kFLEncoderInvert = false;
+        kFLEncoderInvert = TunerConstants.FrontLeft.EncoderInverted;
         kFLXPosMeters = TunerConstants.FrontLeft.LocationX;
         kFLYPosMeters = TunerConstants.FrontLeft.LocationY;
         // Front Right
@@ -153,7 +153,7 @@ public class SwerveConstants {
         kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.EncoderOffset);
         kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted;
         kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted;
-        kFREncoderInvert = false;
+        kFREncoderInvert = TunerConstants.FrontRight.EncoderInverted;
         kFRXPosMeters = TunerConstants.FrontRight.LocationX;
         kFRYPosMeters = TunerConstants.FrontRight.LocationY;
         // Back Left
@@ -170,7 +170,7 @@ public class SwerveConstants {
             -Units.rotationsToRadians(TunerConstants.BackLeft.EncoderOffset) + Math.PI;
         kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted;
         kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted;
-        kBLEncoderInvert = false;
+        kBLEncoderInvert = TunerConstants.BackLeft.EncoderInverted;
         kBLXPosMeters = TunerConstants.BackLeft.LocationX;
         kBLYPosMeters = TunerConstants.BackLeft.LocationY;
         // Back Right
@@ -186,7 +186,7 @@ public class SwerveConstants {
         kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.EncoderOffset);
         kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted;
         kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted;
-        kBREncoderInvert = false;
+        kBREncoderInvert = TunerConstants.BackRight.EncoderInverted;
         kBRXPosMeters = TunerConstants.BackRight.LocationX;
         kBRYPosMeters = TunerConstants.BackRight.LocationY;
         break;