From 1340d86719378bbe42d989f390daa0594a8a997c Mon Sep 17 00:00:00 2001 From: Ray Date: Fri, 7 Feb 2025 12:53:21 +0800 Subject: [PATCH 1/3] add button bindings and change name --- .../frc/robot/Constants$FieldConstants.class | Bin 0 -> 2538 bytes .../frc/robot/Constants$RobotConstants.class | Bin 572 -> 572 bytes .../Constants$SuperstructureConstants.class | Bin 1867 -> 4089 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 2840 bytes bin/main/frc/robot/Constants.class | Bin 538 -> 605 bytes bin/main/frc/robot/RobotContainer.class | Bin 1939 -> 1243 bytes .../frc/robot/commands/GeneratePath.class | Bin 0 -> 2081 bytes .../frc/robot/commands/TeleopSwerve.class | Bin 2684 -> 2897 bytes bin/main/frc/robot/subsystems/Grabber.class | Bin 1978 -> 5208 bytes bin/main/frc/robot/subsystems/Intake.class | Bin 1523 -> 4555 bytes bin/main/frc/robot/subsystems/Swerve.class | Bin 10671 -> 10900 bytes src/main/java/frc/robot/Constants.java | 34 ++++++++++++ src/main/java/frc/robot/RobotContainer.java | 36 +++++++++++-- .../java/frc/robot/commands/GeneratePath.java | 43 +++++++++++++++ .../java/frc/robot/commands/TeleopSwerve.java | 51 +++++++++--------- .../java/frc/robot/subsystems/Swerve.java | 8 +++ 16 files changed, 143 insertions(+), 29 deletions(-) create mode 100644 bin/main/frc/robot/Constants$FieldConstants.class create mode 100644 bin/main/frc/robot/commands/GeneratePath.class create mode 100644 src/main/java/frc/robot/commands/GeneratePath.java diff --git a/bin/main/frc/robot/Constants$FieldConstants.class b/bin/main/frc/robot/Constants$FieldConstants.class new file mode 100644 index 0000000000000000000000000000000000000000..46100a6f35743e068f9e1c099adb8117c3d9268a GIT binary patch literal 2538 zcmai$e{2**6vw}_NB7Dxv=tU8$ZsmNR6L8KLV+&aQChqfawW%)0CLycwpXuru(t)Q zsGvqcR1^h_h>G|tNDu)5DPjeMgrM;sV@ym8{y;Gx{sI()_-4v=yNNlP%zbC)Gw;ot zd2c2?e)Yg1026S`4TWK7omuNOlaZw5^(7N2E1a-W1?4d#9_{#aqYFb`V|Yo}8xJSy zz0)I&MyPo z6^zw4kP(|iZ89|6C;>810yJBx61^(X3yNNldafD4 z>GbQX3CdrZpFO@d9!tcmi8N4H6rx~#$*AGRP}=eJ!*!#8p(xY5nPxbViifROGBG-; z>iy5G@vUE}s`|j@!)xaiUhrTTMsgJ6W(IZqwLr28{r;kms@JWc^9MY*6{FoKVHloi zrC`#!whYH@xSgR}otbR%8}+7Pq!@||{Y4oQVB42^Fa&pqsP1G46=t+@?5{NA)J_W& zg}Q=aEJqgTECvt#;>U%PR;zmc-YHv-w>=_Q8HZai&A;i5?`!@RLq3jdG1PU@)~`Gh z7OQ$*&Bf2Z?)|u6l^k6K%l@`BI=BCJRnIy4*165QMhP~Z(-9KuE>X6syS0{+Q+H^p zUMAN{$|a<>o(U9c+Av*a{h++SX~Kt2ug#N&NCp z@io2;^wx)UxkFT|}&ETc|x!;p-ia8$}S8WCC;4vQwjS#5A?- z9(&Smj{d^a02$v1Rqw<;;1~dvAvX|97}d>95yPArj>HXy9)VRSylx+w*L>pyk$PI+pQ&1S7p9uuB(Iv&AW#QVpIe!*LqMJK;nJ)SYm$4gDI* z3Ff4oR@i8ohCpXjZKD|)g3{&8v|Y%Cvo*|h!g)4apkbkek+fyhhIJYmoUqY`aSaIx zI}Tux1DZ}~*>H)5B@!-6ui0Y5lJn4i_+wd6;>m=-m z_W4Y3g9EbDIsfYuF*-&NSTRfO{m|n}++} zljVGXej{`-fuHHEIgN4n1+#DlCeGq1{EBTjhhz8+Kj3%9@CO@+KUpdMVzY6cHRA$X zhl}i8{KHP*5<88{ii&@gVYs5`xT?&j|5aL;qHJU?w93^$7l$6G_Ui*LDWmbMk^6Aa m4o8)Pr=$sQpMXR7h$gzwCf$6D!=xT*egvP=YjRYMANdcjs8!ei literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 434dd6e48181a286b0c222f1fb53694d8e592cab..93b3b6016386dd85fd0bed14ffc4c5753bc3c7da 100644 GIT binary patch delta 13 UcmdnPvWH~@I}@YeWDX`102^`x9RL6T delta 13 UcmdnPvWH~@I};=SWDX`102^on8vpQ=ey^g z`{-wXU;h}uYP=J~FoyZDRW^ki;(F`3~_ax$e1r_f-STpz7o zH|UT_9*s?!<(lkuIAgOt1zt}iTpNj3R64jY;Tjis9%=8M3A78p#{&v2LiN>{kSMw)wvSU9>1ZAT5*6YY3E` zy0-E6{WGpHOsK98mzPsEH`V#pv>@{}7T@gqJSp=zTw5Kh^mTYF-b8t2cwZg3y&i$J zOK!MoXJpvC&_Z@*W&l|g0-`8`@CyrbKu) z$sSdjNUEA@tzj5mJeQ_FB0^;v#A-TchOp6cQHnANYq63cP-+wAPzjey7=t`+RY(|% zJPxapP>ppIS=D+J!&2H9N{YA6{SVu^TsHa1#`kp&)*}|g28LyKR+$|)JKC01YG{HI zYCwlcptIbNI~9f%LkjQFtl|@@lduu>d_pccLr3xdb96 zCoQ>yQq|S1Xr_JA@ElhRk(_gGmGBU@F^o66WW$Q23`5bZnA)Y%9a2&p9U>zygoh=J zLtYS%Fwo)H6&k{EnTr;bagKz9@yO31mlm=e9Sl;9+@UE}1I<@Ai1{`ru^;JFlesET zcGJmY4m!~lM3U~ATZFsw#CV4L2-Rik`1_y$8mSG9c%q?&JK}Sn?4hI>gblb_$HR7a0$E4&{ z$q9>>%8EPgi$RGgM-0zPn8WLk;ROlz@!7A(OA=moj+Tfno3ySBuSi(xBv4B{<@1-DJE{jAEKqjcD1PSPOG(vdTWHhMDN;!#SY zYZcR4r<)c^C_ns#IR}Lpi2!*)@`hm)`8k50EBK=YUlM$}dWfr#Up#h8=Z@Xd*B5^mcyk8cLUH6vXicN_GTI8T{R(zCcHR}}j+KUS1-%~i?lg9faFmegQtx)D zJ=ESb_PPSdbWwXnR9_l>E{aSS)#u6V8@Ghdia?j~L;;HWv7e^sYCe?4A)jV$JDRyA zXg-|AVK)w$FFM&fEUGz@#t|1qrmN;`_tYxXSoUuX2xbxIT=Y8ibW zIqxctXQ#=}GGLoML~;Olon(V5`h-n$5^qwQPia|CQ#if0kWgpvHtmpecK?}w0IFXf Aw*UYD delta 797 zcmZ9KNl#Nz6o9`=3y(gpEi#otWgdhIjUb4~V6hb{luAJj#)LSuT8CnwabvU#BWuZx zdzZ$Y1tihLAD|nf{snjX15AwZzSoM#=ALuDGoRd_O&{Inhu?4C17lp+FrROHzEj$o zh)zv!{AgC|;$oEyC1T65_+mV<91RP++En*NZ9~OWA~ZP}N$l}86p`hx^F-$sLo<;* z?s}Uj;qX}jF7-UGjG}<}@BJ84{ zVaZ8`1TuE@W+s`9r&4Rltcz2O8H}p$#(-m-3DsIYKip49GRcI%vyjZJt*nOAtI6xx z^h(BRAC??KTH0AjRY9AU)F9O-S3vmHFIP$=)C)5#r21wK=A=Hl&sxTodURV>P^L>5 zS}&no`*ls5SMKAwgUze=aow+&TZ>m$s?*jN9QuXaJA6w!G;GqCC)enBL-VCw)Sjok zNIG@UkxjbubZ-%;Cb-S9g6`htL_y0lvd!t;>@&MsBv0i3%2R*IYdu2=QT>kK&S`NC zlN(f$!p|+*xlJE;weucfGMp#NWghB?N18t20Z)0%I_K;%=;idEDXtjQQfOYQFNmC2tyGc&1N;5M6lR|m6?qzg3W3++O%2C z)~%g%eC_LNPVG$Mq|l@>$xtRsH&UNG6ACP`SYflK$c7SID(tCpq{fLlXPR7S5zyv_ p!<`NfdOV}y#efe(zKrAkVIuW3p F6adl@8O#6x delta 60 zcmcc1GK+=l)W2Q(7#J9A8Kft2-I*-UsK_Wf*^$wUQDSlrV+|wkWKJeUW+?{d$+}F^ Q+zbpH49pCI3?dBT0Q-pya{vGU diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 402aec187e4d5e503b1c5c8a4f826c53ff843e62..3f83aa14a866f2ba011afe74841a251b5ce69f9a 100644 GIT binary patch literal 1243 zcmbVL-%k@k5S|63<*0xK1X1Kr)Z&Ai5aWaVNT8xG7ml=*=u_Rh+qO&YcAMS3qJNo* zCi?ClWt_VfDajd)FFUilGv9tQ-|UZ{U%mmrK5Um@g1~A=hg7RTS=z|fx{{U&$#n^) z2rQhl0i!XKUFrwtJhTMLN&A%+1Fi||dpt_%MPJa4(8gw7EQ0eL8mc5=GNM6ht&+4p z+`bFc#|&PpIOcknfRnWSKG)1D1Vh~B1=j;kVBNbSWzxWWGL|QX4zDq7wJ7bvD9+T=1~LCi;a2s6_ZUCC^!k>!W~ z0@*9hy6V}eS-S`L++5Or!%+)n`6;tK0)?fHhhWlzFhhYZdre@=%XLhxh-oL4p%tjg zky0^dvYGb#vB>q9-V;*T0|Jw^?K1*XbrtavtP+^N8iQl4l`g{?*8f&&Zd5$rk&A6d z?OBHF_HmkGp1gFO8UOUSaoF-3?Ye)`YWSXa+-RTrjg}iSDV22t?*4{Q+7-slPMO;g zJdPX(R^VO*mSCO0a&1^bK8j7Nh3xKT9Opfp)sr*{xNc=q5vY19Wbql(BHND+r)*Cc z0&8QFa(&vdZ!4ja9nnQ?kMglS5;DSPOc}PY@qaCrz*3jnLmV@y5|x^fjs%|8w*OUg z{1Q!->X5${St*OxIX=r&fK5Ps@B^37-hgTJ2>O|=LK$Xq>%eSoD{vEU=g^;kdAwhM zTe)9_MYNN6w*iG6#g2J3B#ChQwS?bKqR}1(wYkU)`#maTGAQ(AQfG!}-Di!$>*uq>W)rJGg=g3vV!#{zuI%%rY!> zYfaryT7+6Hx4WN;HfC_ugq)`w^1FqDF)+O4;5Cd|xW#~bDov=GmL6qA;k)%oXKTsAV}_N1G5WHiL0pZJ zxF#R@raaTHzI)C1rUvgCbd>|4lvEa;l3tgG9qKb&Zd<41$0Ffmn0^u?v=ig$G-a6F z+KD)=oj^oiU#IgBE|WGj4HL9Z(#;QK#c1Cvo@1)`3uX?7&oOt39Gw>q&vE?}H|V%@ zSbRZ;7s#Ds`4l%Z>E;;<#WTEDe2Mo@u|9Q=?$=-9qn}!_9?-f%^aW-qKNE8jIZR^> zbGVOr`myD(L(G1RYj{XnCQHf$+bGjMWGI?txk|G6#=4G2TNjHxhAcB=v9`46vzKcjMs5Qo3V_@@VB= zzA!uz%mB^63=hCVF&ynCt|yqp*|D`N>FB3(|NEam{sOR$#~x-Fu6Ipa7~RrVv~|B9 zM4ebXkdZWjmCeBJdB`v<><7;R5e89DY`6Ah+cM0zbrf3@sK~|)`8nDtA$44Inl$UJ5-&*35G;Y|jAT3oieK*rFMt#*G(KJvJP zw>%UWZpzM3JRd00RVKEgPl|}1)O~5qOYub8z$zWBb~qMsnGEV0-LK1@k&+Br@$03= zbmuGX#;Y9H@HPYQNqdAQ3|UyuR-YYd#&O-*_&$T@*Xb$k&Vk_u#~a8poNuT|ZVme_ zX?B8EC>bs`bUO%l1Ebvha4}=|6rqcaQzA#6KIfdjsiMw~Zt9qTTmFegIib}~<+$bK zLkj2q>QRP>TAC?jc^MyhSYcS26tG}uwYW>ey`c&_(s10uZPLo+Nq0=E7gK39eTq%3 zKSSOIhC-HlXK5PeP*g@<-ILL#4nzH%Va;#6E|RC#sHiu+A4!ks)wd6U0yL<81MbJopC5qi<+g@%IHsh zDjBJPAxA57G>b5w?D^$a$d~^F|Dk|$$#3BR7k|f<0^UhKUg5ppQ2d3?%-{nWZx910 zAVVE^4wu1k1?O><#%pxzB0i+OC6eGNAIrE&Ge+zF;oa**nxVD1mGZAB9-#CyO;UI^ zIrMGf;lwCc}E{?JJh|N*Q r{e->wKkPNyb?lsDAK<}p?(!HnH{#|-+(%Pz8xy#tQ@GoS@0$MtWPu;8 literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/commands/TeleopSwerve.class b/bin/main/frc/robot/commands/TeleopSwerve.class index af250cb97c1eb1ede23d041f1572c931a1e2ed9f..257942fe420d41f4be56e8c1d3af179befeddee7 100644 GIT binary patch delta 983 zcmZ9K-ES0S6vltEA2Yk-w0v}}Ej3m&uw7u?R1he#DXA-8p$VZWX@gRR?shxubh~V4 zTUuBbgn%FTQAR;PNFWN~hJ*kM!9)`;xX>&901`~R@D>xeAZR?(u3((WocBG?Iq!3x zbKZFxx!0)w^7ow|fh6ar^`1azWT0ny;CoHhRM8s0UZ*_tg(BIE(`63kuKNRV<>dzP zwq%B!uF^;$Q|u}^&S+le9o{o|-(41LsXP!mdDPDDrheOSHL_pr{9_i;rAo$Nh^#`W z#?r6gZ;PdLEOHuq6k0OrQevWDC5Eixn3EVWo#8~cm|n-S4Qxgf>dZpHo=j!T^lmes zRtUDGQXWMUqXvZvdlkZiraf4)O(&xeXq$;988dMHTf5H_G;T2AfngD}&B_itf=;qu zV@jcZk2!88Y%`yokzjDZD<2vhEll=U*}TRF3bFs|b$?QuU(_jc&|p4?YVx*g9CjD0 z?V&Qq3_jvxcR)?5pBfzJGxwsJj0eW-(Fq|>KWod_H`!gX9jjnlnWDlAQ_ns3zS^pk z-M`fJ%C~MxdlG+zfE1fg1FsU{4H{X_ORScWBw0Wgt!$9U78cUSYi=&IJbV$2u-waI z_i`wfpR;kv4H8w>R_Qy#+-J^?(`XTPZj7*N^PG-ggxww55{b6XW~3)#w;8UIzsVvG zUN{REpTkiPeti}${R_M8;fBh-+L@>XqdCvqMljEf|3JB4)hhbVQl(Lzk#Wh^DmyE2nawA0O-Y?aycQh7^$rj;C) zV`pSgSLxtq-sUcA`CW$gkWT&-{}JmX3M&YTeJojPWL?Uye2#%4gje`Nc7CC~#Fw%Q z3i$=5Wv6jMazprIPigV{CEMjIv7W?DZnIwBTkgNb*Y*Bd3%;2abbJy|F;_%;l^bXL dd9}(gkCU7d79Zb8)W>Pj54s>9a+Y&6{{w22<30cY delta 776 zcmYk3-Afc<6o;SLuW^Ra($>vL(+FK>Z8MX~%#9Q?0_h?ll(3q%b=94AU0t0}sxs?5 z3bd7FxnOXSOU{QtERZ+2%oV3fjX85_osxB)sC~+P=ZpF)T8odhVo*tg#BBs=rc_hfOC0d?S7Fi=XBF*~J z!0@c(mQ(?2iAI-iF7d=F9(MVb5+7Z~O_w*iC*_8hq*gq_kwxdXrsbtBYE5aI?u=QS zam(myvO}&)PI#HAAZH7lcSqzYFLMnI(V diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class index 75ac9b5bee19c298589183f513d88eefd3a5a4f5..8660d7b115eaf6041effb166cca356b1b2310c95 100644 GIT binary patch literal 5208 zcmb7I`Fk718Ga|Sv$nFiTnJ5p;2ePL9LfbLIHd8FL{vT^*>aKs=}I2k8_Qa;yRu^n zy`dMqZ|F_a(v+6s5+xxmO%Ga1>3z}CyHB5|KlCr?4}JR1?#h;J$+3UY?96=ge)l)u z=!O41^DKZn@J|iP1g;yhGf~@0TW-`Tq@78}m19oyux+H%($-KTuuTpYq7!*DI%L|8 ztG=>k`qb`d#u^(la)Z%i=~$cLNCB;o@3LIW7HEuz-}dh!~UE;SK25{aKO#*Ju}LPV3UZRUnsn&xN|cyM8x)I;C~9V?*MVWYrR{rN`E zH;tZ_jgdreS5p|9aifMU0&8cO#&WJ4mbQ+Ya5FU=H?jrUGbFGs5^E}xEg)+NqX}Cz zGz;urqUzLF$2M%IfsH{rZJ>hcWej`Lr)0)mq%q8~d{z#agET88jAECDor`o%*-;(4 zv4_DmVY(x|mSei6l@mA+iO~{ERJPq3%Ud~_^ToME$E^yvP9Z1F`(!;{joUTcCa`M} zz-&9}vevXb7>=&pX?USe7lqn|!_AExc*(jcw4(NCd4hn>oTt2+ONMtc+K6T$@ zUM|u_WjS2ts5Tw#=%8ijIWIw3zan$3y+Asvus@_@C05nph``lxYhr%fE22AftX4#E zfop;+ZfsQ*tT8euhixgHCPmh*V+~em=oQ$oa5dDB5ZF3XjoYo9;~F{F*^(^eB`dF8 z$hZYtPOsOa4=D}(0^1irrUG#}F0)&_>{a&FF@R(A(P*1pa7I|r97gs+rFuRDo!x^I zs*b&m!4ior@pMy4ndWXCwOC${qd2MK^)P6m!->|mw$8-K{@#;aJ$*filkGj-hhm5O z6Rmx*o^C2yg;B=J4VkQW!No!oaoSRk45S8@hNcA!q;9P(3F%0;iv?Q85QbUqM!l7! zF(i8p+ZdBBJwW$V;qNh?4MgrHMm3z`sVi~sCAjGX{IeKS{JX7SwVZ!7{b%@>L3vKH zbmVa_v)eZj<4533q>38lW>ne)cC11@!*tquEmtHhm#=9ab)qvBH`ATAZP~P+0Be)x zhe$!kxC)UQN+A;O8K@K^>eOi+b^fXQbi6^G+E_Z(+I=|QS?QdH2Lv`NM_r-Q(C{XK zO_kC#p_(X~%+K07yhYvbeaH6g+Oso^`|&oFE^qhl!LnPabVYj^@sN&sgcb2Sb-WAj z7Ff+P=6j)F8(vejo7HT-n7-$n>3*pE3@v#S59@d@-dC1l?3~(rI+$y7Qf$|f(0QRT z%szk*Y51VPj-?5v6v(<4kC%X${ z>}A<5WEuPoaVukFQ-*D-d9YaHj+jjE4e=Sj&TobZw~AGw-*0bIR@P-x7wKq9sbcsk z9iPS{tg56?TgEVX)sFLvxPlkb>Y1Zn2c-^uLC0h2P>6e^cKqg3QL^NE_1fpMrfel> zGS_m+yKc(J8fL8pgermpk%JYsqaBL-6FRbSPV6W;qE~l-^UO55*V_rvC^u#E*KqidY@_t6#6oL zsN+XUAmxz`zlmTcljmc@8Z!-k z$L4<4HCUo`xU!7-DmtaXmpC^0{0*I>40ReC3E zq_!bGgX_?sx)6RfsH?^sd3ksvYcWTtH;^~7DfCA6ecs5%&fDelho>nMARZ%{;8@>$ z9#=Itth<0$p2LRb=BKftp)qs;n~I2pLPhMjjA(655qmjKVP6q<@S}*#vdYaxJUSzA zn1TZ_#>kx>$U1LcAAo>&^=I%|!U%k>D%|Ji!gUd@y9(SF1GqC4;p|G%9VJ|%3S3FL zFI9z`CtW|`QdQte(tWui+(aemjuY-g6}Xagk5`48C*9qIyQd0VNjjFtz_a`4E~X+@ zD$f@2)r)vi;7Km{;kew}G(0#jJQ$Rv@L)Nt0Ag1IkrhE$Ndu6UK$5k7Y7~oj<^sM` z#B+0{G0T9~PD}HAsk$&VLY~xH3DntK#E&oHCm{@Q7US%f)3Ywle(ld@;;(_32jp0^ zEdFVfM>Fm%W4AS6w+z4aO!Pbao?|o1nTeP1$Ajznn0g5>bL_*P@E4A6#9#4u{&97b I<3F(e|LO6z4*&oF delta 847 zcmYk3NmCP15Xb+IAv4MF7)U@MtdTW|V35UyC6W+8kTsDYxKNYD7>h8eVX*YXN3g7S z^k|iLQwWGvegJRYJbU%(*|H~qa_O#q-Tmu-zy9Grtx>=IIsFP?65DUpq{~q-EW>2* ziAkk3xTL=tXOJiIyLl_jAo&AHhRSIplT{(1ibH`@K{Z3W2rDfzl`E(b?;L}&8+8gi z;=5x=s>c-}%Mm9>qutOXR^+kfW)25r7c{hTw4t4$WiB3xMrY#UgFNfIL1jGWy>0l%boMP{Iyl7a_+CW*Jpx)d*uPHKmcZ=Wk#$j&rdLM3CwL4s_yv4Z zDxvlm4WHl*9>QBfO9>q%^c>;Z#z!iwK!|K}8C8o)(6tHOM%G8Z8}%5ZJU@ft28JkN zI2gvr`NRk*km%86s0FuihcFCd{}zas7Xo~qaT|Dq z$rD62JacEk{AVmNoM1b0NxW8`-->FI-GTN?f_>+sDi9|j6-eL#`A%B17wg!V^3sy) Rcu3xjO*|s6;W3`V`xkO!lFR@A diff --git a/bin/main/frc/robot/subsystems/Intake.class b/bin/main/frc/robot/subsystems/Intake.class index 41d732bdeac946c0aedcea0f7499be3b8c450c59..3c3e4926c96c78882b9732394d42c310bc65d236 100644 GIT binary patch literal 4555 zcmb7H`+pn75&v%FU=SmbUawpY%mrT1x4=jf<58T1pEoEp6%J59+70cW3MTkYoR%)!xm1XXZPzGjnhM z@0Hg8ya)ePu#I8wlwDM9t6;gRQ!6<0jw>pTnlfE&Mkr`uI4nvvb*`$bQ@ZWA@}Hp> z&iAWDt5VU-lA2pOHK;j)p{BV-8))zZJ}1DEn(^x?p3EXyELcKljK0g6QlDHS1T~wonF^W)tRnM zO&OwK(MdHI+DvE#IviM=s|rzK=vf6~3U|)3XGnn|9CbvP!q$t!rfbhPjt)rc8FoCN zo4R{~%!$SG3@u4=B8mvE=h%hoT5%(Z_@qX06ZR;07lXP*lR0k2UcxC0m#k+v6-%Gj zW;N9y2GyKv>t;C-U*$p&u8nzU1H*ofDB4?bD?^uWXt&2ocPu+T8js>O98_?Cp|hcq z#6(%x9EWf_gEFfbH8D2DusfEDhb#-k5>ecVxPll%{}vr5Z#jCq$_a@v|pi&$O3At;O~Ovv)|D5XIxwZ+r%yrgE5V>@=V;WW;2Jcjo&+?>ix44fW5H##;kmOYmo z%bZM&JenPtNR4I4$tFsQmN}(U4GX#g(L>ur8{Us|3O>LP-+&z1b`E8QBivD{xf~ya z#&E-o_avuHiE+)=D#9i9DKt&=dx}>Ak@g~}15&x^y_WWI0@s~D{}jqnf5r-)hV?h| zzo9<__{BV|PXII4;PXpZD5SsA(+qPcN-M#L3l#LFXodab0txnENCdQ9Y8&8<0d4x(XcS6VkkrdBBko0_>Cn7YzdQ8V(It;>Dz zu*IF$>6-3OH^RDRC?&KGy6k?r&RYg$K}&2Xo|on9%N$?9GgKEzK)%w-q-pys{UcO1 z#ISSuq&M8iGvDC&raTj&S0wECqhKfPyjI|qTwl#j$^l<(FfY=qHzXmx$MJnUPinnZ z6wI#&gZu~&zgb~uu0AV<{UJBejvwMj3SMBib#;wdCCc$4o}+n{G~|V06?K;)C0n0o zq|DTxaQqZMW4K;EEI5`?b46nmIk0ZF`Z3DsWm7XKe|~{qDfng3qikZ6WLMqk2Zjw{k2N&k@Fw~3o74eM4(FGi^jy~^<&JlBrj;|~g6V~DM^ulba~ z@jCuU)t9ot_t8(b$0X<*96$Ed=B%1s6eo3=*6n_R9Fd>{bQM;XNrs!6 z%qX~xoQ=>Y;|_GmZcqQZWLfuCDwDKAemi(8jZ@xA0~4(Zoutnn`t1bR2|CT_>6<;5 z(bdy+!xija#J--Mm$9$wmdF)!*KtQAQpe%fab$Z-9sRT};GQ}X^rwyo7m@UZhXdi3 z4TVQ~UPbC`WC5d>ky}Kb^yC+CW&!V8K%tK5$(8R7dT%ViT=D+#vyraaWy~$&BE7y? z$A=g2WF4QFyhMI%!)NI?j~nUt9<-pBd_Ii5^bygEqcpawxSu`?PN5HH&<_pAP^RI1 z7RT`mDY!(@o%(tfLznID4EtkcrbFovq`$SMdWzHE1aUJG_^f5Mv$7jL{7ZKoS8+ryt0d z>iGE;{HBijYM^WgsB;QWkGjEI-k>?fdPPaUsZ#qT1Rq}>SZ{^ai({O$DOG)bQj zXRr@vL*V-Z@NIb0^X1R@3#~nLONw|4e|u;T&A)HqZCW$>=vj!GEyl E|12FRzyJUM delta 486 zcmYk1&rTCz6vcmEr!!w?_@+t=r31A5X~A~*S3yM)D#V3hg&Q$0jEjP?3#n0;COpIB zdlH%=i6%S%K7ngrz{JF*@wO0gb5HKM_ndR@x5}@f;Ns7hb6|^|BzTtc-X^c(s<+ub zYJY4hoLYTfk$&7cI1F$YGH5cIP*F}gWk++vzH%G>FnLW*v6OTs>~A;c8b%nk7w%4I z80(9QinZEqax^L9e4)PYqZl{1cs^Ms4JFFrl}_8c&5D*1~ zD=GrAxPU7VU=oP|6%|}+wOajJ>(W+ht8JCq+Qo&Q`(^@BYx2#Td(VCMzyCe=Kj)t0 zX!TvQjhDYWeFnfddTEC-JJwjfXlhf7-`!N}A!JnSaHP?g;|qIHmVIn(V?$GeznEaj z&6`88PVm-wB0)$JutAgWMdigu)qDI`rf25nDS*N;2EUaikdAnQHa3hBUF~<*E~|7m z&vMr^dJI^QrXx!ppd9KYFHkP^kryc^sUJa~JZs*F!9xZa$V9G!$&>vfGDLxZ4!e&2 zglt~MQR%H)(a4OlF;HL-2FrC3qmG%-$ zRt%=di6|qr0ho;KbFno@Rf8w@IKuol0!8uc66rp2#epyVj#)ncSmg(sJ3MrA94< zS21elvzBqr1d%rsrES=9ePV4MRrT)2z1;a@deFL@VjTi2P$9fDLM~TUjc(Wo2b~bF`AguCbeOn;P3f zn4K$++Vl7`P`g85E4Imx?4#*UnHkwXawqQ6AqjmLI#D)6##?sb?h|Vx$C6lSU^n*Y zxQ|eZfvY!!RmNG zx}$QU4gb*QC$)@& zSD)iMchn&$Pa! zQ;xhdIfwFOdvf38e(-7~jxL#$&;UULX^{LfxjMU$iUbXzbb<(Fh^iIKYdk&!g)~gi za2g@&Q#SQ72=-==ufbc_P|N2^#e%M)64u~~<~p~ZbK!dCm?U4fM9Kj@8itJ*_%FUT zFrUfvT2ehaH2HJXL9Bt1ntD4iECNq z@vZb^dF#B(J$~PstThG>;j}#GY|<7A{0eW&5vlVd>u9k~Oun~T`e>@dQcp|dbE$dS zQfky`nY@^q;#^KOgvf@L=}NRseuI`0M|%r3%j`6#<|DsOEwUtSuvkGWS*?t<-4(f>Zq;c6p(wQTB)K=;ZWFYT1Ll+T z{_z~f)@BYZgVxg>g0|9T*)L2BGPkz=`+cFT`5K8(7{LCvu?Ds`~HUhkRO z?|!~f=%BzQ9AWbT2afOy%J#4zjdZp>_3A-t6i?z?Fz~F)?jeA8Kzj+ilof za5{k70c$6=yDa(D?byK(_WYit+)+$U~$;7 z8@VML+xG?yFq5s2WfR1?-1DC0d ztk9|9aXx48Qd{r@o@B~fnch(xW2LC$PK4!BfVU8#gJ)l-9*eEf{MvD}h6KRZi=C?{ zU~H0gH6Ivbhb%fI9+-uo03j)Z;A`Iiz$FO^p7>iBLW5e8eX}p40nT~4x9Iu%wAB$h$b>2%pEA>OXsTc`-gdk!Ytl= zua5Wb;h0>^fm3f%%MZ=JOf3cP@#1_XaG1Q$tq6Ww!v{>m!d9-?!YhyD5w(Gska|80 z;-hx_Sp{W8pj%uoH5U<~SHp`CKIan-C54F$t71G_!LZXW2Jw%%SCWHa4mLr+YXT*Q zUcSsW8vkaY5dZR>6oxDcm#RV#T&X&Ot?&u z%j7WBTBfLyUY6cT2^E%chY??p&alPlML~*Bs@@Pm>H5J-pBIF)r2L|9nwb{OC_+{k z(uOp0QYwS$NTD=JXNF2^yuEL^w5JS?K2jyj|5>)@50}mQ(W;D;%sK`(@^e#wVK=j3 z6Q}7d=Kg+6rVs1vr^xhGWK_KuCUb&4pi)6e!5Mu&VmhdwD}i#`DF19hA{De#f8`I? zXxo|LR!i|=Bn6zdjH1fIg2IzDn7uS~gM~7ZtOqZB_QPP_#<1I2_B%MBcXE1@oXvJ} z2HVw*bV=BQygZFGNf*MPQ8YU2!4z2HE+Gk;%0t9gAL#)a!@A(s*si;XQB_a@)4MAz zNaN;me<3TRG)NN?Xp-u+)070NQgY!SX{YIP6KGcGjX9y-+)&S@dRK%RN>964sP_es zhP|A{_hATM1Erjpr{O?1olOsI$q}InhOm#zcsojG8pkFrxB?rxv`f1=M7!(|jjlB_ z2+yKLbR8lbhAKA@2t~Y)A=RGK+NrjjIXCR)Knqe+D_#FEY}Usu{L8~uGhjnmYclty z@L!KpxiBcd%-G73T}7+;D(9<_GX$4ZriHIaQ^b*o_y#FD7j?WPu5DCWP<6;;CZNJQ zR=G8SZVJ*`-rUcwY%Ubs(-DIi=3=3w6MK1sv7y*zRIZM~psw$O-WCGA{fe~suRFEq zEJmIE6R6v0JFBB>Gfa@S(VdKMp&dLkf_9SR?jG@6AkJl1XN=ZxexJG7}600~jC#2q2IJNLYrjI3z26 z8WLewks@jhg{CyZ5J4`7xZRLU8KTQjn36?Mr#CDdGjh}j8=T0I_POF6Lypy@!3;&k zC4!6vmQ)PYFboBv#5mcK54W@{5}S-=LsH?@P>cdXLQP%O$YoRNYU>*&)i%^yk%~(- zl<=g}Mn_R9MrtTMZ#QbN9pN$!Dl95S6Gn+MrXuI8U|V2~KNM_QCt=2FxE$qTrm4nK zhVgRk38Kq1+B#8#6*d)<#Ji@k$`lP%m@2Z(70PrCS73&?(mY42#4I_Xny|+$PA3c# zF0zX+%}v%?4RO#^%oalhtEs(hMX<$&WoVWh18m_I?-`u7WZvi&4J#28PpdPO)f!r{Mx0fv z6I+sbTh~hcDnf-@xYD!5FiWb~ZizRoBMwrRmL^`c*+icu%e0U4R^EtMO+TuBNDa3_`Pu0 zP>KB-?!y5wMH_2(qD#XAGIr*Uz`8(4yiP?HQ)p$79mcLX=^_XxSMPH}@NEwO4*(R>@8#$grD5NGJ=_-qjuAJ2LCERJwtv;;y6^(v{7 zR!21)J3s8RA%b2NJp`|qli=k|e-0KCzFmzKi!%{EoPw&f0T_XgDL+vNMJG{?_$@N$aZG z*Rvlse1PAp_)sR>>3ErJf51ngGig)a2l!aSU*sekPpS&EbOZwHWSM@d;WK_ zO8i~JKk$_}o$O}iUmE_6vq#C{AjP1OkxU}fIm=Q;ibg6~#2V*DON4Cf8^wtvN4jvC z;)FeAg(;p;%9S@%JYY_+*^p0(8rdmHL{j#v4wcdgC9&6vIP5g*CYc&#QMTBXI@k1u zN-ptHY9U=B64QpI=Al`sw4YC@GAh)_O+{j6T77;#c{M7g6oOXM-n_mwP`iH3@<4}N z@=}dTXoNVKwk_2{u&fPq1lyZ~O`I(>Mx(KGxoAumrQyDem+CR)kWO+kXwT~vLcm((p^ z6=({Lv(jo>qf#rOSbUK=MD%CH8QZ8`yq5K{a|0J`$lt*UAw#fHqfK;;cqH3ry^d~B z>3X@1e#H|XMJjF9D9lw>kuyB`7Eb@Q{?JMrT}N9q+DbQx>vQf@?x4F=x>F?Q78-X^ zrx>66-mIN=3MQJV+>WngNBSZu`7rHAd*V5deo_Jl@@s4-EY$9W(Id03+mL#p(Q4yiA5Orsm=CLTL# zXVz!(vgw5QCU2bSBw?61lba^OE>*0_RTY(9kmVK~$}pN?5c^$)VLNwrJ_q@k1kh(J z8~HoY(}QGBADlkBJ2|rl*~gLRQTvhaGxo_xH-_|}u-<2qnyDLX&YD?j=57qvHAQMl zH%3V9I7TsCnXemTdNA%dDp;%Zd5(oyieNJTPwT;{}B$*7HntRMh?(HF5j)l z=cgMcWWj+PWFnWfJdB2m&)R&{V+i~hij641E%5N{VxB)7J8>y?VI=nOiQk3rXgq>3 zcoJiAgb(r_l(P%t@CwG`EtY?Q33wY7cpp>oA@|vjFdd&`7QW{E=HrZ43~C(}EG$6&zYJnn??9D86;!IS67M`PF&&t^|B0X+fG4+~#6fbl7& zeMt32@D%@dyX7AB;`kvr`VjFA4ELa~+{_WG$S_Cne7WMuP$GEY2+xnkC|Ad7;M>wX zq+>qEr-37}fE`)LPWqU780?)Q7mRV2()aNLrX!`7IT8k4`XPSAzIc#~SMeGHmNE3} zc!TFk&-mJxUZ?UYMY0jad|AybO#Cv<>%>oc@n(%=%Bl2xt!H?QrXj5FWwLI!n0;Tu zWw4a5Rm=3SdZP0OsHO3SyvFDjE|a%eGO)FRUos6NkMa#Gw(?khBOOSHX3wu8_-!xV zf0joK`g)4~c6ro)EovHDgz!iFDF!o@VWs!+5{85R=LkMoAfp>avFR^YKtqepHZ^KI zz&0LV;7f)gc__pnOLZNvFl0>xe+yqsYLyNZG&kNxPU#M!o%8Y?zFg zQIHiLDCuu4o5Okb|CY^_%gT#aY}un60a;d+-jiw<68b35=b-!^8gk0(q@lf3AmiaH zH=i7BGFBWyN`^V7w5Hfwe2hG$kuVM4Y^0nN)4_8eUtG*v8Fm||{dO+s9o*7xn|AYD1I)s-|-CtVC1mC~qKtkPgiI)@bb6N+zMxg0N*F5{G9p-dh{VHSZ-y{LfM zgEK8cqZhC~moub1LgO7YQEI(3$wAX)x^R*7(u@TT;@p%Lby010RAa*#g9<3xo_6t) zo!rEC^N+Ya7{yoe@Fc$E)Zv~%c{V%hlHDLr>R61c*d3W?ipLwFc^C3OTHTiq3Wzcg z5QP@#9fW7mLh`|2x5;xphSbknLwVKzNwIsH=}f>Nn{uKI ztAo}@=xTQE+6&G_o6Z$T;Bu2Y9&I{_0}dueiO zL2D#v_`9gIjc(=iIPVN&@yK?@H_{G(V{6pYGG81M~nL z)TM{$Q97hckI|F#lrB9@&(aZH>L%}T?mj&XcL_{<-CRPwl^IB<=ctckoXX$NBZK diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index efa081f..25d404b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,8 @@ import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import frc.FSLib2025.swerve.SwerveModuleConstants; @@ -224,4 +226,36 @@ public static final class SuperstructureConstants { public static final double INTAKE_KI = 0; public static final double INTAKE_KD = 0; } + + public static final class FieldConstants { + // starting pose + public static final Pose2d Left = new Pose2d(new Translation2d(7.231, 7.615), Rotation2d.fromDegrees(180)); + public static final Pose2d Mid = new Pose2d(new Translation2d(8.068, 6.178), Rotation2d.fromDegrees(180)); + public static final Pose2d Right = new Pose2d(new Translation2d(8.068, 5.067), Rotation2d.fromDegrees(180)); + // REEF scoring pose + public static final Pose2d A = new Pose2d(new Translation2d(3.180, 4.082), Rotation2d.fromDegrees(0)); + public static final Pose2d B = new Pose2d(new Translation2d(3.109, 3.847), Rotation2d.fromDegrees(0)); + public static final Pose2d C = new Pose2d(new Translation2d(3.654, 2.885), Rotation2d.fromDegrees(60)); + public static final Pose2d D = new Pose2d(new Translation2d(3.920, 2.731), Rotation2d.fromDegrees(60)); + public static final Pose2d E = new Pose2d(new Translation2d(5.058, 2.713), Rotation2d.fromDegrees(120)); + public static final Pose2d F = new Pose2d(new Translation2d(5.325, 2.880), Rotation2d.fromDegrees(120)); + public static final Pose2d G = new Pose2d(new Translation2d(5.883, 3.851), Rotation2d.fromDegrees(180)); + public static final Pose2d H = new Pose2d(new Translation2d(5.883, 4.190), Rotation2d.fromDegrees(180)); + public static final Pose2d I = new Pose2d(new Translation2d(5.341, 5.341), Rotation2d.fromDegrees(-120)); + public static final Pose2d J = new Pose2d(new Translation2d(5.072, 5.320), Rotation2d.fromDegrees(-120)); + public static final Pose2d K = new Pose2d(new Translation2d(3.887, 5.375), Rotation2d.fromDegrees(-60)); + public static final Pose2d L = new Pose2d(new Translation2d(3.623, 5.215), Rotation2d.fromDegrees(-60)); + // Algae & Coral + public static final Pose2d LA = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d LC = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d MA = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d MC = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d RA = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + public static final Pose2d RC = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + // PROCESSOR + public static final Pose2d PRO = new Pose2d(new Translation2d(6.347, 0.573), Rotation2d.fromDegrees(-90)); + // Coral Station + public static final Pose2d CSR = new Pose2d(new Translation2d(1.666, 0.693), Rotation2d.fromDegrees(-126)); + public static final Pose2d CSL = new Pose2d(new Translation2d(1.666, 7.338), Rotation2d.fromDegrees(126)); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1260dcc..7846fda 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,27 +1,35 @@ package frc.robot; +import java.util.function.BooleanSupplier; + import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.RobotConstants; +import frc.robot.commands.GeneratePath; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Swerve; public class RobotContainer { - - public CommandXboxController controller = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT); - private final Swerve swerve = new Swerve(); + public CommandXboxController m_Driver = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT); + public CommandXboxController m_Operator = new CommandXboxController(RobotConstants.OPERATOR_CONTROLLER_PORT); + + private final Swerve m_Swerve = new Swerve(); - private final TeleopSwerve teleopSwerve = new TeleopSwerve(swerve, controller); + private final TeleopSwerve teleopSwerve = new TeleopSwerve(m_Swerve, m_Driver); private final SendableChooser autoChooser; + private final GeneratePath m_GeneratePath = new GeneratePath(); + public RobotContainer() { - swerve.setDefaultCommand(teleopSwerve); + m_Swerve.setDefaultCommand(teleopSwerve); configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); @@ -29,7 +37,25 @@ public RobotContainer() { SmartDashboard.putData("Auto Chooser", autoChooser); } + private BooleanSupplier isDriverControll = () -> { + return m_Driver.getHID().getLeftX() >= 0.02 + || m_Driver.getHID().getLeftY() >= 0.02 + || m_Driver.getHID().getRightX() >= 0.02 + || m_Driver.getHID().getAButton() + || m_Driver.getHID().getBButton() + || m_Driver.getHID().getXButton() + || m_Driver.getHID().getYButton(); + }; + private void configureButtonBindings() { + /***** SOME EXAMPLE BELOW, ADD LATER*****/ + // run a path to certain pose automatically + m_Operator.a().and(m_Operator.b()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.A).until(isDriverControll)); + m_Operator.a().and(m_Operator.x()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.CSL).until(isDriverControll)); + m_Operator.a().and(m_Operator.y()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.CSR).until(isDriverControll)); + // driver can reset odometry while operator is scoring/intaking + m_Driver.back().and(m_Driver.leftBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSL), m_Swerve)); + m_Driver.back().and(m_Operator.rightBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSR), m_Swerve)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/GeneratePath.java b/src/main/java/frc/robot/commands/GeneratePath.java new file mode 100644 index 0000000..e384a0b --- /dev/null +++ b/src/main/java/frc/robot/commands/GeneratePath.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.io.IOException; + +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.FileVersionException; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class GeneratePath { + + PathConstraints constraints = new PathConstraints( + 4.0, + 4.0, + Rotation2d.fromDegrees(180).getRotations(), + Rotation2d.fromDegrees(360).getRotations()); + + public Command PathFindToPose(Pose2d targetPose) { + return AutoBuilder.pathfindToPose(targetPose, constraints); + } + + public Command PathFindThenFollow(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile(path), constraints); + } + + public Command FollowPath(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.followPath(PathPlannerPath.fromPathFile(path)); + } + + public Command FollowChoreoPath(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(path)); + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index e426f25..d9d2470 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,17 +1,19 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.Swerve; public class TeleopSwerve extends Command { - private final Swerve swerve; + private final Swerve m_Swerve; private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); @@ -23,45 +25,46 @@ public class TeleopSwerve extends Command { private double reduction = 1; - private CommandXboxController controller; + private CommandXboxController m_Driver; - public TeleopSwerve(Swerve swerve, CommandXboxController controller) { - this.swerve = swerve; - this.controller = controller; - addRequirements(swerve); + public TeleopSwerve(Swerve m_Swerve, CommandXboxController m_Driver) { + this.m_Swerve = m_Swerve; + this.m_Driver = m_Driver; + addRequirements(m_Swerve); } @Override public void execute() { - if (controller.getHID().getAButtonPressed()) { - swerve.setOdometryPosition(new Pose2d()); - swerve.setGyroYaw(new Rotation2d()); + if (m_Driver.getHID().getBackButtonPressed()) { + m_Swerve.setOdometryPosition(new Pose2d()); + m_Swerve.setGyroYaw(new Rotation2d()); } - if (controller.getHID().getRightBumperButton()) { - reduction = 0.3; - } else { - reduction = 1; + double slow = 1; + if (m_Driver.getHID().getLeftBumperButton() || m_Driver.getHID().getRightBumperButton()) { + slow = 0.5; } - xSpeed = xLimiter.calculate(-controller.getLeftY() * reduction); - ySpeed = yLimiter.calculate(-controller.getLeftX() * reduction); - rotSpeed = rotLimiter.calculate(-controller.getRightX() * reduction); + double deadband = 0.04; + double xyMultiplier = 1; + double zMultiplier = 0.75; - // square the input to inprove driving experience + xSpeed = xLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftY(), deadband)); + ySpeed = yLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftX(), deadband)); + rotSpeed = rotLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getRightX(), deadband)); xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); - rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); - - swerve.drive( - new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED), - rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED, + ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); + // rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); + m_Swerve.drive( + new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED).times(xyMultiplier).times(slow), + rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * zMultiplier * slow, + // 0, true); } @Override public void end(boolean interrupted) { - swerve.drive(new Translation2d(), 0, false); + m_Swerve.drive(new Translation2d(), 0, false); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 640a7eb..e50fe29 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,14 @@ public class Swerve extends SubsystemBase { + private static Swerve m_Instance = null; + public static Swerve getInstance() { + if (m_Instance == null) { + m_Instance = new Swerve(); + } + return m_Instance; + } + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); private SwerveModule[] modules = new SwerveModule[] { From bffe53ae04c74a242a1e133fed37ecc8b28237f3 Mon Sep 17 00:00:00 2001 From: Ray Date: Sat, 8 Feb 2025 14:21:20 +0800 Subject: [PATCH 2/3] add abs func --- bin/main/frc/robot/RobotContainer.class | Bin 1243 -> 1728 bytes src/main/java/frc/robot/RobotContainer.java | 12 ++++++------ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 3f83aa14a866f2ba011afe74841a251b5ce69f9a..c71c7baaf49ab2e61a1e5b8a9aa4149afc010979 100644 GIT binary patch delta 536 zcmcc3d4QMe)W2Q(7#J9A8MHTY*)cIO+E4akl4C5MoWbN&&l!@Ds*synl98XHP>@)X z;g*?~5|ST~U!1A|B8*Zr6*7wzO7l`u(=zi?Qxwwjixf(LT1qMlQWe}&^HPfvOHu=X z8o9_aDUNzZVKZs+N~RtrMh1|7>x-=v$a5n$gGo0jiMj!Y%~6xXnO)g97&I6e6oEcg hoIHp50i)^U{VbxBC$UH{s!v|cqRFlaObAR2S^zA1waWki delta 68 zcmX@Wdz+K%)W2Q(7#J9A88kO?*)cIOBuq|VRh*o|WIwr+xqEUPtLkJ4R;S60tPdEC YCMU7!PWELJnY@@yj9ne5iHSi20OPF^#{d8T diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7846fda..d337ac4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,9 +38,9 @@ public RobotContainer() { } private BooleanSupplier isDriverControll = () -> { - return m_Driver.getHID().getLeftX() >= 0.02 - || m_Driver.getHID().getLeftY() >= 0.02 - || m_Driver.getHID().getRightX() >= 0.02 + return Math.abs(m_Driver.getHID().getLeftX()) >= 0.02 + || Math.abs(m_Driver.getHID().getLeftY()) >= 0.02 + || Math.abs(m_Driver.getHID().getRightX()) >= 0.02 || m_Driver.getHID().getAButton() || m_Driver.getHID().getBButton() || m_Driver.getHID().getXButton() @@ -50,9 +50,9 @@ public RobotContainer() { private void configureButtonBindings() { /***** SOME EXAMPLE BELOW, ADD LATER*****/ // run a path to certain pose automatically - m_Operator.a().and(m_Operator.b()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.A).until(isDriverControll)); - m_Operator.a().and(m_Operator.x()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.CSL).until(isDriverControll)); - m_Operator.a().and(m_Operator.y()).onTrue(m_GeneratePath.PathFindToPose(FieldConstants.CSR).until(isDriverControll)); + m_Operator.a().and(m_Operator.b()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.A).until(isDriverControll)); + m_Operator.a().and(m_Operator.x()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.CSL).until(isDriverControll)); + m_Operator.a().and(m_Operator.y()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.CSR).until(isDriverControll)); // driver can reset odometry while operator is scoring/intaking m_Driver.back().and(m_Driver.leftBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSL), m_Swerve)); m_Driver.back().and(m_Operator.rightBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSR), m_Swerve)); From e0d495e425e2e7aa9efd6b41c8fc7b26cc81a313 Mon Sep 17 00:00:00 2001 From: Ray Date: Sat, 8 Feb 2025 14:30:22 +0800 Subject: [PATCH 3/3] move constraints into Constants --- .../frc/robot/Constants$FieldConstants.class | Bin 2538 -> 2538 bytes .../frc/robot/Constants$RobotConstants.class | Bin 572 -> 620 bytes .../Constants$SuperstructureConstants.class | Bin 4089 -> 4089 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 3204 bytes bin/main/frc/robot/Constants.class | Bin 605 -> 605 bytes bin/main/frc/robot/RobotContainer.class | Bin 1728 -> 1885 bytes .../frc/robot/commands/GeneratePath.class | Bin 2081 -> 1447 bytes src/main/java/frc/robot/Constants.java | 8 ++++++ src/main/java/frc/robot/RobotContainer.java | 24 ------------------ .../java/frc/robot/commands/GeneratePath.java | 19 +++++--------- 10 files changed, 15 insertions(+), 36 deletions(-) diff --git a/bin/main/frc/robot/Constants$FieldConstants.class b/bin/main/frc/robot/Constants$FieldConstants.class index 46100a6f35743e068f9e1c099adb8117c3d9268a..0ff10f699ea71694b3a289a597ff01d906ab22eb 100644 GIT binary patch delta 111 zcmV-#0FeLc6Y3MNCJ9~e0387F06GBj09OF?0CE8J0F40l0IC4@0LK9M0OSDq0RaH| z0UiMR0XqQv0ayV20d)ZW0g?d!0j~i80n7mc0qFq)0tNvD0wVzh0zd%<0$l+J0(t=o R0+#^`lYDgV diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 93b3b6016386dd85fd0bed14ffc4c5753bc3c7da..c1e57313a2644c56b3b982b54ee466718070f724 100644 GIT binary patch delta 187 zcmdnP@`i=$)W2Q(7#J9A8I&h-9g>sq4{!}~4Dk<&clP%S3G(;xaSe(O@DB=M2C8A4 zcuI;*oQFY}L1f~q7Dk21`Ha?#ER$C=8Wu=02r}?9Ffy diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 22339f8461647f1180bd9c2d8c49c064b30a5397..7307486d85b283641b919052494d3c9a82f775e4 100644 GIT binary patch delta 200 zcmWN_F-QUd07l{WFAfgf;&|ZZ91elt)*`e8EiNuMG>IhEL6i=X;p)wYlthZ;84)E2 z4UTSdCsKl;F1EP|4jpg#c)R#6?)eAh0#S;@C}DC*f-4TONpa0FH=Jp^^Rvq{cfw delta 200 zcmWN`F-QUd07l{WFAfgf;wZS?95)1kTZ`Z(xQO7=a1d%J9T06JHjOiXZW$1aqZ$ oQHEEY@}>gsa`=$Pr|K-#@?{mScDc+AWZ{l=|$RNn=qkGXtrr+nyt&W zqM|7ZI(R|g8x`;ZUO{=0#I{ODcyN3{ebYDf$yeSS9UXN%yBXR(y1Vo3o^yWR?VSI& z=PHkTowxqIegnXET)pP}yJDYfOwAwGwNy5*A7=3PjHy$B>6AV(olE8OiWbOa51Mvh zFF89_*dT+wZ7i3~XZ0NnmO9@MgSA6VD>Al;YDV5bfn>Hi>45+ z3V)vy5k!snRtFfmIQ*!0kguDg2X`~@h?bpIdR1N3y3$6VkK<7@FsLhvwkpuiF>uy; zzZAg`$FSI7t!j;PG@xF(d2_DfzF&T;GG;iy(MYbHx1#@iBi3CpTq%wwgSs&%?RopB zhzv!j*3DIEM2+YKM*y1~NaGO4By!?QYf#E#T81kAvNo~Dh0nI$N-3)smuwqF&gQDp z$~|W}9zwGd8jf&0iKoOVTf5^Zo|W+o!xo|&M7QL)hogWw25B;^r6v`I7ID$mND*$= z@EphUI4(w|nXrr(80!9SRix1P92FOE;w8Ky<7I|g@f}-NGob1zJ*(!M(;Tnj6oYGA zQ>P+IMpG1H6Lk@v_?E5jU*Uqr5T`lL;4GCRqv&@+gj2aAQfT(hR44F;QOY+Nc?gK9k#+N8FbGEO?DkX@?cCV+*#@L@faOopwwY3(H@jHEvm=#)C)b zSw!1l+`4RsOxsqrLmrfEY1uaU62eP}#22xZWXyy8tI0$W3HMr)Ot>peGC^Y#nebp_HJL0T>8>`(q)^Z8%B)nA0SS^JvEh=)hI#Rl+V@$8LO$E_{z}{79$$XY}G%^x-xJ@CWuW z3x?TUh_kiW&#G~N)nJ6xVwCxiWX(8|VnK4YBZarjbyCrd-$+>pbY>adfX%;%cULwd rnNF|Sg_i+b#3cePc+VW>eOe=MFUjgCKE%f%FHx8A3B35!j9>l_-;+H; delta 1009 zcmYk5Uu;ul6u^IXx4muW2Q*sNji4-q7{=5o;Lr^Q-CqVPFvhyAnnJhTZbj&wZJ7*} zP9z}yoqj%u(Zs|Tqw#^(v4km@7#|?v(HEmW`RW^qAyMMFx5-@Y&3AIn`JL}O=R4=S z*zieR?VSfVZUTGw_*(5hH9BV2`*s;L>A}E8J!jSjTWMG9W{Q?IYcM@iUX`OP~G|LtQ~iXPQHKAbqpxto@1zJ+xb(UMPTZcDPUo_P(?`?AtP|j6gJ%6jaP#JP`fQhz%Vei&nW>&oR6N6W zSDjV7!dZRWAG6N!T7Xw|*lIIh=e&+vJ;52~6>o4se`=l44+2fmOH%a7^lZB2R5~}+ z5>FSWYcc839l@0GzWzM8+xSrbD(fRU8oFPqA;=DPN=eUx@jW4FAx2~12U^F>WmzIT zC;t*{Lu0-by~ES}RlOrZRkv1jkFT(Ana;5#x-0aS>GdtLUxa^=gCfjD4wWhOMi^XV zXa!7^NrWHr$V6CqXhH%7O+*-7O(x4E!|OdV8Lsolr0gwZGQx|i$yAwCc#}t_!i^r8 zvOF?Xig3b1$0g6n3U(R$JEkHC_R=ylV$D}5lqs0kIWr=%%-nTKBiH1bYIsxD3pC2Q z6^kb7WOg5?nO2#wZn5>rlnhEAN7>GCx#x`Bw8JjucwTze#+4H7EXe3zWiMY6;~N?8 z3SHcg@Si07F8%x>&)|0s@+SlQ#S#AIs9`c}tYO4hA9F)n)PNax&VRBfglZO8}y xZA)DKKVQo;M!gW#S|xakD?*yQ?d|gp?}~g_3OvVqe6X)Us0BWv0qyx0{sYu#fWKO0K03NUdfB*mh delta 13 Ucmcc1a+hTT2NNUzWKO0K03NFYe*gdg diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index c71c7baaf49ab2e61a1e5b8a9aa4149afc010979..6f1557b5fd214a228911d072a29fc327b9d60948 100644 GIT binary patch literal 1885 zcmbtV|8o;X6#r}+dhPWD8d{-Huq{xMpsezxB55m-0u~|_QZn`j2ltX~d!5T|=60{> zf8(E^Gn6tq9mp^S?RZsX;8Q^vw50wdhYD;e)ehV$iaP&)DT z_u9yY=B>|k&oeTVs@XB0>oIS8)_kMKgszN>#HiAaQrZT-I5=>w8zMG!p?7>G;ySO$ zs4o0aw(k+lSS8qwgh^tG^4tFevNZTsH)ri+6<5gJ$WjmmW`$umm#;EpN~$hh3}e#6 zNlZ9+pP~3aO5orO!+bZ_R2*$YWb~y@E3{M>Q#fl~o}(CYYx(LJs7M};VZ=%wcyKV{ zV2>fDE3JSwmM*o&`X*!`0kTt~w>mS6zI98xF41qv?ITDa=lj*hKW=#1w9u zlHa4{eiDRr8GE>iTf{ows`mV_V{hm3gS{Trvo_(MdhqbxC_W?VSk&1ztcxO+9V{{2 z7}!O9*p?ov9eQ|J!RHK4Godphs1?fa# zM_6<41;fpMmzj4bg@?y@LPLk*sL&j$8Cj>O$bC0It#Mj(@Fl~-Kp%Zk(ZH;zB(BMa zflbe;ci(-@VpD7KQ0 zB|U4&k?zz?^z}H&LpVV{>R3$Bnx&H;QUst{IKZjGFPPdKKEU)oW>Ptm$`?qU-8{hj zJ}#1Yd9(1Egx8okz?FRz+FIcaJ}kV&$NN~!-lns~x483D>hmtG3u#Q}k-;Q+pTZ1I z;|iv62N&>w7+l6YUf?3`k*!CuQAD_p673ADWYLQD7VY-ixW#{ihrglxBdJ5UOY2Db bX`u&VybUpgr>WICzM|bFyVH1v3a0-AWCa%x literal 1728 zcmds1-A@xi5T6C4A5@A31X1)*)D{yjA?kyC7^vV&Lz}h|eX4tR+jhy_?qzSU$X{ln ziN5|0&1%MrRl7R^V%U$K#N;*>8O(UyPXvzgs z8JHq4cR~l$_NnOEb?1b+nm{(_yjFa`6oH)@^J4onN}Du&u%f{QO9SaIE2Zycu!Cr zE6no4Ux93=M_trxRIIICylyt=ykW=%)vQ5vpFrwU$6YXJjz>d+&TCCzy_WD8Ywp|K zSh$)aEBjLVjEYtqhCWaDnBL}s>s_@+H#on3qvZ4 zN0!;8R+ls1%jepCW(7?5rDuis{zERjwrn7WWg|AdiiwDY$GX_Dp4CMNjUoChWY&09 z{z;@fEWw=uEWj#(#qyA$WYSt%anai{6clUtu%kF|m};9&Bv7hJm-@$4akCz6PU$|6 z2&{|^!sTJcwpend=<*)6?tU_r`&dl8Cd$G(A#epQ|7$V`EcBS(!%srUK*rH%3JGkK zD_5P^ER=CV|6EBN3jFt4GFC1-mTmbD6$4^dGT1uY@wA68qU7Bh@)%TUHU48z%#BZiT@@`R0HjHPi4qYUE(8H!>j zs0v+itG-~!6jaUik6bNHe6ctZH)X`IShzt(j^S>b+*r^JqpQW@TBgd?|PF)J$vu#XZ z#&|W$Fq17@9$SvJ3>!IPYNlUtQ-u4<_tgo*b`p5d8X4@p&o$9D@*@im8P=&FZq8}b z_0qL-GKY6!>GOT*izA^Ug7$MPJYtyYij|5V9A4R2U|3eV!CR3Ec_f3jFNjx1L|HOZ zA4(eP-CE4^vhf5<7M?O>lY|&ylpZ%rJ$2?Yjb%JzNY}LrOl)*A>?GPUC}++_7ONIk z7?zT9+)k|cE)DNG()WZ;!@(LuC0j^BC#fF%9a1wH0@E=6yb5Z?o_G1C(n6W?je{@K zcx619HlFmhD%v%Ts`M%OT~-}k6Fs9QuXgu_IRT5HHl}|9=s7aezBF1CahjP~T|GnU zD}4;XrqOHzGq_D-x(A^74n@?Y7^l%BZxQ7X&GvUpe9C0bF#VIFs|<5j80U#_Whx@@yO%!N9m=>^s)lVM{3UY@|a LbOY~ZSI6r=jS^wz literal 2081 zcmb7^>v9`46vzKcjMs5Qo3V_@@VB= zzA!uz%mB^63=hCVF&ynCt|yqp*|D`N>FB3(|NEam{sOR$#~x-Fu6Ipa7~RrVv~|B9 zM4ebXkdZWjmCeBJdB`v<><7;R5e89DY`6Ah+cM0zbrf3@sK~|)`8nDtA$44Inl$UJ5-&*35G;Y|jAT3oieK*rFMt#*G(KJvJP zw>%UWZpzM3JRd00RVKEgPl|}1)O~5qOYub8z$zWBb~qMsnGEV0-LK1@k&+Br@$03= zbmuGX#;Y9H@HPYQNqdAQ3|UyuR-YYd#&O-*_&$T@*Xb$k&Vk_u#~a8poNuT|ZVme_ zX?B8EC>bs`bUO%l1Ebvha4}=|6rqcaQzA#6KIfdjsiMw~Zt9qTTmFegIib}~<+$bK zLkj2q>QRP>TAC?jc^MyhSYcS26tG}uwYW>ey`c&_(s10uZPLo+Nq0=E7gK39eTq%3 zKSSOIhC-HlXK5PeP*g@<-ILL#4nzH%Va;#6E|RC#sHiu+A4!ks)wd6U0yL<81MbJopC5qi<+g@%IHsh zDjBJPAxA57G>b5w?D^$a$d~^F|Dk|$$#3BR7k|f<0^UhKUg5ppQ2d3?%-{nWZx910 zAVVE^4wu1k1?O><#%pxzB0i+OC6eGNAIrE&Ge+zF;oa**nxVD1mGZAB9-#CyO;UI^ zIrMGf;lwCc}E{?JJh|N*Q r{e->wKkPNyb?lsDAK<}p?(!HnH{#|-+(%Pz8xy#tQ@GoS@0$MtWPu;8 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 25d404b..504472d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.pathplanner.lib.path.PathConstraints; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -18,6 +19,7 @@ public static final class RobotConstants { public static final double PERIODIC_INTERVAL = 0.02; // the periodic ,in seconds public static final String CANBUS_NAME = "GTX7130"; public static final int DRIVE_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; } public static final class SwerveConstants { @@ -87,6 +89,12 @@ public static final class SwerveConstants { STEER_MOTOR_CONFIGURATION.inverted(false); STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake); } + + public static final PathConstraints constraints = new PathConstraints( + 4.0, + 4.0, + Rotation2d.fromDegrees(180).getRotations(), + Rotation2d.fromDegrees(360).getRotations()); } public static final class SuperstructureConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d337ac4..2bc54e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,38 +26,14 @@ public class RobotContainer { private final SendableChooser autoChooser; - private final GeneratePath m_GeneratePath = new GeneratePath(); - public RobotContainer() { m_Swerve.setDefaultCommand(teleopSwerve); - configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); } - private BooleanSupplier isDriverControll = () -> { - return Math.abs(m_Driver.getHID().getLeftX()) >= 0.02 - || Math.abs(m_Driver.getHID().getLeftY()) >= 0.02 - || Math.abs(m_Driver.getHID().getRightX()) >= 0.02 - || m_Driver.getHID().getAButton() - || m_Driver.getHID().getBButton() - || m_Driver.getHID().getXButton() - || m_Driver.getHID().getYButton(); - }; - - private void configureButtonBindings() { - /***** SOME EXAMPLE BELOW, ADD LATER*****/ - // run a path to certain pose automatically - m_Operator.a().and(m_Operator.b()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.A).until(isDriverControll)); - m_Operator.a().and(m_Operator.x()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.CSL).until(isDriverControll)); - m_Operator.a().and(m_Operator.y()).onTrue(m_GeneratePath.pathFindToPose(FieldConstants.CSR).until(isDriverControll)); - // driver can reset odometry while operator is scoring/intaking - m_Driver.back().and(m_Driver.leftBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSL), m_Swerve)); - m_Driver.back().and(m_Operator.rightBumper()).onTrue(Commands.runOnce(() -> m_Swerve.setOdometryPosition(FieldConstants.CSR), m_Swerve)); - } - public Command getAutonomousCommand() { return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/commands/GeneratePath.java b/src/main/java/frc/robot/commands/GeneratePath.java index e384a0b..8115f49 100644 --- a/src/main/java/frc/robot/commands/GeneratePath.java +++ b/src/main/java/frc/robot/commands/GeneratePath.java @@ -16,28 +16,23 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SwerveConstants; public class GeneratePath { - PathConstraints constraints = new PathConstraints( - 4.0, - 4.0, - Rotation2d.fromDegrees(180).getRotations(), - Rotation2d.fromDegrees(360).getRotations()); - - public Command PathFindToPose(Pose2d targetPose) { - return AutoBuilder.pathfindToPose(targetPose, constraints); + public Command pathFindToPose(Pose2d targetPose) { + return AutoBuilder.pathfindToPose(targetPose, SwerveConstants.CONCTRAINTS); } - public Command PathFindThenFollow(String path) throws FileVersionException, IOException, ParseException { - return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile(path), constraints); + public Command pathFindThenFollow(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile(path), SwerveConstants.CONCTRAINTS); } - public Command FollowPath(String path) throws FileVersionException, IOException, ParseException { + public Command followPath(String path) throws FileVersionException, IOException, ParseException { return AutoBuilder.followPath(PathPlannerPath.fromPathFile(path)); } - public Command FollowChoreoPath(String path) throws FileVersionException, IOException, ParseException { + public Command followChoreoPath(String path) throws FileVersionException, IOException, ParseException { return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(path)); } }