From 9aeb8053885cf7fd94e7e530cfa7de43e571f020 Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Thu, 10 Oct 2024 09:01:46 -0500 Subject: [PATCH 1/3] Use fused CANCoder mode --- src/main/java/frc/lib/util/swerve/SwerveModuleReal.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 34538eb1..ed90f1b8 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -52,7 +52,11 @@ private void configAngleMotor() { swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; /* Gear Ratio and Wrapping Config */ - swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; + swerveAngleFXConfig.Feedback.FeedbackRemoteSensorID = angleEncoder.getDeviceID(); + swerveAngleFXConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + // swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; + swerveAngleFXConfig.Feedback.SensorToMechanismRatio = 1.0; + swerveAngleFXConfig.Feedback.RotorToSensorRatio = Constants.Swerve.angleGearRatio; swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; /* Current Limiting */ From 09324a93262a81655279daa341c3e2242d9d06bb Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Mon, 14 Oct 2024 14:58:45 -0500 Subject: [PATCH 2/3] fix imports --- src/main/java/frc/lib/util/swerve/SwerveModuleReal.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index ed90f1b8..2084f621 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.Constants; From 36b0bd1dac431364860854e64226816ee480e3b7 Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Sun, 15 Dec 2024 10:16:46 -0600 Subject: [PATCH 3/3] Update based on Marty/Jetsi tests --- README.md | 8 ++++---- src/main/java/frc/lib/util/swerve/SwerveModule.java | 10 ---------- .../java/frc/lib/util/swerve/SwerveModuleReal.java | 11 ++++++++++- src/main/java/frc/robot/subsystems/swerve/Swerve.java | 9 --------- .../java/frc/robot/subsystems/swerve/SwerveReal.java | 2 +- 5 files changed, 15 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index fb1e31e0..17bd236b 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,13 @@ # FRC2024 + [![CI](https://github.com/Frc5572/FRC2024/actions/workflows/main.yml/badge.svg)](https://github.com/Frc5572/FRC2024/actions/workflows/main.yml) [![Java Docs](https://img.shields.io/badge/docs-2024-blue)](https://frc5572.github.io/FRC2024/) Code for our 2024 robot - ## CAN IDs ### Swerve + Front Left Drive Motor - 10 Front Left Angle Motor - 8 Front Left canCoder - 10 @@ -21,6 +22,7 @@ Back Right Angle Motor - 51 Back Right canCoder - 4 ### Other + Shooter Top - 13 Shooter Bottom - 15 Elevator - 57 @@ -30,14 +32,12 @@ Indexer - 32 Climber Left - 60 Climber Right - 48 - ## PDH Ports ### Swerve - - ### Other + 0 - Wrist 1 - Left Climber 2 - Elevator diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 492e9055..b3a23d7b 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -35,7 +35,6 @@ public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io) // lastAngle = getState().angle.getDegrees(); io.updateInputs(inputs); - resetToAbsolute(); Logger.processInputs("SwerveModule" + moduleNumber, inputs); } @@ -90,15 +89,6 @@ public Rotation2d getCANcoder() { return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder); } - /** - * Reset the Swerve Module angle to face forward - */ - public void resetToAbsolute() { - double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); - io.setPositionAngleMotor(absolutePosition); - inputs.angleMotorSelectedPosition = absolutePosition; - } - /** * Get the current Swerve Module State * diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 517f1077..05a5dc29 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -9,7 +9,9 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.math.Conversions; import frc.robot.Constants; @@ -36,9 +38,13 @@ public class SwerveModuleReal implements SwerveModuleIO { /* angle motor control requests */ private final PositionVoltage anglePosition = new PositionVoltage(0); + private final Rotation2d angleOffset; /** Instantiating motors and Encoders */ - public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID) { + public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID, + Rotation2d cancoderOffset) { + + this.angleOffset = cancoderOffset; angleEncoder = new CANcoder(cancoderID, "canivore"); mDriveMotor = new TalonFX(driveMotorID, "canivore"); @@ -127,6 +133,9 @@ private void configDriveMotor() { private void configAngleEncoder() { /* Angle Encoder Config */ swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; + swerveCANcoderConfig.MagnetSensor.AbsoluteSensorRange = + AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + swerveCANcoderConfig.MagnetSensor.MagnetOffset = -angleOffset.getRotations(); angleEncoder.getConfigurator().apply(swerveCANcoderConfig); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 368e3d00..63e815c8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -225,15 +225,6 @@ public void resetFieldRelativeOffset() { fieldOffset = getGyroYaw().getDegrees() + 180; } - /** - * Reset all modules to their front facing position - */ - public void resetModulesToAbsolute() { - for (SwerveModule mod : swerveMods) { - mod.resetToAbsolute(); - } - } - public void resetPvInitialization() { hasInitialized = false; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index 410e8ca3..7e3dfca2 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -25,7 +25,7 @@ public void updateInputs(SwerveInputs inputs) { public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, Rotation2d angleOffset) { return new SwerveModule(moduleNumber, angleOffset, - new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID)); + new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID, angleOffset)); } @Override