From f455d59b39b7191496909cc9dd41271074900d3f Mon Sep 17 00:00:00 2001 From: Alex <75352922+agrinmanriv0537@users.noreply.github.com> Date: Thu, 25 Jan 2024 17:08:54 -0600 Subject: [PATCH] updated constants --- .../frc/lib/util/swerve/SwerveModule.java | 11 +++-- .../frc/lib/util/swerve/SwerveModuleIO.java | 17 ++++---- .../frc/lib/util/swerve/SwerveModuleReal.java | 17 ++++---- src/main/java/frc/robot/Constants.java | 40 +++++++++---------- .../frc/robot/subsystems/swerve/Swerve.java | 24 +++++------ 5 files changed, 53 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 5f737e3..2f72f4f 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -90,7 +90,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { * @return The rotation of the CANCoder in {@link Rotation2d} */ public Rotation2d getCANcoder() { - return Rotation2d.fromRotations(io.getAbsolutePositionAngleEncoder().getValue()); + return Rotation2d.fromRotations(io.getAbsolutePositionAngleEncoder()); } /** @@ -108,9 +108,8 @@ public void resetToAbsolute() { */ public SwerveModuleState getState() { return new SwerveModuleState( - Conversions.RPSToMPS(io.getVelocityDriveMotor().getValue(), - Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(io.getPositionAngleMotor().getValue())); + Conversions.RPSToMPS(io.getVelocityDriveMotor(), Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(io.getPositionAngleMotor())); } /** @@ -120,9 +119,9 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - Conversions.rotationsToMeters(io.getPositionDriveMotor().getValue(), + Conversions.rotationsToMeters(io.getPositionDriveMotor(), Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(io.getPositionAngleMotor().getValue())); + Rotation2d.fromRotations(io.getPositionAngleMotor())); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index 8e60d3b..fcca1de 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -1,7 +1,6 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.AutoLog; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.ControlRequest; /** IO Class for SwerveModule */ @@ -26,22 +25,22 @@ public default void setAngleMotor(ControlRequest request) {} public default void setAngleSelectedSensorPosition(double angle) {} - public default StatusSignal getAbsolutePositionAngleEncoder() { - return null; + public default double getAbsolutePositionAngleEncoder() { + return 0.0; } public default void setPositionAngleMotor(double absolutePosition) {} - public default StatusSignal getVelocityDriveMotor() { - return null; + public default double getVelocityDriveMotor() { + return 0.0; } - public default StatusSignal getPositionAngleMotor() { - return null; + public default double getPositionAngleMotor() { + return 0.0; } - public default StatusSignal getPositionDriveMotor() { - return null; + public default double getPositionDriveMotor() { + return 0.0; } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 6c13b4e..408e794 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -1,6 +1,5 @@ package frc.lib.util.swerve; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.ControlRequest; @@ -111,8 +110,8 @@ public void setDriveMotor(ControlRequest request) { } @Override - public StatusSignal getAbsolutePositionAngleEncoder() { - return angleEncoder.getAbsolutePosition(); + public double getAbsolutePositionAngleEncoder() { + return angleEncoder.getAbsolutePosition().getValueAsDouble(); } @Override @@ -121,18 +120,18 @@ public void setPositionAngleMotor(double absolutePosition) { } @Override - public StatusSignal getVelocityDriveMotor() { - return mDriveMotor.getVelocity(); + public double getVelocityDriveMotor() { + return mDriveMotor.getVelocity().getValueAsDouble(); } @Override - public StatusSignal getPositionAngleMotor() { - return mAngleMotor.getPosition(); + public double getPositionAngleMotor() { + return mAngleMotor.getPosition().getValueAsDouble(); } @Override - public StatusSignal getPositionDriveMotor() { - return mDriveMotor.getPosition(); + public double getPositionDriveMotor() { + return mDriveMotor.getPosition().getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f9ac7fb..25072a9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -105,43 +105,43 @@ public static final class Swerve { /* Module Specific Constants */ /** - * Front Left Module - Module 0 + * Front Left Module - Module 0. */ public static final class Mod0 { - public static final int driveMotorID = 10; - public static final int angleMotorID = 8; - public static final int canCoderID = 10; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(115.400390625); + public static final int DRIVE_MOTOR_ID = 6; + public static final int ANGLE_MOTOR_ID = 8; + public static final int CAN_CODER_ID = 4; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(138.604); } /** - * Front Right Module - Module 1 + * Front Right Module - Module 1. */ public static final class Mod1 { - public static final int driveMotorID = 3; - public static final int angleMotorID = 9; - public static final int canCoderID = 1; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-55.37109375); + public static final int DRIVE_MOTOR_ID = 1; + public static final int ANGLE_MOTOR_ID = 4; + public static final int CAN_CODER_ID = 1; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(280.107); } /** - * Back Left Module - Module 2 + * Back Left Module - Module 2. */ public static final class Mod2 { - public static final int driveMotorID = 2; - public static final int angleMotorID = 40; - public static final int canCoderID = 2; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(145.01953125); + public static final int DRIVE_MOTOR_ID = 3; + public static final int ANGLE_MOTOR_ID = 2; + public static final int CAN_CODER_ID = 2; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(121.553); } /** - * Back Right Module - Module 3 + * Back Right Module - Module 3. */ public static final class Mod3 { - public static final int driveMotorID = 6; - public static final int angleMotorID = 51; - public static final int canCoderID = 4; - public static final Rotation2d angleOffset = Rotation2d.fromDegrees(3.955078125); + public static final int DRIVE_MOTOR_ID = 7; + public static final int ANGLE_MOTOR_ID = 5; + public static final int CAN_CODER_ID = 3; + public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(248.027); } public static final HolonomicPathFollowerConfig pathFollowerConfig = diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 1713d21..f01c98a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -32,18 +32,18 @@ public class Swerve extends SubsystemBase { */ public Swerve(SwerveIO swerveIO) { swerveMods = new SwerveModule[] { - swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID, - Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, - Constants.Swerve.Mod0.angleOffset), - swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID, - Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID, - Constants.Swerve.Mod1.angleOffset), - swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID, - Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID, - Constants.Swerve.Mod2.angleOffset), - swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID, - Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, - Constants.Swerve.Mod3.angleOffset)}; + swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.DRIVE_MOTOR_ID, + Constants.Swerve.Mod0.ANGLE_MOTOR_ID, Constants.Swerve.Mod0.CAN_CODER_ID, + Constants.Swerve.Mod0.ANGLE_OFFSET), + swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.DRIVE_MOTOR_ID, + Constants.Swerve.Mod1.ANGLE_MOTOR_ID, Constants.Swerve.Mod1.CAN_CODER_ID, + Constants.Swerve.Mod1.ANGLE_OFFSET), + swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.DRIVE_MOTOR_ID, + Constants.Swerve.Mod2.ANGLE_MOTOR_ID, Constants.Swerve.Mod2.CAN_CODER_ID, + Constants.Swerve.Mod2.ANGLE_OFFSET), + swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.DRIVE_MOTOR_ID, + Constants.Swerve.Mod3.ANGLE_MOTOR_ID, Constants.Swerve.Mod3.CAN_CODER_ID, + Constants.Swerve.Mod3.ANGLE_OFFSET)}; swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions());