ok just going to paste what she has in the swerve module drive subsystem
package frc.robot.subsystems.Drive;
//import com.ctre.phoenix6.controls.DutyCycleOut;
//import com.ctre.phoenix6.controls.PositionVoltage;
//import com.ctre.phoenix6.controls.VelocityVoltage;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
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.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.Publisher;
import edu.wpi.first.networktables.StructArrayPublisher;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.math.OnboardModuleState;
import frc.lib.util.LoggedTunableNumber;
import frc.lib.util.SwerveModuleConstants;
import frc.robot.Constants;
//import frc.robot.subsystems.Encoders.ModuleEncoder;
import frc.robot.subsystems.Encoders.ModuleEncoderThrifty;
public class SwerveModule {
/* Module details */
public int moduleNumber;
/* Motors */
private SparkMax angleMotor;
private SparkMax driveMotor;
/* Encoders and their values /
private RelativeEncoder driveEncoder;
private RelativeEncoder integratedAngleEncoder;
private ModuleEncoderThrifty angleEncoder;
private double lastAngle;
private double states;
/ Controllers */
public final SparkClosedLoopController driveController;
public final SparkClosedLoopController angleController;
public final double anglePID;
public final double driveSVA;
public final double drivePID;
public final PIDController drivePIDController;
public SimpleMotorFeedforward feedforward;
//private VelocityVoltage drVelocityVoltage;
//private DutyCycleOut drDutyCycleOut;
//private PositionVoltage angPositionVoltage;
public LoggedTunableNumber driveSpeed;
// For logging
private double driveSetpoint = 0f;
private double angleSetpoint = 0f;
public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) {
this.moduleNumber = moduleNumber;
this.anglePID = moduleConstants.anglePID;
this.driveSVA = moduleConstants.driveSVA;
this.drivePID = moduleConstants.drivePID;
this.drivePIDController = new PIDController(drivePID[0], drivePID[1], drivePID[2]);
/* Angle Encoder Config */
angleEncoder = new ModuleEncoderThrifty(moduleConstants.cancoderID);
angleEncoder.setOffset(moduleConstants.angleOffset);
/* Angle Motor Config */
angleMotor = new SparkMax(moduleConstants.angleMotorID, MotorType.kBrushless);
integratedAngleEncoder = angleMotor.getEncoder();
angleController = angleMotor.getClosedLoopController();
configAngleMotor();
/* Drive Motor Config */
driveMotor = new SparkMax(moduleConstants.driveMotorID, MotorType.kBrushless);
driveEncoder = driveMotor.getEncoder();
driveController = driveMotor.getClosedLoopController();
configDriveMotor();
lastAngle = getState().angle.getDegrees();
}
public void setDesiredState(SwerveModuleState desiredState) {
desiredState = OnboardModuleState.optimize(
desiredState,
getState().angle); // Custom optimize command, since default WPILib optimize assumes
// continuous controller which REV and CTRE are not
this.setSpeed(desiredState);
this.setAngle(desiredState);
}
public void resetToAbsolute() {
double integratedAngleEncoderPosition = this.integratedAngleEncoder.getPosition();
double absolutePosition = integratedAngleEncoderPosition - integratedAngleEncoderPosition % 360
+ angleEncoder.getAbsolutePosition().getDegrees();
integratedAngleEncoder.setPosition(absolutePosition);
// double absolutePosition = getAngle().getRotations() - angleOffset.getRotations();
//angleMotor.set(absolutePosition);
}
private void configAngleMotor() {
SparkMaxConfig config = new SparkMaxConfig();
// Set motor configurations
config
.inverted(true)
.idleMode(IdleMode.kBrake);
// Set encoder configurations
config.encoder
.positionConversionFactor(Constants.SwerveConstants.angleConversionFactor) // Conversion factor for angle encoder
.velocityConversionFactor(1.0); // Adjust as needed if velocity conversion is relevant
// Set PID configurations
config.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.pid(anglePID[0], anglePID[1], anglePID[2]);
// Apply the configuration
angleMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
this.resetToAbsolute(); // Reset to absolute position as part of configuration
}
private void configDriveMotor() {
SparkMaxConfig config = new SparkMaxConfig();
// Set motor configurations
config
.idleMode(IdleMode.kCoast)
.inverted(moduleNumber == 1 || moduleNumber == 3); // Invert for specific modules
// Set encoder configurations
config.encoder
.positionConversionFactor(Constants.SwerveConstants.driveConversionPositionFactor)
.velocityConversionFactor(Constants.SwerveConstants.driveConversionVelocityFactor);
// Set PID configurations
config.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
.pid(1.0, 0.0, 0.0); // Example PID values
// Apply the configuration
driveMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
// Additional configurations and initializations
this.feedforward = new SimpleMotorFeedforward(driveSVA[0], driveSVA[1], driveSVA[2]);
driveEncoder.setPosition(0.0); // Reset encoder position
}
private void setSpeed(SwerveModuleState desiredState) {
driveSetpoint = desiredState.speedMetersPerSecond;
}
private void setAngle(SwerveModuleState desiredState) {
// Prevent rotating module if speed is less then 1%. Prevents jittering.
double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.02))
? lastAngle
: desiredState.angle
.getDegrees();
angleSetpoint = angle;
angleController.setReference(angle, ControlType.kPosition);
lastAngle = angle;
}
public void logValues() {
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " Desired Speed", driveSetpoint);
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " Actual Speed", this.getSpeed());
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " Desired Angle",
angleSetpoint % 360);
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " Actual Angle",
angleEncoder.getAbsolutePosition().getDegrees());
SwerveModuleState states = new SwerveModuleState {
new SwerveModuleState(),
new SwerveModuleState(),
new SwerveModuleState(),
new SwerveModuleState()
};
StructArrayPublisher publisher = NetworkTableInstance.getDefault()
.getStructArrayTopic(“MyStates”, SwerveModuleState.struct).publish();
}
public void goToHome() {
Rotation2d angle = getAngle();
angleController.setReference(angle.getDegrees() - angle.getDegrees() % 360,
ControlType.kPosition);
lastAngle = angle.getDegrees() - angle.getDegrees() % 360;
}
private Rotation2d getAngle() {
return Rotation2d.fromDegrees(this.integratedAngleEncoder.getPosition());
}
public SwerveModuleState getState() {
return new SwerveModuleState(this.getSpeed(), this.getAngle());
}
public double getSpeed() {
return this.driveEncoder.getVelocity();
}
public double getDistance() {
return this.driveEncoder.getPosition();
}
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(this.getDistance(), this.getAngle());
}
public SwerveModulePosition getRedPosition() {
return new SwerveModulePosition(this.getDistance(), Rotation2d.fromDegrees(-this.getAngle().getDegrees()));
}
public SparkMax getDriveMotor() {
return this.driveMotor;
}
public void periodic() {
var pidOutput = MathUtil.clamp(drivePIDController.calculate(getSpeed(),
driveSetpoint),
-Constants.SwerveConstants.maxSpeed,
Constants.SwerveConstants.maxSpeed);
var ffOutput = feedforward.calculate(driveSetpoint);
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " PID output", pidOutput);
SmartDashboard.putNumber(Constants.SwerveConstants.moduleNames[moduleNumber] + " FF output", ffOutput);
driveController.setReference((pidOutput / Constants.SwerveConstants.maxSpeed)
* 12 + ffOutput,
ControlType.kVoltage);
Publisher.set(states);
}
}