fix: make swerve offset units consistent, swerve offsets happen in th…

…e same place
flamingchickens1540 · Jan 17, 2024 · fb2088e · fb2088e
1 parent 32d3ff0
commit fb2088e
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 18 deletions.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
@@ -43,15 +43,12 @@ public class ModuleIOTalonFX implements ModuleIO {
     private final StatusSignal<Double> turnAppliedVolts;
     private final StatusSignal<Double> turnCurrent;
 
-    private final Rotation2d absoluteEncoderOffset;
-
     private final boolean isCANFD;
 
     public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
         driveTalon = hw.driveMotor;
         turnTalon = hw.turnMotor;
         cancoder = hw.cancoder;
-        absoluteEncoderOffset = hw.cancoderOffset;
 
         isCANFD = CANBus.isNetworkFD(CAN_BUS);
 
@@ -113,7 +110,7 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
         inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
 
-        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).plus(absoluteEncoderOffset);
+        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
         inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO);
         inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO;
         inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();

diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
@@ -7,7 +7,6 @@
 import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import edu.wpi.first.math.geometry.Rotation2d;
 import org.team1540.robot2024.Constants;
 
 public class SwerveFactory {
@@ -28,21 +27,20 @@ public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) {
 
     public enum SwerveCorner {
         FRONT_LEFT(0),
-        FRONT_RIGHT(90),
-        BACK_LEFT(270),
-        BACK_RIGHT(180);
+        FRONT_RIGHT(0.25),
+        BACK_LEFT(0.75),
+        BACK_RIGHT(0.5);
 
-        private final double offset;
-        SwerveCorner(double offset) {
-            this.offset = offset;
+        private final double offsetRots;
+        SwerveCorner(double offsetRots) {
+            this.offsetRots = offsetRots;
         }
     }
 
     public static class SwerveModuleHW {
         public final TalonFX driveMotor;
         public final TalonFX turnMotor;
         public final CANcoder cancoder;
-        public final Rotation2d cancoderOffset;
 
         private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             if (id < 1 || id > 8) {
@@ -65,7 +63,7 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
             turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
-            canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1];
+            canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1] + corner.offsetRots;
             canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
             canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
 
@@ -78,12 +76,7 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
 
             this.cancoder = new CANcoder(10 + id, canbus);
 
-
             this.cancoder.getConfigurator().apply(canCoderConfig);
-
-            this.cancoderOffset = Rotation2d.fromDegrees(corner.offset);
-
-
         }
     }
 }