diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index e08200d..1779348 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -110,6 +110,19 @@ public SwerveModuleState getState() { Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); } + /** + * Get the current Swerve Module State + * + * @return The current {@link SwerveModuleState} + */ + public SwerveModuleState getStateAbs() { + return new SwerveModuleState( + Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity, + Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations( + inputs.absolutePositionAngleEncoder - this.angleOffset.getRotations())); + } + /** * Get the current Swerve Module Position * diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ad854cc..33bfc2e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.util.Color; import frc.lib.util.FieldConstants; +import frc.robot.subsystems.swerve.Swerve.SwerveGyro; /** * Constants file. @@ -188,7 +189,11 @@ public static final class Swerve { public static final edu.wpi.first.wpilibj.SPI.Port navXID = edu.wpi.first.wpilibj.SPI.Port.kMXP; - public static final boolean invertGyro = true; + + public static final SwerveGyro selectedGyro = + frc.robot.subsystems.swerve.Swerve.SwerveGyro.CANAND; + public static final boolean invertGyro = true; // Not inverted to use with CANand Gyro, + // invert for NavX public static final boolean isFieldRelative = true; public static final boolean isOpenLoop = false; @@ -215,7 +220,8 @@ public static final class Swerve { public static final double angleGearRatio = (12.8 / 1.0); // (150 / 7) : 1 /* Motor Inverts */ - public static final InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + public static final InvertedValue angleMotorInvert = + InvertedValue.CounterClockwise_Positive; public static final InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; @@ -259,8 +265,8 @@ public static final class Swerve { /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 10.0; - public static final double AUTO_MAX_SPEED = 3.0; + public static final double maxSpeed = 1; + public static final double AUTO_MAX_SPEED = 1; /** Radians per Second */ public static final double maxAngularVelocity = 15.0; @@ -278,7 +284,7 @@ public static final class Mod0 { public static final int angleMotorID = 6; public static final int canCoderID = 4; // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(183.955078125); - public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.376221 + 0.75); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.37451171875); } @@ -290,7 +296,7 @@ public static final class Mod1 { public static final int angleMotorID = 5; public static final int canCoderID = 3; // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(325.01953125); - public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.355957 + 0.25); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.146728515625); } @@ -302,7 +308,7 @@ public static final class Mod2 { public static final int angleMotorID = 7; public static final int canCoderID = 2; // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(124.62890625); - public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.223145 + 0.75); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.219970703125); } @@ -314,7 +320,7 @@ public static final class Mod3 { public static final int angleMotorID = 8; public static final int canCoderID = 1; // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(295.400390625); - public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.095459 + 0.25); + public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.40771484375); } public static final HolonomicPathFollowerConfig pathFollowerConfig = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3074c10..6d68ef5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,7 +53,7 @@ public RobotContainer(RobotRunType runtimeType) { s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop)); // Configure the button bindings - CanandEventLoop.getInstance(); + // CanandEventLoop.getInstance(); configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index f8c7d00..5890ec9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -35,6 +35,10 @@ public class Swerve extends SubsystemBase { private SwerveIO swerveIO; + public enum SwerveGyro { + NAVX, CANAND + } + /** * Swerve Subsystem */ @@ -132,6 +136,15 @@ public SwerveModuleState[] getModuleStates() { return states; } + @AutoLogOutput(key = "Absolute States") + public SwerveModuleState[] gModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (SwerveModule mod : swerveMods) { + states[mod.moduleNumber] = mod.getStateAbs(); + } + return states; + } + /** * Get Swerve Module Positions * @@ -180,9 +193,21 @@ public Rotation2d getHeading() { * @return Current rotation/yaw of gyro as {@link Rotation2d} */ public Rotation2d getGyroYaw() { - float yaw = inputs.yaw; - return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw) - : Rotation2d.fromDegrees(yaw); + Rotation2d robotYaw = switch (Constants.Swerve.selectedGyro) { + case NAVX -> { + float yaw = inputs.yaw; + yield (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw) + : Rotation2d.fromDegrees(yaw); + + } + case CANAND -> { + double yaw = inputs.newyaw; + yield Rotation2d.fromRotations(yaw); + + } + default -> new Rotation2d(); + }; + return robotYaw; } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java index b0ae91b..6b1b469 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -15,6 +15,10 @@ public static class SwerveInputs { public float yaw; public float roll; public float pitch; + + public double newyaw; + public double newroll; + public double newpitch; } public default void updateInputs(SwerveInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index 410e8ca..09e855a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swerve; import com.kauailabs.navx.frc.AHRS; +import com.reduxrobotics.sensors.canandgyro.Canandgyro; import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.util.swerve.SwerveModule; import frc.lib.util.swerve.SwerveModuleReal; @@ -10,6 +11,7 @@ public class SwerveReal implements SwerveIO { private AHRS gyro = new AHRS(Constants.Swerve.navXID); + private Canandgyro newGyro = new Canandgyro(1); /** Real Swerve Initializer */ public SwerveReal() {} @@ -19,6 +21,9 @@ public void updateInputs(SwerveInputs inputs) { inputs.yaw = gyro.getYaw(); inputs.pitch = gyro.getPitch(); inputs.roll = gyro.getRoll(); + inputs.newyaw = newGyro.getYaw(); + inputs.newpitch = newGyro.getPitch(); + inputs.newroll = newGyro.getRoll(); }