From 52d7659f3e0b619ccc2b9fa0051dd5e4474918d7 Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Mon, 20 Jan 2025 15:37:37 -0700
Subject: [PATCH] make autos

---
 .../deploy/pathplanner/autos/New Auto.auto    | 12 +++++
 .../pathplanner/paths/Example Path.path       | 20 +++----
 .../deploy/pathplanner/paths/New Path.path    | 54 +++++++++++++++++++
 src/main/java/frc4388/robot/Constants.java    |  4 +-
 .../java/frc4388/robot/RobotContainer.java    |  5 ++
 5 files changed, 84 insertions(+), 11 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
index 70b7ab2..5a4618b 100644
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -9,6 +9,18 @@
           "data": {
             "pathName": "Example Path"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoGotoPosition"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
         }
       ]
     }
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index 946269c..4bd51e2 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.415049342105263,
-        "y": 4.785115131578947
+        "x": 15.106960227272728,
+        "y": 2.120426136363636
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.905756578947368,
-        "y": 4.794736842105262
+        "x": 15.101585096858368,
+        "y": 2.396686756913594
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.8294407894736837,
-        "y": 5.862746710526316
+        "x": 15.106960227272728,
+        "y": 4.005056818181817
       },
       "prevControl": {
-        "x": 3.0693256578947365,
-        "y": 5.872368421052631
+        "x": 15.10799254912145,
+        "y": 3.6525397317875603
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -179.09061955080082
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -2.4366482468102095
+    "rotation": 178.99491399474587
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..c729144
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 14.398977272727272,
+        "y": 4.015028409090909
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.651107954545454,
+        "y": 4.005056818181817
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.525767045454545,
+        "y": 5.271448863636363
+      },
+      "prevControl": {
+        "x": 14.857670454545453,
+        "y": 5.690255681818181
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 177.06432655357875
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -178.8308606720925
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 25d3767..6027489 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -354,7 +354,9 @@ public final class Constants {
 
     public static final class FieldConstants {
         public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
+        public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0;
+        // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
+
         public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
 
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e211436..2f9d7cc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -35,6 +35,8 @@ import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoPositionCommand;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
+
+import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
 // import frc4388.robot.subsystems.LED;
@@ -92,12 +94,15 @@ public class RobotContainer {
     () -> autoplaybackName.get(), // lastAutoName
            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
            true, false);
+    private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision);
     
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         
+        NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition);
+
         configureButtonBindings();        
         configureVirtualButtonBindings();
         new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());