From 5ef4432f198944de628fb90c8023b8fe06f12e0c Mon Sep 17 00:00:00 2001
From: issy <lshields822@gmail.com>
Date: Thu, 23 Jan 2025 09:33:17 -0800
Subject: [PATCH 01/44] setup basic input

---
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 networktables.json                            |   1 +
 simgui-ds.json                                |  98 ++++++++++++++++++
 simgui-window.json                            |  65 ++++++++++++
 simgui.json                                   |  19 ++++
 .../org/ironriders/core/RobotContainer.java   |   6 ++
 .../org/ironriders/drive/DriveCommands.java   |   6 ++
 .../org/ironriders/drive/DriveConstants.java  |   1 +
 11 files changed, 196 insertions(+)
 create mode 100644 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
 create mode 100644 networktables.json
 create mode 100644 simgui-ds.json
 create mode 100644 simgui-window.json
 create mode 100644 simgui.json

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..4c5e507e4644718398c1d5ffbeb2754e08bad02b
GIT binary patch
literal 2048
zcmcgtJ!n%=6uz1?2}R_g4uMkoYAu38Nf$v%%+ngvf@pP+wseRhqM#5oh**cGh=?F$
zk!Ez$fVP>OihVkWLJAoKrGu@5H5I|<Ac$1sJNKL~yxhUXp9|r<bH4MPzx$K->iAdn
zpV#O>WuH=RX0AWIf8ywjlQ8o)A_2TKY?{OR>$BUP#ab=Bs^&kE3}gTL`)}6&RArif
zQt$U;YnzGxZTY(0F|AbfeseGy$gbYvDCwU&IXZC9>vzb0S^SD3%e5og-#7Tt4(0=|
z_g>U~WTiYQhL!(wr;a=Rfb*->y{8Dzi;g^FeUW*xj}Nj0U&5%e_!op<dd_j+?+cE;
z>!Q!@;(m8XAn>JpgjX9-?=y~TRk&X8qk`KF#!Wr~p{^I@YkKY!m-_N0F6K~``kKKa
zF8r|%$!?xI@o8Vc#~g@53w-#)k5Q$P2YH@JKd$E+7a%45{V4H#%zjW$>BDW^N8=0g
z?MJeYOKH821(CR`i0_Ku+s7PV(z>V%5_5MpvIRWg249T`;E24}Lo*1T1<41Q%SI(4
zBDKrA=f^7N5^GPuXY{bs?qz81rQhLs!#k|HL4|t)!%)Z={uZ;D-*ZC#L8A-py0TjO
z^tRdDyDK*`ZT_mz(l@mOO0Iv)iv5Ejh7XZA|Ax7xzB!<5I_Hq!+YkA?<gW65K*D}Y
zB9ybthus=x9d=Lfl1ShK%h@93g`c{xb*i&3r#vGs`YTjN^tnZzhSUxJS1TOPk~}z{
zO3rzHk?)fubp!V$-zSj`k#jfMAA_1X8PoR=dTM?5%ZZ*73wB_bq_==R=oc)C9sJ|z
z_k(o3q`&$W?!$^CbUfrke$*9L>&<4wjd_!Ybl+VPW<~1jK0T%1ZA<RYnV~!SzW)MU
C7Ck8d

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..06b0cffa4eb6c1e4b7b05a185b9956de49f215b3
GIT binary patch
literal 2048
zcmcgtO=uHQ5FTxslp?ZV!9Xc)TZ<r4+M|@1tu@A8H1!}=dI%z-pb#{O*h>@<5rkZ%
zIeOE8wmEq!cJ&}0QYa!QJ=l7%rXsi=1g&V?*_m(Iyn`2i9)#JM@B8NGy?vW^bnFZL
z=QTc99VIHI=LS;yk59}v9cKMTn-4FKnc=Aa>TI90Sg)tnX#NA+DE6<P|7QPBDBb*%
zem9P;Z)N@0m1}nUG*Rtdb2#SP?%v`!`=2@`J8;(>a5#Qhc}2CA`jLR|8Twee=z%Zw
zUJQ5~<vK$GllOC1kazOF)Tgz5rx?$xk2<;jwt0?E4vU3e(@;hE1=W|INgnV$#WDAi
z`m?)b-kq8VdTk%+X%qT=DtWD{^b37Val0Y7sYfF8^}KR5*mus$b9tH<`%u&Vn*L%f
z_&A4TkL)|^`COpKKCp%t^x%UhC{gmD>}S$T1pAv%U`XftQI`EN=RrT^_qT#M8lOdP
zKh$$vP6g*!P>Z{Y{3YeRc_jI@YzckAV&9$35`hl5p{GL%IBM_i;63m^3tA60n~7;g
zWNNi{&gZJ<I@TXUFX&;Xm1TIPOTWYO20QAO{0{f}fssfs`mABIzUxHz{-6u3K3SnZ
z-8Gwid*yn%g-?ah-_i<@UjHzP^Fttx2eo+rjd19`K0r3V=djS*59ECr)$@Qw{HA88
zWJHg+H6}jduHt32zz0_{MXn1TUD!6&*`FoXsOxAQcSGM}#T(i;_%Bx_pQUyDe;ir*
zp63_kJ~<i&?rFY{$g6hlhVYr5OLo!B$#`%NRZidHd$uQDOf1-b-~@jQ_<?`^;=955
sdA@Mze9m{71M!;H>p49Ye1})nKQnSWxW86kF1WAVDgX|?rS}K?U(32XA^-pY

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..03453a98009b1163e10942b1d4e8ed81a2038071
GIT binary patch
literal 2048
zcmcgtOK4M35FKs$p@;-@5h%r1YY|+Ob}1$1X^gR(T3wVX6@rK;SO^+K?B*#&L=du(
zX6a4?+GZsP#YP3`B84slr3+gZ)>H(a3&9T>-`tt=c)1HVelCQ0GiT13x!=5{vCs6M
z$8djnKarQ3>q{Ouessp|u<O@b0(d2Dhok<>b35IoS}nOw^Y7V4v48#jH~W7=sm7o5
z`|a4qR_1?Mz2>w|5moLs24jKk>M0Df|FKilfmf@2F2^rxUO{cCcEsfS7C+W1eBjGH
z7fl{Vxz2#V<p11lbjR;WeOljln(>_as59EzGSBhxL9yVg43#uLul(Y3(LuhiIOe^q
z{_Gx^?=BSrU)zT~Z9>0KMK@lSe!-6^?$iah`4B>1i>0e(-C39C<*P2%p`!gYg27zm
zV;>UTvhK|1eE}cqz#JanBOiH!5+x4FdM5mKv%c*L4CwrR6lHzv`JkWTyPIYn^-scg
z9_T)<B+WkN)#9!~-_yL;4@F<gdgu!l>+Wur2zbCPJ{?xTReNU#uR-uEXg%0$CZ>u|
zYPPp0N6Y6sHXeg7=!n~#WqIDE-{CncJL-BthkJv-P$U@r*05c_>PGT|i7qtz6c7EW
z?X%f8SEf@<`IHa+EzJPw^$(-iKLpzFp%&-g3Wx5igXHizhXvocFXtt#`vHsi4OJ**
zgpYVUEk5E^#fxfz_pfFOTo-wCVcSG!Z<eZ7T^F5x*K@3RUHeA<i#5@UYn||3=sC|V
z$$f$za9{O4Kv(VDb;&p76GSto+RQyfJ6{(cO^-c0x!?qW6Z{tN2mgY>cO&@6Gw%oc
qX6XDpod?1dt=D~K!o0(4>Yp9DW$tf2J7Vr@mlA*@Kd$!&`M&@edpy(t

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..b0ed0ccf76b5e41a435a4bd55620d2d4e4b811bd
GIT binary patch
literal 2048
zcmcgsKWGzS6n~nu2}R^khd?P_twnGs?I<PYw8pfHRzaxHA%lp5La0H+I-DXRf{;bb
z=%xW}Gr1^wI*3CG83d(+txJ0q!E+Eq)Og?b-tV~kh7MJl<=uO~-|zkVzAvlv?DIma
zm8VVoO$;>-5xIr=!TjOVC+<60v$mfK;l(jC9QWU!-{YvhSLoFDcziRy|7ZWNsL=VB
zo`0TP+phFKmTuTxvqa5@oso2Cd;6;s%KzHK*@1iBprhhfgjW??ZyneCz|g0=SP#72
ze@XLHRMi<~n1Vn1bl#~)T%T4Bo>6#NeAJm7NIh5asS&o&OB$*RzaskD8_olMAUNi3
zi$AxY=iMubpqKWMp0=Rh*PPdCaKF%}1-E_1O+6B!uebH<dhaT)nk$gJ*h5qL>x4h0
z1s~^->*KvEy*d}@u?MA5Kj^^+k5QuBQQl`R$msoM1Q?e2e%E+E<~-=9wt7d;;eTVj
z{Y1`jF|W_DA{K8I`CZ|Ce9HNytc$*2v3F-HN1y|4=xJO4N9=>$Y6s!pg4Ba871NRt
znUeP2#mUBn?Ai<H89nACS%&9cUJoxD?6~WO9Uce+qp@K8Tf%02&x!HFL0=|)vLb(W
z*Ien-)tiL`pDK~REeVi3|0s&{L!cWUVsZbCaO8eCLbkf+u+ZC&`M#8{@p-@^ep@os
zi>yc78e<=EPw<*p;6qErs;Ubf&Fq}+87Prw)Wv+2=D5DMsN+lD;J;hue3sO~eI~i*
z<pq9Ej`R&Yka}Ol`eNsAF+W<=20qZcmdbqX9XyAX%IbHGdZ>@SGW4s{Nx9wN_e%X`
U>96nXwEo|=<o%o*y{q5*U&SFZ&;S4c

literal 0
HcmV?d00001

diff --git a/networktables.json b/networktables.json
new file mode 100644
index 0000000..fe51488
--- /dev/null
+++ b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..c2bdbe4
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,98 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {},
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}
diff --git a/simgui-window.json b/simgui-window.json
new file mode 100644
index 0000000..a18dffa
--- /dev/null
+++ b/simgui-window.json
@@ -0,0 +1,65 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "font": "Proggy Dotted",
+      "fps": "120",
+      "height": "720",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1280",
+      "xpos": "207",
+      "ypos": "23"
+    }
+  },
+  "Window": {
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "5,540",
+      "Size": "169,184"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "338,448",
+      "Size": "796,155"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "250,277",
+      "Size": "750,185"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "250,130",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "223,218"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "103,73",
+      "Size": "135,219"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "99,116"
+    }
+  }
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 0000000..56b55ad
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,19 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Alerts": "Alerts",
+      "/SmartDashboard/Auto Select": "String Chooser",
+      "/SmartDashboard/Encoders": "Alerts",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/IMU": "Alerts",
+      "/SmartDashboard/JSON": "Alerts",
+      "/SmartDashboard/Motors": "Alerts",
+      "/SmartDashboard/Pigeon 2 (v6) [11]": "Gyro",
+      "/SmartDashboard/Swerve Drive": "Alerts"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}
diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 4bbeb03..da24425 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -30,6 +31,9 @@ public class RobotContainer {
 
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
+  
+  private final CommandGenericHID secondaryController =
+            new CommandGenericHID(DriveConstants.SECONDARY_CONTROLLER_PORT);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -51,6 +55,8 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
+    secondaryController.button(1).onTrue(driveCommands.autoLeave());
+
     driveSubsystem.setDefaultCommand(
         driveCommands.driveTeleop(
             () -> Utils.controlCurve(
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index bd1b16f..5dc5d1f 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -39,4 +39,10 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 			);
 		});
 	}
+
+	public Command autoLeave() {
+		return driveSubsystem.runOnce(() -> {
+			System.out.println("testjdff");
+		});
+	}
 }
diff --git a/src/main/java/org/ironriders/drive/DriveConstants.java b/src/main/java/org/ironriders/drive/DriveConstants.java
index 45d7a92..0b83f3c 100644
--- a/src/main/java/org/ironriders/drive/DriveConstants.java
+++ b/src/main/java/org/ironriders/drive/DriveConstants.java
@@ -9,6 +9,7 @@ public class DriveConstants {
 
     // Ports, IDs, Configs, etc.
     public static final int PRIMARY_CONTROLLER_PORT = 0;
+    public static final int SECONDARY_CONTROLLER_PORT = 1;
 
     public static final File SWERVE_JSON_DIRECTORY = new File(Filesystem.getDeployDirectory(), "swerve");
     public static final PPHolonomicDriveController HOLONOMIC_CONFIG = 

From f0d8f04052538a17343b5dfa185a8e509e2811b1 Mon Sep 17 00:00:00 2001
From: Mr George the dying leaf
 <108304257+ChloroticLeaf@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:39:28 -0800
Subject: [PATCH 02/44] Update README.md

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index dc633b9..3946b41 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,5 @@
+# This is the Auto branch. 
+
 # *Iron Riders* - Season 2025 Repository
 
 ![image](https://github.com/user-attachments/assets/65a361c1-4db4-4ecb-b378-381c4e049c19)

From 7d54a632c63e761e87aca554553c2eabbd473985 Mon Sep 17 00:00:00 2001
From: issy <lshields822@gmail.com>
Date: Tue, 28 Jan 2025 16:33:00 -0800
Subject: [PATCH 03/44] kinda work

---
 .wpilib/wpilib_preferences.json               |   6 ++
 build.gradle                                  |   3 +-
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 simgui-ds.json                                |   4 +-
 simgui-window.json                            |  22 +++----
 src/deploy/pathplanner/navgrid.json           |   1 +
 src/deploy/pathplanner/paths/NAMEME.path      |  54 ++++++++++++++++++
 src/deploy/pathplanner/settings.json          |  32 +++++++++++
 .../org/ironriders/core/RobotContainer.java   |  17 ++++--
 .../org/ironriders/drive/DriveCommands.java   |  27 +++++++--
 13 files changed, 142 insertions(+), 24 deletions(-)
 create mode 100644 .wpilib/wpilib_preferences.json
 create mode 100644 src/deploy/pathplanner/navgrid.json
 create mode 100644 src/deploy/pathplanner/paths/NAMEME.path
 create mode 100644 src/deploy/pathplanner/settings.json

diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
new file mode 100644
index 0000000..a7d6bd1
--- /dev/null
+++ b/.wpilib/wpilib_preferences.json
@@ -0,0 +1,6 @@
+{
+    "currentLanguage": "none",
+    "enableCppIntellisense": false,
+    "projectYear": "none",
+    "teamNumber": 4180
+}
\ No newline at end of file
diff --git a/build.gradle b/build.gradle
index 2d1fedd..8d370d9 100644
--- a/build.gradle
+++ b/build.gradle
@@ -33,8 +33,7 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
-                                           // longer exist in deploy directory of this project
+                    deleteOldFiles = true // helps with pathplaner but could cause issue maybe
                 }
             }
         }
diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index 4c5e507e4644718398c1d5ffbeb2754e08bad02b..32ba0e8f95b8315a9dab472ca1f27c83bb434f3d 100644
GIT binary patch
delta 33
ocmZn=Xb{-Yz{q--Va~sW69uigjxbF5vw{H#m?jFEEn;E=0L!8ZyZ`_I

delta 83
zcmZn=Xb{-Yz{qlt!T$e5K^wbYK`;3jfZ)gdCI!}N28IpG{~NLZX=%k0PZ0k-7f2Q%
Tf8c<s14z6AtZwr^MkY1@Vf`8l

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 06b0cffa4eb6c1e4b7b05a185b9956de49f215b3..a038c85650370896e53a790387370c1fea55fc73 100644
GIT binary patch
delta 62
zcmZn=Xb|9d$*}X)*)wNsCkiG{RN$Og;=}TSA^0np#|z}K9%h*HZ{b8iYpx><Q~s=A
N00O3of@X`D*Z>`k7a0Hm

delta 112
zcmZn=Xb@m|$`JN-qG0kw1>T7zJ{->&_PjcK=8P>^gcB&ja*)CP|3pC>yI(;s`51uU
t$NeS+)@lZZ4a@%<vH)pm#S%{t|2-E-79oG&fT{yXyaKFl^FKx=HUJ4=CTIWv

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index 03453a98009b1163e10942b1d4e8ed81a2038071..e7fc986c50fabee53128659293b764ea4ed5c8b9 100644
GIT binary patch
delta 60
zcmZn=Xb|9d&anH{*)wNsCkiG{RN$Og;>Q&F70l+HSmMKam|^z6MH2<BxsEVQ{<D$+
N2$&`cnk`~t0|4ig7Bc_<

delta 110
zcmZn=Xb@m|$`JN-qG0kw1>T7zer$VQojr5L7R=|ISmMKSkiq`{L_r(7UqLVV7=Yl%
s{U!z0Y6gZ4%l{j)0BLE(5>F8SJr_t8A%Eb2ssl*80<3QHKSm}t0I1_7f&c&j

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index b0ed0ccf76b5e41a435a4bd55620d2d4e4b811bd..16cfd6b81ddececa35eb93059420fa3ea3f5558a 100644
GIT binary patch
delta 62
zcmZn=Xb|9d%dqv;*)wNsCkiG{RN$Og;=}TW!T&3m#|z}KG62D%iGtQ#hZv^(Sq0)T
LO%ya+#KZ;w=C~BS

delta 112
zcmZn=Xb|9d&anH{*)wNsCkiG{RN$Og;=}TcA@nPl#|z>eWU&7~QP9TjSI|p71|axx
sze$0$nt@@%^8bb`Kw4U{#1q7S&jpf2$R9YM>Hre20IS>lkCBND03IkNXaE2J

diff --git a/simgui-ds.json b/simgui-ds.json
index c2bdbe4..dd6a35c 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -90,7 +90,9 @@
     }
   ],
   "robotJoysticks": [
-    {},
+    {
+      "guid": "Keyboard1"
+    },
     {
       "guid": "Keyboard0"
     }
diff --git a/simgui-window.json b/simgui-window.json
index a18dffa..19dd3bd 100644
--- a/simgui-window.json
+++ b/simgui-window.json
@@ -6,39 +6,39 @@
     "GLOBAL": {
       "font": "Proggy Dotted",
       "fps": "120",
-      "height": "720",
-      "maximized": "0",
+      "height": "511",
+      "maximized": "1",
       "style": "0",
       "userScale": "2",
-      "width": "1280",
-      "xpos": "207",
-      "ypos": "23"
+      "width": "959",
+      "xpos": "607",
+      "ypos": "125"
     }
   },
   "Window": {
     "###FMS": {
       "Collapsed": "0",
-      "Pos": "5,540",
-      "Size": "169,184"
+      "Pos": "5,492",
+      "Size": "283,140"
     },
     "###Joysticks": {
       "Collapsed": "0",
-      "Pos": "338,448",
+      "Pos": "175,192",
       "Size": "796,155"
     },
     "###NetworkTables": {
       "Collapsed": "0",
-      "Pos": "250,277",
+      "Pos": "717,587",
       "Size": "750,185"
     },
     "###NetworkTables Info": {
       "Collapsed": "0",
-      "Pos": "250,130",
+      "Pos": "640,480",
       "Size": "750,145"
     },
     "###Other Devices": {
       "Collapsed": "0",
-      "Pos": "1025,20",
+      "Pos": "940,20",
       "Size": "250,695"
     },
     "###System Joysticks": {
diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/NAMEME.path b/src/deploy/pathplanner/paths/NAMEME.path
new file mode 100644
index 0000000..3f475e5
--- /dev/null
+++ b/src/deploy/pathplanner/paths/NAMEME.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "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": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/settings.json b/src/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..5bf438c
--- /dev/null
+++ b/src/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index da24425..b17fd3f 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -8,12 +8,16 @@
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.path.PathPlannerPath;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -33,7 +37,7 @@ public class RobotContainer {
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
   
   private final CommandGenericHID secondaryController =
-            new CommandGenericHID(DriveConstants.SECONDARY_CONTROLLER_PORT);
+      new CommandGenericHID(DriveConstants.SECONDARY_CONTROLLER_PORT);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -55,7 +59,10 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
-    secondaryController.button(1).onTrue(driveCommands.autoLeave());
+
+    secondaryController.button(1).onTrue(driveCommands.runPath("Example Path"));
+    //secondaryController.button(2).onTrue(driveCommands.runPath("NAMEME"));
+
 
     driveSubsystem.setDefaultCommand(
         driveCommands.driveTeleop(
@@ -72,7 +79,7 @@ private void configureBindings() {
               DriveConstants.ROTATION_CONTROL_EXPONENT, 
               DriveConstants.ROTATION_CONTROL_DEADBAND)
         )
-    );
+  );
   }
 
   /**
@@ -81,7 +88,7 @@ private void configureBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An example command will be run in autonomous. THIS IS A PLACEHOLDER!
-    return autoChooser.getSelected();
+    return null;
   }
+  
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 5dc5d1f..0a58807 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -1,10 +1,17 @@
 package org.ironriders.drive;
 
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.ironriders.core.RobotContainer;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import swervelib.SwerveDrive;
 
 public class DriveCommands {
@@ -25,6 +32,7 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 	 */
 	public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inputTranslationY, DoubleSupplier inputRotation) {
 		return driveSubsystem.runOnce(() -> {
+
 			// No driver input while autonomous
 			if (DriverStation.isAutonomous()) return;
 
@@ -40,9 +48,18 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 		});
 	}
 
-	public Command autoLeave() {
-		return driveSubsystem.runOnce(() -> {
-			System.out.println("testjdff");
-		});
-	}
+	public Command runPath(String Path) {
+    try{
+        // Load the path you want to follow using its name in the GUI
+        PathPlannerPath autoPath = PathPlannerPath.fromPathFile(Path);
+		System.out.println(Path);
+        // Create a path following command using AutoBuilder. This will also trigger event markers.
+        return AutoBuilder.followPath(autoPath);
+    } catch (Exception e) {
+		System.out.println("eroor");
+        DriverStation.reportError("Something bad happen !!: " + e.getMessage(), e.getStackTrace());
+        return Commands.none();
+		
+    }
+  }
 }

From a297b63281b739bae11ef8d6719538e50dbd81b3 Mon Sep 17 00:00:00 2001
From: issy <lshields822@gmail.com>
Date: Tue, 28 Jan 2025 16:54:21 -0800
Subject: [PATCH 04/44] small chanmges s

---
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui-window.json                           |  41 +++++++++++--------
 simgui.json                                  |  28 +++++++++++++
 src/main/java/org/ironriders/core/Robot.java |   5 ++-
 7 files changed, 55 insertions(+), 19 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index 32ba0e8f95b8315a9dab472ca1f27c83bb434f3d..e4db747a58746032ab748138ddfb10c0ac60d7e8 100644
GIT binary patch
delta 62
zcmZn=Xb@m|$Po2)qG0kw1>T7zJ{*r24!k;h=8P>^gcB&jdYmEY-}Q-t)?6nT9RGZ0
N00O3of@X`D*Z>E!7a0Hm

delta 62
zcmZn=Xb@m|$`JN-qG0kw1>T7zJ{->&_PjcK=8P>^gcB&jdYEC(zl9S8t+|dcO!>2d
O0SK5T3YslqVgmpOy%!k(

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index a038c85650370896e53a790387370c1fea55fc73..50b42fdf251ff0951482630f174ef3370b4acc2b 100644
GIT binary patch
delta 62
zcmZn=Xb|9d%CPs<*)wNsCkiG{RN$Og;=}TUA^a<t#|z}K9%o4UcYUIuHP;CS$3Nd0
NfPiVDpxGiOHUJ!h7a0Hm

delta 62
zcmZn=Xb|9d$*}X)*)wNsCkiG{RN$Og;=}TSA^0np#|z}K9%h*HZ{b8iYpx><Q~s=A
N00O3of@X`D*Z>`k7a0Hm

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index e7fc986c50fabee53128659293b764ea4ed5c8b9..3d0e198dfe2aca4d742da047d431656362da9d43 100644
GIT binary patch
delta 62
zcmZn=Xb|9d%&`B}*)wNsCkiG{RN$Og;=}TYA@VDj#|z}K9%o4WcVnWUHP;CShd)0U
NfPiVDpxGiOHUJ!V7a0Hm

delta 62
zcmZn=Xb|9d&anH{*)wNsCkiG{RN$Og;=}TcA@nPl#|z}K9%h*RZ_z|SYpx><lmDz_
N00O3of@X`D*Z>`Y7a0Hm

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index 16cfd6b81ddececa35eb93059420fa3ea3f5558a..da71c7ac7e5cce37947094bdc8dcbd96a31d7bd3 100644
GIT binary patch
delta 62
zcmZn=Xb|9d$*}X)*)wNsCkiG{RN$Og;=}TSA^0np#|z}K9%V@ScXOhkHP<l)$3H(A
NfPiVDpxGiOHUJ!h7a0Hm

