diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index d91c5b5e..6dc7fe71 100644 --- 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 turnAppliedVolts; private final StatusSignal 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 index 7a05ca20..867f5d0a 100644 --- 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,13 +27,13 @@ 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; } } @@ -42,7 +41,6 @@ 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); - - } } }