Swerve Working

Team3128 · Jan 27, 2024 · 2e7cbce · 2e7cbce
1 parent f853429
commit 2e7cbce
Show file tree

Hide file tree

Showing 5 changed files with 42 additions and 9 deletions.
diff --git a/build.gradle b/build.gradle
@@ -70,6 +70,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }
 
 repositories {

diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java
@@ -119,9 +119,9 @@ public static class SwerveConstants {
         /* Angle Encoder Invert */
         public static final boolean canCoderInvert = false;
 
-        public static final MotorConfig driveMotorConfig = new MotorConfig(SwerveConversions.rotationsToMeters(1, wheelCircumference, driveGearRatio), 60, driveLimit, false, Neutral.COAST);
+        public static final MotorConfig driveMotorConfig = new MotorConfig(SwerveConversions.rotationsToMeters(1, wheelCircumference, driveGearRatio), 60, driveLimit, driveMotorInvert, Neutral.COAST);
 
-        public static final MotorConfig angleMotorConfig = new MotorConfig(SwerveConversions.rotationsToDegrees(1, angleGearRatio), 1, angleLimit, false, Neutral.COAST);
+        public static final MotorConfig angleMotorConfig = new MotorConfig(SwerveConversions.rotationsToDegrees(1, angleGearRatio), 1, angleLimit, angleMotorInvert, Neutral.COAST);
 
         public static final PIDFFConfig drivePIDConfig = new PIDFFConfig(driveKP, driveKI, driveKD, driveKS, driveKV, driveKA);
 
@@ -150,7 +150,7 @@ public static class SwerveConstants {
             new SwerveMotorConfig(5, driveMotorConfig, drivePIDConfig),
             new SwerveMotorConfig(6, angleMotorConfig, anglePIDConfig),
             22,
-            109.775390625,
+            109.775390625 - 180,
             canCoderInvert,
             maxSpeed);
 

diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java
@@ -1,12 +1,16 @@
 package frc.team3128;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.team3128.commands.CmdSwerveDrive;
 import common.hardware.input.NAR_ButtonBoard;
 import common.hardware.input.NAR_Joystick;
 import common.hardware.input.NAR_XboxController;
+import common.hardware.input.NAR_XboxController.XboxButton;
 import common.utility.narwhaldashboard.NarwhalDashboard;
+import common.utility.shuffleboard.NAR_Shuffleboard;
 import frc.team3128.subsystems.Swerve;
 
 /**
@@ -27,6 +31,7 @@ public class RobotContainer {
     private NarwhalDashboard dashboard;
 
     public RobotContainer() {
+        NAR_Shuffleboard.WINDOW_WIDTH = 10;
 
         swerve = Swerve.getInstance();
 
@@ -42,6 +47,9 @@ public RobotContainer() {
     }   
 
     private void configureButtonBindings() {
-
+        controller.getButton(XboxButton.kB).onTrue(Commands.runOnce(()-> swerve.resetEncoders()));
+
+        rightStick.getButton(1).onTrue(Commands.runOnce(()-> swerve.zeroGyro(0)));
+
     }
 }
diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java
@@ -3,10 +3,12 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 
 import common.core.swerve.SwerveBase;
+import common.utility.shuffleboard.NAR_Shuffleboard;
 
 import static frc.team3128.Constants.SwerveConstants.*;
 import static frc.team3128.Constants.VisionConstants.*;
 
+import java.lang.reflect.Field;
 import java.util.function.Supplier;
 
 public class Swerve extends SwerveBase {
@@ -26,14 +28,34 @@ public static synchronized Swerve getInstance() {
         return instance;
     }
 
-    public Swerve() {
-        super(swerveKinematics, Mod0, Mod1, Mod2, Mod3);
+    private Swerve() {
+        super(swerveKinematics, SVR_STATE_STD, SVR_VISION_MEASUREMENT_STD, Mod0, Mod1, Mod2, Mod3);
         gyro = new Pigeon2(pigeonID);
         yaw = gyro.getYaw().asSupplier();
         pitch = gyro.getPitch().asSupplier();
         roll = gyro.getRoll().asSupplier();
-        initSwerveOdometry(SVR_STATE_STD, SVR_VISION_MEASUREMENT_STD);
         initShuffleboard();
+        // NAR_Shuffleboard.addData("Tab", "Tab1", ()-> getRobotVelocity().toString(), 1, 0);
+        // NAR_Shuffleboard.addData("Tab", "Tab2", ()-> getRobotVelocity().toString(), 2, 0);
+        // NAR_Shuffleboard.addData("Tab", "Tab3", ()-> getRobotVelocity().toString(), 3, 0);
+
+        // try {
+        //     final Field field = SwerveBase.class.getDeclaredField("useShuffleboard");
+        //     field.setAccessible(true);
+        //     field.setBoolean(this, false);
+        // } catch (NoSuchFieldException e) {
+        //     // TODO Auto-generated catch block
+        //     e.printStackTrace();
+        // } catch (SecurityException e) {
+        //     // TODO Auto-generated catch block
+        //     e.printStackTrace();
+        // } catch (IllegalArgumentException e) {
+        //     // TODO Auto-generated catch block
+        //     e.printStackTrace();
+        // } catch (IllegalAccessException e) {
+        //     // TODO Auto-generated catch block
+        //     e.printStackTrace();
+        // }
     }
 
     @Override

diff --git a/vendordeps/3128-common.json b/vendordeps/3128-common.json
@@ -1,6 +1,6 @@
 {
     "name": "3128-common",
-    "version": "1.1.7",
+    "version": "1.2.1",
     "uuid": "ae3fa5a2-78d9-47e8-921a-dba45b889445",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.github.Team3128",
             "artifactId": "3128-common",
-            "version": "1.1.7"
+            "version": "1.2.1"
         }
     ],
     "jniDependencies": [],