delta 62
zcmZn=Xb|9d%dqv;*)wNsCkiG{RN$Og;=}TW!T&3m#|z}KG62D%iGtQ#hZv^(Sq0)T
LO%ya+#KZ;w=C~BS

diff --git a/simgui-window.json b/simgui-window.json
index 19dd3bd..b621789 100644
--- a/simgui-window.json
+++ b/simgui-window.json
@@ -4,52 +4,57 @@
   },
   "MainWindow": {
     "GLOBAL": {
-      "font": "Proggy Dotted",
+      "font": "Fira Code Retina",
       "fps": "120",
-      "height": "511",
-      "maximized": "1",
-      "style": "0",
+      "height": "635",
+      "maximized": "0",
+      "style": "3",
       "userScale": "2",
-      "width": "959",
-      "xpos": "607",
-      "ypos": "125"
+      "width": "1198",
+      "xpos": "1119",
+      "ypos": "334"
     }
   },
   "Window": {
     "###FMS": {
       "Collapsed": "0",
-      "Pos": "5,492",
-      "Size": "283,140"
+      "Pos": "636,616",
+      "Size": "288,138"
     },
     "###Joysticks": {
       "Collapsed": "0",
-      "Pos": "175,192",
-      "Size": "796,155"
+      "Pos": "72,289",
+      "Size": "796,169"
     },
     "###NetworkTables": {
       "Collapsed": "0",
-      "Pos": "717,587",
+      "Pos": "152,616",
       "Size": "750,185"
     },
     "###NetworkTables Info": {
       "Collapsed": "0",
-      "Pos": "640,480",
+      "Pos": "636,544",
       "Size": "750,145"
     },
     "###Other Devices": {
       "Collapsed": "0",
-      "Pos": "940,20",
+      "Pos": "636,25",
       "Size": "250,695"
     },
+    "###Plot <0>": {
+      "Collapsed": "0",
+      "Pos": "825,385",
+      "Size": "700,400"
+    },
     "###System Joysticks": {
       "Collapsed": "0",
       "Pos": "5,350",
-      "Size": "223,218"
+      "Size": "231,236"
     },
     "###Timing": {
       "Collapsed": "0",
-      "Pos": "103,73",
-      "Size": "135,219"
+      "Pos": "194,44",
+      "Size": "139,217"
     },
     "Debug##Default": {
       "Collapsed": "0",
@@ -59,7 +64,7 @@
     "Robot State": {
       "Collapsed": "0",
       "Pos": "5,20",
-      "Size": "99,116"
+      "Size": "108,122"
     }
   }
 }
diff --git a/simgui.json b/simgui.json
index 56b55ad..98ecb11 100644
--- a/simgui.json
+++ b/simgui.json
@@ -15,5 +15,33 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.10000000149011612,
+            0.10000000149011612,
+            0.10000000149011612,
+            1.0
+          ],
+          "height": 154
+        },
+        {
+          "backgroundColor": [
+            0.10000000149011612,
+            0.10000000149011612,
+            0.10000000149011612,
+            1.0
+          ],
+          "height": 154
+        }
+      ],
+      "window": {
+        "name": "/SmartDashboard/Field",
+        "visible": false
+      }
+    }
   }
 }
diff --git a/src/main/java/org/ironriders/core/Robot.java b/src/main/java/org/ironriders/core/Robot.java
index 71c79e0..8e6870f 100644
--- a/src/main/java/org/ironriders/core/Robot.java
+++ b/src/main/java/org/ironriders/core/Robot.java
@@ -4,6 +4,7 @@
 
 package org.ironriders.core;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -94,7 +95,9 @@ public void testPeriodic() {}
 
   /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationInit() {}
+  public void simulationInit() {
+    DriverStation.silenceJoystickConnectionWarning(true);
+  }
 
   /** This function is called periodically whilst in simulation. */
   @Override

From 6e452c8ecb5cddc865d09d5e350034030a9ddf61 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 7 Jan 2025 09:24:26 -0800
Subject: [PATCH 05/44] f# Conflicts:

---
 .gitignore                                    |  3 +-
 .../org/ironriders/vision/VisionCommands.java |  5 ++
 .../ironriders/vision/VisionConstants.java    |  5 ++
 .../ironriders/vision/VisionSubsystem.java    |  5 ++
 vendordeps/photonlib.json                     | 71 +++++++++++++++++++
 5 files changed, 88 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/org/ironriders/vision/VisionCommands.java
 create mode 100644 src/main/java/org/ironriders/vision/VisionConstants.java
 create mode 100644 src/main/java/org/ironriders/vision/VisionSubsystem.java
 create mode 100644 vendordeps/photonlib.json

diff --git a/.gitignore b/.gitignore
index 014ae3a..6fe50da 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,4 +26,5 @@ replay_pid*
 .gradle/
 .vscode/
 .idea/ # please do not use IntelliJ!
-build
\ No newline at end of file
+build/
+gradlew
\ No newline at end of file
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
new file mode 100644
index 0000000..5a6cbc7
--- /dev/null
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -0,0 +1,5 @@
+package org.ironriders.vision;
+
+public class VisionCommands {
+
+}
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
new file mode 100644
index 0000000..cfda5c9
--- /dev/null
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -0,0 +1,5 @@
+package org.ironriders.vision;
+
+public class VisionConstants {
+
+}
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
new file mode 100644
index 0000000..c7bfb38
--- /dev/null
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -0,0 +1,5 @@
+package org.ironriders.vision;
+
+public class VisionSubsystem {
+
+}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..db43d6d
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2025.0.0-beta-8",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.0.0-beta-8",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2025.0.0-beta-8",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.0.0-beta-8",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2025.0.0-beta-8"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2025.0.0-beta-8"
+        }
+    ]
+}
\ No newline at end of file

From 42886e39144ee4b0f2d5ea3c3e43cf5d7b1b50cf Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 7 Jan 2025 14:34:22 -0800
Subject: [PATCH 06/44] added like code

---
 .gitignore                                    |  6 +--
 .../org/ironriders/vision/VisionCommands.java | 41 +++++++++++++++++++
 .../ironriders/vision/VisionConstants.java    |  2 +-
 .../ironriders/vision/VisionSubsystem.java    | 10 ++++-
 4 files changed, 53 insertions(+), 6 deletions(-)

diff --git a/.gitignore b/.gitignore
index 6fe50da..a418055 100644
--- a/.gitignore
+++ b/.gitignore
@@ -22,9 +22,7 @@
 # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
 hs_err_pid*
 replay_pid*
-
+build/
 .gradle/
 .vscode/
-.idea/ # please do not use IntelliJ!
-build/
-gradlew
\ No newline at end of file
+.idea/ # please do not use IntelliJ!
\ No newline at end of file
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 5a6cbc7..49b7784 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -1,5 +1,46 @@
 package org.ironriders.vision;
 
+import java.util.List;
+
+import org.ironriders.vision.VisionSubsystem;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.photonvision.*;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
 public class VisionCommands {
+    private final VisionSubsystem VisionSubsystem;
+    PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
+    AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+    public VisionCommands(VisionSubsystem visionSubsystem) {
+		this.VisionSubsystem = visionSubsystem;
+	}
+    public Command alignCoral(){
+        return VisionSubsystem.runOnce(()->{
+
+        }
+
+        );
+    }
 
+    private Transform3d getPathToTag(int id){
+        var result = camera.getLatestResult();
+        boolean hasTargets = result.hasTargets();
+        if(!hasTargets){
+            return new Transform3d();
+        }
+        List<PhotonTrackedTarget> targets = result.getTargets();
+        for (PhotonTrackedTarget target : targets) {
+            if(target.getFiducialId()==id){
+                double poseAmbiguity = target.getPoseAmbiguity();
+                Transform3d pose = target.getBestCameraToTarget();
+                return pose;
+            }
+        }
+                return new Transform3d();
+    }
 }
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index cfda5c9..d961e20 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -1,5 +1,5 @@
 package org.ironriders.vision;
 
 public class VisionConstants {
-
+    public static final String CAM_NAME="camera";
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index c7bfb38..82ea6d7 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -1,5 +1,13 @@
 package org.ironriders.vision;
 
-public class VisionSubsystem {
+import org.ironriders.drive.DriveCommands;
 
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class VisionSubsystem extends SubsystemBase {
+    private VisionCommands commands;
+
+    public VisionCommands getCommands() {
+        return commands;
+    }
 }

From 34992c74e900bc1bc9f8bb664e1b2a5a79ba90b4 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 7 Jan 2025 16:22:12 -0800
Subject: [PATCH 07/44] more code

C:w
C
:i# Conflicts:
---
 src/main/java/org/ironriders/core/Robot.java  |  1 +
 .../org/ironriders/vision/VisionCommands.java | 37 +++++++++++++++----
 .../ironriders/vision/VisionConstants.java    |  2 +
 .../ironriders/vision/VisionSubsystem.java    |  2 -
 4 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/src/main/java/org/ironriders/core/Robot.java b/src/main/java/org/ironriders/core/Robot.java
index 8e6870f..f962597 100644
--- a/src/main/java/org/ironriders/core/Robot.java
+++ b/src/main/java/org/ironriders/core/Robot.java
@@ -4,6 +4,7 @@
 
 package org.ironriders.core;
 
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 49b7784..ea303bb 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -2,45 +2,66 @@
 
 import java.util.List;
 
-import org.ironriders.vision.VisionSubsystem;
+import org.ironriders.drive.DriveSubsystem;
+
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.photonvision.*;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 public class VisionCommands {
     private final VisionSubsystem VisionSubsystem;
+    private final DriveSubsystem driveSubsystem;
     PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
     AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
-    public VisionCommands(VisionSubsystem visionSubsystem) {
+    public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsystem) {
 		this.VisionSubsystem = visionSubsystem;
+        this.driveSubsystem = driveSubsystem;
 	}
     public Command alignCoral(){
         return VisionSubsystem.runOnce(()->{
-
+            int[] tags=null;
+            if(!DriverStation.getAlliance().isPresent()){
+                return;
+            }
+            if(DriverStation.getAlliance().get()== Alliance.Red){
+            tags=VisionConstants.REEF_TAG_IDS_RED;
+            }
+            else{
+            tags=VisionConstants.REEF_TAG_IDS_BLUE;  
+            }
+            for(int i :tags){
+                if(getPathToTag(i)!=null){
+                    Translation2d path =getPathToTag(i);
+                    driveSubsystem.drive(path, 0, true);
+                }
+            }
         }
 
         );
     }
 
-    private Transform3d getPathToTag(int id){
+    private Translation2d getPathToTag(int id){
         var result = camera.getLatestResult();
         boolean hasTargets = result.hasTargets();
         if(!hasTargets){
-            return new Transform3d();
+            return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
         for (PhotonTrackedTarget target : targets) {
             if(target.getFiducialId()==id){
-                double poseAmbiguity = target.getPoseAmbiguity();
+                //double poseAmbiguity = target.getPoseAmbiguity();
                 Transform3d pose = target.getBestCameraToTarget();
-                return pose;
+                
+                return new Translation2d(pose.getX(),pose.getY());
             }
         }
-                return new Transform3d();
+                return null;
     }
 }
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index d961e20..ed806ce 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -2,4 +2,6 @@
 
 public class VisionConstants {
     public static final String CAM_NAME="camera";
+    public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
+    public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 82ea6d7..826e84b 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -1,7 +1,5 @@
 package org.ironriders.vision;
 
-import org.ironriders.drive.DriveCommands;
-
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {

From 2d19df5fee21dba272e74d8954988de8cc383cc8 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 7 Jan 2025 18:25:22 -0800
Subject: [PATCH 08/44] update backup

---
 .../org/ironriders/core/RobotContainer.java   | 14 ++++---
 .../org/ironriders/vision/VisionCommands.java | 41 +++++++++----------
 .../ironriders/vision/VisionSubsystem.java    |  7 +++-
 3 files changed, 33 insertions(+), 29 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index b17fd3f..2f6d70d 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -7,9 +7,9 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import org.ironriders.vision.VisionCommands;
+import org.ironriders.vision.VisionSubsystem;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
@@ -30,8 +30,8 @@ public class RobotContainer {
   private final DriveSubsystem driveSubsystem = new DriveSubsystem();
   private final DriveCommands driveCommands = driveSubsystem.getCommands();
 
-  // Auto support
-  private final SendableChooser<Command> autoChooser;
+  private final VisionSubsystem visionSubsystem = new VisionSubsystem();
+  private final VisionCommands visionCommands = visionSubsystem.getCommands();
 
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
@@ -79,7 +79,9 @@ private void configureBindings() {
               DriveConstants.ROTATION_CONTROL_EXPONENT, 
               DriveConstants.ROTATION_CONTROL_DEADBAND)
         )
-  );
+        
+    );
+    primaryController.a().onTrue(visionCommands.alignCoral());
   }
 
   /**
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index ea303bb..0fd4ae5 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -4,7 +4,6 @@
 
 import org.ironriders.drive.DriveSubsystem;
 
-
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.*;
@@ -21,24 +20,24 @@ public class VisionCommands {
     AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
     public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsystem) {
-		this.VisionSubsystem = visionSubsystem;
+        this.VisionSubsystem = visionSubsystem;
         this.driveSubsystem = driveSubsystem;
-	}
-    public Command alignCoral(){
-        return VisionSubsystem.runOnce(()->{
-            int[] tags=null;
-            if(!DriverStation.getAlliance().isPresent()){
+    }
+
+    public Command alignCoral() {
+        return VisionSubsystem.runOnce(() -> {
+            int[] tags = null;
+            if (!DriverStation.getAlliance().isPresent()) {
                 return;
             }
-            if(DriverStation.getAlliance().get()== Alliance.Red){
-            tags=VisionConstants.REEF_TAG_IDS_RED;
-            }
-            else{
-            tags=VisionConstants.REEF_TAG_IDS_BLUE;  
+            if (DriverStation.getAlliance().get() == Alliance.Red) {
+                tags = VisionConstants.REEF_TAG_IDS_RED;
+            } else {
+                tags = VisionConstants.REEF_TAG_IDS_BLUE;
             }
-            for(int i :tags){
-                if(getPathToTag(i)!=null){
-                    Translation2d path =getPathToTag(i);
+            for (int i : tags) {
+                if (getPathToTag(i) != null) {
+                    Translation2d path = getPathToTag(i);
                     driveSubsystem.drive(path, 0, true);
                 }
             }
@@ -47,21 +46,19 @@ public Command alignCoral(){
         );
     }
 
-    private Translation2d getPathToTag(int id){
+    private Translation2d getPathToTag(int id) {
         var result = camera.getLatestResult();
         boolean hasTargets = result.hasTargets();
-        if(!hasTargets){
+        if (!hasTargets) {
             return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
         for (PhotonTrackedTarget target : targets) {
-            if(target.getFiducialId()==id){
-                //double poseAmbiguity = target.getPoseAmbiguity();
+            if (target.getFiducialId() == id) {
                 Transform3d pose = target.getBestCameraToTarget();
-                
-                return new Translation2d(pose.getX(),pose.getY());
+                return new Translation2d(pose.getX(), pose.getY());
             }
         }
-                return null;
+        return null;
     }
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 826e84b..b9289dd 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -1,11 +1,16 @@
 package org.ironriders.vision;
 
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands;
-
+    /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
     }
+    
+    public void alignwithCoral(){
+        commands.alignCoral();
+    }
 }

From 04e057d559bc6249d0c9b7caa399c4a5d4421357 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 7 Jan 2025 18:55:17 -0800
Subject: [PATCH 09/44] =?UTF-8?q?no=20warnings=20=F0=9F=98=8A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/main/java/org/ironriders/core/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 2f6d70d..b7f0b9f 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -11,7 +11,6 @@
 import org.ironriders.vision.VisionSubsystem;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -33,6 +32,7 @@ public class RobotContainer {
   private final VisionSubsystem visionSubsystem = new VisionSubsystem();
   private final VisionCommands visionCommands = visionSubsystem.getCommands();
 
+  private final SendableChooser<Command> autoChooser;
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
   

From dbc3f66f847351de2d5e46c66b832ab1caaafd5c Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 14 Jan 2025 19:42:57 -0800
Subject: [PATCH 10/44] swerve updates + test code for motors

---
 src/main/java/org/ironriders/core/RobotContainer.java |  2 +-
 src/main/java/org/ironriders/drive/DriveCommands.java | 11 ++++++++++-
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index b7f0b9f..ad503ea 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -81,7 +81,7 @@ private void configureBindings() {
         )
         
     );
-    primaryController.a().onTrue(visionCommands.alignCoral());
+  //  primaryController.a().onTrue(driveCommands.driveTest());
   }
 
   /**
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 0a58807..87688da 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -8,6 +8,9 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathPlannerPath;
 
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,7 +25,13 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.driveSubsystem = driveSubsystem;
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
-
+	SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
+	public Command driveTest(){
+		return driveSubsystem.runOnce(()->{
+				sparkMax.set(1);
+		}
+		);
+	}
 	/**
 	 * Command to drive the robot given controller input.
 	 * 

From 191c7ff85d83eb2c0d4838d82ae71f17e693e6cf Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 16 Jan 2025 16:24:26 -0800
Subject: [PATCH 11/44] got rid of swerve stuff while it does not work

---
 .../java/org/ironriders/core/RobotContainer.java  | 15 ++++++++++-----
 .../java/org/ironriders/drive/DriveCommands.java  | 11 ++++++-----
 .../java/org/ironriders/drive/DriveSubsystem.java |  5 +++--
 .../org/ironriders/vision/VisionCommands.java     |  3 +++
 .../org/ironriders/vision/VisionConstants.java    |  2 +-
 .../org/ironriders/vision/VisionSubsystem.java    |  4 +++-
 6 files changed, 26 insertions(+), 14 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index ad503ea..4f1a916 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -32,7 +32,7 @@ public class RobotContainer {
   private final VisionSubsystem visionSubsystem = new VisionSubsystem();
   private final VisionCommands visionCommands = visionSubsystem.getCommands();
 
-  private final SendableChooser<Command> autoChooser;
+  //private final SendableChooser<Command> autoChooser;
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
   
@@ -45,8 +45,8 @@ public RobotContainer() {
     configureBindings();
 
     // Init auto chooser
-    autoChooser = AutoBuilder.buildAutoChooser();
-    SmartDashboard.putData("Auto Select", autoChooser);
+   // autoChooser = AutoBuilder.buildAutoChooser();
+    //SmartDashboard.putData("Auto Select", autoChooser);
   }
 
   /**
@@ -81,7 +81,8 @@ private void configureBindings() {
         )
         
     );
-  //  primaryController.a().onTrue(driveCommands.driveTest());
+   // primaryController.a().onTrue(driveCommands.driveTest());
+    primaryController.a().onTrue(visionCommands.alignCoral());
   }
 
   /**
@@ -90,7 +91,11 @@ private void configureBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return null;
+    // An example command will be run in autonomous. THIS IS A PLACEHOLDER!
+   // return autoChooser.getSelected();
+   return new Command() {
+    
+   };
   }
   
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 87688da..a0d647f 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -25,13 +25,13 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.driveSubsystem = driveSubsystem;
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
-	SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
+	/*SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
 	public Command driveTest(){
 		return driveSubsystem.runOnce(()->{
 				sparkMax.set(1);
 		}
 		);
-	}
+	}*/
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
@@ -48,10 +48,11 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
 				new Translation2d(
-					inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
-					inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+					//inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
+					//inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
 				),
-				inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+				//inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+				0,
 				true // Gus likes it this way
 			);
 		});
