diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 3594d85..4f28b35 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -16,7 +16,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.math.Conversions; -import frc.lib.util.ctre.CTREModuleState; import frc.robot.Constants; import frc.robot.Robot; @@ -31,6 +30,7 @@ public class SwerveModule { private CANCoder angleEncoder; private RelativeEncoder angleEncoderBuiltIn; private double lastAngle; + private double absolutePosition; SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); @@ -76,7 +76,7 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants * @param isOpenLoop Whether to use open or closed loop formula */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { - desiredState = CTREModuleState.optimize(desiredState, getState().angle); + // desiredState = CTREModuleState.optimize(desiredState, getState().angle); // Custom optimize // command, since // default WPILib @@ -99,7 +99,9 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) ? lastAngle - : desiredState.angle.getDegrees(); // Prevent rotating module if speed is + : (desiredState.angle.getDegrees()); // Prevent + // rotating + // module if speed is // less then 1%. Prevents // Jittering. // angleMotor.set(ControlMode.Position, @@ -108,10 +110,29 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) // System.out.println("Angle Motor PID Controller Setting Reference Status: " // + angleMotorPIDController.setReference(angle, ControlType.kPosition)); SmartDashboard.putNumber("Module: " + moduleNumber + "PID Set Angle", angle); + SmartDashboard.putNumber("Mod" + moduleNumber + "PID Setpoint", + desiredState.angle.getDegrees()); + + SmartDashboard.putNumber("Mod" + moduleNumber + "Diff", + desiredState.angle.minus(getCanCoder()).getDegrees()); + + angleMotorPIDController.setReference(angle, ControlType.kPosition); lastAngle = angle; } + /** + * + */ + private void resetToAbsolute() { + // double absolutePosition = Conversions.degreesToFalcon( + // getCanCoder().getDegrees() - angleOffset, Constants.Swerve.angleGearRatio); + // angleEncoderBuiltIn.setPosition(0); + absolutePosition = getCanCoder().getDegrees() - angleOffset; + SmartDashboard.putString("Mod: " + moduleNumber + "SparkMax Status", + "" + angleEncoderBuiltIn.setPosition(absolutePosition)); + } + // /** // * // * @@ -133,7 +154,8 @@ public SwerveModulePosition getPosition() { double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(), Constants.Swerve.driveGearRatio, Constants.Swerve.wheelCircumference); - Rotation2d angle = Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition()); + Rotation2d angle = + Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition() - absolutePosition); return new SwerveModulePosition(position, angle); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8c87cfb..3d2eb3d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -104,7 +104,7 @@ public static final class Swerve { public static final double angleMotorEncoderPositionPIDMinInput = 0; public static final double angleMotorEncoderPositionPIDMaxInput = 360; - public static final double turningDegreesPerEncoderRevolution = 360 / angleGearRatio; + public static final double turningDegreesPerEncoderRevolution = 360.0; /* Drive Motor PID Values */ public static final double driveKP = 0.10; @@ -129,7 +129,7 @@ public static final class Swerve { public static final NeutralMode driveNeutralMode = NeutralMode.Coast; /* Motor Inverts */ - public static final boolean driveMotorInvert = true; + public static final boolean driveMotorInvert = false; public static final boolean angleMotorInvert = true; /* Angle Encoder Invert */ diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 29ec93a..33d81f6 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,7 +1,7 @@ { "fileName": "NavX.json", "name": "KauaiLabs_navX_FRC", - "version": "2023.0.3", + "version": "2023.0.4", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://dev.studica.com/maven/release/2023/" @@ -11,7 +11,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2023.0.3" + "version": "2023.0.4" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.3", + "version": "2023.0.4", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, @@ -30,7 +30,7 @@ "linuxraspbian", "linuxarm32", "linuxarm64", - "linux86-64", + "linuxx86-64", "osxuniversal", "windowsx86-64" ] diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 68a48fc..3e18a45 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,8 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.2", + "version": "2024.0.0-beta-6.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -11,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.2" + "version": "2024.0.0-beta-6.2" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.2", + "version": "2024.0.0-beta-6.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -28,7 +29,9 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena" + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ] diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 27ee06e..614dc3a 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix (v5)", - "version": "5.30.4+23.0.8", + "version": "5.31.0+23.2.2", "frcYear": 2023, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -12,19 +12,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.30.4" + "version": "5.31.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "5.31.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,9 +48,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +61,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +74,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +87,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +100,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +113,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +126,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +139,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +152,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +165,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.8", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +182,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -197,7 +197,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +212,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -225,9 +225,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -257,7 +257,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -285,9 +285,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -300,9 +300,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -315,9 +315,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -330,9 +330,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -345,9 +345,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -360,9 +360,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -375,9 +375,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -390,9 +390,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -405,9 +405,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.8", + "version": "23.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,