From a0393dc541829b26c4092f3529f631127219f282 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 26 Jan 2024 18:24:16 -0500
Subject: [PATCH 1/7] removed unused redudant method

---
 .../java/frc/robot/subsystems/drive/DriveSubsystem.java    | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index f1a73157..d7a61e61 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -404,13 +404,6 @@ private Pose2d parseEndPoint(TomlParseResult parseResult, String pose) {
     }
   }
 
-  private Pose2d parsePose2d(TomlParseResult parseResult, String pose) {
-    return new Pose2d(
-        parseResult.getTable(pose).getDouble("x"),
-        parseResult.getTable(pose).getDouble("y"),
-        Rotation2d.fromDegrees(parseResult.getTable(pose).getDouble("angle")));
-  }
-
   // Control Methods
   public void lockZero() {
     SwerveModule[] swerveModules = io.getSwerveModules();

From 85cf8cd4f9851b1a1eea02ff54b8a04398f93933 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 26 Jan 2024 19:37:49 -0500
Subject: [PATCH 2/7] paths, middle initial 1 to all middle notes

---
 .../paths/MiddleInitial1_MiddleNote1.toml     | 16 +++++++++++++++
 .../paths/MiddleInitial1_MiddleNote2.toml     | 16 +++++++++++++++
 .../paths/MiddleInitial1_MiddleNote3.toml     |  2 +-
 .../paths/MiddleInitial1_MiddleNote4.toml     | 20 +++++++++++++++++++
 .../paths/MiddleInitial1_MiddleNote5.toml     | 20 +++++++++++++++++++
 5 files changed, 73 insertions(+), 1 deletion(-)
 create mode 100644 src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml
 create mode 100644 src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml
 create mode 100644 src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
 create mode 100644 src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml

diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml
new file mode 100644
index 00000000..2c5009f3
--- /dev/null
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml
@@ -0,0 +1,16 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "MI1"
+
+[end_pose]
+    dataPoint = "M1"
+
+[[internal_points]]
+    x = 5.82
+    y = 7.45
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml
new file mode 100644
index 00000000..9e11f9b1
--- /dev/null
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml
@@ -0,0 +1,16 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "MI1"
+
+[end_pose]
+    dataPoint = "M2"
+
+[[internal_points]]
+    x = 5.80
+    y = 7.45
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
index e472b1cc..def2f495 100644
--- a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
@@ -17,4 +17,4 @@ target_yaw = 0.0 #degrees
 
 [[internal_points]]
     x = 4.71
-    y = 4.105
\ No newline at end of file
+    y = 4.11
\ No newline at end of file
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
new file mode 100644
index 00000000..e6d9dd99
--- /dev/null
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
@@ -0,0 +1,20 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "MI1"
+
+[end_pose]
+    dataPoint = "M4"
+
+[[internal_points]]
+    x = 4.41
+    y = 4.83
+
+[[internal_points]]
+    x = 4.71
+    y = 4.05
\ No newline at end of file
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml
new file mode 100644
index 00000000..9ed7a8b4
--- /dev/null
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml
@@ -0,0 +1,20 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "MI1"
+
+[end_pose]
+    dataPoint = "M5"
+
+[[internal_points]]
+    x = 4.41
+    y = 4.83
+
+[[internal_points]]
+    x = 5.00
+    y = 4.05
\ No newline at end of file

From 721c9ec606114d607d8e0a488ef0d39fa3718b8f Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 26 Jan 2024 19:38:18 -0500
Subject: [PATCH 3/7] middle note positions, M1 to M2 path

---
 .../deploy/paths/MiddleNote1_MiddleNote2.toml    | 16 ++++++++++++++++
 .../java/frc/robot/constants/AutonConstants.java | 10 +++++-----
 2 files changed, 21 insertions(+), 5 deletions(-)
 create mode 100644 src/main/deploy/paths/MiddleNote1_MiddleNote2.toml

diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml
new file mode 100644
index 00000000..1b6e61d7
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml
@@ -0,0 +1,16 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "M1"
+
+[end_pose]
+    dataPoint = "M2"
+
+[[internal_points]]
+    x = 8.27
+    y = 1.59
diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java
index 58c94b37..dbf4dcd7 100644
--- a/src/main/java/frc/robot/constants/AutonConstants.java
+++ b/src/main/java/frc/robot/constants/AutonConstants.java
@@ -14,11 +14,11 @@ public final class Setpoints {
     public static final Pose2d W3 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
 
     // Middle Notes
-    public static final Pose2d M1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
-    public static final Pose2d M2 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
-    public static final Pose2d M3 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
-    public static final Pose2d M4 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
-    public static final Pose2d M5 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));
+    public static final Pose2d M1 = new Pose2d(8.270494, 0.752856, Rotation2d.fromDegrees(0.0));
+    public static final Pose2d M2 = new Pose2d(8.270494, 2.429256, Rotation2d.fromDegrees(0.0));
+    public static final Pose2d M3 = new Pose2d(8.270494, 4.105275, Rotation2d.fromDegrees(0.0));
+    public static final Pose2d M4 = new Pose2d(8.270494, 5.781294, Rotation2d.fromDegrees(0.0));
+    public static final Pose2d M5 = new Pose2d(8.270494, 7.457694, Rotation2d.fromDegrees(0.0));
 
     // Shooting Positions
     public static final Pose2d AS1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));

