diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 9bc07dc..fa72d40 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -26,6 +27,7 @@ public class SwerveModule { private CANSparkMax angleMotor; private TalonFX driveMotor; private CANCoder angleEncoder; + private RelativeEncoder angleEncoderBuiltIn; private double lastAngle; SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); @@ -54,7 +56,11 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants driveMotor = new TalonFX(constants.driveMotorID); configDriveMotor(); - + angleEncoderBuiltIn = angleMotor.getEncoder(); + angleEncoderBuiltIn + .setPositionConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution); + angleEncoderBuiltIn + .setVelocityConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution / 60); lastAngle = getState().angle.getDegrees(); } @@ -107,7 +113,7 @@ private void resetToAbsolute() { // getCanCoder().getDegrees() - angleOffset, Constants.Swerve.angleGearRatio); double absolutePosition = getCanCoder().getDegrees() - angleOffset; - angleMotor.setSelectedSensorPosition(absolutePosition); + angleEncoderBuiltIn.setPosition(absolutePosition); } // /** @@ -131,8 +137,7 @@ public SwerveModulePosition getPosition() { double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(), Constants.Swerve.driveGearRatio, Constants.Swerve.wheelCircumference); - Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees( - angleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio)); + Rotation2d angle = Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition()); return new SwerveModulePosition(position, angle); } @@ -191,7 +196,7 @@ public Rotation2d getCanCoder() { public SwerveModuleState getState() { double velocity = Conversions.falconToMPS(driveMotor.getSelectedSensorVelocity(), Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio); - Rotation2d angle = Rotation2d.fromDegrees(angleMotor.); + Rotation2d angle = Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition()); return new SwerveModuleState(velocity, angle); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 83c2954..f94cc06 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -72,7 +72,7 @@ public static final class Swerve { public static final double closedLoopRamp = 0.4; public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1 - public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 + public static final double angleGearRatio = ((150 / 7) / 1.0); // 12.8:1 public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(new Translation2d(wheelBase / 2.0, trackWidth / 2.0), @@ -92,13 +92,14 @@ public static final class Swerve { public static final boolean driveEnableCurrentLimit = true; /* Angle Motor PID Values */ - public static final double angleKP = 0.6; + public static final double angleKP = 0.0; public static final double angleKI = 0.0; - public static final double angleKD = 12.0; + public static final double angleKD = 0.0; public static final double angleKF = 0.0; public static final double angleMotorEncoderPositionPIDMinInput = 0; public static final double angleMotorEncoderPositionPIDMaxInput = 360; + public static final double turningDegreesPerEncoderRevolution = 360 / angleGearRatio; /* Drive Motor PID Values */ public static final double driveKP = 0.10;