feat: update swerve config for krakens and adjusted inversion

flamingchickens1540 · Aug 1, 2024 · e3d090b · e3d090b
1 parent ec0f05d
commit e3d090b
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 12 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -52,10 +52,10 @@ public static class SwerveConfig {
         public static final String CAN_BUS  = "swerve";
         public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0;
 
-        public static final int FRONT_LEFT  = IS_COMPETITION_ROBOT ? 1 : 9;
-        public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 2;
-        public static final int BACK_LEFT   = IS_COMPETITION_ROBOT ? 4 : 6;
-        public static final int BACK_RIGHT  = IS_COMPETITION_ROBOT ? 3 : 5;
+        public static final int FRONT_LEFT  = 4;
+        public static final int FRONT_RIGHT = 3;
+        public static final int BACK_LEFT   = 1;
+        public static final int BACK_RIGHT  = 7;
 
         public static final int PIGEON_ID = 9;
     }
@@ -208,8 +208,8 @@ public static class Elevator {
         public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT;
 
         public static final double GEAR_RATIO = 11.571;
-        public static final int LEADER_ID = 7;
-        public static final int FOLLOWER_ID = 8;
+        public static final int LEADER_ID = 7; // RIGHT
+        public static final int FOLLOWER_ID = 8; // LEFT
 
         public static final int LEFT_FLIPPER_ID = 1;
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
@@ -6,7 +6,6 @@
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
@@ -130,15 +129,15 @@ public void setTurnVoltage(double volts) {
     @Override
     public void setDriveBrakeMode(boolean enable) {
         MotorOutputConfigs config = new MotorOutputConfigs();
-        config.Inverted = InvertedValue.CounterClockwise_Positive;
+        driveTalon.getConfigurator().refresh(config);
         config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
         driveTalon.getConfigurator().apply(config);
     }
 
     @Override
     public void setTurnBrakeMode(boolean enable) {
         MotorOutputConfigs config = new MotorOutputConfigs();
-        config.Inverted = IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
+        turnTalon.getConfigurator().refresh(config);
         config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
         turnTalon.getConfigurator().apply(config);
     }

diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
@@ -47,7 +47,6 @@ public static class SwerveModuleHW {
         public final TalonFX driveMotor;
         public final TalonFX turnMotor;
         public final CANcoder cancoder;
-
         private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             if (id < 1 || id > 9) {
                 throw new IllegalArgumentException("Swerve module id must be between 1 and 9");
@@ -56,6 +55,27 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
                 canbus = "";
             }
 
+            boolean invertTurn = true;
+            boolean invertDrive = true;
+            switch (corner) {
+                case FRONT_LEFT:
+                    invertTurn = false;
+                    invertDrive = false;
+                    break;
+                case FRONT_RIGHT:
+                    invertTurn = true;
+                    invertDrive = false;
+                    break;
+                case BACK_LEFT:
+                    invertTurn = true;
+                    invertDrive = false;
+                    break;
+                case BACK_RIGHT:
+                    invertTurn = false;
+                    invertDrive = true;
+                    break;
+            }
+
             int driveID = 30 + id;
             int turnID = 20 + id;
             int canCoderID = 10 + id;
@@ -72,11 +92,11 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0;
             driveConfig.CurrentLimits.SupplyTimeThreshold = 0.1;
             driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
-            driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+            driveConfig.MotorOutput.Inverted = invertDrive ? InvertedValue.CounterClockwise_Positive: InvertedValue.Clockwise_Positive;
 
             turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0;
             turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
-            turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+            turnConfig.MotorOutput.Inverted = invertTurn ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
             turnConfig.Feedback.FeedbackRemoteSensorID = canCoderID;
             turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
             turnConfig.Feedback.SensorToMechanismRatio = 1.0;