Merge branch 'alexNeoCode' of https://github.com/Frc5572/Jetsi-Offseason

into alexNeoCode
Frc5572 · Jan 9, 2024 · 5a31e69 · 5a31e69
2 parents e2e3f56 + 62be459
commit 5a31e69
Show file tree

Hide file tree

Showing 5 changed files with 90 additions and 65 deletions.
diff --git 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
@@ -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
@@ -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,15 +11,15 @@
         {
             "groupId": "com.kauailabs.navx.frc",
             "artifactId": "navx-frc-java",
-            "version": "2023.0.3"
+            "version": "2023.0.4"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "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
@@ -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,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2023.4.2"
+            "version": "2024.0.0-beta-6.2"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "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"
             ]
         }
     ]