First tinkering iteration, missing some stuff

2026-01-12 23:25:11 -05:00
parent ec73c42cfb
commit 58a6971704
20 changed files with 1752 additions and 3 deletions
--- a/src/main/java/frc/robot/utilities/SwerveModule.java
+++ b/src/main/java/frc/robot/utilities/SwerveModule.java
@@ -0,0 +1,123 @@
+package frc.robot.utiltiies;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import frc.robot.constants.ModuleConstants;
+
+/*
+ * This thread
+ * 
+ * https://www.chiefdelphi.com/t/best-easiest-way-to-connect-wire-and-program-thrifty-absolute-magnetic-encoder-to-rev-spark-max-motor-controller/439040/30
+ * 
+ * implies that the best use of the thrifty absolute encoder is to use it as a reference for the Spark relative encoder and then
+ * used the closed loop control on the controller for turning
+ * 
+ * IDK if that's really necessary, the read rate of the analog ports is 100HZ, I suppose the only benefit is the higher rate of
+ * the controller closed loop controller. 
+ */
+public class SwerveModule {
+    private TalonFX drive;
+    private SparkMax turning;
+
+    private RelativeEncoder turningRelativeEncoder;
+
+    private AnalogEncoder turningAbsoluteEncoder;
+
+    private SparkClosedLoopController turningClosedLoopController;
+
+    private VelocityVoltage driveVelocityRequest;
+
+    private String moduleName;
+
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        drive = new TalonFX(drivingCANID);
+        turning = new SparkMax(turningCANID, MotorType.kBrushless);
+
+        turningRelativeEncoder = turning.getEncoder();
+
+        turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
+
+        turningClosedLoopController = turning.getClosedLoopController();
+
+        drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
+        turning.configure(
+            ModuleConstants.turningConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
+
+        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        drive.setPosition(0);
+
+        this.moduleName = "Drivetrain/Modules/" + moduleName;
+    }
+
+    public void periodic() {
+        Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(turningRelativeEncoder.getPosition())
+        );
+    }
+
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+            drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(turningRelativeEncoder.getPosition())
+        );
+    }
+
+    public void setDesiredState(SwerveModuleState desiredState) {
+        // TODO is this really necessary, the offset is managed by the Absolute Encoder
+        // and its "source of truth" behavior in relation to the relative encoder
+        // Probably doesn't *hurt* that it's here, but it may not be needed
+        desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition()));
+
+        drive.setControl(
+            driveVelocityRequest.withVelocity(
+                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+            ).withFeedForward(
+                desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
+            )
+        );
+
+        turningClosedLoopController.setSetpoint(
+            desiredState.angle.getRadians(), 
+            ControlType.kPosition
+        );
+    }
+
+    public void resetEncoders() {
+        drive.setPosition(0);
+
+        zeroTurningEncoder();
+    }
+
+    public void zeroTurningEncoder() {
+        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+    }
+}