diff --git a/src/main/java/org/ironriders/drive/DriveSubsystem.java b/src/main/java/org/ironriders/drive/DriveSubsystem.java
index e553066..7e57fb9 100644
--- a/src/main/java/org/ironriders/drive/DriveSubsystem.java
+++ b/src/main/java/org/ironriders/drive/DriveSubsystem.java
@@ -22,7 +22,8 @@ public class DriveSubsystem extends SubsystemBase {
 	private SwerveDrive swerveDrive;
 
 	public DriveSubsystem() throws RuntimeException {
-		try {
+
+	try {
 			swerveDrive = 
 				new SwerveParser(DriveConstants.SWERVE_JSON_DIRECTORY) // YAGSL reads from the deply/swerve directory.
 					.createSwerveDrive(DriveConstants.SWERVE_MAXIMUM_SPEED);
@@ -70,7 +71,7 @@ public DriveSubsystem() throws RuntimeException {
 	 * @param fieldRelative If not field relative, the robot will move relative to its own rotation.
 	 */
 	public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
-		swerveDrive.drive(translation, rotation, fieldRelative, false);
+		//swerveDrive.drive(translation, rotation, fieldRelative, false);
 	}
 
 	/** Fetch the DriveCommands instance */
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 0fd4ae5..7d2cdef 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -26,8 +26,10 @@ public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsy
 
     public Command alignCoral() {
         return VisionSubsystem.runOnce(() -> {
+            System.out.println("started coral");
             int[] tags = null;
             if (!DriverStation.getAlliance().isPresent()) {
+                System.err.println("no alliace!");
                 return;
             }
             if (DriverStation.getAlliance().get() == Alliance.Red) {
@@ -39,6 +41,7 @@ public Command alignCoral() {
                 if (getPathToTag(i) != null) {
                     Translation2d path = getPathToTag(i);
                     driveSubsystem.drive(path, 0, true);
+                    System.out.println("ran drive");
                 }
             }
         }
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index ed806ce..f5fe3e0 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -1,7 +1,7 @@
 package org.ironriders.vision;
 
 public class VisionConstants {
-    public static final String CAM_NAME="camera";
+    public static final String CAM_NAME="Arducam_OV9281_USB_Camera";
     public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
     public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index b9289dd..6682a71 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -1,10 +1,12 @@
 package org.ironriders.vision;
 
 
+import org.ironriders.drive.DriveSubsystem;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {
-    private VisionCommands commands;
+    private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;

From 175fca9f7d019a280de44e1c9608d032a996bb73 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 16 Jan 2025 16:30:39 -0800
Subject: [PATCH 12/44] read the drive stuff i commented out

---
 .../java/org/ironriders/core/RobotContainer.java    | 13 +++++--------
 .../java/org/ironriders/drive/DriveCommands.java    | 12 ++++++------
 .../java/org/ironriders/drive/DriveSubsystem.java   |  2 +-
 3 files changed, 12 insertions(+), 15 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 4f1a916..1fd96b7 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -32,7 +32,7 @@ public class RobotContainer {
   private final VisionSubsystem visionSubsystem = new VisionSubsystem();
   private final VisionCommands visionCommands = visionSubsystem.getCommands();
 
-  //private final SendableChooser<Command> autoChooser;
+  private final SendableChooser<Command> autoChooser;
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
   
@@ -45,8 +45,8 @@ public RobotContainer() {
     configureBindings();
 
     // Init auto chooser
-   // autoChooser = AutoBuilder.buildAutoChooser();
-    //SmartDashboard.putData("Auto Select", autoChooser);
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Select", autoChooser);
   }
 
   /**
@@ -81,7 +81,7 @@ private void configureBindings() {
         )
         
     );
-   // primaryController.a().onTrue(driveCommands.driveTest());
+    primaryController.a().onTrue(driveCommands.driveTest());
     primaryController.a().onTrue(visionCommands.alignCoral());
   }
 
@@ -92,10 +92,7 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous. THIS IS A PLACEHOLDER!
-   // return autoChooser.getSelected();
-   return new Command() {
-    
-   };
+    return autoChooser.getSelected();
   }
   
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index a0d647f..f9e25d6 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -25,13 +25,13 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.driveSubsystem = driveSubsystem;
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
-	/*SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
+	SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
 	public Command driveTest(){
 		return driveSubsystem.runOnce(()->{
 				sparkMax.set(1);
 		}
 		);
-	}*/
+	}
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
@@ -48,11 +48,11 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
 				new Translation2d(
-					//inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
-					//inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+					inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
+					inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
 				),
-				//inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
-				0,
+				inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+				
 				true // Gus likes it this way
 			);
 		});
diff --git a/src/main/java/org/ironriders/drive/DriveSubsystem.java b/src/main/java/org/ironriders/drive/DriveSubsystem.java
index 7e57fb9..38e2dff 100644
--- a/src/main/java/org/ironriders/drive/DriveSubsystem.java
+++ b/src/main/java/org/ironriders/drive/DriveSubsystem.java
@@ -71,7 +71,7 @@ public DriveSubsystem() throws RuntimeException {
 	 * @param fieldRelative If not field relative, the robot will move relative to its own rotation.
 	 */
 	public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
-		//swerveDrive.drive(translation, rotation, fieldRelative, false);
+		swerveDrive.drive(translation, rotation, fieldRelative, false);
 	}
 
 	/** Fetch the DriveCommands instance */

From 01e3a3d06cd3b4313119ce91e59b68f985d238d8 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 16 Jan 2025 18:23:37 -0800
Subject: [PATCH 13/44] made numbers come out + put ids to smartdashboard

---
 .../org/ironriders/core/RobotContainer.java   | 37 ++++++++----
 .../org/ironriders/drive/DriveCommands.java   | 12 ++--
 .../org/ironriders/drive/DriveSubsystem.java  |  4 +-
 .../org/ironriders/vision/VisionCommands.java | 57 ++++++++++++++++---
 .../ironriders/vision/VisionConstants.java    |  2 +-
 .../ironriders/vision/VisionSubsystem.java    | 29 +++++++++-
 6 files changed, 108 insertions(+), 33 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 1fd96b7..f3adfb3 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -19,9 +19,12 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
@@ -39,23 +42,30 @@ public class RobotContainer {
   private final CommandGenericHID secondaryController =
       new CommandGenericHID(DriveConstants.SECONDARY_CONTROLLER_PORT);
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
 
     // Init auto chooser
-    autoChooser = AutoBuilder.buildAutoChooser();
-    SmartDashboard.putData("Auto Select", autoChooser);
+    // autoChooser = AutoBuilder.buildAutoChooser();
+    // SmartDashboard.putData("Auto Select", autoChooser);
   }
 
   /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
+   * Use this method to define your trigger->command mappings. Triggers can be
+   * created via the
+   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
+   * an arbitrary
    * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
+   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for
+   * {@link
+   * CommandXboxController
+   * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
+   * PS4} controllers or
+   * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
    * joysticks}.
    */
   private void configureBindings() {
@@ -92,7 +102,10 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous. THIS IS A PLACEHOLDER!
-    return autoChooser.getSelected();
+    // return autoChooser.getSelected();
+    return new Command() {
+
+    };
   }
   
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index f9e25d6..a0d647f 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -25,13 +25,13 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.driveSubsystem = driveSubsystem;
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
-	SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
+	/*SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
 	public Command driveTest(){
 		return driveSubsystem.runOnce(()->{
 				sparkMax.set(1);
 		}
 		);
-	}
+	}*/
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
@@ -48,11 +48,11 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
 				new Translation2d(
-					inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
-					inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+					//inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
+					//inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
 				),
-				inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
-				
+				//inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+				0,
 				true // Gus likes it this way
 			);
 		});
diff --git a/src/main/java/org/ironriders/drive/DriveSubsystem.java b/src/main/java/org/ironriders/drive/DriveSubsystem.java
index 38e2dff..591005f 100644
--- a/src/main/java/org/ironriders/drive/DriveSubsystem.java
+++ b/src/main/java/org/ironriders/drive/DriveSubsystem.java
@@ -58,7 +58,7 @@ public DriveSubsystem() throws RuntimeException {
 				return false;
 			},
 			this
-		);
+		);*/
 	}
 
 	/** 
@@ -71,7 +71,7 @@ public DriveSubsystem() throws RuntimeException {
 	 * @param fieldRelative If not field relative, the robot will move relative to its own rotation.
 	 */
 	public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
-		swerveDrive.drive(translation, rotation, fieldRelative, false);
+		//swerveDrive.drive(translation, rotation, fieldRelative, false);
 	}
 
 	/** Fetch the DriveCommands instance */
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 7d2cdef..e1a9a67 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -1,5 +1,6 @@
 package org.ironriders.vision;
 
+
 import java.util.List;
 
 import org.ironriders.drive.DriveSubsystem;
@@ -11,12 +12,12 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.photonvision.*;
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 public class VisionCommands {
     private final VisionSubsystem VisionSubsystem;
     private final DriveSubsystem driveSubsystem;
-    PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
     AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
     public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsystem) {
@@ -24,35 +25,61 @@ public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsy
         this.driveSubsystem = driveSubsystem;
     }
 
