appears to be working

Frc5572 · Jan 7, 2024 · 5ee5709 · 5ee5709
1 parent 8f0721b
commit 5ee5709
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 23 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -34,7 +34,7 @@
     "editor.defaultFormatter": "redhat.java",
   },
   "editor.codeActionsOnSave": {
-    "source.organizeImports": true
+    "source.organizeImports": "explicit"
   },
   "editor.formatOnSave": true,
   "editor.formatOnPaste": true,

diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -32,7 +32,7 @@ public class SwerveModule {
     SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS,
         Constants.Swerve.driveKV, Constants.Swerve.driveKA);
 
-    SparkMaxPIDController angleMotorPIDController = angleMotor.getPIDController();
+    SparkMaxPIDController angleMotorPIDController;
 
     /**
      * Creates an instance of a Swerve Module
@@ -50,13 +50,13 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants
 
         /* Angle Motor Config */
         angleMotor = new CANSparkMax(constants.angleMotorID, MotorType.kBrushless);
+        angleMotorPIDController = angleMotor.getPIDController();
+        angleEncoderBuiltIn = angleMotor.getEncoder();
         configAngleMotor();
 
         /* Drive Motor Config */
         driveMotor = new TalonFX(constants.driveMotorID);
         configDriveMotor();
-
-        angleEncoderBuiltIn = angleMotor.getEncoder();
         angleEncoderBuiltIn
             .setPositionConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution);
         angleEncoderBuiltIn

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -92,7 +92,12 @@ public static final class Swerve {
         public static final boolean driveEnableCurrentLimit = true;
 
         /* Angle Motor PID Values */
-        public static final double angleKP = 0.0;
+        // public static final double angleKP = 0.6;
+        // public static final double angleKI = 0.0;
+        // public static final double angleKD = 12.0;
+        // public static final double angleKF = 0.0;
+
+        public static final double angleKP = 0.008;
         public static final double angleKI = 0.0;
         public static final double angleKD = 0.0;
         public static final double angleKF = 0.0;
@@ -121,7 +126,7 @@ public static final class Swerve {
         // public static final NeutralMode angleNeutralMode = NeutralMode.Coast;
         public static final IdleMode angleNeutralMode = IdleMode.kCoast;
 
-        public static final NeutralMode driveNeutralMode = NeutralMode.Brake;
+        public static final NeutralMode driveNeutralMode = NeutralMode.Coast;
 
         /* Motor Inverts */
         public static final boolean driveMotorInvert = true;
@@ -135,10 +140,10 @@ public static final class Swerve {
          * Front Left Module - Module 0.
          */
         public static final class Mod0 {
-            public static final int driveMotorID = 50;
+            public static final int driveMotorID = 10;
             public static final int angleMotorID = 8;
-            public static final int canCoderID = 3;
-            public static final double angleOffset = 139.043;
+            public static final int canCoderID = 10;
+            public static final double angleOffset = 292.588;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }
@@ -147,10 +152,10 @@ public static final class Mod0 {
          * Front Right Module - Module 1. PROBLEM CHILD
          */
         public static final class Mod1 {
-            public static final int driveMotorID = 51;
+            public static final int driveMotorID = 3;
             public static final int angleMotorID = 52;
-            public static final int canCoderID = 2;
-            public static final double angleOffset = 118.564;
+            public static final int canCoderID = 1;
+            public static final double angleOffset = 304.014;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }
@@ -159,10 +164,10 @@ public static final class Mod1 {
          * Back Left Module - Module 2.
          */
         public static final class Mod2 {
-            public static final int driveMotorID = 4;
+            public static final int driveMotorID = 2;
             public static final int angleMotorID = 2;
-            public static final int canCoderID = 1;
-            public static final double angleOffset = 259.805;
+            public static final int canCoderID = 2;
+            public static final double angleOffset = 146.426;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }
@@ -171,10 +176,10 @@ public static final class Mod2 {
          * Back Right Module - Module 3.
          */
         public static final class Mod3 {
-            public static final int driveMotorID = 3;
+            public static final int driveMotorID = 6;
             public static final int angleMotorID = 53;
             public static final int canCoderID = 4;
-            public static final double angleOffset = 31.816;
+            public static final double angleOffset = 5.273;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,9 +4,6 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -15,7 +12,6 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.TeleopSwerve;
-import frc.robot.commands.TestTransform;
 import frc.robot.subsystems.Swerve;
 
 /**
@@ -52,8 +48,8 @@ public RobotContainer() {
     private void configureButtonBindings() {
         /* Driver Buttons */
         driver.y().whileTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
-        driver.x().whileTrue(new TestTransform(s_Swerve,
-            new Transform2d(new Translation2d(1, 0), Rotation2d.fromDegrees(180)), 7));
+        // driver.x().whileTrue(new TestTransform(s_Swerve,
+        // new Transform2d(new Translation2d(1, 0), Rotation2d.fromDegrees(180)), 7));
         driver.a().whileTrue(new InstantCommand(() -> s_Swerve.resetInitialized()));
     }