From 6899a10a92ed6b39da142c3b43b61ad4a6341ad3 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 26 Jan 2024 20:01:43 -0500
Subject: [PATCH 4/7] updated constant parsing to allow optional angle
 parameter

---
 .../subsystems/drive/DriveSubsystem.java      | 48 +++++++++++++------
 1 file changed, 34 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index d7a61e61..644511e4 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -355,47 +355,67 @@ public PathData generateTrajectory(String trajectoryName) {
     }
   }
 
-  private Pose2d parseEndPoint(TomlParseResult parseResult, String pose) {
-    TomlTable table = parseResult.getTable(pose);
+  private Pose2d parseEndPoint(TomlParseResult parseResult, String poseName) {
+    TomlTable table = parseResult.getTable(poseName);
 
     if (table.contains("dataPoint")) {
+      Pose2d pose;
+
       switch (table.getString("dataPoint")) {
           // Starting Positions
         case "MI1":
-          return Setpoints.MI1;
+          pose = Setpoints.MI1;
+          break;
 
           // Wing Notes
         case "W1":
-          return Setpoints.W1;
+          pose = Setpoints.W1;
+          break;
         case "W2":
-          return Setpoints.W2;
+          pose = Setpoints.W2;
+          break;
         case "W3":
-          return Setpoints.W3;
+          pose = Setpoints.W3;
+          break;
 
           // Middle Notes
         case "M1":
-          return Setpoints.M1;
+          pose = Setpoints.M1;
+          break;
         case "M2":
-          return Setpoints.M2;
+          pose = Setpoints.M2;
+          break;
         case "M3":
-          return Setpoints.M3;
+          pose = Setpoints.M3;
+          break;
         case "M4":
-          return Setpoints.M4;
+          pose = Setpoints.M4;
+          break;
         case "M5":
-          return Setpoints.M5;
+          pose = Setpoints.M5;
+          break;
 
           // Shooting Positions
         case "AS1":
-          return Setpoints.AS1;
+          pose = Setpoints.AS1;
+          break;
         case "MS1":
-          return Setpoints.MS1;
+          pose = Setpoints.MS1;
+          break;
         case "NAS1":
-          return Setpoints.NAS1;
+          pose = Setpoints.NAS1;
+          break;
 
         default:
           logger.warn("Bad data point {}", table.getString("dataPoint"));
           return new Pose2d();
       }
+
+      if (table.contains("angle")) {
+        pose = new Pose2d(pose.getX(), pose.getX(), Rotation2d.fromDegrees(table.getDouble("angle")));
+      }
+
+      return pose;
     } else {
       return new Pose2d(
           table.getDouble("x"),

From 0f66f573ab0be155c446a90345bd7b5d58de8b95 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 26 Jan 2024 20:27:18 -0500
Subject: [PATCH 5/7] created middle note to middle note paths

---
 .../paths/MiddleInitial1_MiddleNote3.toml      |  2 +-
 .../paths/MiddleInitial1_MiddleNote4.toml      |  2 +-
 .../paths/MiddleInitial1_MiddleNote5.toml      |  2 +-
 .../deploy/paths/MiddleNote1_MiddleNote2.toml  |  4 +++-
 .../deploy/paths/MiddleNote2_MiddleNote1.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote2_MiddleNote3.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote2_MiddleNote4.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote3_MiddleNote2.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote3_MiddleNote4.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote4_MiddleNote2.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote4_MiddleNote3.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote4_MiddleNote5.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote5_MiddleNote4.toml  | 18 ++++++++++++++++++
 13 files changed, 168 insertions(+), 4 deletions(-)
 create mode 100644 src/main/deploy/paths/MiddleNote2_MiddleNote1.toml
 create mode 100644 src/main/deploy/paths/MiddleNote2_MiddleNote3.toml
 create mode 100644 src/main/deploy/paths/MiddleNote2_MiddleNote4.toml
 create mode 100644 src/main/deploy/paths/MiddleNote3_MiddleNote2.toml
 create mode 100644 src/main/deploy/paths/MiddleNote3_MiddleNote4.toml
 create mode 100644 src/main/deploy/paths/MiddleNote4_MiddleNote2.toml
 create mode 100644 src/main/deploy/paths/MiddleNote4_MiddleNote3.toml
 create mode 100644 src/main/deploy/paths/MiddleNote4_MiddleNote5.toml
 create mode 100644 src/main/deploy/paths/MiddleNote5_MiddleNote4.toml

diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
index def2f495..b6b3886b 100644
--- a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml
@@ -17,4 +17,4 @@ target_yaw = 0.0 #degrees
 
 [[internal_points]]
     x = 4.71
-    y = 4.11
\ No newline at end of file
+    y = 4.11
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
index e6d9dd99..5366f31d 100644
--- a/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml
@@ -17,4 +17,4 @@ target_yaw = 0.0 #degrees
 
 [[internal_points]]
     x = 4.71
-    y = 4.05
\ No newline at end of file
+    y = 4.05
diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml
index 9ed7a8b4..51b28901 100644
--- a/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml
+++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml
@@ -17,4 +17,4 @@ target_yaw = 0.0 #degrees
 
 [[internal_points]]
     x = 5.00
-    y = 4.05
\ No newline at end of file
+    y = 4.05
diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml
index 1b6e61d7..1b26b19e 100644
--- a/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml
+++ b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml
@@ -3,13 +3,15 @@ max_acceleration = 1.0 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false
-target_yaw = 0.0 #degrees
+target_yaw = -90.0 #degrees
 
 [start_pose]
     dataPoint = "M1"
+    angle = -90.0
 
 [end_pose]
     dataPoint = "M2"
+    angle = -90.0
 
 [[internal_points]]
     x = 8.27
diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml
new file mode 100644
index 00000000..03af096a
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M2"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M1"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 1.59
diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml
new file mode 100644
index 00000000..d9d6e5f2
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M2"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M3"
+    angle = -90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 3.27
diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml
new file mode 100644
index 00000000..293b4b1f
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M2"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M4"
+    angle = -90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 4.11
diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml
new file mode 100644
index 00000000..33c6d52f
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M3"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M2"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 3.27
diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml
new file mode 100644
index 00000000..42a4561b
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M3"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M4"
+    angle = -90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 4.94
diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml
new file mode 100644
index 00000000..1e897eac
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M2"
+    angle = 90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 4.11
diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml
new file mode 100644
index 00000000..b036de74
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M3"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 4.94
diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml
new file mode 100644
index 00000000..3506813f
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M5"
+    angle = -90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 6.62
diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml
new file mode 100644
index 00000000..d19ebdf6
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M4"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 6.62

From 5824f6fdaf8c5b293a68b749443298693476e834 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 2 Feb 2024 17:42:54 -0500
Subject: [PATCH 6/7] added paths M1 to all middle ntoes

---
 .../deploy/paths/MiddleNote1_MiddleNote3.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote1_MiddleNote4.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote1_MiddleNote5.toml  | 18 ++++++++++++++++++
 3 files changed, 54 insertions(+)
 create mode 100644 src/main/deploy/paths/MiddleNote1_MiddleNote3.toml
 create mode 100644 src/main/deploy/paths/MiddleNote1_MiddleNote4.toml
 create mode 100644 src/main/deploy/paths/MiddleNote1_MiddleNote5.toml

diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml
new file mode 100644
index 00000000..7ed07b88
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M1"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M3"
+    angle = -90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 2.43
diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml
new file mode 100644
index 00000000..60c52423
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M1"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M4"
+    angle = -90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 3.27
diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml
new file mode 100644
index 00000000..54e128c4
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M1"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M4"
+    angle = -90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 4.11

From f34a6c2231858226a2daef0903ddaccf44d808a3 Mon Sep 17 00:00:00 2001
From: IRoot <imroot07@gmail.com>
Date: Fri, 2 Feb 2024 18:30:17 -0500
Subject: [PATCH 7/7] all middle note to middle note paths

---
 .../deploy/paths/MiddleNote2_MiddleNote5.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote3_MiddleNote1.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote3_MiddleNote5.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote4_MiddleNote1.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote5_MiddleNote1.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote5_MiddleNote2.toml  | 18 ++++++++++++++++++
 .../deploy/paths/MiddleNote5_MiddleNote3.toml  | 18 ++++++++++++++++++
 .../frc/robot/constants/AutonConstants.java    |  3 +++
 8 files changed, 129 insertions(+)
 create mode 100644 src/main/deploy/paths/MiddleNote2_MiddleNote5.toml
 create mode 100644 src/main/deploy/paths/MiddleNote3_MiddleNote1.toml
 create mode 100644 src/main/deploy/paths/MiddleNote3_MiddleNote5.toml
 create mode 100644 src/main/deploy/paths/MiddleNote4_MiddleNote1.toml
 create mode 100644 src/main/deploy/paths/MiddleNote5_MiddleNote1.toml
 create mode 100644 src/main/deploy/paths/MiddleNote5_MiddleNote2.toml
 create mode 100644 src/main/deploy/paths/MiddleNote5_MiddleNote3.toml

diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml
new file mode 100644
index 00000000..1d864122
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M2"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M5"
+    angle = -90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 4.94
diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml
new file mode 100644
index 00000000..ebb3894b
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M3"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M1"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 2.43
diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml
new file mode 100644
index 00000000..03f51953
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -90.0 #degrees
+
+[start_pose]
+    dataPoint = "M3"
+    angle = -90.0
+
+[end_pose]
+    dataPoint = "M5"
+    angle = -90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 5.78
diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml
new file mode 100644
index 00000000..8df061a1
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M1"
+    angle = 90.0
+
+[[internal_points]]
+    x = 7.60
+    y = 3.27
diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml
new file mode 100644
index 00000000..3d148507
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M1"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 4.11
diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml
new file mode 100644
index 00000000..3a053103
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M2"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 4.94
diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml
new file mode 100644
index 00000000..1d333ccb
--- /dev/null
+++ b/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml
@@ -0,0 +1,18 @@
+max_velocity = 4.0 #m/s
+max_acceleration = 1.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 90.0 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    angle = 90.0
+
+[end_pose]
+    dataPoint = "M3"
+    angle = 90.0
+
+[[internal_points]]
+    x = 8.27
+    y = 5.78
diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java
index dbf4dcd7..f113dc7c 100644
--- a/src/main/java/frc/robot/constants/AutonConstants.java
+++ b/src/main/java/frc/robot/constants/AutonConstants.java
@@ -4,6 +4,9 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public final class AutonConstants {
+  // field width (x) = 16.540988 m;
+  // field height (y) = 8.21055 m;
+
   public final class Setpoints {
     // Starting Positions
     public static final Pose2d MI1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0));