-    public Command alignCoral() {
+    public Command alignCoral(PhotonCamera camera) {
         return VisionSubsystem.runOnce(() -> {
+
             System.out.println("started coral");
             int[] tags = null;
             if (!DriverStation.getAlliance().isPresent()) {
-                System.err.println("no alliace!");
+                error("no alliace!");
                 return;
             }
             if (DriverStation.getAlliance().get() == Alliance.Red) {
                 tags = VisionConstants.REEF_TAG_IDS_RED;
+                print("red");
             } else {
                 tags = VisionConstants.REEF_TAG_IDS_BLUE;
+                print("blue");
+            }
+            boolean moved = false;
+            PhotonPipelineResult result = camera.getLatestResult();
+            if (!result.hasTargets()) {
+                error("no targets!");
+                return;
             }
             for (int i : tags) {
-                if (getPathToTag(i) != null) {
-                    Translation2d path = getPathToTag(i);
-                    driveSubsystem.drive(path, 0, true);
-                    System.out.println("ran drive");
+                if (getPathToTag(i, result) != null) {
+                    Translation2d path = getPathToTag(i, result);
+                    if (path.getX() > 0 || path.getY() > 0) {
+                        print("running drive");
+                        driveSubsystem.drive(path, 0, true);
+                        print("ran drive");
+                        print("x:" + path.getX());
+                        print("y:" + path.getY());
+                        moved = true;
+                    }
                 }
+
+            }
+            if (!moved) {
+                print("no tags I want");
+                List<PhotonTrackedTarget> targets = result.getTargets();
+                for (PhotonTrackedTarget target : targets) {
+                    print(target.fiducialId);
+                }
+                return;
             }
+
         }
 
         );
     }
 
-    private Translation2d getPathToTag(int id) {
-        var result = camera.getLatestResult();
+    private Translation2d getPathToTag(int id, PhotonPipelineResult result) {
+        print("getting path");
         boolean hasTargets = result.hasTargets();
         if (!hasTargets) {
+            print("no valid targets!");
             return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
@@ -64,4 +91,16 @@ private Translation2d getPathToTag(int id) {
         }
         return null;
     }
+
+    private void print(String input) {
+        System.out.println(input);
+    }
+
+    private void print(int input) {
+        System.out.println(input);
+    }
+
+    private void error(String input) {
+        System.err.println(input);
+    }
 }
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index f5fe3e0..01c4e93 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -1,7 +1,7 @@
 package org.ironriders.vision;
 
 public class VisionConstants {
-    public static final String CAM_NAME="Arducam_OV9281_USB_Camera";
+    public static final String CAM_NAME="apriltagcam";
     public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
     public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 6682a71..459f777 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -1,18 +1,41 @@
 package org.ironriders.vision;
 
+import java.util.List;
 
 import org.ironriders.drive.DriveSubsystem;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
+    private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
     }
-    
-    public void alignwithCoral(){
-        commands.alignCoral();
+    public PhotonCamera getCamera(){
+        return this.camera;
+    }
+    public void alignwithCoral() {
+        commands.alignCoral(camera);
+    }
+
+    @Override
+    public void periodic() {
+        PhotonPipelineResult result=camera.getLatestResult();
+        if(!result.hasTargets()){
+            SmartDashboard.putString("IDs", "None");
+            return;
+        }
+        List<PhotonTrackedTarget> targets=result.getTargets();
+        String ids="IDs: ";
+        for (PhotonTrackedTarget target :targets){
+            ids+=(target.fiducialId)+" ";
+        }
+        SmartDashboard.putString("IDs", ids);
     }
 }

From f5d44b742ed639434cf0488fc16efb36cbbab31b Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 08:48:26 -0800
Subject: [PATCH 14/44] not super sure acctually

---
 src/main/deploy/vision/iron-hallway.tags.json | 112 ++++++++++++++++++
 src/main/java/org/ironriders/core/Main.java   |  12 +-
 src/main/java/org/ironriders/core/Robot.java  |  52 +++++---
 .../org/ironriders/drive/DriveCommands.java   |  39 +++---
 .../org/ironriders/drive/DriveConstants.java  |  10 +-
 .../org/ironriders/vision/VisionCommands.java |   8 +-
 .../ironriders/vision/VisionSubsystem.java    |  17 +--
 7 files changed, 196 insertions(+), 54 deletions(-)
 create mode 100644 src/main/deploy/vision/iron-hallway.tags.json

diff --git a/src/main/deploy/vision/iron-hallway.tags.json b/src/main/deploy/vision/iron-hallway.tags.json
new file mode 100644
index 0000000..cdf55f9
--- /dev/null
+++ b/src/main/deploy/vision/iron-hallway.tags.json
@@ -0,0 +1,112 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pos": {
+        "translation": {
+          "x": 1.5239999512320015,
+          "y": 3.962399873203204,
+          "z": 0.6921499778512008
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pos": {
+        "translation": {
+          "x": 4.5719998536960045,
+          "y": 3.962399873203204,
+          "z": 1.9113499388368018
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pos": {
+        "translation": {
+          "x": 7.6199997561600075,
+          "y": 3.962399873203204,
+          "z": 1.3017499583440013
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pos": {
+        "translation": {
+          "x": 7.6199997561600075,
+          "y": 0,
+          "z": 1.3017499583440013
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pos": {
+        "translation": {
+          "x": 4.5719998536960045,
+          "y": 0,
+          "z": 1.9113499388368018
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pos": {
+        "translation": {
+          "x": 1.5239999512320015,
+          "y": 0,
+          "z": 0.6921499778512008
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0,
+            "Y": 0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/src/main/java/org/ironriders/core/Main.java b/src/main/java/org/ironriders/core/Main.java
index 0c242f4..9d3958a 100644
--- a/src/main/java/org/ironriders/core/Main.java
+++ b/src/main/java/org/ironriders/core/Main.java
@@ -7,17 +7,21 @@
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what
+ * you are doing, do not modify this file except to change the parameter class
+ * to the startRobot
  * call.
  */
 public final class Main {
-  private Main() {}
+  private Main() {
+  }
 
   /**
    * Main initialization function. Do not perform any initialization here.
    *
-   * <p>If you change your main robot class, change the parameter type.
+   * <p>
+   * If you change your main robot class, change the parameter type.
    */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
diff --git a/src/main/java/org/ironriders/core/Robot.java b/src/main/java/org/ironriders/core/Robot.java
index f962597..3af5b4c 100644
--- a/src/main/java/org/ironriders/core/Robot.java
+++ b/src/main/java/org/ironriders/core/Robot.java
@@ -4,15 +4,16 @@
 
 package org.ironriders.core;
 
-
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The methods in this class are called automatically corresponding to each mode, as described in
- * the TimedRobot documentation. If you change the name of this class or the package after creating
+ * The methods in this class are called automatically corresponding to each
+ * mode, as described in
+ * the TimedRobot documentation. If you change the name of this class or the
+ * package after creating
  * this project, you must also update the Main.java file in the project.
  */
 public class Robot extends TimedRobot {
@@ -22,39 +23,52 @@ public class Robot extends TimedRobot {
   private Command autonomousCommand;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
+   * This function is run when the robot is first started up and should be used
+   * for any
    * initialization code.
    */
   public Robot() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
   }
 
   /**
-   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * This function is called every 20 ms, no matter the mode. Use this for items
+   * like diagnostics
    * that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and
    * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // and running subsystem periodic() methods. This must be called from the
+    // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
   @Override
   public void autonomousInit() {
     autonomousCommand = robotContainer.getAutonomousCommand();
@@ -67,7 +81,8 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
   public void teleopInit() {
@@ -82,7 +97,8 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
   public void testInit() {
@@ -92,7 +108,8 @@ public void testInit() {
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override
@@ -102,5 +119,6 @@ public void simulationInit() {
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+  }
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index a0d647f..0298487 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -25,35 +25,40 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.driveSubsystem = driveSubsystem;
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
-	/*SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
-	public Command driveTest(){
-		return driveSubsystem.runOnce(()->{
-				sparkMax.set(1);
-		}
-		);
-	}*/
+
+	/*
+	 * SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
+	 * public Command driveTest(){
+	 * return driveSubsystem.runOnce(()->{
+	 * sparkMax.set(1);
+	 * }
+	 * );
+	 * }
+	 */
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
 	 * @param inputTranslationX DoubleSupplier, value from 0-1.
 	 * @param inputTranslationY DoubleSupplier, value from 0-1.
-	 * @param inputRotation DoubleSupplier, value from 0-1.
+	 * @param inputRotation     DoubleSupplier, value from 0-1.
 	 */
-	public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inputTranslationY, DoubleSupplier inputRotation) {
+	public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inputTranslationY,
+			DoubleSupplier inputRotation) {
 		return driveSubsystem.runOnce(() -> {
 
 			// No driver input while autonomous
-			if (DriverStation.isAutonomous()) return;
+			if (DriverStation.isAutonomous())
+				return;
 
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
-				new Translation2d(
-					//inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
-					//inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
-				),
-				//inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
-				0,
-				true // Gus likes it this way
+					new Translation2d(
+			// inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
+			// inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+			),
+					// inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+					0,
+					true // Gus likes it this way
 			);
 		});
 	}
diff --git a/src/main/java/org/ironriders/drive/DriveConstants.java b/src/main/java/org/ironriders/drive/DriveConstants.java
index 0b83f3c..4eed2e7 100644
--- a/src/main/java/org/ironriders/drive/DriveConstants.java
+++ b/src/main/java/org/ironriders/drive/DriveConstants.java
@@ -12,11 +12,15 @@ public class DriveConstants {
     public static final int SECONDARY_CONTROLLER_PORT = 1;
 
     public static final File SWERVE_JSON_DIRECTORY = new File(Filesystem.getDeployDirectory(), "swerve");
-    public static final PPHolonomicDriveController HOLONOMIC_CONFIG = 
-        new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+    public static final PPHolonomicDriveController HOLONOMIC_CONFIG = new PPHolonomicDriveController( // PPHolonomicController
+                                                                                                      // is the built in
+                                                                                                      // path following
+                                                                                                      // controller for
+                                                                                                      // holonomic drive
+                                                                                                      // trains
             new PIDConstants(5.0, 0.0, 0.0), // Translation PID
             new PIDConstants(5.0, 0.0, 0.0) // Rotation PID
-        );
+    );
 
     // Mathematical Constants
     public static final double TRANSLATION_CONTROL_EXPONENT = 3.0;
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index e1a9a67..7247e56 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -1,8 +1,6 @@
 package org.ironriders.vision;
 
-
 import java.util.List;
-
 import org.ironriders.drive.DriveSubsystem;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -28,7 +26,7 @@ public VisionCommands(VisionSubsystem visionSubsystem, DriveSubsystem driveSubsy
     public Command alignCoral(PhotonCamera camera) {
         return VisionSubsystem.runOnce(() -> {
 
-            System.out.println("started coral");
+            System.out.println("started coral mini auto");
             int[] tags = null;
             if (!DriverStation.getAlliance().isPresent()) {
                 error("no alliace!");
@@ -36,10 +34,8 @@ public Command alignCoral(PhotonCamera camera) {
             }
             if (DriverStation.getAlliance().get() == Alliance.Red) {
                 tags = VisionConstants.REEF_TAG_IDS_RED;
-                print("red");
             } else {
                 tags = VisionConstants.REEF_TAG_IDS_BLUE;
-                print("blue");
             }
             boolean moved = false;
             PhotonPipelineResult result = camera.getLatestResult();
@@ -103,4 +99,4 @@ private void print(int input) {
     private void error(String input) {
         System.err.println(input);
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 459f777..68fcb65 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -13,28 +13,31 @@
 public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
     private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
+
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
     }
-    public PhotonCamera getCamera(){
+
+    public PhotonCamera getCamera() {
         return this.camera;
     }
+
     public void alignwithCoral() {
         commands.alignCoral(camera);
     }
 
     @Override
     public void periodic() {
-        PhotonPipelineResult result=camera.getLatestResult();
-        if(!result.hasTargets()){
+        PhotonPipelineResult result = camera.getLatestResult();
+        if (!result.hasTargets()) {
             SmartDashboard.putString("IDs", "None");
             return;
         }
-        List<PhotonTrackedTarget> targets=result.getTargets();
-        String ids="IDs: ";
-        for (PhotonTrackedTarget target :targets){
-            ids+=(target.fiducialId)+" ";
+        List<PhotonTrackedTarget> targets = result.getTargets();
+        String ids = "IDs: ";
+        for (PhotonTrackedTarget target : targets) {
+            ids += (target.fiducialId) + " ";
         }
         SmartDashboard.putString("IDs", ids);
     }

From d90ce07fd5fbf0255abe01ca5e6f8a4d2d6951ad Mon Sep 17 00:00:00 2001
From: saturncorgi <saturncorgi@gmail.com>
Date: Wed, 22 Jan 2025 22:22:17 -0800
Subject: [PATCH 15/44] some *limited* commenting + added canAlignCoral smart
 dashboard value I mostley just wanted an excuse to use my new keyboard

---
 .../org/ironriders/vision/VisionCommands.java |  9 ++++----
 .../ironriders/vision/VisionSubsystem.java    | 22 ++++++++++++++++++-
 2 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 7247e56..68fc347 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -60,8 +60,8 @@ public Command alignCoral(PhotonCamera camera) {
             if (!moved) {
                 print("no tags I want");
                 List<PhotonTrackedTarget> targets = result.getTargets();
-                for (PhotonTrackedTarget target : targets) {
-                    print(target.fiducialId);
+                for (PhotonTrackedTarget target : targets) { // this whole if statement is really just for debugging and is probably outdated due to the smart dashboard layout
+                    print(target.fiducialId);//my experiance however is that as soon as i remove it i'll need it so it stays
                 }
                 return;
             }
@@ -72,14 +72,13 @@ public Command alignCoral(PhotonCamera camera) {
     }
 
     private Translation2d getPathToTag(int id, PhotonPipelineResult result) {
-        print("getting path");
         boolean hasTargets = result.hasTargets();
         if (!hasTargets) {
-            print("no valid targets!");
+            print("no valid targets!"); //this should be caught before this function is ran but might as well double check
             return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
-        for (PhotonTrackedTarget target : targets) {
+        for (PhotonTrackedTarget target : targets) {//see comment on nested for loops in visionSubsystem
             if (target.getFiducialId() == id) {
                 Transform3d pose = target.getBestCameraToTarget();
                 return new Translation2d(pose.getX(), pose.getY());
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 68fcb65..b5a8789 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -7,13 +7,15 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
     private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
-
+    public boolean canAlignCoral;//you can get this if you want!
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
@@ -40,5 +42,23 @@ public void periodic() {
             ids += (target.fiducialId) + " ";
         }
         SmartDashboard.putString("IDs", ids);
+
+        int[] tags = null;
+        if (DriverStation.getAlliance().get() == Alliance.Red) {
+            tags = VisionConstants.REEF_TAG_IDS_RED;
+
+        } else {
+            tags = VisionConstants.REEF_TAG_IDS_BLUE;
+        }
+        boolean foundTag=false;
+        for (int i : tags) {
+            for (PhotonTrackedTarget target : targets) {//nested for loops feel slow but also i don't really care
+                if(i==target.fiducialId){
+                    foundTag=true;
+                }
+            }
+        }
+        canAlignCoral=foundTag;
+        SmartDashboard.putBoolean("Can Align Coral", canAlignCoral);
     }
 }

From 6c4a13f66da47d6263bff03be2d9eb0435bca524 Mon Sep 17 00:00:00 2001
From: saturncorgi <saturncorgi@gmail.com>
Date: Wed, 22 Jan 2025 23:32:29 -0800
Subject: [PATCH 16/44] added pose estimation based on all 4 cameras possibly

---
 .../ironriders/vision/VisionConstants.java    |  4 ++
 .../ironriders/vision/VisionSubsystem.java    | 63 ++++++++++++++++++-
 2 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index 01c4e93..3445581 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -1,7 +1,11 @@
 package org.ironriders.vision;
 
+import edu.wpi.first.math.geometry.Transform3d;
+
 public class VisionConstants {
     public static final String CAM_NAME="apriltagcam";
+    public static final String[] CAM_NAMES={"frontLeft","frontRight","backLeft","backRight"};//eventually we will have more cameras
+    public static final Transform3d[] CAM_OFFSETS={new Transform3d(),new Transform3d(),new Transform3d(),new Transform3d()};//put the offsets in here
     public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
     public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index b5a8789..669c3a0 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -4,9 +4,17 @@
 
 import org.ironriders.drive.DriveSubsystem;
 import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -15,6 +23,7 @@
 public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
     private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
+    private DriveSubsystem driveSubsystem;
     public boolean canAlignCoral;//you can get this if you want!
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
@@ -28,7 +37,10 @@ public PhotonCamera getCamera() {
     public void alignwithCoral() {
         commands.alignCoral(camera);
     }
-
+    public void setDriveSubsystem(DriveSubsystem driveSubsystem){
+        this.driveSubsystem=driveSubsystem;
+    }
+    @SuppressWarnings("null")
     @Override
     public void periodic() {
         PhotonPipelineResult result = camera.getLatestResult();
@@ -58,7 +70,54 @@ public void periodic() {
                 }
             }
         }
-        canAlignCoral=foundTag;
+        canAlignCoral=foundTag; //update public var so others can see it
         SmartDashboard.putBoolean("Can Align Coral", canAlignCoral);
+
+
+
+        //begin pose test space area. there's always a chance this will look nicer later
+
+        AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+        //Forward Camera
+        List<PhotonCamera> cams=null;
+        for(String name :VisionConstants.CAM_NAMES){
+            cams.add(new PhotonCamera(name));
+        }
+
+        List<PhotonPoseEstimator>poseEstimators=null;
+        // Construct PhotonPoseEstimator
+        for(Transform3d offsett:VisionConstants.CAM_OFFSETS){
+            poseEstimators.add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));  
+        }
+        int index=0;
+        for(PhotonPoseEstimator estimate :poseEstimators){
+            result=cams.get(index).getLatestResult();
+            estimate.update(result);
+            index++;
+        }
+        //great now we have a estimate from all the cameras. I don't really know what to do with this so i'll contruct an average pose i guess;
+        double averageX=0;
+        double averageY=0;
+        double averageZ=0;
+        double averageRotationX=0;
+        double averageRotationY=0;
+        double averageRotationZ=0;
+
+        for(PhotonPoseEstimator estimate :poseEstimators){
+            averageX+=estimate.getReferencePose().getX();
+            averageY+=estimate.getReferencePose().getY();
+            averageZ+=estimate.getReferencePose().getZ();
+            averageRotationX+=estimate.getReferencePose().getRotation().getX();
+            averageRotationY+=estimate.getReferencePose().getRotation().getY();
+            averageRotationZ+=estimate.getReferencePose().getRotation().getZ();
+        }
+        averageX=averageX/4;
+        averageY=averageY/4;
+        averageZ=averageZ/4;
+        averageRotationX=averageRotationX/4;
+        averageRotationY=averageRotationY/4;
+        averageRotationZ=averageRotationZ/4;
+        Pose3d averagePose=new Pose3d(averageX,averageY,averageZ,new Rotation3d(averageRotationX,averageRotationY,averageRotationZ));//yay
+        driveSubsystem.getSwerveDrive().addVisionMeasurement(new Pose2d(averagePose.getX(),averagePose.getY(),averagePose.getRotation().toRotation2d()),System.nanoTime());//update the swerve drives position stuff
     }
 }

From 0184e77f97a9ab5e65ecaa37f7cc596ed6db428e Mon Sep 17 00:00:00 2001
From: saturncorgi <saturncorgi@gmail.com>
Date: Wed, 22 Jan 2025 23:33:49 -0800
Subject: [PATCH 17/44] FYI that was all writen at 11:30 at night and probably
 won't work

---
 src/main/java/org/ironriders/vision/VisionSubsystem.java | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 669c3a0..db48cfc 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -75,7 +75,8 @@ public void periodic() {
 
 
 
-        //begin pose test space area. there's always a chance this will look nicer later
+        //begin pose test space area. there's always a chance this will look nicer later. probably not.
+
 
         AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
         //Forward Camera

From 4225bf7309f673f2bb895f25b3ea86a2de370f67 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 08:50:51 -0800
Subject: [PATCH 18/44] cleaned up comments a bit

---
 src/main/java/org/ironriders/vision/VisionConstants.java | 2 +-
 src/main/java/org/ironriders/vision/VisionSubsystem.java | 2 --
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index 3445581..b57bb78 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -4,7 +4,7 @@
 
 public class VisionConstants {
     public static final String CAM_NAME="apriltagcam";
-    public static final String[] CAM_NAMES={"frontLeft","frontRight","backLeft","backRight"};//eventually we will have more cameras
+    public static final String[] CAM_NAMES={"frontLeft","frontRight","backLeft","backRight"};
     public static final Transform3d[] CAM_OFFSETS={new Transform3d(),new Transform3d(),new Transform3d(),new Transform3d()};//put the offsets in here
     public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
     public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index db48cfc..d48293d 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -79,14 +79,12 @@ public void periodic() {
 
 
         AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-        //Forward Camera
         List<PhotonCamera> cams=null;
         for(String name :VisionConstants.CAM_NAMES){
             cams.add(new PhotonCamera(name));
         }
 
         List<PhotonPoseEstimator>poseEstimators=null;
-        // Construct PhotonPoseEstimator
         for(Transform3d offsett:VisionConstants.CAM_OFFSETS){
             poseEstimators.add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));  
         }

From 7f97df9e3a597b4ad49733b7593d58e422f42c6f Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 09:02:44 -0800
Subject: [PATCH 19/44] re-add drive stuff + format vision stuff

---
 .../org/ironriders/core/RobotContainer.java   | 13 +--
 .../org/ironriders/drive/DriveCommands.java   | 19 +---
 .../org/ironriders/drive/DriveSubsystem.java  |  7 +-
 .../org/ironriders/vision/VisionCommands.java | 11 ++-
 .../ironriders/vision/VisionConstants.java    | 11 +--
 .../ironriders/vision/VisionSubsystem.java    | 90 ++++++++++---------
 6 files changed, 75 insertions(+), 76 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index f3adfb3..137dfac 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -7,6 +7,10 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import org.ironriders.vision.VisionCommands;
 import org.ironriders.vision.VisionSubsystem;
 
@@ -50,8 +54,8 @@ public RobotContainer() {
     configureBindings();
 
     // Init auto chooser
-    // autoChooser = AutoBuilder.buildAutoChooser();
-    // SmartDashboard.putData("Auto Select", autoChooser);
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Select", autoChooser);
   }
 
   /**
@@ -102,10 +106,7 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous. THIS IS A PLACEHOLDER!
-    // return autoChooser.getSelected();
-    return new Command() {
-
-    };
+    return autoChooser.getSelected();
   }
   
 }
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 0298487..4c8af27 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -8,8 +8,6 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathPlannerPath;
 
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -26,15 +24,7 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
 
-	/*
-	 * SparkMax sparkMax=new SparkMax(13, MotorType.kBrushless);
-	 * public Command driveTest(){
-	 * return driveSubsystem.runOnce(()->{
-	 * sparkMax.set(1);
-	 * }
-	 * );
-	 * }
-	 */
+
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
@@ -53,11 +43,10 @@ public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inpu
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
 					new Translation2d(
-			// inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
-			// inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+			 inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
+			 inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
 			),
-					// inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
-					0,
+					 inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
 					true // Gus likes it this way
 			);
 		});
diff --git a/src/main/java/org/ironriders/drive/DriveSubsystem.java b/src/main/java/org/ironriders/drive/DriveSubsystem.java
index 591005f..e553066 100644
--- a/src/main/java/org/ironriders/drive/DriveSubsystem.java
+++ b/src/main/java/org/ironriders/drive/DriveSubsystem.java
@@ -22,8 +22,7 @@ public class DriveSubsystem extends SubsystemBase {
 	private SwerveDrive swerveDrive;
 
 	public DriveSubsystem() throws RuntimeException {
-
-	try {
+		try {
 			swerveDrive = 
 				new SwerveParser(DriveConstants.SWERVE_JSON_DIRECTORY) // YAGSL reads from the deply/swerve directory.
 					.createSwerveDrive(DriveConstants.SWERVE_MAXIMUM_SPEED);
@@ -58,7 +57,7 @@ public DriveSubsystem() throws RuntimeException {
 				return false;
 			},
 			this
-		);*/
+		);
 	}
 
 	/** 
@@ -71,7 +70,7 @@ public DriveSubsystem() throws RuntimeException {
 	 * @param fieldRelative If not field relative, the robot will move relative to its own rotation.
 	 */
 	public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
-		//swerveDrive.drive(translation, rotation, fieldRelative, false);
+		swerveDrive.drive(translation, rotation, fieldRelative, false);
 	}
 
 	/** Fetch the DriveCommands instance */
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 68fc347..5a5706c 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -60,8 +60,10 @@ public Command alignCoral(PhotonCamera camera) {
             if (!moved) {
                 print("no tags I want");
                 List<PhotonTrackedTarget> targets = result.getTargets();
-                for (PhotonTrackedTarget target : targets) { // this whole if statement is really just for debugging and is probably outdated due to the smart dashboard layout
-                    print(target.fiducialId);//my experiance however is that as soon as i remove it i'll need it so it stays
+                for (PhotonTrackedTarget target : targets) { // this whole if statement is really just for debugging and
+                                                             // is probably outdated due to the smart dashboard layout
+                    print(target.fiducialId);// my experiance however is that as soon as i remove it i'll need it so it
+                                             // stays
                 }
                 return;
             }
@@ -74,11 +76,12 @@ public Command alignCoral(PhotonCamera camera) {
     private Translation2d getPathToTag(int id, PhotonPipelineResult result) {
         boolean hasTargets = result.hasTargets();
         if (!hasTargets) {
-            print("no valid targets!"); //this should be caught before this function is ran but might as well double check
+            print("no valid targets!"); // this should be caught before this function is ran but might as well double
+                                        // check
             return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
-        for (PhotonTrackedTarget target : targets) {//see comment on nested for loops in visionSubsystem
+        for (PhotonTrackedTarget target : targets) {// see comment on nested for loops in visionSubsystem
             if (target.getFiducialId() == id) {
                 Transform3d pose = target.getBestCameraToTarget();
                 return new Translation2d(pose.getX(), pose.getY());
diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index b57bb78..9f2ef1a 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -3,9 +3,10 @@
 import edu.wpi.first.math.geometry.Transform3d;
 
 public class VisionConstants {
-    public static final String CAM_NAME="apriltagcam";
-    public static final String[] CAM_NAMES={"frontLeft","frontRight","backLeft","backRight"};
-    public static final Transform3d[] CAM_OFFSETS={new Transform3d(),new Transform3d(),new Transform3d(),new Transform3d()};//put the offsets in here
-    public static final int[] REEF_TAG_IDS_RED={17,18,19,20,21,22};
-    public static final int[] REEF_TAG_IDS_BLUE={6,7,8,9,10,11};
+    public static final String CAM_NAME = "apriltagcam";
+    public static final String[] CAM_NAMES = { "frontLeft", "frontRight", "backLeft", "backRight" };
+    public static final Transform3d[] CAM_OFFSETS = { new Transform3d(), new Transform3d(), new Transform3d(),
+            new Transform3d() };// put the offsets in here
+    public static final int[] REEF_TAG_IDS_RED = { 17, 18, 19, 20, 21, 22 };
+    public static final int[] REEF_TAG_IDS_BLUE = { 6, 7, 8, 9, 10, 11 };
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index d48293d..ec4442f 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -24,7 +24,8 @@ public class VisionSubsystem extends SubsystemBase {
     private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
     private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
     private DriveSubsystem driveSubsystem;
-    public boolean canAlignCoral;//you can get this if you want!
+    public boolean canAlignCoral;// you can get this if you want!
+
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
@@ -37,9 +38,11 @@ public PhotonCamera getCamera() {
     public void alignwithCoral() {
         commands.alignCoral(camera);
     }
-    public void setDriveSubsystem(DriveSubsystem driveSubsystem){
-        this.driveSubsystem=driveSubsystem;
+
+    public void setDriveSubsystem(DriveSubsystem driveSubsystem) {
+        this.driveSubsystem = driveSubsystem;
     }
+
     @SuppressWarnings("null")
     @Override
     public void periodic() {
@@ -62,61 +65,64 @@ public void periodic() {
         } else {
             tags = VisionConstants.REEF_TAG_IDS_BLUE;
         }
-        boolean foundTag=false;
+        boolean foundTag = false;
         for (int i : tags) {
-            for (PhotonTrackedTarget target : targets) {//nested for loops feel slow but also i don't really care
-                if(i==target.fiducialId){
-                    foundTag=true;
+            for (PhotonTrackedTarget target : targets) {// nested for loops feel slow but also i don't really care
+                if (i == target.fiducialId) {
+                    foundTag = true;
                 }
             }
         }
-        canAlignCoral=foundTag; //update public var so others can see it
+        canAlignCoral = foundTag; // update public var so others can see it
         SmartDashboard.putBoolean("Can Align Coral", canAlignCoral);
 
-
-
-        //begin pose test space area. there's always a chance this will look nicer later. probably not.
-
+        // begin pose test space area. there's always a chance this will look nicer
+        // later. probably not.
 
         AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-        List<PhotonCamera> cams=null;
-        for(String name :VisionConstants.CAM_NAMES){
+        List<PhotonCamera> cams = null;
+        for (String name : VisionConstants.CAM_NAMES) {
             cams.add(new PhotonCamera(name));
         }
 
-        List<PhotonPoseEstimator>poseEstimators=null;
-        for(Transform3d offsett:VisionConstants.CAM_OFFSETS){
-            poseEstimators.add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));  
+        List<PhotonPoseEstimator> poseEstimators = null;
+        for (Transform3d offsett : VisionConstants.CAM_OFFSETS) {
+            poseEstimators
+                    .add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));
         }
-        int index=0;
-        for(PhotonPoseEstimator estimate :poseEstimators){
-            result=cams.get(index).getLatestResult();
+        int index = 0;
+        for (PhotonPoseEstimator estimate : poseEstimators) {
+            result = cams.get(index).getLatestResult();
             estimate.update(result);
             index++;
         }
-        //great now we have a estimate from all the cameras. I don't really know what to do with this so i'll contruct an average pose i guess;
-        double averageX=0;
-        double averageY=0;
-        double averageZ=0;
-        double averageRotationX=0;
-        double averageRotationY=0;
-        double averageRotationZ=0;
+        // great now we have a estimate from all the cameras. I don't really know what
+        // to do with this so i'll contruct an average pose i guess;
+        double averageX = 0;
+        double averageY = 0;
+        double averageZ = 0;
+        double averageRotationX = 0;
+        double averageRotationY = 0;
+        double averageRotationZ = 0;
 
-        for(PhotonPoseEstimator estimate :poseEstimators){
-            averageX+=estimate.getReferencePose().getX();
-            averageY+=estimate.getReferencePose().getY();
-            averageZ+=estimate.getReferencePose().getZ();
-            averageRotationX+=estimate.getReferencePose().getRotation().getX();
-            averageRotationY+=estimate.getReferencePose().getRotation().getY();
-            averageRotationZ+=estimate.getReferencePose().getRotation().getZ();
+        for (PhotonPoseEstimator estimate : poseEstimators) {
+            averageX += estimate.getReferencePose().getX();
+            averageY += estimate.getReferencePose().getY();
+            averageZ += estimate.getReferencePose().getZ();
+            averageRotationX += estimate.getReferencePose().getRotation().getX();
+            averageRotationY += estimate.getReferencePose().getRotation().getY();
+            averageRotationZ += estimate.getReferencePose().getRotation().getZ();
         }
-        averageX=averageX/4;
-        averageY=averageY/4;
-        averageZ=averageZ/4;
-        averageRotationX=averageRotationX/4;
-        averageRotationY=averageRotationY/4;
-        averageRotationZ=averageRotationZ/4;
-        Pose3d averagePose=new Pose3d(averageX,averageY,averageZ,new Rotation3d(averageRotationX,averageRotationY,averageRotationZ));//yay
-        driveSubsystem.getSwerveDrive().addVisionMeasurement(new Pose2d(averagePose.getX(),averagePose.getY(),averagePose.getRotation().toRotation2d()),System.nanoTime());//update the swerve drives position stuff
+        averageX = averageX / 4;
+        averageY = averageY / 4;
+        averageZ = averageZ / 4;
+        averageRotationX = averageRotationX / 4;
+        averageRotationY = averageRotationY / 4;
+        averageRotationZ = averageRotationZ / 4;
+        Pose3d averagePose = new Pose3d(averageX, averageY, averageZ,
+                new Rotation3d(averageRotationX, averageRotationY, averageRotationZ));// yay
+        driveSubsystem.getSwerveDrive().addVisionMeasurement(
+                new Pose2d(averagePose.getX(), averagePose.getY(), averagePose.getRotation().toRotation2d()),
+                System.nanoTime());// update the swerve drives position stuff
     }
 }

From 24b0d32e5fa45972d90ab8f42f4344d77050d023 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 10:33:14 -0800
Subject: [PATCH 20/44] IDK stuff

---
 src/main/java/org/ironriders/vision/VisionCommands.java  | 2 +-
 src/main/java/org/ironriders/vision/VisionSubsystem.java | 5 ++---
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index 5a5706c..b579d7d 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -77,7 +77,7 @@ private Translation2d getPathToTag(int id, PhotonPipelineResult result) {
         boolean hasTargets = result.hasTargets();
         if (!hasTargets) {
             print("no valid targets!"); // this should be caught before this function is ran but might as well double
-                                        // check
+                                              // check
             return null;
         }
         List<PhotonTrackedTarget> targets = result.getTargets();
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index ec4442f..d30105c 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -87,8 +87,7 @@ public void periodic() {
 
         List<PhotonPoseEstimator> poseEstimators = null;
         for (Transform3d offsett : VisionConstants.CAM_OFFSETS) {
-            poseEstimators
-                    .add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));
+            poseEstimators.add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));
         }
         int index = 0;
         for (PhotonPoseEstimator estimate : poseEstimators) {
@@ -105,7 +104,7 @@ public void periodic() {
         double averageRotationY = 0;
         double averageRotationZ = 0;
 
-        for (PhotonPoseEstimator estimate : poseEstimators) {
+        for (PhotonPoseEstimator estimate : poseEstimators) {//i could probably combine this with the last loop but i didn't do that initially and if it aint broke
             averageX += estimate.getReferencePose().getX();
             averageY += estimate.getReferencePose().getY();
             averageZ += estimate.getReferencePose().getZ();

From 5855b6099fe9cbc343034087d3f5504a57207d8e Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 10:39:11 -0800
Subject: [PATCH 21/44] Allow for an arbitrary number of cameras

---
 .../java/org/ironriders/vision/VisionSubsystem.java  | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index d30105c..262ff1b 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -112,12 +112,12 @@ public void periodic() {
             averageRotationY += estimate.getReferencePose().getRotation().getY();
             averageRotationZ += estimate.getReferencePose().getRotation().getZ();
         }
-        averageX = averageX / 4;
-        averageY = averageY / 4;
-        averageZ = averageZ / 4;
-        averageRotationX = averageRotationX / 4;
-        averageRotationY = averageRotationY / 4;
-        averageRotationZ = averageRotationZ / 4;
+        averageX = averageX / cams.size();
+        averageY = averageY / cams.size();
+        averageZ = averageZ / cams.size();
+        averageRotationX = averageRotationX / cams.size();
+        averageRotationY = averageRotationY / cams.size();
+        averageRotationZ = averageRotationZ / cams.size();
         Pose3d averagePose = new Pose3d(averageX, averageY, averageZ,
                 new Rotation3d(averageRotationX, averageRotationY, averageRotationZ));// yay
         driveSubsystem.getSwerveDrive().addVisionMeasurement(

From a79b52dda6f745fa55c3cd970feab2335f36df0f Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 10:44:43 -0800
Subject: [PATCH 22/44] also stuff

---
 src/main/java/org/ironriders/vision/VisionConstants.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index 9f2ef1a..39726fe 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -4,9 +4,9 @@
 
 public class VisionConstants {
     public static final String CAM_NAME = "apriltagcam";
-    public static final String[] CAM_NAMES = { "frontLeft", "frontRight", "backLeft", "backRight" };
-    public static final Transform3d[] CAM_OFFSETS = { new Transform3d(), new Transform3d(), new Transform3d(),
-            new Transform3d() };// put the offsets in here
+    public static final String[] CAM_NAMES = { "frontLeft", "frontRight", "backLeft", "backRight" };//you can put any number of cameras here
+    public static final Transform3d[] CAM_OFFSETS = { new Transform3d(), new Transform3d(), new Transform3d(),//just make sure these arrays are 1:1
+            new Transform3d() };
     public static final int[] REEF_TAG_IDS_RED = { 17, 18, 19, 20, 21, 22 };
     public static final int[] REEF_TAG_IDS_BLUE = { 6, 7, 8, 9, 10, 11 };
 }

From 6abc22231720be7048e0075d0b6baa696e0442be Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 10:48:56 -0800
Subject: [PATCH 23/44] a bit of input validation

---
 .../java/org/ironriders/vision/VisionConstants.java    |  5 ++---
 .../java/org/ironriders/vision/VisionSubsystem.java    | 10 ++++++++--
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionConstants.java b/src/main/java/org/ironriders/vision/VisionConstants.java
index 39726fe..0cc3629 100644
--- a/src/main/java/org/ironriders/vision/VisionConstants.java
+++ b/src/main/java/org/ironriders/vision/VisionConstants.java
@@ -4,9 +4,8 @@
 
 public class VisionConstants {
     public static final String CAM_NAME = "apriltagcam";
-    public static final String[] CAM_NAMES = { "frontLeft", "frontRight", "backLeft", "backRight" };//you can put any number of cameras here
-    public static final Transform3d[] CAM_OFFSETS = { new Transform3d(), new Transform3d(), new Transform3d(),//just make sure these arrays are 1:1
-            new Transform3d() };
+    public static final String[] CAM_NAMES = {};//you can put any number of cameras here
+    public static final Transform3d[] CAM_OFFSETS = {};
     public static final int[] REEF_TAG_IDS_RED = { 17, 18, 19, 20, 21, 22 };
     public static final int[] REEF_TAG_IDS_BLUE = { 6, 7, 8, 9, 10, 11 };
 }
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 262ff1b..424905e 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -78,13 +78,19 @@ public void periodic() {
 
         // begin pose test space area. there's always a chance this will look nicer
         // later. probably not.
-
+        if(VisionConstants.CAM_OFFSETS.length==0){
+            System.out.println("no cameras set skipping!");  
+            return;
+        }
         AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
         List<PhotonCamera> cams = null;
         for (String name : VisionConstants.CAM_NAMES) {
             cams.add(new PhotonCamera(name));
         }
-
+        if(cams.size()!=VisionConstants.CAM_OFFSETS.length){
+            System.out.println("VISION ARRAY MISMATCH!!!!!!!");
+            return;
+        }
         List<PhotonPoseEstimator> poseEstimators = null;
         for (Transform3d offsett : VisionConstants.CAM_OFFSETS) {
             poseEstimators.add(new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, offsett));

From c9b5a1058dcc9338f3cb02985001914f11f7cbc4 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 11:59:16 -0800
Subject: [PATCH 24/44] stuff

---
 src/main/java/org/ironriders/vision/VisionSubsystem.java | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 424905e..5fe9a43 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -76,8 +76,6 @@ public void periodic() {
         canAlignCoral = foundTag; // update public var so others can see it
         SmartDashboard.putBoolean("Can Align Coral", canAlignCoral);
 
-        // begin pose test space area. there's always a chance this will look nicer
-        // later. probably not.
         if(VisionConstants.CAM_OFFSETS.length==0){
             System.out.println("no cameras set skipping!");  
             return;

From 843d90208049cce7fa58b1a29654b6a3c47db573 Mon Sep 17 00:00:00 2001
From: saturncorgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 14:56:14 -0800
Subject: [PATCH 25/44] update all vendordeps

---
 vendordeps/PathplannerLib.json                |  6 +-
 vendordeps/Phoenix6-frc2025-latest.json       | 82 +++++++++++++------
 vendordeps/REVLib.json                        | 13 ++-
 ...b-2025.0.0.json => ReduxLib-2025.0.1.json} | 12 +--
 vendordeps/ThriftyLib.json                    |  4 +-
 vendordeps/maple-sim.json                     |  4 +-
 vendordeps/photonlib.json                     | 12 +--
 ...agsl-2025.1.3.json => yagsl-2025.2.2.json} | 10 +--
 8 files changed, 85 insertions(+), 58 deletions(-)
 rename vendordeps/{ReduxLib-2025.0.0.json => ReduxLib-2025.0.1.json} (90%)
 rename vendordeps/{yagsl-2025.1.3.json => yagsl-2025.2.2.json} (91%)

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 396f92d..7dbcb1f 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2025.1.1",
+    "version": "2025.2.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.1.1"
+            "version": "2025.2.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.1.1",
+            "version": "2025.2.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json
index 7f4bd2e..cf87357 100644
--- a/vendordeps/Phoenix6-frc2025-latest.json
+++ b/vendordeps/Phoenix6-frc2025-latest.json
@@ -1,7 +1,7 @@
 {
     "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index 2c39628..7194603 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/ReduxLib-2025.0.0.json b/vendordeps/ReduxLib-2025.0.1.json
similarity index 90%
rename from vendordeps/ReduxLib-2025.0.0.json
rename to vendordeps/ReduxLib-2025.0.1.json
index f15ff90..6cc750e 100644
--- a/vendordeps/ReduxLib-2025.0.0.json
+++ b/vendordeps/ReduxLib-2025.0.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "ReduxLib-2025.0.0.json",
+    "fileName": "ReduxLib-2025.0.1.json",
     "name": "ReduxLib",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "frcYear": "2025",
     "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "ReduxLib",
             "headerClassifier": "headers",
             "sourcesClassifier": "sources",
@@ -54,7 +54,7 @@
         {
             "groupId": "com.reduxrobotics.frc",
             "artifactId": "ReduxLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.1",
             "libName": "ReduxCore",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json
index 0244506..00d3f73 100644
--- a/vendordeps/ThriftyLib.json
+++ b/vendordeps/ThriftyLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "ThriftyLib.json",
     "name": "ThriftyLib",
-    "version": "2025.0.1",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.thethriftybot.frc",
             "artifactId": "ThriftyLib-java",
-            "version": "2025.0.1"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [],
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
index 1ebe33c..d8f5051 100644
--- a/vendordeps/maple-sim.json
+++ b/vendordeps/maple-sim.json
@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.2.8",
+    "version": "0.3.3",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.2.8"
+            "version": "0.3.3"
         },
         {
             "groupId": "org.dyn4j",
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index db43d6d..6af3d3e 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2025.0.0-beta-8",
+    "version": "v2025.1.1",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
     "frcYear": "2025",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.1.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -28,7 +28,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.1.1",
             "libName": "photonlib",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -43,7 +43,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.1.1",
             "libName": "photontargeting",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -60,12 +60,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-java",
-            "version": "v2025.0.0-beta-8"
+            "version": "v2025.1.1"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-java",
-            "version": "v2025.0.0-beta-8"
+            "version": "v2025.1.1"
         }
     ]
 }
\ No newline at end of file
diff --git a/vendordeps/yagsl-2025.1.3.json b/vendordeps/yagsl-2025.2.2.json
similarity index 91%
rename from vendordeps/yagsl-2025.1.3.json
rename to vendordeps/yagsl-2025.2.2.json
index 7441cbd..fa43cc7 100644
--- a/vendordeps/yagsl-2025.1.3.json
+++ b/vendordeps/yagsl-2025.2.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "yagsl-2025.1.3.json",
+    "fileName": "yagsl-2025.2.2.json",
     "name": "YAGSL",
-    "version": "2025.1.3",
+    "version": "2025.2.2",
     "frcYear": "2025",
     "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "swervelib",
             "artifactId": "YAGSL-java",
-            "version": "2025.1.3"
+            "version": "2025.2.2"
         }
     ],
     "requires": [
@@ -42,7 +42,7 @@
         },
         {
             "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
-            "errorMessage": "NavX is required!",
+            "errorMessage": "Studica is required!",
             "offlineFileName": "Studica-2025.0.0.json",
             "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json"
         },
@@ -54,7 +54,7 @@
         },
         {
             "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
-            "errorMessage": "MapleSim is requires for simulation",
+            "errorMessage": "maple-sim is required for simulation",
             "offlineFileName": "maple-sim.json",
             "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json"
         }

From 70153f2b8aacf41d610bc69a709e88ebd8252e03 Mon Sep 17 00:00:00 2001
From: Ryan Elbert <ryan.elbert5125@gmail.com>
Date: Mon, 20 Jan 2025 08:22:26 -0800
Subject: [PATCH 26/44] Created Coral Wrist Command, Subsystem and Constants
 file plus util feature from 2024 for converting between radians and degrees
 (Its not building cause unrelated gradle issues)

---
 src/main/java/org/ironriders/core/Utils.java  | 26 +++++
 .../manipulators/CoralWristCommands.java      | 36 +++++++
 .../manipulators/CoralWristSubsystem.java     | 95 +++++++++++++++++++
 .../manipulators/ManipulatorConstants.java    | 47 +++++++++
 4 files changed, 204 insertions(+)
 create mode 100644 src/main/java/org/ironriders/manipulators/CoralWristCommands.java
 create mode 100644 src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
 create mode 100644 src/main/java/org/ironriders/manipulators/ManipulatorConstants.java

diff --git a/src/main/java/org/ironriders/core/Utils.java b/src/main/java/org/ironriders/core/Utils.java
index 055e53d..fbd4221 100644
--- a/src/main/java/org/ironriders/core/Utils.java
+++ b/src/main/java/org/ironriders/core/Utils.java
@@ -1,5 +1,7 @@
 package org.ironriders.core;
 
+import edu.wpi.first.units.measure.Voltage;
+
 public class Utils {
 
     /**
@@ -13,4 +15,28 @@ public class Utils {
     public static double controlCurve(double input, double exponent, double deadband) {
                 return Math.pow(input, exponent);
     }
+
+
+
+     /**
+     * Normalizes a rotational input value to the range [0, 360) degrees.
+     *
+     * @param input The input rotational value.
+     * @return The normalized rotational value within the range [0, 360) degrees.
+     */
+    public static double absoluteRotation(double input) {
+        return (input % 360 + 360) % 360;
+    }
+    /**
+     * Normalizes Added Voltage from Feed Forward to a number between (0.0, 1.0).
+     *
+     * @param input The additional voltage.
+     * @return The normalized volatge value within the range (0.0, 1.0).
+     */
+    public static double percentOfMaxVoltage(double voltage, int maxVoltage){
+        return (voltage/maxVoltage);
+    }
+
+    
+
 }
diff --git a/src/main/java/org/ironriders/manipulators/CoralWristCommands.java b/src/main/java/org/ironriders/manipulators/CoralWristCommands.java
new file mode 100644
index 0000000..1172048
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulators/CoralWristCommands.java
@@ -0,0 +1,36 @@
+package org.ironriders.manipulators;
+
+
+import org.ironriders.manipulators.ManipulatorConstants.State;
+import edu.wpi.first.wpilibj2.command.Command;
+import com.pathplanner.lib.auto.NamedCommands;
+
+public class CoralWristCommands {
+    private final CoralWristSubsystem wrist;
+
+    public CoralWristCommands(CoralWristSubsystem wrist){
+        this.wrist = wrist;
+
+        NamedCommands.registerCommand("Wrist Coral Station", set(State.STATION));
+        NamedCommands.registerCommand("Wrist Upright", set(State.STOWED));
+        NamedCommands.registerCommand("Wrist L1-L3", set(State.L1toL3));
+        NamedCommands.registerCommand("Wrist L4", set(State.L4));
+    }
+
+    public Command set(State state){
+        return wrist
+            .runOnce(() -> wrist.set(state.getPostion()))
+            .until(wrist::atPosition)
+            .handleInterrupt(wrist::reset);
+    }
+
+    public Command reset(){
+        return wrist.runOnce(wrist::reset);
+    }
+
+    public CoralWristSubsystem getCoralWrist(){
+        return wrist;
+    }
+
+
+}
diff --git a/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
new file mode 100644
index 0000000..546945d
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
@@ -0,0 +1,95 @@
+package org.ironriders.manipulators;
+
+import org.ironriders.core.Utils;
+
+import com.revrobotics.spark.SparkLimitSwitch;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.LimitSwitchConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import static org.ironriders.manipulators.ManipulatorConstants.*;
+import static org.ironriders.core.Utils.*;
+
+public class CoralWristSubsystem extends SubsystemBase {
+    //Why do we extend subsystem base?
+    private final CoralWristCommands commands;
+
+    //find acutal motor IDs
+    private final SparkMax motor = new SparkMax(CORALWRISTMOTOR, MotorType.kBrushless);
+    private final ProfiledPIDController pid = new ProfiledPIDController(0.1, 0, 0, PROFILE);
+    private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(CORALWRISTENCODER);
+    private final SparkLimitSwitch forwardLimitSwitch = motor.getForwardLimitSwitch();
+    private final SparkLimitSwitch reverseLimitSwitch = motor.getReverseLimitSwitch();
+    private final LimitSwitchConfig forwardLimitSwitchConfig = new LimitSwitchConfig();
+    private final LimitSwitchConfig reverseLimitSwitchConfig = new LimitSwitchConfig();
+    private final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    // private ArmFeedforward coralFeedforward = new ArmFeedforward(CORALWRISTKS,CORALWRISTKG,CORALWRISTKV); 
+    public CoralWristSubsystem(){
+
+        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true).forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // this sets allows the limit switch to disable the motor  
+        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true).forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // It also sets the Type to k normally closed see https://docs.revrobotics.com/brushless/spark-max/specs/data-port#limit-switch-operation
+        motorConfig
+            .smartCurrentLimit(CORAL_WRIST_CURRENT_STALL_LIMIT, CORAL_WRIST_CURRENT_FREE_LIMIT)
+            .voltageCompensation(CORAL_WRIST_COMPENSATED_VOLTAGE)
+            .idleMode(IdleMode.kBrake)
+            .limitSwitch
+            .apply(forwardLimitSwitchConfig)
+            .apply(reverseLimitSwitchConfig);
+        motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        
+        
+        
+
+        set(getRotation());
+
+        pid.setTolerance(Coral_Wrist_TOLERANCE);
+       
+    
+        commands = new CoralWristCommands(this);
+    }
+
+    @Override
+    public void periodic(){
+        double output = pid.calculate(getRotation()); 
+        motor.set(output);
+
+        SmartDashboard.putNumber(DASHBOARD_PREFIX + "rotation", getRotation());
+        SmartDashboard.putNumber(DASHBOARD_PREFIX + "output", output);
+        SmartDashboard.putNumber(DASHBOARD_PREFIX + "setPoint", pid.getGoal().position);
+        SmartDashboard.putBoolean(DASHBOARD_PREFIX + "fowardSwitch", forwardLimitSwitch.isPressed());
+        SmartDashboard.putBoolean(DASHBOARD_PREFIX + "reverseSwitch", reverseLimitSwitch.isPressed());
+    }
+
+    public void set(double position){
+        pid.setGoal(position);
+    }
+
+    public void reset(){
+        pid.setGoal(getRotation()); // Stops the wrist from moving
+        pid.reset(getRotation());   //sets the error to zero but asssums it has no velocity
+    }
+
+    private double getRotation(){
+        return Utils.absoluteRotation(absoluteEncoder.get() * 360 -CORAL_WRIST_ENCODER_OFFSET);
+    }
+
+    public boolean atPosition(){
+        return pid.atGoal();
+    }
+
+    public CoralWristCommands getWristCommands(){
+        return commands;
+    }
+
+}
+
diff --git a/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java b/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java
new file mode 100644
index 0000000..3650012
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java
@@ -0,0 +1,47 @@
+package org.ironriders.manipulators;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class ManipulatorConstants {
+    
+    public static final String DASHBOARD_PREFIX = "coralwrist/";
+
+    public static final int CORALWRISTMOTOR = -1;
+    public static final int CORALINTAKEMOTOR = -1;
+    public static final int CORALWRISTENCODER = -1;
+    // Need to tune
+    public static final double CORALWRISTKP = 0.55; 
+    public static final double CORALWRISTKI = 0;
+    public static final double CORALWRISTKD = 0.0;
+    // public static final double CORALWRISTKS = 0.0; //The static gain in volts. // Need to test
+    // public static final double CORALWRISTKG = 0.0; //The gravity gain in volts. // Need to test
+    // public static final double CORALWRISTKV = 0.0; // The velocity gain in V/(rad/s).
+
+    public static final int CORAL_WRIST_CURRENT_STALL_LIMIT = 30; //please test
+    public static final int CORAL_WRIST_CURRENT_FREE_LIMIT = 30; //please test
+    public static final int CORAL_WRIST_COMPENSATED_VOLTAGE = 10; // ASK A MENTOR PLEASE
+    public static final double CORAL_WRIST_ENCODER_OFFSET = -1; //please test
+    public static final double Coral_Wrist_TOLERANCE = 10; //tune me please
+
+
+    public enum State {
+        STATION(0),
+        STOWED(0),
+        L1toL3(0),
+        L4(0);
+
+        final double postion;
+
+        State(int postion){
+            this.postion = postion;
+        }
+
+        public double getPostion(){
+            return postion;
+        }
+
+    }
+
+    public static final TrapezoidProfile.Constraints PROFILE =
+                new TrapezoidProfile.Constraints(500, 850);
+}

From 2458538af3646a17acbda9ff9b3976da89736b6f Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:28:44 -0800
Subject: [PATCH 27/44] Create gradle.yml

---
 .github/workflows/gradle.yml | 67 ++++++++++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 .github/workflows/gradle.yml

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
new file mode 100644
index 0000000..87d5ecb
--- /dev/null
+++ b/.github/workflows/gradle.yml
@@ -0,0 +1,67 @@
+# This workflow uses actions that are not certified by GitHub.
+# They are provided by a third-party and are governed by
+# separate terms of service, privacy policy, and support
+# documentation.
+# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time
+# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle
+
+name: Java CI with Gradle
+
+on:
+  push:
+    branches: [ "main" ]
+  pull_request:
+    branches: [ "main" ]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+    permissions:
+      contents: read
+
+    steps:
+    - uses: actions/checkout@v4
+    - name: Set up JDK 17
+      uses: actions/setup-java@v4
+      with:
+        java-version: '17'
+        distribution: 'temurin'
+
+    # Configure Gradle for optimal use in GitHub Actions, including caching of downloaded dependencies.
+    # See: https://github.com/gradle/actions/blob/main/setup-gradle/README.md
+    - name: Setup Gradle
+      uses: gradle/actions/setup-gradle@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0
+
+    - name: Build with Gradle Wrapper
+      run: ./gradlew build
+
+    # NOTE: The Gradle Wrapper is the default and recommended way to run Gradle (https://docs.gradle.org/current/userguide/gradle_wrapper.html).
+    # If your project does not have the Gradle Wrapper configured, you can use the following configuration to run Gradle with a specified version.
+    #
+    # - name: Setup Gradle
+    #   uses: gradle/actions/setup-gradle@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0
+    #   with:
+    #     gradle-version: '8.9'
+    #
+    # - name: Build with Gradle 8.9
+    #   run: gradle build
+
+  dependency-submission:
+
+    runs-on: ubuntu-latest
+    permissions:
+      contents: write
+
+    steps:
+    - uses: actions/checkout@v4
+    - name: Set up JDK 17
+      uses: actions/setup-java@v4
+      with:
+        java-version: '17'
+        distribution: 'temurin'
+
+    # Generates and submits a dependency graph, enabling Dependabot Alerts for all project dependencies.
+    # See: https://github.com/gradle/actions/blob/main/dependency-submission/README.md
+    - name: Generate and submit dependency graph
+      uses: gradle/actions/dependency-submission@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0

From dd5bb185ad32e931f29fb4326b50f04933a09d3b Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:31:01 -0800
Subject: [PATCH 28/44] Update gradle.yml

---
 .github/workflows/gradle.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index 87d5ecb..84c1ac7 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -22,10 +22,10 @@ jobs:
 
     steps:
     - uses: actions/checkout@v4
-    - name: Set up JDK 17
+    - name: Set up JDK 13
       uses: actions/setup-java@v4
       with:
-        java-version: '17'
+        java-version: '13'
         distribution: 'temurin'
 
     # Configure Gradle for optimal use in GitHub Actions, including caching of downloaded dependencies.

From 3e083dbf8a0ad0b0c38ca3da8a3b34008fc64b93 Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:32:10 -0800
Subject: [PATCH 29/44] Update gradle.yml

---
 .github/workflows/gradle.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index 84c1ac7..d9b8188 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -22,10 +22,10 @@ jobs:
 
     steps:
     - uses: actions/checkout@v4
-    - name: Set up JDK 13
+    - name: Set up JDK 11
       uses: actions/setup-java@v4
       with:
-        java-version: '13'
+        java-version: '11'
         distribution: 'temurin'
 
     # Configure Gradle for optimal use in GitHub Actions, including caching of downloaded dependencies.

From b627614aaae51384b025f3f8635120a5c3763062 Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:34:11 -0800
Subject: [PATCH 30/44] Update gradle.yml

---
 .github/workflows/gradle.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index d9b8188..87d5ecb 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -22,10 +22,10 @@ jobs:
 
     steps:
     - uses: actions/checkout@v4
-    - name: Set up JDK 11
+    - name: Set up JDK 17
       uses: actions/setup-java@v4
       with:
-        java-version: '11'
+        java-version: '17'
         distribution: 'temurin'
 
     # Configure Gradle for optimal use in GitHub Actions, including caching of downloaded dependencies.

From 75122bf206792f460cdc5338b594e3c2b4781c37 Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 09:35:17 -0800
Subject: [PATCH 31/44] Update gradle.yml

---
 .github/workflows/gradle.yml | 81 ++++++++++--------------------------
 1 file changed, 22 insertions(+), 59 deletions(-)

diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index 87d5ecb..ff7cce9 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -1,67 +1,30 @@
-# This workflow uses actions that are not certified by GitHub.
-# They are provided by a third-party and are governed by
-# separate terms of service, privacy policy, and support
-# documentation.
-# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time
-# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle
-
-name: Java CI with Gradle
-
+# This is a basic workflow to build robot code.
+name: CI
+# Controls when the action will run. Triggers the workflow on push or pull request
+# events but only for the main branch.
 on:
   push:
-    branches: [ "main" ]
+    branches: [ main ]
   pull_request:
-    branches: [ "main" ]
-
+    branches: [ main ]
+# A workflow run is made up of one or more jobs that can run sequentially or in parallel
 jobs:
+  # This workflow contains a single job called "build"
   build:
-
-    runs-on: ubuntu-latest
-    permissions:
-      contents: read
-
+    # The type of runner that the job will run on
+    runs-on: ubuntu-22.04
+    # This grabs the WPILib docker container
+    container: wpilib/roborio-cross-ubuntu:2025-22.04
+    # Steps represent a sequence of tasks that will be executed as part of the job
     steps:
+    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
     - uses: actions/checkout@v4
-    - name: Set up JDK 17
-      uses: actions/setup-java@v4
-      with:
-        java-version: '17'
-        distribution: 'temurin'
-
-    # Configure Gradle for optimal use in GitHub Actions, including caching of downloaded dependencies.
-    # See: https://github.com/gradle/actions/blob/main/setup-gradle/README.md
-    - name: Setup Gradle
-      uses: gradle/actions/setup-gradle@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0
-
-    - name: Build with Gradle Wrapper
+    # Declares the repository safe and not under dubious ownership.
+    - name: Add repository to git safe directories
+      run: git config --global --add safe.directory $GITHUB_WORKSPACE
+    # Grant execute permission for gradlew
+    - name: Grant execute permission for gradlew
+      run: chmod +x gradlew
+    # Runs a single command using the runners shell
+    - name: Compile and run tests on robot code
       run: ./gradlew build
-
-    # NOTE: The Gradle Wrapper is the default and recommended way to run Gradle (https://docs.gradle.org/current/userguide/gradle_wrapper.html).
-    # If your project does not have the Gradle Wrapper configured, you can use the following configuration to run Gradle with a specified version.
-    #
-    # - name: Setup Gradle
-    #   uses: gradle/actions/setup-gradle@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0
-    #   with:
-    #     gradle-version: '8.9'
-    #
-    # - name: Build with Gradle 8.9
-    #   run: gradle build
-
-  dependency-submission:
-
-    runs-on: ubuntu-latest
-    permissions:
-      contents: write
-
-    steps:
-    - uses: actions/checkout@v4
-    - name: Set up JDK 17
-      uses: actions/setup-java@v4
-      with:
-        java-version: '17'
-        distribution: 'temurin'
-
-    # Generates and submits a dependency graph, enabling Dependabot Alerts for all project dependencies.
-    # See: https://github.com/gradle/actions/blob/main/dependency-submission/README.md
-    - name: Generate and submit dependency graph
-      uses: gradle/actions/dependency-submission@af1da67850ed9a4cedd57bfd976089dd991e2582 # v4.0.0

From ae1f477b9a202dbf9e64a0bfefc59bdb131fc0a3 Mon Sep 17 00:00:00 2001
From: Tyler <104324278+Saturncorgi@users.noreply.github.com>
Date: Thu, 23 Jan 2025 10:03:26 -0800
Subject: [PATCH 32/44] Add workflow status badge to readme

obviously only merge if you actually create the workflow
---
 README.md | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/README.md b/README.md
index 3946b41..4ab338a 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,4 @@
-# This is the Auto branch. 
-
+[![CI](https://github.com/IronRiders/reefscape/actions/workflows/gradle.yml/badge.svg)](https://github.com/IronRiders/reefscape/actions/workflows/gradle.yml)
 # *Iron Riders* - Season 2025 Repository
 
 ![image](https://github.com/user-attachments/assets/65a361c1-4db4-4ecb-b378-381c4e049c19)

From d786b53c8d808bbd4131734393750f084599a1b9 Mon Sep 17 00:00:00 2001
From: a-mishmash <reldamail@icloud.com>
Date: Thu, 23 Jan 2025 15:53:16 -0800
Subject: [PATCH 33/44] Updated drive

---
 src/main/deploy/swerve/modules/backleft.json  |  2 +-
 src/main/deploy/swerve/modules/backright.json |  2 +-
 src/main/deploy/swerve/modules/frontleft.json |  2 +-
 .../deploy/swerve/modules/frontright.json     |  2 +-
 .../org/ironriders/core/RobotContainer.java   | 10 ++-
 .../manipulator/ManipulatorCommands.java      |  5 ++
 .../manipulator/ManipulatorConstants.java     |  9 +++
 .../manipulator/ManipulatorSubsystem.java     | 66 +++++++++++++++++++
 8 files changed, 91 insertions(+), 7 deletions(-)
 create mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
 create mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
 create mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java

diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json
index 9c16fdf..75dba43 100644
--- a/src/main/deploy/swerve/modules/backleft.json
+++ b/src/main/deploy/swerve/modules/backleft.json
@@ -3,7 +3,7 @@
       "front": -12,
       "left": 12
     },
-    "absoluteEncoderOffset": 193.0,
+    "absoluteEncoderOffset": 99.884,
     "drive": {
       "type": "talonfx",
       "id": 5,
diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json
index 90a70db..f8ac24d 100644
--- a/src/main/deploy/swerve/modules/backright.json
+++ b/src/main/deploy/swerve/modules/backright.json
@@ -3,7 +3,7 @@
       "front": -12,
       "left": -12
     },
-    "absoluteEncoderOffset": 185.0,
+    "absoluteEncoderOffset": 205.782,
     "drive": {
       "type": "talonfx",
       "id": 3,
diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json
index c72ac86..37abbc3 100644
--- a/src/main/deploy/swerve/modules/frontleft.json
+++ b/src/main/deploy/swerve/modules/frontleft.json
@@ -3,7 +3,7 @@
       "front": 12,
       "left": 12
     },
-    "absoluteEncoderOffset": 266.0,
+    "absoluteEncoderOffset": 83.670,
     "drive": {
       "type": "talonfx",
       "id": 7,
diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json
index 4785930..cf0b51d 100644
--- a/src/main/deploy/swerve/modules/frontright.json
+++ b/src/main/deploy/swerve/modules/frontright.json
@@ -3,7 +3,7 @@
       "front": 12,
       "left": -12
     },
-    "absoluteEncoderOffset": 22.0,
+    "absoluteEncoderOffset": 33.034,
     "drive": {
       "type": "talonfx",
       "id": 1,
diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 137dfac..495fd30 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -7,7 +7,10 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
+import org.ironriders.manipulator.ManipulatorCommands;
+import org.ironriders.manipulator.ManipulatorSubsystem;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -36,9 +39,10 @@ public class RobotContainer {
   private final DriveSubsystem driveSubsystem = new DriveSubsystem();
   private final DriveCommands driveCommands = driveSubsystem.getCommands();
 
-  private final VisionSubsystem visionSubsystem = new VisionSubsystem();
-  private final VisionCommands visionCommands = visionSubsystem.getCommands();
+  // private final ManipulatorSubsystem manipulatorSubsystem = new ManipulatorSubsystem();
+  // private final ManipulatorCommands manipulatorCommands = manipulatorSubsystem.getCommands();
 
+  // Auto support
   private final SendableChooser<Command> autoChooser;
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
@@ -89,7 +93,7 @@ private void configureBindings() {
               DriveConstants.TRANSLATION_CONTROL_EXPONENT, 
               DriveConstants.TRANSLATION_CONTROL_DEADBAND),
             () -> Utils.controlCurve(
-              primaryController.getLeftX(), 
+              primaryController.getRightX(), 
               DriveConstants.ROTATION_CONTROL_EXPONENT, 
               DriveConstants.ROTATION_CONTROL_DEADBAND)
         )
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java b/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
new file mode 100644
index 0000000..612e8d6
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
@@ -0,0 +1,5 @@
+package org.ironriders.manipulator;
+
+public class ManipulatorCommands {
+
+}
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java b/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
new file mode 100644
index 0000000..c63c465
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
@@ -0,0 +1,9 @@
+package org.ironriders.manipulator;
+
+public class ManipulatorConstants {
+    // CAN IDs
+    public static final int CORAL_WRIST_CAN_ID = 0;
+    public static final int CORAL_INTAKE_CAN_ID = 0;
+    public static final int ALGAE_MOTOR_ONE_CAN_ID = 0;
+    public static final int ALGAE_MOTOR_TWO_CAN_ID = 0;
+}
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java b/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java
new file mode 100644
index 0000000..98f03d5
--- /dev/null
+++ b/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java
@@ -0,0 +1,66 @@
+package org.ironriders.manipulator;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.REVLibError;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkMaxAlternateEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import org.ironriders.manipulator.ManipulatorConstants.*;
+
+/**
+ * The ManipulatorSubsystem controls the intake and placement of coral and algae.
+ * This subsystem is SEPERATE from the ElevatorSubsystem, which controls the (vertical) position of the manipulator.
+ */
+public class ManipulatorSubsystem {
+
+    private ManipulatorCommands commands;
+
+    // Motors & such
+    private SparkMax coralWrist;
+    private AbsoluteEncoder coralWristEncoder;
+    private SparkMax coralIntake;
+
+    private SparkMax algaeMotorOne;
+    private SparkMax algaeMotorTwo;
+
+    public ManipulatorSubsystem() {
+        commands = new ManipulatorCommands();
+
+        // initialize motors
+        coralWrist = new SparkMax(ManipulatorConstants.CORAL_WRIST_CAN_ID, MotorType.kBrushless);
+        coralWristEncoder = coralWrist.getAbsoluteEncoder();
+        coralIntake = new SparkMax(ManipulatorConstants.CORAL_INTAKE_CAN_ID, MotorType.kBrushless);
+        algaeMotorOne = new SparkMax(ManipulatorConstants.ALGAE_MOTOR_ONE_CAN_ID, MotorType.kBrushless);
+        algaeMotorTwo = new SparkMax(ManipulatorConstants.ALGAE_MOTOR_TWO_CAN_ID, MotorType.kBrushless);
+
+        // set up the wrist motor
+        SparkMaxConfig coralWristConfig = new SparkMaxConfig();
+        coralWristConfig
+            .idleMode(IdleMode.kBrake)
+            .smartCurrentLimit(40)
+            .closedLoop
+                .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+                .pidf(0.55, 0, 0, 0.00375);
+        coralWrist.configure(coralWristConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+
+        // set up other motors
+        SparkMaxConfig otherMotorsConfig = new SparkMaxConfig();
+        otherMotorsConfig
+            .idleMode(IdleMode.kBrake)
+            .smartCurrentLimit(15);
+        coralIntake.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+        algaeMotorOne.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+        algaeMotorTwo.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+    }
+
+    public ManipulatorCommands getCommands() {
+        return commands;
+    }
+}

From 65a79fadfc4fe591933e4211237783398aa6772e Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 16:01:11 -0800
Subject: [PATCH 34/44] stuff again

---
 src/main/java/org/ironriders/vision/VisionSubsystem.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 5fe9a43..9bc7389 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -104,7 +104,7 @@ public void periodic() {
         double averageX = 0;
         double averageY = 0;
         double averageZ = 0;
-        double averageRotationX = 0;
+        double averageRotationX = 0;//could this be an array? yes. will it be? no
         double averageRotationY = 0;
         double averageRotationZ = 0;
 

From 2b69c2898bcff925cf41b8e0689a0b6017a8aedb Mon Sep 17 00:00:00 2001
From: a-mishmash <reldamail@icloud.com>
Date: Thu, 23 Jan 2025 16:03:40 -0800
Subject: [PATCH 35/44] bingus

---
 .../org/ironriders/core/RobotContainer.java   |  8 +--
 .../manipulator/ManipulatorCommands.java      |  5 --
 .../manipulator/ManipulatorConstants.java     |  9 ---
 .../manipulator/ManipulatorSubsystem.java     | 66 -------------------
 .../manipulators/CoralWristCommands.java      | 17 ++---
 .../manipulators/CoralWristSubsystem.java     | 56 ++++++++--------
 .../manipulators/ManipulatorConstants.java    | 31 ++++-----
 7 files changed, 55 insertions(+), 137 deletions(-)
 delete mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
 delete mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
 delete mode 100644 src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 495fd30..baaf249 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -7,8 +7,8 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
-import org.ironriders.manipulator.ManipulatorCommands;
-import org.ironriders.manipulator.ManipulatorSubsystem;
+import org.ironriders.manipulators.CoralWristCommands;
+import org.ironriders.manipulators.CoralWristSubsystem;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -39,8 +39,8 @@ public class RobotContainer {
   private final DriveSubsystem driveSubsystem = new DriveSubsystem();
   private final DriveCommands driveCommands = driveSubsystem.getCommands();
 
-  // private final ManipulatorSubsystem manipulatorSubsystem = new ManipulatorSubsystem();
-  // private final ManipulatorCommands manipulatorCommands = manipulatorSubsystem.getCommands();
+  private final CoralWristSubsystem coralWristSubsystem = new CoralWristSubsystem();
+  private final CoralWristCommands coralWristCommands = coralWristSubsystem.getCommands();
 
   // Auto support
   private final SendableChooser<Command> autoChooser;
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java b/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
deleted file mode 100644
index 612e8d6..0000000
--- a/src/main/java/org/ironriders/manipulator/ManipulatorCommands.java
+++ /dev/null
@@ -1,5 +0,0 @@
-package org.ironriders.manipulator;
-
-public class ManipulatorCommands {
-
-}
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java b/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
deleted file mode 100644
index c63c465..0000000
--- a/src/main/java/org/ironriders/manipulator/ManipulatorConstants.java
+++ /dev/null
@@ -1,9 +0,0 @@
-package org.ironriders.manipulator;
-
-public class ManipulatorConstants {
-    // CAN IDs
-    public static final int CORAL_WRIST_CAN_ID = 0;
-    public static final int CORAL_INTAKE_CAN_ID = 0;
-    public static final int ALGAE_MOTOR_ONE_CAN_ID = 0;
-    public static final int ALGAE_MOTOR_TWO_CAN_ID = 0;
-}
diff --git a/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java b/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java
deleted file mode 100644
index 98f03d5..0000000
--- a/src/main/java/org/ironriders/manipulator/ManipulatorSubsystem.java
+++ /dev/null
@@ -1,66 +0,0 @@
-package org.ironriders.manipulator;
-
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.REVLibError;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkMaxAlternateEncoder;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.config.SparkBaseConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import org.ironriders.manipulator.ManipulatorConstants.*;
-
-/**
- * The ManipulatorSubsystem controls the intake and placement of coral and algae.
- * This subsystem is SEPERATE from the ElevatorSubsystem, which controls the (vertical) position of the manipulator.
- */
-public class ManipulatorSubsystem {
-
-    private ManipulatorCommands commands;
-
-    // Motors & such
-    private SparkMax coralWrist;
-    private AbsoluteEncoder coralWristEncoder;
-    private SparkMax coralIntake;
-
-    private SparkMax algaeMotorOne;
-    private SparkMax algaeMotorTwo;
-
-    public ManipulatorSubsystem() {
-        commands = new ManipulatorCommands();
-
-        // initialize motors
-        coralWrist = new SparkMax(ManipulatorConstants.CORAL_WRIST_CAN_ID, MotorType.kBrushless);
-        coralWristEncoder = coralWrist.getAbsoluteEncoder();
-        coralIntake = new SparkMax(ManipulatorConstants.CORAL_INTAKE_CAN_ID, MotorType.kBrushless);
-        algaeMotorOne = new SparkMax(ManipulatorConstants.ALGAE_MOTOR_ONE_CAN_ID, MotorType.kBrushless);
-        algaeMotorTwo = new SparkMax(ManipulatorConstants.ALGAE_MOTOR_TWO_CAN_ID, MotorType.kBrushless);
-
-        // set up the wrist motor
-        SparkMaxConfig coralWristConfig = new SparkMaxConfig();
-        coralWristConfig
-            .idleMode(IdleMode.kBrake)
-            .smartCurrentLimit(40)
-            .closedLoop
-                .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-                .pidf(0.55, 0, 0, 0.00375);
-        coralWrist.configure(coralWristConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
-
-        // set up other motors
-        SparkMaxConfig otherMotorsConfig = new SparkMaxConfig();
-        otherMotorsConfig
-            .idleMode(IdleMode.kBrake)
-            .smartCurrentLimit(15);
-        coralIntake.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
-        algaeMotorOne.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
-        algaeMotorTwo.configure(otherMotorsConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
-    }
-
-    public ManipulatorCommands getCommands() {
-        return commands;
-    }
-}
diff --git a/src/main/java/org/ironriders/manipulators/CoralWristCommands.java b/src/main/java/org/ironriders/manipulators/CoralWristCommands.java
index 1172048..ba0b44d 100644
--- a/src/main/java/org/ironriders/manipulators/CoralWristCommands.java
+++ b/src/main/java/org/ironriders/manipulators/CoralWristCommands.java
@@ -1,6 +1,5 @@
 package org.ironriders.manipulators;
 
-
 import org.ironriders.manipulators.ManipulatorConstants.State;
 import edu.wpi.first.wpilibj2.command.Command;
 import com.pathplanner.lib.auto.NamedCommands;
@@ -8,7 +7,7 @@
 public class CoralWristCommands {
     private final CoralWristSubsystem wrist;
 
-    public CoralWristCommands(CoralWristSubsystem wrist){
+    public CoralWristCommands(CoralWristSubsystem wrist) {
         this.wrist = wrist;
 
         NamedCommands.registerCommand("Wrist Coral Station", set(State.STATION));
@@ -17,20 +16,18 @@ public CoralWristCommands(CoralWristSubsystem wrist){
         NamedCommands.registerCommand("Wrist L4", set(State.L4));
     }
 
-    public Command set(State state){
+    public Command set(State state) {
         return wrist
-            .runOnce(() -> wrist.set(state.getPostion()))
-            .until(wrist::atPosition)
-            .handleInterrupt(wrist::reset);
+                .runOnce(() -> wrist.set(state.getPostion()))
+                .until(wrist::atPosition)
+                .handleInterrupt(wrist::reset);
     }
 
-    public Command reset(){
+    public Command reset() {
         return wrist.runOnce(wrist::reset);
     }
 
-    public CoralWristSubsystem getCoralWrist(){
+    public CoralWristSubsystem getCoralWrist() {
         return wrist;
     }
-
-
 }
diff --git a/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
index 546945d..8190af1 100644
--- a/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
+++ b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
@@ -21,10 +21,10 @@
 import static org.ironriders.core.Utils.*;
 
 public class CoralWristSubsystem extends SubsystemBase {
-    //Why do we extend subsystem base?
+    // Why do we extend subsystem base?
     private final CoralWristCommands commands;
 
-    //find acutal motor IDs
+    // find acutal motor IDs
     private final SparkMax motor = new SparkMax(CORALWRISTMOTOR, MotorType.kBrushless);
     private final ProfiledPIDController pid = new ProfiledPIDController(0.1, 0, 0, PROFILE);
     private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(CORALWRISTENCODER);
@@ -33,34 +33,36 @@ public class CoralWristSubsystem extends SubsystemBase {
     private final LimitSwitchConfig forwardLimitSwitchConfig = new LimitSwitchConfig();
     private final LimitSwitchConfig reverseLimitSwitchConfig = new LimitSwitchConfig();
     private final SparkMaxConfig motorConfig = new SparkMaxConfig();
-    // private ArmFeedforward coralFeedforward = new ArmFeedforward(CORALWRISTKS,CORALWRISTKG,CORALWRISTKV); 
-    public CoralWristSubsystem(){
 
-        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true).forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // this sets allows the limit switch to disable the motor  
-        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true).forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // It also sets the Type to k normally closed see https://docs.revrobotics.com/brushless/spark-max/specs/data-port#limit-switch-operation
+    // private ArmFeedforward coralFeedforward = new
+    // ArmFeedforward(CORALWRISTKS,CORALWRISTKG,CORALWRISTKV);
+    public CoralWristSubsystem() {
+
+        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true)
+                .forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // this sets allows the limit switch to
+                                                                                 // disable the motor
+        forwardLimitSwitchConfig.forwardLimitSwitchEnabled(true)
+                .forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyClosed); // It also sets the Type to k normally
+                                                                                 // closed see
+                                                                                 // https://docs.revrobotics.com/brushless/spark-max/specs/data-port#limit-switch-operation
         motorConfig
-            .smartCurrentLimit(CORAL_WRIST_CURRENT_STALL_LIMIT, CORAL_WRIST_CURRENT_FREE_LIMIT)
-            .voltageCompensation(CORAL_WRIST_COMPENSATED_VOLTAGE)
-            .idleMode(IdleMode.kBrake)
-            .limitSwitch
-            .apply(forwardLimitSwitchConfig)
-            .apply(reverseLimitSwitchConfig);
+                .smartCurrentLimit(CORAL_WRIST_CURRENT_STALL_LIMIT, CORAL_WRIST_CURRENT_FREE_LIMIT)
+                .voltageCompensation(CORAL_WRIST_COMPENSATED_VOLTAGE)
+                .idleMode(IdleMode.kBrake).limitSwitch
+                .apply(forwardLimitSwitchConfig)
+                .apply(reverseLimitSwitchConfig);
         motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        
-        
-        
 
         set(getRotation());
 
         pid.setTolerance(Coral_Wrist_TOLERANCE);
-       
-    
+
         commands = new CoralWristCommands(this);
     }
 
     @Override
-    public void periodic(){
-        double output = pid.calculate(getRotation()); 
+    public void periodic() {
+        double output = pid.calculate(getRotation());
         motor.set(output);
 
         SmartDashboard.putNumber(DASHBOARD_PREFIX + "rotation", getRotation());
@@ -70,26 +72,24 @@ public void periodic(){
         SmartDashboard.putBoolean(DASHBOARD_PREFIX + "reverseSwitch", reverseLimitSwitch.isPressed());
     }
 
-    public void set(double position){
+    public void set(double position) {
         pid.setGoal(position);
     }
 
-    public void reset(){
+    public void reset() {
         pid.setGoal(getRotation()); // Stops the wrist from moving
-        pid.reset(getRotation());   //sets the error to zero but asssums it has no velocity
+        pid.reset(getRotation()); // sets the error to zero but asssums it has no velocity
     }
 
-    private double getRotation(){
-        return Utils.absoluteRotation(absoluteEncoder.get() * 360 -CORAL_WRIST_ENCODER_OFFSET);
+    private double getRotation() {
+        return Utils.absoluteRotation(absoluteEncoder.get() * 360 - CORAL_WRIST_ENCODER_OFFSET);
     }
 
-    public boolean atPosition(){
+    public boolean atPosition() {
         return pid.atGoal();
     }
 
-    public CoralWristCommands getWristCommands(){
+    public CoralWristCommands getCommands() {
         return commands;
     }
-
 }
-
diff --git a/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java b/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java
index 3650012..0be7122 100644
--- a/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java
+++ b/src/main/java/org/ironriders/manipulators/ManipulatorConstants.java
@@ -3,26 +3,28 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class ManipulatorConstants {
-    
+
     public static final String DASHBOARD_PREFIX = "coralwrist/";
 
     public static final int CORALWRISTMOTOR = -1;
     public static final int CORALINTAKEMOTOR = -1;
     public static final int CORALWRISTENCODER = -1;
     // Need to tune
-    public static final double CORALWRISTKP = 0.55; 
+    public static final double CORALWRISTKP = 0.55;
     public static final double CORALWRISTKI = 0;
     public static final double CORALWRISTKD = 0.0;
-    // public static final double CORALWRISTKS = 0.0; //The static gain in volts. // Need to test
-    // public static final double CORALWRISTKG = 0.0; //The gravity gain in volts. // Need to test
-    // public static final double CORALWRISTKV = 0.0; // The velocity gain in V/(rad/s).
-
-    public static final int CORAL_WRIST_CURRENT_STALL_LIMIT = 30; //please test
-    public static final int CORAL_WRIST_CURRENT_FREE_LIMIT = 30; //please test
+    // public static final double CORALWRISTKS = 0.0; //The static gain in volts. //
+    // Need to test
+    // public static final double CORALWRISTKG = 0.0; //The gravity gain in volts.
+    // // Need to test
+    // public static final double CORALWRISTKV = 0.0; // The velocity gain in
+    // V/(rad/s).
+
+    public static final int CORAL_WRIST_CURRENT_STALL_LIMIT = 30; // please test
+    public static final int CORAL_WRIST_CURRENT_FREE_LIMIT = 30; // please test
     public static final int CORAL_WRIST_COMPENSATED_VOLTAGE = 10; // ASK A MENTOR PLEASE
-    public static final double CORAL_WRIST_ENCODER_OFFSET = -1; //please test
-    public static final double Coral_Wrist_TOLERANCE = 10; //tune me please
-
+    public static final double CORAL_WRIST_ENCODER_OFFSET = -1; // please test
+    public static final double Coral_Wrist_TOLERANCE = 10; // tune me please
 
     public enum State {
         STATION(0),
@@ -32,16 +34,15 @@ public enum State {
 
         final double postion;
 
-        State(int postion){
+        State(int postion) {
             this.postion = postion;
         }
 
-        public double getPostion(){
+        public double getPostion() {
             return postion;
         }
 
     }
 
-    public static final TrapezoidProfile.Constraints PROFILE =
-                new TrapezoidProfile.Constraints(500, 850);
+    public static final TrapezoidProfile.Constraints PROFILE = new TrapezoidProfile.Constraints(500, 850);
 }

From 1da318aef5eb962cfe315f7cd95fc357d6920052 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 16:08:31 -0800
Subject: [PATCH 36/44] no warnings

---
 src/main/java/org/ironriders/core/RobotContainer.java        | 5 ++---
 src/main/java/org/ironriders/core/Utils.java                 | 1 -
 .../org/ironriders/manipulators/CoralWristSubsystem.java     | 4 +---
 3 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index baaf249..2f0a284 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -12,7 +12,6 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.ironriders.vision.VisionCommands;
 import org.ironriders.vision.VisionSubsystem;
@@ -39,8 +38,8 @@ public class RobotContainer {
   private final DriveSubsystem driveSubsystem = new DriveSubsystem();
   private final DriveCommands driveCommands = driveSubsystem.getCommands();
 
-  private final CoralWristSubsystem coralWristSubsystem = new CoralWristSubsystem();
-  private final CoralWristCommands coralWristCommands = coralWristSubsystem.getCommands();
+ // private final CoralWristSubsystem coralWristSubsystem = new CoralWristSubsystem();
+ // private final CoralWristCommands coralWristCommands = coralWristSubsystem.getCommands(); no warnings :)
 
   // Auto support
   private final SendableChooser<Command> autoChooser;
diff --git a/src/main/java/org/ironriders/core/Utils.java b/src/main/java/org/ironriders/core/Utils.java
index fbd4221..041b582 100644
--- a/src/main/java/org/ironriders/core/Utils.java
+++ b/src/main/java/org/ironriders/core/Utils.java
@@ -1,6 +1,5 @@
 package org.ironriders.core;
 
-import edu.wpi.first.units.measure.Voltage;
 
 public class Utils {
 
diff --git a/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
index 8190af1..f792b83 100644
--- a/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
+++ b/src/main/java/org/ironriders/manipulators/CoralWristSubsystem.java
@@ -10,15 +10,13 @@
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
+
 import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import static org.ironriders.manipulators.ManipulatorConstants.*;
-import static org.ironriders.core.Utils.*;
 
 public class CoralWristSubsystem extends SubsystemBase {
     // Why do we extend subsystem base?

From 37d14550c24eba04c555a640fc6a9400bda8de87 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 16:08:54 -0800
Subject: [PATCH 37/44] actually no warnings

---
 src/main/java/org/ironriders/core/RobotContainer.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 2f0a284..363e3ed 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -7,8 +7,8 @@
 import org.ironriders.drive.DriveCommands;
 import org.ironriders.drive.DriveConstants;
 import org.ironriders.drive.DriveSubsystem;
-import org.ironriders.manipulators.CoralWristCommands;
-import org.ironriders.manipulators.CoralWristSubsystem;
+//import org.ironriders.manipulators.CoralWristCommands;
+//import org.ironriders.manipulators.CoralWristSubsystem;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;

From 02261798b24039fad2dae2d90cdb65c409463b32 Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Thu, 23 Jan 2025 19:09:31 -0800
Subject: [PATCH 38/44] stuff

---
 src/main/java/org/ironriders/core/RobotContainer.java |  5 ++++-
 .../java/org/ironriders/vision/VisionSubsystem.java   | 11 ++++++-----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 363e3ed..ab802b8 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -41,7 +41,10 @@ public class RobotContainer {
  // private final CoralWristSubsystem coralWristSubsystem = new CoralWristSubsystem();
  // private final CoralWristCommands coralWristCommands = coralWristSubsystem.getCommands(); no warnings :)
 
-  // Auto support
+  private final VisionSubsystem visionSubsystem = new VisionSubsystem(driveSubsystem);
+  private final VisionCommands visionCommands = visionSubsystem.getCommands();
+  private PhotonCamera camera = visionSubsystem.getCamera();
+
   private final SendableChooser<Command> autoChooser;
   private final CommandXboxController primaryController =
       new CommandXboxController(DriveConstants.PRIMARY_CONTROLLER_PORT);
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 9bc7389..37ad601 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -21,11 +21,16 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class VisionSubsystem extends SubsystemBase {
-    private VisionCommands commands = new VisionCommands(this, new DriveSubsystem());
+    private VisionCommands commands;
     private PhotonCamera camera = new PhotonCamera(VisionConstants.CAM_NAME);
     private DriveSubsystem driveSubsystem;
     public boolean canAlignCoral;// you can get this if you want!
 
+    public VisionSubsystem(DriveSubsystem driveSubsystem) {
+        this.driveSubsystem = driveSubsystem;
+        this.commands = new VisionCommands(this, driveSubsystem);
+    }
+
     /** Fetch the VisionCommands instance */
     public VisionCommands getCommands() {
         return commands;
@@ -39,10 +44,6 @@ public void alignwithCoral() {
         commands.alignCoral(camera);
     }
 
-    public void setDriveSubsystem(DriveSubsystem driveSubsystem) {
-        this.driveSubsystem = driveSubsystem;
-    }
-
     @SuppressWarnings("null")
     @Override
     public void periodic() {

From d0e160b8656adba99dbc83ad0cd977d7f0569636 Mon Sep 17 00:00:00 2001
From: a-mishmash <reldamail@icloud.com>
Date: Thu, 23 Jan 2025 19:34:44 -0800
Subject: [PATCH 39/44] Offset changes

---
 src/main/deploy/swerve/modules/backleft.json           | 4 ++--
 src/main/deploy/swerve/modules/backright.json          | 2 +-
 src/main/deploy/swerve/modules/frontleft.json          | 2 +-
 src/main/deploy/swerve/modules/frontright.json         | 2 +-
 src/main/java/org/ironriders/core/RobotContainer.java  | 6 +++---
 src/main/java/org/ironriders/drive/DriveSubsystem.java | 2 +-
 6 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json
index 75dba43..c9c54a2 100644
--- a/src/main/deploy/swerve/modules/backleft.json
+++ b/src/main/deploy/swerve/modules/backleft.json
@@ -3,7 +3,7 @@
       "front": -12,
       "left": 12
     },
-    "absoluteEncoderOffset": 99.884,
+    "absoluteEncoderOffset": 26.51,
     "drive": {
       "type": "talonfx",
       "id": 5,
@@ -20,7 +20,7 @@
       "canbus": null
     },
     "inverted": {
-      "drive": true,
+      "drive": false,
       "angle": true
     },
     "absoluteEncoderInverted": false
diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json
index f8ac24d..f73563f 100644
--- a/src/main/deploy/swerve/modules/backright.json
+++ b/src/main/deploy/swerve/modules/backright.json
@@ -3,7 +3,7 @@
       "front": -12,
       "left": -12
     },
-    "absoluteEncoderOffset": 205.782,
+    "absoluteEncoderOffset": 299.0,
     "drive": {
       "type": "talonfx",
       "id": 3,
diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json
index 37abbc3..0bf85f9 100644
--- a/src/main/deploy/swerve/modules/frontleft.json
+++ b/src/main/deploy/swerve/modules/frontleft.json
@@ -3,7 +3,7 @@
       "front": 12,
       "left": 12
     },
-    "absoluteEncoderOffset": 83.670,
+    "absoluteEncoderOffset": 341.8,
     "drive": {
       "type": "talonfx",
       "id": 7,
diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json
index cf0b51d..9e6dccc 100644
--- a/src/main/deploy/swerve/modules/frontright.json
+++ b/src/main/deploy/swerve/modules/frontright.json
@@ -3,7 +3,7 @@
       "front": 12,
       "left": -12
     },
-    "absoluteEncoderOffset": 33.034,
+    "absoluteEncoderOffset": 98.2,
     "drive": {
       "type": "talonfx",
       "id": 1,
diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index ab802b8..02cafd7 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -87,15 +87,15 @@ private void configureBindings() {
     driveSubsystem.setDefaultCommand(
         driveCommands.driveTeleop(
             () -> Utils.controlCurve(
-              primaryController.getLeftX(), 
+              0.0, // -primaryController.getLeftX(), 
               DriveConstants.TRANSLATION_CONTROL_EXPONENT, 
               DriveConstants.TRANSLATION_CONTROL_DEADBAND),
             () -> Utils.controlCurve(
-              primaryController.getLeftY(), 
+              0.0, //primaryController.getLeftY(), 
               DriveConstants.TRANSLATION_CONTROL_EXPONENT, 
               DriveConstants.TRANSLATION_CONTROL_DEADBAND),
             () -> Utils.controlCurve(
-              primaryController.getRightX(), 
+              0.4, // primaryController.getRightX(), 
               DriveConstants.ROTATION_CONTROL_EXPONENT, 
               DriveConstants.ROTATION_CONTROL_DEADBAND)
         )
diff --git a/src/main/java/org/ironriders/drive/DriveSubsystem.java b/src/main/java/org/ironriders/drive/DriveSubsystem.java
index e553066..c8fa086 100644
--- a/src/main/java/org/ironriders/drive/DriveSubsystem.java
+++ b/src/main/java/org/ironriders/drive/DriveSubsystem.java
@@ -72,7 +72,7 @@ public DriveSubsystem() throws RuntimeException {
 	public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
 		swerveDrive.drive(translation, rotation, fieldRelative, false);
 	}
-
+	
 	/** Fetch the DriveCommands instance */
 	public DriveCommands getCommands() {
 		return commands;

From 7e238f417aa76dfc71b7beccbfe2300f53e0fbfa Mon Sep 17 00:00:00 2001
From: Saturn corgi <tylerclayton183@gmail.com>
Date: Tue, 28 Jan 2025 16:49:36 -0800
Subject: [PATCH 40/44] update

---
 .../org/ironriders/core/RobotContainer.java   | 31 +++++++++----------
 .../org/ironriders/vision/VisionCommands.java |  2 +-
 .../ironriders/vision/VisionSubsystem.java    |  2 +-
 3 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 02cafd7..4ffa4d0 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -38,8 +38,10 @@ public class RobotContainer {
   private final DriveSubsystem driveSubsystem = new DriveSubsystem();
   private final DriveCommands driveCommands = driveSubsystem.getCommands();
 
- // private final CoralWristSubsystem coralWristSubsystem = new CoralWristSubsystem();
- // private final CoralWristCommands coralWristCommands = coralWristSubsystem.getCommands(); no warnings :)
+  // private final CoralWristSubsystem coralWristSubsystem = new
+  // CoralWristSubsystem();
+  // private final CoralWristCommands coralWristCommands =
+  // coralWristSubsystem.getCommands(); no warnings :)
 
   private final VisionSubsystem visionSubsystem = new VisionSubsystem(driveSubsystem);
   private final VisionCommands visionCommands = visionSubsystem.getCommands();
@@ -87,22 +89,19 @@ private void configureBindings() {
     driveSubsystem.setDefaultCommand(
         driveCommands.driveTeleop(
             () -> Utils.controlCurve(
-              0.0, // -primaryController.getLeftX(), 
-              DriveConstants.TRANSLATION_CONTROL_EXPONENT, 
-              DriveConstants.TRANSLATION_CONTROL_DEADBAND),
+                primaryController.getLeftY(),
+                DriveConstants.TRANSLATION_CONTROL_EXPONENT,
+                DriveConstants.TRANSLATION_CONTROL_DEADBAND),
             () -> Utils.controlCurve(
-              0.0, //primaryController.getLeftY(), 
-              DriveConstants.TRANSLATION_CONTROL_EXPONENT, 
-              DriveConstants.TRANSLATION_CONTROL_DEADBAND),
+                primaryController.getLeftX(),
+                DriveConstants.TRANSLATION_CONTROL_EXPONENT,
+                DriveConstants.TRANSLATION_CONTROL_DEADBAND),
             () -> Utils.controlCurve(
-              0.4, // primaryController.getRightX(), 
-              DriveConstants.ROTATION_CONTROL_EXPONENT, 
-              DriveConstants.ROTATION_CONTROL_DEADBAND)
-        )
-        
-    );
-    primaryController.a().onTrue(driveCommands.driveTest());
-    primaryController.a().onTrue(visionCommands.alignCoral());
+                primaryController.getRightX(),
+                DriveConstants.ROTATION_CONTROL_EXPONENT,
+                DriveConstants.ROTATION_CONTROL_DEADBAND)));
+
+    primaryController.a().onTrue(visionCommands.alignCoral(camera));
   }
 
   /**
diff --git a/src/main/java/org/ironriders/vision/VisionCommands.java b/src/main/java/org/ironriders/vision/VisionCommands.java
index b579d7d..0c047c1 100644
--- a/src/main/java/org/ironriders/vision/VisionCommands.java
+++ b/src/main/java/org/ironriders/vision/VisionCommands.java
@@ -48,7 +48,7 @@ public Command alignCoral(PhotonCamera camera) {
                     Translation2d path = getPathToTag(i, result);
                     if (path.getX() > 0 || path.getY() > 0) {
                         print("running drive");
-                        driveSubsystem.drive(path, 0, true);
+                        driveSubsystem.drive(path, 0, false);
                         print("ran drive");
                         print("x:" + path.getX());
                         print("y:" + path.getY());
diff --git a/src/main/java/org/ironriders/vision/VisionSubsystem.java b/src/main/java/org/ironriders/vision/VisionSubsystem.java
index 37ad601..a90b1cd 100644
--- a/src/main/java/org/ironriders/vision/VisionSubsystem.java
+++ b/src/main/java/org/ironriders/vision/VisionSubsystem.java
@@ -78,7 +78,7 @@ public void periodic() {
         SmartDashboard.putBoolean("Can Align Coral", canAlignCoral);
 
         if(VisionConstants.CAM_OFFSETS.length==0){
-            System.out.println("no cameras set skipping!");  
+          //  System.out.println("no cameras set skipping!");  
             return;
         }
         AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

From 96432383fe719cd524e65e4da5f7b72d99d66d72 Mon Sep 17 00:00:00 2001
From: issy <lshields822@gmail.com>
Date: Tue, 28 Jan 2025 17:52:36 -0800
Subject: [PATCH 41/44] somewhat auto worke

---
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 simgui-window.json                            |  20 ++++++++--------
 .../org/ironriders/core/RobotContainer.java   |   1 +
 .../org/ironriders/drive/DriveCommands.java   |  22 +++++++-----------
 .../org/ironriders/drive/DriveConstants.java  |   2 +-
 8 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index e4db747a58746032ab748138ddfb10c0ac60d7e8..5b6fd755a031f5035107eab99a1440066ac171a0 100644
GIT binary patch
delta 63
zcmZn=Xb@m|$q@8)qG0kw1>T7zJ{+$YcDy=!=8P>^gcB&jcAi1_w;anvK`X8U48lKk
QSr`}?m?jFEEn;E=0QQ*`8vp<R

delta 62
zcmZn=Xb@m|$Po2)qG0kw1>T7zJ{*r24!k;h=8P>^gcB&jdYmEY-}Q-t)?6nT9RGZ0
N00O3of@X`D*Z>E!7a0Hm

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 50b42fdf251ff0951482630f174ef3370b4acc2b..14eda26df81a0eb5c4c55267135b16feab16bd7b 100644
GIT binary patch
delta 63
zcmZn=Xb@m|&*1ZQqG0kw1>T7zJ{%7i4!t^i=8P>^gcB&jcAi1_w;anvK`X8U48lKk
QSr`}?m?jFEEn;E=0QQj;8vp<R

delta 62
zcmZn=Xb|9d%CPs<*)wNsCkiG{RN$Og;=}TUA^a<t#|z}K9%o4UcYUIuHP;CS$3Nd0
NfPiVDpxGiOHUJ!h7a0Hm

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index 3d0e198dfe2aca4d742da047d431656362da9d43..ffc973be80cebe07071e930de068999b1ea6f045 100644
GIT binary patch
delta 72
zcmZn=Xb|9d&#?K`*)wNsCkiG{RN$Og;=}QP;UJ2L03+wbR%^EN3_`!<StbfvaUEa~
S`l-jlz`(#XQP6A=6B__0n;AC%

delta 62
zcmZn=Xb|9d%&`B}*)wNsCkiG{RN$Og;=}TYA@VDj#|z}K9%o4WcVnWUHP;CShd)0U
NfPiVDpxGiOHUJ!V7a0Hm

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index da71c7ac7e5cce37947094bdc8dcbd96a31d7bd3..41e633163309af599a8d26eca4aac21741a0a779 100644
GIT binary patch
delta 63
zcmZn=Xb|9d%CPs<*)wNsCkiG{RN$Og;=}TUA^a<t#|z}Kon;XIt-vx-(2DCE1M^RP
P76t|erip@Pi<sB|3uqM^

delta 62
zcmZn=Xb|9d$*}X)*)wNsCkiG{RN$Og;=}TSA^0np#|z}K9%V@ScXOhkHP<l)$3H(A
NfPiVDpxGiOHUJ!h7a0Hm

diff --git a/simgui-window.json b/simgui-window.json
index b621789..e389f29 100644
--- a/simgui-window.json
+++ b/simgui-window.json
@@ -10,21 +10,21 @@
       "maximized": "0",
       "style": "3",
       "userScale": "2",
-      "width": "1198",
-      "xpos": "1119",
-      "ypos": "334"
+      "width": "638",
+      "xpos": "1281",
+      "ypos": "387"
     }
   },
   "Window": {
     "###FMS": {
       "Collapsed": "0",
-      "Pos": "636,616",
-      "Size": "288,138"
+      "Pos": "619,616",
+      "Size": "172,198"
     },
     "###Joysticks": {
       "Collapsed": "0",
-      "Pos": "72,289",
-      "Size": "796,169"
+      "Pos": "155,183",
+      "Size": "632,169"
     },
     "###NetworkTables": {
       "Collapsed": "0",
@@ -33,12 +33,12 @@
     },
     "###NetworkTables Info": {
       "Collapsed": "0",
-      "Pos": "636,544",
+      "Pos": "619,544",
       "Size": "750,145"
     },
     "###Other Devices": {
       "Collapsed": "0",
-      "Pos": "636,25",
+      "Pos": "619,25",
       "Size": "250,695"
     },
     "###Plot <0>": {
@@ -54,7 +54,7 @@
     "###Timing": {
       "Collapsed": "0",
       "Pos": "194,44",
-      "Size": "139,217"
+      "Size": "139,171"
     },
     "Debug##Default": {
       "Collapsed": "0",
diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 4ffa4d0..0ad5429 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -18,6 +18,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
+import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import com.pathplanner.lib.auto.AutoBuilder;
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 4c8af27..0a58807 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -8,7 +8,6 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathPlannerPath;
 
-
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -24,30 +23,27 @@ public DriveCommands(DriveSubsystem driveSubsystem) {
 		this.swerveDrive = driveSubsystem.getSwerveDrive();
 	}
 
-
 	/**
 	 * Command to drive the robot given controller input.
 	 * 
 	 * @param inputTranslationX DoubleSupplier, value from 0-1.
 	 * @param inputTranslationY DoubleSupplier, value from 0-1.
-	 * @param inputRotation     DoubleSupplier, value from 0-1.
+	 * @param inputRotation DoubleSupplier, value from 0-1.
 	 */
-	public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inputTranslationY,
-			DoubleSupplier inputRotation) {
+	public Command driveTeleop(DoubleSupplier inputTranslationX, DoubleSupplier inputTranslationY, DoubleSupplier inputRotation) {
 		return driveSubsystem.runOnce(() -> {
 
 			// No driver input while autonomous
-			if (DriverStation.isAutonomous())
-				return;
+			if (DriverStation.isAutonomous()) return;
 
 			// Run the drive method with the inputs multiplied by the max speed.
 			driveSubsystem.drive(
-					new Translation2d(
-			 inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
-			 inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
-			),
-					 inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
-					true // Gus likes it this way
+				new Translation2d(
+					inputTranslationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), 
+					inputTranslationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()
+				),
+				inputRotation.getAsDouble() * swerveDrive.getMaximumChassisAngularVelocity(),
+				true // Gus likes it this way
 			);
 		});
 	}
diff --git a/src/main/java/org/ironriders/drive/DriveConstants.java b/src/main/java/org/ironriders/drive/DriveConstants.java
index 4eed2e7..d7fea14 100644
--- a/src/main/java/org/ironriders/drive/DriveConstants.java
+++ b/src/main/java/org/ironriders/drive/DriveConstants.java
@@ -28,5 +28,5 @@ public class DriveConstants {
     public static final double ROTATION_CONTROL_EXPONENT = 3.0;
     public static final double ROTATION_CONTROL_DEADBAND = 0.8;
 
-    public static final double SWERVE_MAXIMUM_SPEED = 7.5; // m/s
+    public static final double SWERVE_MAXIMUM_SPEED = 0.25; // m/s
 }

From a3a420f711d4bee59f860f75be54aca64d55f561 Mon Sep 17 00:00:00 2001
From: issy <lshields822@gmail.com>
Date: Tue, 28 Jan 2025 18:09:03 -0800
Subject: [PATCH 42/44] Worke!! up to date!

---
 src/main/java/org/ironriders/core/RobotContainer.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index 0ad5429..b09f472 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import org.ironriders.vision.VisionCommands;
 import org.ironriders.vision.VisionSubsystem;
+import org.photonvision.PhotonCamera;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;

From a5323c437a0ce166f41df27173774fa2dc9d55a8 Mon Sep 17 00:00:00 2001
From: isabelle <lshields822@gmail.com>
Date: Wed, 26 Feb 2025 15:51:56 -0800
Subject: [PATCH 43/44] auto setup

---
 src/main/java/org/ironriders/core/RobotContainer.java | 1 -
 src/main/java/org/ironriders/drive/DriveCommands.java | 4 ++--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java
index b09f472..602ff79 100644
--- a/src/main/java/org/ironriders/core/RobotContainer.java
+++ b/src/main/java/org/ironriders/core/RobotContainer.java
@@ -85,7 +85,6 @@ public RobotContainer() {
   private void configureBindings() {
 
     secondaryController.button(1).onTrue(driveCommands.runPath("Example Path"));
-    //secondaryController.button(2).onTrue(driveCommands.runPath("NAMEME"));
 
 
     driveSubsystem.setDefaultCommand(
diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 0a58807..ecc26be 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -56,8 +56,8 @@ public Command runPath(String Path) {
         // Create a path following command using AutoBuilder. This will also trigger event markers.
         return AutoBuilder.followPath(autoPath);
     } catch (Exception e) {
-		System.out.println("eroor");
-        DriverStation.reportError("Something bad happen !!: " + e.getMessage(), e.getStackTrace());
+		System.out.println("Error while following path: ".concat(Path));
+        DriverStation.reportError("Error" + e.getMessage(), e.getStackTrace());
         return Commands.none();
 		
     }

From 5fd0bc251d57a7f18866bccf7a1c2c476f773666 Mon Sep 17 00:00:00 2001
From: isabelle <lshields822@gmail.com>
Date: Wed, 26 Feb 2025 16:05:05 -0800
Subject: [PATCH 44/44] add auto controls

---
 src/main/java/org/ironriders/drive/DriveCommands.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/ironriders/drive/DriveCommands.java b/src/main/java/org/ironriders/drive/DriveCommands.java
index 01eee35..9521ca2 100644
--- a/src/main/java/org/ironriders/drive/DriveCommands.java
+++ b/src/main/java/org/ironriders/drive/DriveCommands.java
@@ -3,7 +3,7 @@
 import java.util.OptionalInt;
 import java.util.function.*;
 import java.util.function.DoubleSupplier;
-
+import edu.wpi.first.wpilibj.DriverStation;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
@@ -13,7 +13,7 @@
 import java.util.function.Supplier;
 
 import org.ironriders.core.FieldConstants;
-import org.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Commands;