From d934cdf35b858fe9d74b5c2beba112b93fbc4792 Mon Sep 17 00:00:00 2001
From: Tylr-J42 <Tyler.Jacques00@gmail.com>
Date: Mon, 24 Feb 2025 07:50:18 -0500
Subject: [PATCH] processor placement, advantagekit, and chirp

---
 build.gradle                                  |   8 +++
 src/main/deploy/Orchestra/doomE1M1.chrp       | Bin 0 -> 12774 bytes
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 .../deploy/pathplanner/paths/New Path.path    |  54 ++++++++++++++++++
 src/main/java/frc/robot/Robot.java            |  32 ++++++++++-
 src/main/java/frc/robot/RobotContainer.java   |  26 ++++++---
 .../constants/ManipulatorPivotConstants.java  |   2 +-
 .../java/frc/robot/subsystems/Drivetrain.java |  31 +++++++++-
 .../frc/robot/subsystems/MAXSwerveModule.java |   8 ++-
 .../robot/subsystems/ManipulatorPivot.java    |   2 +-
 vendordeps/AdvantageKit.json                  |  35 ++++++++++++
 11 files changed, 184 insertions(+), 16 deletions(-)
 create mode 100644 src/main/deploy/Orchestra/doomE1M1.chrp
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path
 create mode 100644 vendordeps/AdvantageKit.json

