testing changes

grt192 · Feb 11, 2024 · 2a22f3f · 2a22f3f
1 parent ccaa4aa
commit 2a22f3f
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 8 deletions.
diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json
@@ -7,5 +7,10 @@
   },
   "Server": {
     "open": true
+  },
+  "transitory": {
+    "Testing": {
+      "open": true
+    }
   }
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -74,7 +74,7 @@ public static class SwerveConstants { //ADD 90 degrees to all
     public static final int BR_STEER = 7; 
     public static final double BR_OFFSET = 2.66 + Math.PI * 1.0 / 4.0;
 
-    private static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0);
+    public static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0);
     public static final Translation2d FL_POS = new Translation2d(MODULE_DIST, MODULE_DIST);
     public static final Translation2d FR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST);
     public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);
@@ -140,7 +140,9 @@ public static class AutoAlignConstants {
     // Y: APRILTAG_POS 
     public static final Pose2d BLUE_SOURCE_POSE = new Pose2d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Rotation2d.fromDegrees(120)); 
 
-    public static final Pose2d BLUE_AMP_POSE = new Pose2d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.00 - robotRadius), Rotation2d.fromDegrees(270));
+    // public static final Pose2d BLUE_AMP_POSE = new Pose2d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.00 - robotRadius - 11.0), Rotation2d.fromDegrees(90));
+    public static final Pose2d BLUE_AMP_POSE = new Pose2d(Units.inchesToMeters(323.00 - robotRadius - 11.0), Units.inchesToMeters(72.5), Rotation2d.fromDegrees(90));
+    public static final Pose2d ORIGIN_POSE = new Pose2d(0, 0, new Rotation2d(0));
 
     // X: APRILTAG_POS + SUBWOOFER_OFFSET + ROBOT_X_CENTER
     // ROT: rotated 180 deg from Apriltag pos since robots shoots from back

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -155,7 +155,7 @@ public RobotContainer() {
         // module = new SwerveModule(6, 7, 0);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
 
-        SmartDashboard.putData("AUTO ALIGN BLUE AMP", AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE));
+        swerveCrauton.add("AUTO ALIGN BLUE AMP", AlignCommand.getAlignCommand(AutoAlignConstants.ORIGIN_POSE));
 
         camera1 = new UsbCamera("camera1", 0);
         camera1.setFPS(60);

diff --git a/src/main/java/frc/robot/commands/align/AlignCommand.java b/src/main/java/frc/robot/commands/align/AlignCommand.java
@@ -25,7 +25,7 @@ static public Command getAlignCommand(Pose2d targetPose){
                 Units.degreesToRadians(360), Units.degreesToRadians(540)
             ), 
     0, 
-    2.0
+    0.0
         );
     }
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.Timer;
@@ -159,10 +160,10 @@ public SwerveSubsystem() {
             this::getRobotRelativeChassisSpeeds, 
             this::setRobotRelativeDrivePowers, 
             new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
-                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
-                        4.5, // Max module speed, in m/s
-                        0.4, // Drive base radius in meters. Distance from robot center to furthest module.
+                        new PIDConstants(1.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(1.0, 0.0, 0.0), // Rotation PID constants
+                        1, // Max module speed, in m/s
+                        Constants.SwerveConstants.MODULE_DIST * 1.414, // Drive base radius in meters. Distance from robot center to furthest module.
                         new ReplanningConfig() // Default path replanning config. See the API for the options here
                 ),
             () -> {
-Original file line number
+Diff line change
@@ Expand Up / @@ -25,7 +25,7 @@ static public Command getAlignCommand(Pose2d targetPose){ @@
                     Units.degreesToRadians(360), Units.degreesToRadians(540)
                 ),
 ,
-.0
+.0
             );
         }
@@ Expand Down @@