diff --git a/build.gradle b/build.gradle
index b9f0e50..ba412af 100644
--- a/build.gradle
+++ b/build.gradle
@@ -10,6 +10,11 @@ java {
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
 deploy {
@@ -72,6 +77,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
diff --git a/src/main/deploy/Orchestra/doomE1M1.chrp b/src/main/deploy/Orchestra/doomE1M1.chrp
new file mode 100644
index 0000000000000000000000000000000000000000..b966913d58522f066028c37374b092844a79bf1f
GIT binary patch
literal 12774
zcmeI22bWZ3wuX132#5hmiUb)9ND>3sqGUyEf*=Y=5?W}wY0Ika0s+ZEX=w=pf`SCe
zl7oN^qGVJQ6`4Us+v|v!2LUg`6%m=|{dU#z{()I{t-EN~yPnt0KIc2<J8AE8%FI$O
zw$UkRDU<9Y>tGSnUv<>M;`S`8l47<9UlLA39V}`y;jV=LOA)gTXsX3*8LWDda<c4p
z*gg1|y#X6uY&OOQSR=R%X{<f)c<@ncTh#0yXaMU!1iM}0V~Nv(kJ(Y!vAEe#a$?2I
zPN6aOl>G(=5bwY?jDoWhzIX|<Wk}mU2^POpIoVboHVr;vbK%`3wf)&v9j-*$ckxj0
z*;el|v&~3jZGp8e_pw^T5y8jpV|cQZwm)uNt}rWkm2x71w+A1$DX?Z~-=6YtF4BB1
zf*&S)oib+Ik>+zFtX)<)S=I<<1|PFs@PTqZpW;5Gu@1nq!AGr2d9!~a&F5FJ@6|q5
zCY%?1%#Ooe6?{I2Uc<r0YacIxqmbtFdAKy;%U)}?5otbW!ZH<=lWo;to8Tk19Nu5a
z=ks>>2GV>Mz$3wDThq#BdyvN33sbB3Se;>h@Np}I=l<gJ+2=a5^3{|Rv8!R@;NvzM
z*1z7jry5+2G@onXX9<7T4QB5n&F2ny*Nw`_vNYH~_?UeNAHK=w^AUImX{<tc2^WLb
znE`)An$Ke}ezV8>!xh2D>@1Ab^!XfDgL43{eY^@jk2Iez!PgW1x>_7-r1@M7udA(`
zY^wu12OqIbVjZ8)#&8$Xe2T|{&$c$Vm>obGOKe`($9f!24?b?+!xDG+eCFJ0Rt>Lx
z6w`u_+iS2{J>Q->a3j*bzX`ud`1|UceTp=nAHaL>R8E$4gd>BG*=Mk4s?TRHcm!#z
z?_ue?e4RP)q6R*nXYeEN8cW<1e9Vg7Z8rEGpU<iItMS^$%J3zm`CJIMC48-hW*;EU
z=UP~^k#e%F0qh-o#CE_QjeR~_!H<yUQ#=!Vwsme|R)`8L$9{r!n`)i?0(0=M5YL4L
zl4S;-!56YUVxwjpC+izH0$*UcHWKbduaGZZ-dyom;Dh*GmSN&_RLJ_pL+CX5V&!`k
zj|O(b@3KDD9WF##zjzdB{bHT_v|kas1$GQRZX00R7Ct`>;0~lYe+QmS_;&YmypWFX
zVc4#va<Z%!%nv?h-@+#z@Z&2ULmKN8tdi#I90sp!<;VA8I38)Ne7H0Cn3ZmAHnxo)
z-#Pedcpcvwa3Rw1T@Cjod}>>>!$`+>E4=eT<z(A^FdBTs_QQVd{P=c+pCKLJ&tcK_
zp5GgOk2KZ~u<JuU*3)oh@Nqj2t9JD3C9i{71HAU}ZkQf?+_u3-JNfpsfcug5@gO{x
z@ZCC_{fso9U&F4CDksY#a7OSkI|&DN@%bDC&mfI;9@gmU>wFGY?B?@X3Qk8FOWYrP
z%&K%Zo7}_aa~b{?y!NpkT#YoJTi~IDZ~mCs*GTjEE^OXYIoZ}84hcSDhv9%;KA-9E
zTcr6EOQm~$44y)}IB$zQZq^#<dT9eEqM^hm!B0XwGq6l=<;P6yiWYIbi1R}{H}Ffm
z&i{YF>U}(4%tDK}Uc@yao)dT?#4}Ay%}{<ca4`Nf*UQszJ4#E*4g3S2my&5>i@uyg
z%()l|K5p;BKAF1i;?^D>Mp6FzFIYUweJ1=3X<v@RzWo%>vO#cu@G(0F$7FL1czTS5
zMW67oO2YcVM{OFsDWdB)YL(!8q_M=m2OqN<Pnyk&YTu)_5q~#+HOIFp+=8fzY!Cb@
z;UA1qUm<P(LD)7&IoZ}7jtf3wN8zYkZGW~!;7JrE{})&(?)gLDd8DzdzgaHQSX1Ec
z;Nw<)0CPS_+aI@i_=oVCPjPVYaXSQKgME9t!=p(1_#-U)wEH+LI>hJmEIy9ce2#^y
zf{$5=p=Ohi=2I*+%*VPKHVZy#^Wg2neLkzhl}KZ)g5Lxmv-%^<mW=fI+>UR7*FLs^
zdy)3>06doPT}E-Nk>>MD*k!bGvMm!%2|i*c;Dj+gpM&8!r1|^<-aOXx$H9`%XgxW0
zElfwNxn9HtXc2MoCsfG!|4&%+S;b?4x%e?$FXEaI&$YGi3{w7Yu>LsaF@=4EBTymN
z%SgBvY5iicJkKu<A3{2>I=~k~yiee#A)agEHP30>%)st=-J^QIXM&I05jb?b))TiZ
zcmnAjbsAR9cRvDN^}OE?FM-36?uV1%=HO#ic7oYVq<&!*tTfTbss`HxAGH;*@g%<=
z-T@1c#u85iAG2nY&DOu*_rnkH58-t`><kYe-4DNjXA(YR3iaMp-~ONQ8F<|f`@=cG
zN9-(oX`0^;pMfP_R8Bnbj^MLxD!h8Sk0m~bG}deI^WfuFXNK8>GyVCp37>`6e2SBU
zkK4~MZ<cRQe|Q#YA5&hT2SM`ru<~r5&(d%_(tOT>yMvEe)j4L1k>*pZKG(;(1wI;l
z)HcDic|M;_;BKV#i@ygSvo^1qy*1zG^Dw>#Uh~--eu1=)-@}U+xX*>97y9;}#^>NQ
zpCjS2;3HOS5zl%wmH1>>cCm8eRt`1^KHKKNYD;{q+u#DEv3A0L1s}JTOPRyvKA#`s
z$Kdms&jD}-(!EHrWg#B7-{FK6X5~`iHU^$Y`D9(X(yShmp9*hWrDsRXD#EEKpSgV%
zejGga<kj@QsF1l`4I8ZSam9YYM{N&$c&+XU^ttc@r1c+!SFDqdSvR<MJ@qK@uknxL
z3z^#p{2C1<=On!Rb@zi{#SPjvY8Cuw{7~jF4{iuPVx=~6-$RAO=fcXHloPk>VcX!d
zZ5eF%hPI750WL-Z7;6hWi*)a5UO=DtrtVR3dj~%nuXA-IToQcTif*N!MY{I$Vd=M&
z6SWHP-h`hI>%HygY7O`*(z&`G76u=))NN*4k<Qh(V3X}WRtp#lK5F}6uN{7_c7mTH
zjU|@ZDIc>w@Nc`czfn7mAAr}nIs~3V%CX(tTaf&7u*M$W{>xz=(z!YVZVx_URrd0n
zMLJiP!rJdBCmz@-_-xw%8@$UiBPGWkfa6g?N<Yho2T?9@vFv+1FR4As!7fPWn79BH
za&KP<ze1|*#hUxPwyy>IqG*wrWx}<nW|0hA2ah9-E8g|K#?7<_a1hE(iP~Ve1Er?)
z2|SNK&HC*Fu6?9uM=Ll5>G)pgwf*>B=(RNu|Jhz!*G00|PUCv1+mz=8N+q5P3s5fS
zvv@}TPi-Tr*Un4%v)9h0womrjy1tUVb}sqqxz%ImrTp1z7qI?huYHXCWUpPo^`f3z
zy>=egOS0G2`jfr3)_;_LuXUeaFBf`kzg{l%+J3$KCwuKY&f8?Kt?MP(YoGpKy>=Ss
zr+RMn*m<1)$zD5+>m}K1=W#x(=T@(+^FP^ZD?iz5r*XX`du@%Yo?AV()}QRP^SEBr
zbF0@r#`TiywWk&Nv)8`zq6@wDg<d;q{};Ws?(@lBy9d`x`ZL~Zrx8!~+Nzh5y>=Ss
zzj|)<*hRTslD)Rx9VL5h<tKaXlGJ0#Ub}$nCE06h{mEWi`N>{8jq4@ZYZq|6sOMIX
zt@S5+?V{wX=f2QuXWNBd+xwRby|((43%&M*Ui<$~uU&(8Ao}Zy<ex3hd!7>R^*e$L
zr1+Eye4@YBy?XO`Nb!qm@O>G{Pk?1>x)*0B{2X{=Esu}Cjd!rOyDtrACA_Hb!CH^F
zJmIgrn|Gc_aq+r)+*gIKCA@fRLyxNuT7{I~q=nhrNM7F-KSc7qT2gZ&`P>JnCz1S^
zG=7rO%KcP$RcrSPp}r3)X9K*Wt@}Mt-~SZ<8+@dl`)^@Jd-wkX2R`J!SO>m+Amv~4
zF#Cn%YdpffAbEY`+l=H}buxPo$?IF*XGmV(?|wq^!@KZ~6v<EQYIa#S_j6(8?(Ww>
zeUDSlHrV(v_aDP{J>7o=d-Zbv3(QS-uR4D;(z2@Iry}_qsKFN^`T7~W_d^x&`i8X|
z$#=@+Js6VLx2kWEyuMGJM)J>P^DW>B_cP%&5%<fXzBwsp3v3W||2}NR>odg*VYeLj
z$DqC;DSi$Pi@U#~Ki>h7;@1t}Jt2~>`xNgKk$lsEe8)%f`eyVol7D<K@4=9~z73s3
z^7<ZBWT^WWVVPm>7sDHdyWa@wjc~sg>idqC{S<Z@<^DU^ceMLo;h-_@ON`~cJyO2v
z#+gW7HQ_QOf6qAH2O{}4dHl`<=~(ESNFkDcay-AoLGt?caSqArd&d<M+|PqmCc0k>
z>rQgN12&!P{u8M07h3iROrPTZzhQi;`-`XXJsl~h+>2%}BKaGq^Ij9l-#LTdbs+hc
zFY(S9$#;I4@4`r4-ypt2@=wp=cP~g@-x^BHcCYUX73R2K0c*~6zZKpy&;5Sb=2iD!
z!0z+i{|uj8;Ql-uvC#dMi}<dGG*;Ec+~1JA-ubUZ^39j>4im|Dc#ZEkNWS+nez$<C
z;q}h{43d9#1@CEBx}OfOUFCiWY`fb18?fOT_wT^yTK9)xk9F>UfG@1)II+&(;L6wC
zU%J8Hw<}(8Bfob-^0haay^C~iG<t(`7|FLS;Qcd_e{3`7@D}$mSm90I7rj^iBjI1z
z%I_%N^0?lgSAE;P-kIOF&HYZ;Y`gn|u)_}bM`7=s?*9e#4qS6pbT{AZkbL<){FVU8
zSKrI~d?cUx4!^@e@_LW`+xwhzc>S#};$F|G)ROM?oO-Ja+d+<=Q}<TV`|7Ol4!QdE
z?#0v_-0OE)58UWpzs2full!hPySjUE$j$DDLOtWO4aw)7*01NCo^gsNpLf~@J@4Ap
z@nu)lV%uuFe;a<7h<^mXyv5_<@w)C$K)cO-3e>Yw>r6g3HLjkUdPXXqd~V7s_SgEp
z>>c%(hlJPfrXIP|<NB>sMyh-9sk__{gyR~x&x3l_rnAoE^H<|4w)7s4C!fEH>-k&I
z(3jl|-)rQ4AN;Jbd-2C6`fidH-nE`>>i&0lS#x?%;(AA__o4El-hs-C=?O0`PWUD8
z$AlO4?AJVq^N^NR4KN#NJ5>YpZ0R|l<8Q?)NA<v#L|pa25u~{4fiIEPse0h=NWMyI
zeu{vUquSsc()v{!>~8CE)dtlb^tft+Z`%2?>2MR$vZ@oBw)Y&>391hi7yBTMr8=P}
zl3xgaMaoglFcvAUnqdV}Ts6Z|r0r47@Wi7&&)?$j$16wm!(OBu)eov06c>L)imQHD
z-QAb10mmcdsFo<v!*f(ieDs*dRZG<A>D!}P;^toNBX9@OvcJGK>F!lmWFW;=R}4h*
zsw?u4yy}XVk+w&5MaN8!tFCw*Y2Q^>tVZ&`z?S_yM>WO)q-9lOy!?d6Rb!k+imS#@
zZJ}lB!+fN16)PL{xay6DF^{X>XprM^)f@G4-4BNQk(O2M(K+sM)gB{};;KD{A&slr
zqsc(ec@tj>uN>7OvygIBholeoxayE?NZYA8WHZur^ap%wi07y#`2s0NHA&H79#>6L
zWVpvwll+dfY|{~Z14SBF^-0x{KCbGM??!oCyl=EGtNNtL824l0VWeeMtMo?ls#S)g
zG`{8Oy_V_~#l?xiXWHvfwTt57+~6~97i^v9an&<xg3q+SLcN31oT#=@O`~PSeMtMO
z+GY=uFY!Ecf|R2==Le)^Rp+dk=yBCKmrwGz>YM|Uec3i}KGL$Pd1}Alan(EzPw}{F
zp7v8cu9~Mc(zui1QKV&6|HP2us()r6#Z~`IL)sqIKdO6_^9jB>UO9@bL&{MtG<cTB
zRSO+L%2zFP0O>fEo6S8OX<5}pr;u_~7gd_;an(f?=6PIoQQ230*+*dk(z0K`)cHQH
zYNS6Fcw9A7_l3T!YNSp`>wg8FKw4Jy(hwxCdg(=^b6_)Uw$$UQofZe5Y5Sn+C*_M9
zgU__jp=v3`#l692+E1|mavxXq)i+2vs;`cq)p*rci&y!6RfVIFa#U-P5OP#&sm4-V
zykV`!Rcn2}&X?^CcOm7d?z(rq&xz_T)mw^-nMmWR?y9xPw`~djEK-hYuyIH^s=-uy
zDK0KYimL{TZt-P*z&FM#NA=iyNauj+vB_IKu6pbk(z2?@*1hf9TpLb6T2{5$W!pSQ
zwb>`zJ$?}0y2Il~;i;XzYz%&cw5;m1cDp=Bby}4@9#@^#A8B0GX{yh34y?x)!)sZ^
QrX;*-wiRglo2SnI8x6NeQ~&?~

literal 0
HcmV?d00001

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 23e0db9..42f06e8 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +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
+{"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,false,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/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..e655fcd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.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": 3.4559075342465753,
+        "y": 3.123458904109589
+      },
+      "prevControl": {
+        "x": 2.4559075342465753,
+        "y": 3.123458904109589
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.80426606528677
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3f39471..21f32ac 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,7 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -14,11 +22,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
+
+
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  @SuppressWarnings("resource")
+public Robot() {
+    Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
+
+    if (isReal()) {
+        Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+    } else {
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+        Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+    }
+
+    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+    }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 63f5dbc..3f216d1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,7 @@ package frc.robot;
 import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.ManipulatorPivot;
 import frc.robot.subsystems.Vision;
@@ -64,6 +65,12 @@ public class RobotContainer {
 
 		manipulatorPivot = new ManipulatorPivot();
 
+		//commands for pathplanner
+		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
+		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -143,14 +150,6 @@ public class RobotContainer {
 			manipulator.runUntilCollected(() -> 0.35)
 		);
 
-		driver.leftBumper().whileTrue(
-			manipulator.runUntilCollected(() -> 0.5)
-		);
-
-		driver.rightBumper().whileTrue(
-			manipulator.runManipulator(() -> 1, false)
-		);
-
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
@@ -205,6 +204,8 @@ public class RobotContainer {
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
 		);
 
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition));
+
 	}
 
 	private void configureNamedCommands() {
@@ -418,6 +419,15 @@ public class RobotContainer {
 		.raceWith(elevator.maintainPosition()));
 	}
 
+	private Command moveWithAlgae(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)
diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
index 9134148..e4b9ea2 100644
--- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
+++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
@@ -43,7 +43,7 @@ public class ManipulatorPivotConstants {
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kProcesserPosition = Units.degreesToRadians(175.0);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
     public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 66711ea..f598ceb 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -4,10 +4,12 @@
 
 package frc.robot.subsystems;
 
+import java.io.File;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.ctre.phoenix6.Orchestra;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
@@ -22,6 +24,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
@@ -43,6 +47,9 @@ public class Drivetrain extends SubsystemBase {
 
     private Vision vision;
 
+    public Orchestra m_orchestra = new Orchestra();
+    private Timer musicTimer = new Timer();
+
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
         m_frontLeft = new MAXSwerveModule(
@@ -73,7 +80,7 @@ public class Drivetrain extends SubsystemBase {
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(gyro.getAngle()),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -99,6 +106,21 @@ public class Drivetrain extends SubsystemBase {
             }, 
             this
         );
+
+        m_orchestra.loadMusic(Filesystem.getDeployDirectory()
+        .toPath()
+        .resolve("Orchestra" + File.separator + "doomE1M1.chrp")
+        .toString());
+
+        // Add a single device to the orchestra
+        m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
+        m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
+        m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
+        m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
+
+        m_orchestra.play();
+        musicTimer.reset();
+        musicTimer.start();
     }
 
     @Override
@@ -130,6 +152,13 @@ public class Drivetrain extends SubsystemBase {
         }
         */
         
+        if(musicTimer.get()>8){
+            if (m_orchestra.isPlaying()) {
+                m_orchestra.stop();
+            }
+            musicTimer.stop();
+            musicTimer.reset();
+        }
         
     }
 
diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
index a787c51..3e1d7a7 100644
--- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
@@ -107,9 +107,9 @@ public class MAXSwerveModule {
         // Command driving and turning SPARKS towards their respective setpoints.
         m_drive.setControl(
         driveVelocityRequest.withVelocity(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         ).withFeedForward(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         )
         );
 
@@ -134,6 +134,10 @@ public class MAXSwerveModule {
         return m_turningSpark.get() * RobotController.getBatteryVoltage();
     }
 
+    public TalonFX getDrivingMotor(){
+        return m_drive;
+    }
+
     /** Zeroes all the SwerveModule encoders. */
     public void resetEncoders() {
         m_drive.setPosition(0);
diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
index f4872b7..75ca3c0 100644
--- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
+++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
         );
         pidController.setSetpoint(0);
 
-        pidController.disableContinuousInput();
+        pidController.enableContinuousInput(0, 180);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644
index 0000000..c587313
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.1",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+    ],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.1",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}
\ No newline at end of file