From cceb2b1cc20b648c5b4caea2a7890ef439987aca Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 13 Jun 2024 11:51:23 -0600 Subject: [PATCH 01/18] Janky working prototype, Most code removed, Using phoenix 6 --- .github/workflows/gradle.yml | 4 +- .gitignore | 8 + .wpilib/wpilib_preferences.json | 2 +- WPILib-License.md | 2 +- autos/final_1note_stationary.auto | Bin 0 -> 11345 bytes autos/final_red_center_4note_taxi.auto | Bin 0 -> 76469 bytes build.gradle | 24 +- gradle/wrapper/gradle-wrapper.jar | Bin 60756 -> 43462 bytes gradle/wrapper/gradle-wrapper.properties | 4 +- gradlew | 35 +- gradlew.bat | 1 + networktables.json | 1 + settings.gradle | 5 +- src/main/java/frc4388/robot/Constants.java | 184 +++++--- src/main/java/frc4388/robot/Robot.java | 17 +- .../java/frc4388/robot/RobotContainer.java | 425 ++++++++++++++++-- src/main/java/frc4388/robot/RobotMap.java | 148 ++++-- .../robot/commands/Autos/AutoAlign.java | 208 +++++++++ .../robot/commands/Autos/PlaybackChooser.java | 11 +- .../Autos/neo AutoRecoding format.txt | 20 + .../commands/Autos/neoPlaybackChooser.java | 107 +++++ src/main/java/frc4388/robot/commands/PID.java | 4 +- .../commands/Swerve/JoystickPlayback.java | 8 +- .../commands/Swerve/JoystickRecorder.java | 6 +- .../commands/Swerve/neoJoystickPlayback.java | 197 ++++++++ .../commands/Swerve/neoJoystickRecorder.java | 129 ++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 151 ++++++- .../robot/subsystems/SwerveModule.java | 173 ++++--- src/main/java/frc4388/utility/DataUtils.java | 35 ++ .../frc4388/utility/DualJoystickButton.java | 22 + src/main/java/frc4388/utility/RobotGyro.java | 78 +++- .../java/frc4388/utility/UtilityStructs.java | 14 + .../configurable/ConfigurableDouble.java | 23 + .../configurable/ConfigurableString.java | 23 + .../utility/controller/VirtualController.java | 145 ++++++ .../utility/controller/XboxTriggerButton.java | 4 +- vendordeps/NavX.json | 13 +- vendordeps/Phoenix5.json | 151 +++++++ vendordeps/Phoenix6.json | 339 ++++++++++++++ vendordeps/WPILibNewCommands.json | 1 + 40 files changed, 2426 insertions(+), 296 deletions(-) create mode 100644 autos/final_1note_stationary.auto create mode 100644 autos/final_red_center_4note_taxi.auto create mode 100644 networktables.json create mode 100644 src/main/java/frc4388/robot/commands/Autos/AutoAlign.java create mode 100644 src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt create mode 100644 src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java create mode 100644 src/main/java/frc4388/utility/DataUtils.java create mode 100644 src/main/java/frc4388/utility/DualJoystickButton.java create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableDouble.java create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableString.java create mode 100644 src/main/java/frc4388/utility/controller/VirtualController.java create mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/Phoenix6.json diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 9eaf0d0..7d89e58 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -13,10 +13,10 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v1 - - name: Set up JDK 1.8 + - name: Set up JDK 17 uses: actions/setup-java@v1 with: - java-version: 1.12 + java-version: 17 - name: Change wrapper permissions run: chmod +x ./gradlew - name: Build with Gradle diff --git a/.gitignore b/.gitignore index a8d1911..b5b18bb 100644 --- a/.gitignore +++ b/.gitignore @@ -170,3 +170,11 @@ out/ # Simulation GUI and other tools window save file *-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ +simgui.json +simgui-ds.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9c62ece..f523e9c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2023", + "projectYear": "2024", "teamNumber": 4388 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 3d5a824..43b62ec 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2023 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/autos/final_1note_stationary.auto b/autos/final_1note_stationary.auto new file mode 100644 index 0000000000000000000000000000000000000000..f83851cdd1bb4f57568177ac80723b4358c8981a GIT binary patch literal 11345 zcmd6sPl(K69EWFiW@ly(YB|tGxk!p~k(9$;&;Tfgk>Q=fUB_xsLo-=_E1YfrR}w*KqC(P(yl+rh=9 zHO|g$+A~yZ9gUW1ZB<1ncC80Sh1j)S;ZcfR$AD2Ga7~ZDubT>wQtWyPj0&;qmBOPG zyFLJ;LhSmg@F>NupTMY)y4rn(M=5sA0HZ?e+N|&>#jahzs1UnODm+TD>pUkcp~#IDB*k5cS<35*J{>%GFG6uZ6vqeATZrSK@FuFe1$ z6=K&qg-0oN?FB}K*wx^9*g2y{K`EZ=GB7H{u3HL^QtWE*JnTHjl2wT3dZ+Lx#jXa= z!_E&ZS%q}2Zdc(^id}1fQ6Y9+PyD`-bK-yn|f z6T5DcV^ZvTXz1+3t{3E(6uatv+mtriKljI^ zU=PRd5PQv>hbykrW)gPdxvr99QtTQUIy-@@wfx(rH%E?1vFo*=vlF}O?!N5**LV;6 zQ(Uu2>0JG~JEF3=nmG?w_Hc*UHFn}XJVcI3vFnJTvlF{ckz-Qqx@hR^#I76Um=wG2 z89F<$Yn~jFVprWgTiLns(M-ZlI@e$oIVQ!f4TjE6?Al6>NwMpIp|caaPLN|#?5exd qD|+S!*UTjB#B<#x$E4Wx$k5q|T@9X5gSvN!MbE}m-8;lG*UVpBF_YW? literal 0 HcmV?d00001 diff --git a/autos/final_red_center_4note_taxi.auto b/autos/final_red_center_4note_taxi.auto new file mode 100644 index 0000000000000000000000000000000000000000..b20ab8403b4d97fd6083b9c3b6e3e2d554f754de GIT binary patch literal 76469 zcmd7bd6X2z9S87TaX|#R6y*>EMC6uB6oJ$}-6Y<4##`|W;fX;EL==~;^(dJ1SQ2U;6}RwKkxZPZUVt66KH z)fs5@1zLvzt)qa}X+R5e&E~$on!N+eYaP(q2DEmPB~~0hIZZPKOU6At_jv8zioyac zea$8|UGjE0!39fpUi~Pn*iyGPVQZweYQET!ixH&uK`LXYb)`#w`3O3`j57f--i$Q*41C6 zX{KPwJ~tLnSW%z2^#;&d4Yd4rKKXt4EiSL-ErFJQJ)ihKY(4;(m){;HpVzIxyygL| zCw=RsDQTK1SaRFL=Ibe}s88Jb+P9Ma?9f6#Pf7kdszn!_SK`;$E&R{s$y*bEdEKt9 z#INUDG`ZeL{5lR>M^MKZaKuhp2Q6lehFUL^ACqJ1>15WjAOIuGP8)MTlOqtxnMmT9a*!r3leW zwlyxgL2I(D@f0C?$+oVDZqS-+>uQP+y<}Unq8qd(+nPfWqL*yzq38y!$*|n-A+~;= zB1A9Q){^K3t;x2&qX^MUuCW4+TYtUa~DrJFoWPxzY2X zHQ9M#+If}NwY0U-OLkuR^D@A?C%Qpvvh&ipcxEaSPE<{JzdRT=Td#YE(?Yg21!!F_K7Qt+RCfEf zs{Cl{sT3bMH0hP>yk?8_BQH;9qJ;IG6CXRCK?U1dF4n8QfBN{qldhoeAZ4YaD$h54Bbm1^#j0 zzdD>J(v@#GnP_6`R9>{y$(#UT@5?cs8}9+FTAbCf8PM7XXdQ?< z4?7Ok*P>OmtIN%kE>8`&J?uD%#-|XQdLNyzHA6qIta$FSRGuu%Dt2DC0ua3*@v4d-{^ZG=3-vE=wJ+7cZ^K9#9oV9No*^qKKSf(A?a7)R}Yu{e< z9W%MvLg>Y8>vZ38bhN?~Y-@^dr3w>zDcib9TU9!kWrA%z>RXP*pv7!!skXLIEL+L8 zDzp{3mnz%(250TpQd`xweMif=dF}UGnZ(Kmj$2cu&p`rvr;#*tZ4!?nUu>_UoF3iZ9OU*Q*Ibpqe?%f znv(AZ$LMBTi-6X0+32_l$l6ksNZ_x0Z_M=EDt2D$w6!h7^7>u+9^2X}8ua{UQa_m{Q}To0XC?MNyg*y^O@Jnoa`i(emN%Ja zUC+DU%kDG@Xw8rfrS4YCdM9V=X-0nd$SLJCMy?)JpJv*u~gW@LPMy+pBPg+r4$rHt=*8Q-eOfig2OS|Ohpd@(+&OJ_f7{;3$PzWD{e zHb%?(yMZxUWqt-fa)No@8#ft%ki1&$mT3m$R)&W-MC`+XA-pUH@*&!d%YI>kc!Pt%hyQ zw?#IWv#saMShgCrwZs5shHYJJi)=1uTUVN~Y&C4_Z??$ha<=tvGnTD}ZQW~&Y%XV8nD$V$Q9I1-w$*U+ z>Q-WlY%XV89nDy_8n)Hj7TH|Rwt#JKw^DPvZ8hw?#@Qm9%h?ui?7nV)Ft^)Q!^q1W z$E+LXTGYKq+-KUC4gf?%HzQ&BZ+s~Tv1P0ysn!u;a{nAvgj>Rv3p z{$2p9HPGrx&kzB5^^;yd0P%A0xruqCcI`auUPjNCh0(uDMULejto!=vekMIP7sh>E z&w4|R(dB2!I1hVd|4;YdTdg?yeJSf2y$=`nDs%je*Hf|dF;iBi?|}*{j6PSYUM`KH z?}4(G$Ze%=P+{~x@vJ!dvngw%etjAJPh!lg$UQe2q}R7h7=1$Gwhk!KmeD69cB`|j zRWWHyI5p@0m2n&|cOM?mUt4Q~{S3Xl;^6U0dFGFbP^6fmUyzH5_P_$!fwTcL321$Rvw9W*t+qg`yZ>5F`aJA81ZW+l z_hDL%UG2Wun*5Qmy}o*$BdZ)Ya`N9_?m1Idm3r2e<>l^_?GNMgvfFO-oFg}<+~A@r z>3d&2UjkZ7Wo4<#YisbkS8jElKkR*2AwPALRh@sFKoetIwQ_@sCTgc_NtuE3)Ud5@ zsZXjf`X}dM+$yfst0mCdSAMLvUl@IBW6$d#pfy%{W1|D|Iss@+1zI-&t-FENi`p{! zuE&1PF9lj_d@KKb05dyw8?^;!eW$IuX$@&Ji99vjybjz`TlH6ek+9VTX!Vop)ov2? zYP{P#J1tPf-iM>)MwoT9wrca=(+eb(nPum7ouuoHkTG>*v~D(I*=pE%y=;qYE@xY- z%viP>ww1O;HkY%lMl+VJhHEjpz_8Ex-fbxh`8G}2N_Jj7C^Ep(zsm`*E(PW_1!&zs z_d~5O8SkA7|x9cf!4F4QT+}AJqOE4Dl-#fUQ2-12cq%EAF{as^4g@2m-nnb z!F{*`XZ6{`w^G5B-1+#2}4zTXi?R?0&k&~o5!#-XM=)MN^p)+F#YuMJC z+Vc9)1tXjDY^xGzZO2&$7Xz)~K&w>Lt1%*}&!c%R1Z%kG#=)o3vkibXUSDG;e$F2} zS$NM56V1#04*0>>3vV2yu!W7}=a> zTg!ykk6eD}m|^qQur2RtW!|76m29gfBw&GRw)ItkpdpL6R^Oo^0Si>Kt)mJA4Ozst zfa5vzJvU_I3skf7()aWV1P?4?Tk`^eBFwTaeTO6h;;q8Ao{w(Onrv$cMTlOqt<}*D zT9a)xP=x3u*XjoxOQ+x9==so^?7WVo2+>Qnb#ior)?{1bDMIv;ZB34D(3))PpA;c_ z$*|n<0s7q+-Jmtu)-x0#ddaq4i*C@GZ0jA05WVDD{i73lO}5oehlo7Ewz@|q^onc? z)AqWrul|^}S6Soqu8BOs-iK#LCiIGI3p2)o+J~5N9hC(f-=@EQf~F6Lb%K4op0$QH zR$yBzj8N7JhUJdG(SN5kw6Ox$8h{yF%m${IkS>37>`KD=aGV=1Pn z3jPKGEeV!v>llh867MfMK;H)tpOb1U*?H;j0|KnurK9oySmm-DW<3M6s({v4IBQ@D z(CPrRdIGJXKx+)pI!1cWsFQCu2A(BnquPyu(}312pmi6}dI)C?Dh67efYv~uH411Q z3$)G#TGN5njX>*PKr02bRsyYhptTcc4c-%Ibp~2}f!0`{bso_ABhZ=ww4MT5!1i$P z2f)12K^Qr|}-{7nv zdjPF2K&ubXDg#>MarY{R+$Fv5XeNJObjV}U>sbk~7D?|3Gr(E_wARZ7sC&ypYJt{H zoHg`7pmiwFDx=>-PX3%9Iv;4gAieKT0`giTyE?G5p(qzRmZAR%GJul_SppT9*N>sX%KM(3%UhyeFfO^YA%fUateKl|btgptT)m9X1ANodmQl z1X@=DEl12&zjc@V``2N&i8<=GkO9_Qp!EpQdIe~`1+>=Vti#&^twBI*4A43OXq^YN zCIYSLKx-Dzx&vt45408lt=EB81@I- zp9QeisK*rlxC>+~7Dk4IxIRsKBHqdR9b3+fyL=ynHmIDe&|>O*VlvO+%z5;9Z$|DJ zb4dO}-R=MKr^6EKJiOrJdLnCJn>np?~vU-1sSFgPu;pthGwf}bG z(1db%xwr#x)r+`4AGu$j7~u2MpVs3VpLp|mx&;>`0p0vH6kF+D2CY0fVdQOZ@h;A` z{infNyvmFUiu*XG}RNMNwXrbec_*a3N=2zJ|Wh5z* z5rAX$JJR{#zP>KY**>xHTuw?|-Rg|o24V)74HcfVT;WtQHXlE+_4iPE8QE#DUm%x0 zEKr75ur~W%w#-My3Tj`hH6EuEW+8K-^5P62$7Sc5OK+22qj&Pd1;)1#4tKihi=~8C zHiQSst0cpri6%OeaR`PY>HH_;CPaRNty%WTm4{wDK8V6gCZlG@U3$~JQZ;HPvDJcT1V{ z?>H@13MJcCNe#5z+MecYNi@VT5|&UiN1D4ATT+%M+h4c$t;C#UAs3O_q=GxK0}8%8 z8J(_M9bayxN}69ex4dzM_P3oh@ZGREjVvn%%r7=xjkqxJP4kj}5tlf;QosR=%4L5y zWhgejO=vao5oX%mOHbhJ8V+SG&K5dABn6!WiKl{|oPkq(9z8l&Mm%(=qGcFzI=eLu zWc_oCLyf;hVlB@dnwY98?75B20=n$>u3b|NB28H0u-6Rpl((%KWEBOfElVWJx+5yg z#SGqwza7f}$z;n~g%4HDU{;V{gXIhft*q2=4zSezGK~nBgu9-Q*rZ#2f=Q}i2|qOp z!!y4p)4o=LVUNhlkp#JL{tfkhXNbB=Ox>M=n6soptJw-IDI|_$is2w}(XY>a=H52d z3zE$tjPUhWWS+5h=KVH&uqQS=$v3nRs&p$%11b%5qtF}S2#Pc`IiyBIF4%A!;AVoI zXU8-Rpv!DQNcF~(qQnyyMy=-AN~U>#&X1j5BLDP{?K!%h!;hfJI>$mdLSvktEr*89 zdJHvby^$xEX0^l9g$xW-d?J;L0#(`UT~zpL&*cEh$L|HPAu=P8`OQZV!-}l`noSp_ zQ-1$q$R-gDL)?6YaM!=8H=QGW$NT2SeZlb8PKJdc=F-cT@j7Xags+Pr*jPtlHFnf- zh?q<6;)27IdPc^Wdy-mX%2s84C1xZq9Xms+==F4);O`VUASmu3(RlgE#0+#giLh-& zcxm3_e}n4{%|X zJp{G_j+%`j_q5}k{eW&TlP}J2wtZ2^<^E(O)4OQX8FDp6RJq!F{(6eHWSD3=f~(h} zJXCf7=r<16X{pHkm%yzYI_=VDP&9bmI1*)YXZeB}F? z(%QsB5fo*FUZxK$oX~X^69;x~j7ms8xlzpt-T15e9}$4T-pC z6PFg@;B-j|Ywajpe4~bk#S6(fO^|mm1hKOPfA%8-_iGCfICE|=P_~e;Wz6my&)h_~ zkv&_xSAw7AZ%ThYF(4jADW4vg=oEdJGVOs>FqamoL3Np8>?!W#!R-0%2Bg4h?kz5I zKV-rKN2n(vUL%D<4oj@|`eJ>0i#TmYBtYmfla;c!ATW%;xGQ0*TW@PTlGG><@dxUI zg>+3SiGdZ%?5N=8uoLA|$4isK$aJ%i{hECP$bK{J#0W2gQ3YEa zZQ50Stn6hqdfxJ*9#NuSLwKFCUGk@c=(igyVL;;2^wi4o30YXSIb2g_ud$ zgpCr@H0qWtk2hK8Q|&wx)}4+hTYlf;$a4#oUM=V@Cw#!$(nOFFpZ;0lc!qd=c$S}Z zGGI-0jg~S~cgVT=4Vo)b)|4phjStD49*EqC)IPwyeKBLcN;Wu@Aeph;emROAwJ-0< z_#>wVm$)ygH|qyxZaet&(Vf%pVdnvKWJn9`%DAxj3ot;v>S$I}jJ$FLBF*~iZ!ZXE zkvui&p}fI0Y=IDX)mm0@tAd|fEHl~J&K}ZX(Mm3cm1UAuwJ42+AO5@HwYfDH7ipIc zmI;1J;J@+aCNG1M`Btf>YT>~c&3j~Qi@Py5JT6;zjx$cvOQW@3oQ>|}GH?TW-E z1R;q^QFjm5W~7f}c3Ww|awg1BAJ^slEV~Pk`Kd`PS$7;SqJZNj->it4DW2l15}xP6 zoCl$kyEF%yJni0(L!Z&14m!1urXh6Btj_5JYt1{#+H8w?5QI%% zo-$KYWNMJVH?Hh@1n7OSu~QhSswL8x0=$<8QG_zepi_`y_79=nK=_ZP_`Em2UI*tyQoB+r{1QYZCpb?2OrgUw#oRH$?^Tj!Req>XiE#~B|~ z+%HB;=ic+R@px4Ld8mwpY;W^A%8%l8$@B@1m5n`TlKI6bz2mp*^^^1mK$COW$HOfp zUGTz-cN9?BGEp}5A!mDFjaiWa2_J2Iq8qj0mXzk; z66JBKRP{p%wN7XobR0YjhAuW9T1Gw3FDvR5dWJ8ElNYF94eF3ebu+QwKjtvVu4L zI9ip#mQ@4uqVdkl-TUQMb^XBJVLW(-$s;Nq;@5gr4`UfLgF$adIhd?rHOa%D);whv z=;krPp~@I+-Z|r#s3yCH+c1US?dnm+C*)r{m+86sTJusLdNu^sqLrfWed^ndHXH`m zd3#cOe3>w-ga(Dus_^ppG9AC>Iq{y%%CK+Cro_sqLCs{VLuK=dev>OL1dis4(PQ5R zcz)>DjEkfV+MO;~>VUlYF00SgfUo~@(&9$Iy2|G0T9BSP?&T22>K46D zL*~j#yJ?)^*%J3!16f)@Y2Z^kS*BzwfAQ7K96rFRIh>#$*$_Io;z>ux@}G98!fWR@ zGTFxv4r~v)Gsd|pF91*-eaZ3Qw1MH$K^7JhWIdX%o$2kCbvGDXy)a?@8T&1dY4`;L z4Kn+f%SSFWE_rpEpL9bnlmYq`D!6F%di<&Hh=+!VI~j)2mfil03T#jJ_s?}VV0_hp z7T9bWxc>Jm2Z0WMU?`Z$xE74Gu~%s{mW!d4uvKCx@WD+gPUQ zV0vQS(Ig++z=EHN)BR44*EDSWIyT~R4$FcF*VEY*8@l=218Q05D2$|fXKFhRgBIEE zdDFB}1dKkoO^7}{5crKX!p?dZWNz$m>1icsXG2N+((x0OIST9Zo^DW_tytvlwXGpn zs8?pJXjEG;T@qrZi%#h93?FP$!&P4JA(&H61tqQi=opRzNpm zkrG}$^t9&XduK*Qa1?355wd8G2CI6QEh@Ua>AsD;7oRUNLPb76m4HG3K?)wF~IyS3`fXuNM>${?wmB zpVz;?6_(Fiadfd{vUCBM*_kt$+F3J+IojI;9L(gc9n3{sEZyzR9o!_mOwFC#tQ{Q~ zP3-`#uK#tP3Q7~Q;4H|wjZHO8h7e4IuBxl&vz2w~D8)w=Wtg31zpZhz%+kzSzL*dV zwp@{WU4i;hJ7c2f1O;7Mz6qRKeASoIv0_bV=i@NMG*l<#+;INk-^`5w@}Dj~;k=|}qM1vq_P z|GpBGe_IKq|LNy9SJhKOQ$c=5L{Dv|Q_lZl=-ky*BFBJLW9&y_C|!vyM~rQx=!vun z?rZJQB5t}Dctmui5i31C_;_}CEn}_W%>oSXtt>@kE1=JW*4*v4tPp;O6 zmAk{)m!)}34pTWg8{i>($%NQ(Tl;QC@J@FfBoc%Gr&m560^kgSfodAFrIjF}aIw)X zoXZ`@IsMkc8_=w%-7`D6Y4e*CG8k%Ud=GXhsTR50jUnm+R*0A(O3UKFg0`K;qp1bl z7``HN=?39ic_kR|^R^~w-*pa?Vj#7|e9F1iRx{GN2?wK!xR1GW!qa=~pjJb-#u1K8 zeR?Y2i-pt}yJq;SCiVHODIvQJX|ZJaT8nO+(?HXbLefulKKgM^B(UIO1r+S=7;kLJ zcH}1J=Px2jsh3Tec&v8Jcbng8;V-`#*UHt?hB(pmOipKwf3Lz8rG$heEB30Sg*2rx zV<|KN86$soN(I!BwO`1n^^uF2*x&vJ$2d$>+`(romzHP|)K_KkO6Hc>_dwMW-M(#S zK(~SiXT1@fvc#U+?|?PniDRm01)f^#55;nhM|wi?oG>yBsa?~?^xTU|fX-R(sTA+5 zaq}-8Tx7zrOy#3*JLIIVsBmHYLdD}!0NP!+ITW+Thn0)8SS!$@)HXwB3tY!fMxc#1 zMp3H?q3eD?u&Njx4;KQ5G>32+GRp1Ee5qMO0lZjaRRu&{W<&~DoJNGkcYF<5(Ab+J zgO>VhBl{okDPn78<%&e2mR{jwVCz5Og;*Z;;3%VvoGo_;HaGLWYF7q#jDX=Z#Ml`H z858YVV$%J|e<1n`%6Vsvq7GmnAV0wW4$5qQ3uR@1i>tW{xrl|ExywIc?fNgYlA?C5 zh$ezAFb5{rQu6i7BSS5*J-|9DQ{6^BVQ{b*lq`xS@RyrsJN?-t=MTMPY;WYeKBCNg z^2|pN!Q^WPJuuO4!|P@jzt&tY1Y8d%FNK5xK(!@`jO2aEA*4 zkO6b|UVBipci?){-Ke=+1;mGlND8)6+P;8sq}UXw2hn;fc7nM>g}GSMWu&v&fqh

iViYT=fZ(|3Ox^$aWPp4a8h24tD<|8-!aK0lHgL$N7Efw}J zVIB!7=T$U`ao1?upi5V4Et*-lTG0XvExbf!ya{cua==$WJyVG(CmA6Of*8E@DSE%L z`V^$qz&RU$7G5mg;8;=#`@rRG`-uS18$0WPN@!v2d{H2sOqP|!(cQ@ zUHo!d>>yFArLPf1q`uBvY32miqShLT1B@gDL4XoVTK&@owOoD)OIHXrYK-a1d$B{v zF^}8D3Y^g%^cnvScOSJR5QNH+BI%d|;J;wWM3~l>${fb8DNPg)wrf|GBP8p%LNGN# z3EaIiItgwtGgT&iYCFy9-LG}bMI|4LdmmJt@V@% zb6B)1kc=T)(|L@0;wr<>=?r04N;E&ef+7C^`wPWtyQe(*pD1pI_&XHy|0gIGHMekd zF_*M4yi6J&Z4LQj65)S zXwdM{SwUo%3SbPwFsHgqF@V|6afT|R6?&S;lw=8% z3}@9B=#JI3@B*#4s!O))~z zc>2_4Q_#&+5V`GFd?88^;c1i7;Vv_I*qt!_Yx*n=;rj!82rrR2rQ8u5(Ejlo{15P% zs~!{%XJ>FmJ})H^I9bn^Re&38H{xA!0l3^89k(oU;bZWXM@kn$#aoS&Y4l^-WEn-fH39Jb9lA%s*WsKJQl?n9B7_~P z-XM&WL7Z!PcoF6_D>V@$CvUIEy=+Z&0kt{szMk=f1|M+r*a43^$$B^MidrT0J;RI` z(?f!O<8UZkm$_Ny$Hth1J#^4ni+im8M9mr&k|3cIgwvjAgjH z8`N&h25xV#v*d$qBX5jkI|xOhQn!>IYZK7l5#^P4M&twe9&Ey@@GxYMxBZq2e7?`q z$~Szs0!g{2fGcp9PZEt|rdQ6bhAgpcLHPz?f-vB?$dc*!9OL?Q8mn7->bFD2Si60* z!O%y)fCdMSV|lkF9w%x~J*A&srMyYY3{=&$}H zGQ4VG_?$2X(0|vT0{=;W$~icCI{b6W{B!Q8xdGhF|D{25G_5_+%s(46lhvNLkik~R z>nr(&C#5wwOzJZQo9m|U<;&Wk!_#q|V>fsmj1g<6%hB{jGoNUPjgJslld>xmODzGjYc?7JSuA?A_QzjDw5AsRgi@Y|Z0{F{!1=!NES-#*f^s4l0Hu zz468))2IY5dmD9pa*(yT5{EyP^G>@ZWumealS-*WeRcZ}B%gxq{MiJ|RyX-^C1V=0 z@iKdrGi1jTe8Ya^x7yyH$kBNvM4R~`fbPq$BzHum-3Zo8C6=KW@||>zsA8-Y9uV5V z#oq-f5L5}V<&wF4@X@<3^C%ptp6+Ce)~hGl`kwj)bsAjmo_GU^r940Z-|`<)oGnh7 zFF0Tde3>ui?8Yj{sF-Z@)yQd~CGZ*w-6p2U<8}JO-sRsVI5dBji`01W8A&3$?}lxBaC&vn0E$c5tW* zX>5(zzZ=qn&!J~KdsPl;P@bmA-Pr8T*)eh_+Dv5=Ma|XSle6t(k8qcgNyar{*ReQ8 zTXwi=8vr>!3Ywr+BhggHDw8ke==NTQVMCK`$69fhzEFB*4+H9LIvdt-#IbhZvpS}} zO3lz;P?zr0*0$%-Rq_y^k(?I{Mk}h@w}cZpMUp|ucs55bcloL2)($u%mXQw({Wzc~ z;6nu5MkjP)0C(@%6Q_I_vsWrfhl7Zpoxw#WoE~r&GOSCz;_ro6i(^hM>I$8y>`!wW z*U^@?B!MMmb89I}2(hcE4zN2G^kwyWCZp5JG>$Ez7zP~D=J^LMjSM)27_0B_X^C(M z`fFT+%DcKlu?^)FCK>QzSnV%IsXVcUFhFdBP!6~se&xxrIxsvySAWu++IrH;FbcY$ z2DWTvSBRfLwdhr0nMx+URA$j3i7_*6BWv#DXfym?ZRDcX9C?cY9sD3q)uBDR3uWg= z(lUIzB)G$Hr!){>E{s4Dew+tb9kvToZp-1&c?y2wn@Z~(VBhqz`cB;{E4(P3N2*nJ z_>~g@;UF2iG{Kt(<1PyePTKahF8<)pozZ*xH~U-kfoAayCwJViIrnqwqO}7{0pHw$ zs2Kx?s#vQr7XZ264>5RNKSL8|Ty^=PsIx^}QqOOcfpGUU4tRkUc|kc7-!Ae6!+B{o~7nFpm3|G5^=0#Bnm6`V}oSQlrX(u%OWnC zoLPy&Q;1Jui&7ST0~#+}I^&?vcE*t47~Xq#YwvA^6^} z`WkC)$AkNub|t@S!$8CBlwbV~?yp&@9h{D|3z-vJXgzRC5^nYm+PyPcgRzAnEi6Q^gslXYRv4nycsy-SJu?lMps-? zV`U*#WnFsdPLL)Q$AmD|0`UaC4ND07+&UmOu!eHruzV|OUox<+Jl|Mr@6~C`T@P%s zW7sgXLF2SSe9Fl^O(I*{9wsFSYb2l%-;&Pi^dpv!{)C3d0AlNY6!4fgmSgj_wQ*7Am7&$z;Jg&wgR-Ih;lUvWS|KTSg!&s_E9_bXBkZvGiC6bFKDWZxsD$*NZ#_8bl zG1P-#@?OQzED7@jlMJTH@V!6k;W>auvft)}g zhoV{7$q=*;=l{O>Q4a@ ziMjf_u*o^PsO)#BjC%0^h>Xp@;5$p{JSYDt)zbb}s{Kbt!T*I@Pk@X0zds6wsefuU zW$XY%yyRGC94=6mf?x+bbA5CDQ2AgW1T-jVAJbm7K(gp+;v6E0WI#kuACgV$r}6L? zd|Tj?^%^*N&b>Dd{Wr$FS2qI#Ucs1yd4N+RBUQiSZGujH`#I)mG&VKoDh=KKFl4=G z&MagXl6*<)$6P}*Tiebpz5L=oMaPrN+caUXRJ`D?=K9!e0f{@D&cZLKN?iNP@X0aF zE(^pl+;*T5qt?1jRC=5PMgV!XNITRLS_=9{CJExaQj;lt!&pdzpK?8p>%Mb+D z?yO*uSung=-`QQ@yX@Hyd4@CI^r{2oiu`%^bNkz+Nkk!IunjwNC|WcqvX~k=><-I3 zDQdbdb|!v+Iz01$w@aMl!R)koD77Xp;eZwzSl-AT zr@Vu{=xvgfq9akRrrM)}=!=xcs+U1JO}{t(avgz`6RqiiX<|hGG1pmop8k6Q+G_mv zJv|RfDheUp2L3=^C=4aCBMBn0aRCU(DQwX-W(RkRwmLeuJYF<0urcaf(=7)JPg<3P zQs!~G)9CT18o!J4{zX{_e}4eS)U-E)0FAt}wEI(c0%HkxgggW;(1E=>J17_hsH^sP z%lT0LGgbUXHx-K*CI-MCrP66UP0PvGqM$MkeLyqHdbgP|_Cm!7te~b8p+e6sQ_3k| zVcwTh6d83ltdnR>D^)BYQpDKlLk3g0Hdcgz2}%qUs9~~Rie)A-BV1mS&naYai#xcZ z(d{8=-LVpTp}2*y)|gR~;qc7fp26}lPcLZ#=JpYcn3AT9(UIdOyg+d(P5T7D&*P}# zQCYplZO5|7+r19%9e`v^vfSS1sbX1c%=w1;oyruXB%Kl$ACgKQ6=qNWLsc=28xJjg zwvsI5-%SGU|3p>&zXVl^vVtQT3o-#$UT9LI@Npz~6=4!>mc431VRNN8od&Ul^+G_kHC`G=6WVWM z%9eWNyy(FTO|A+@x}Ou3CH)oi;t#7rAxdIXfNFwOj_@Y&TGz6P_sqiB`Q6Lxy|Q{`|fgmRG(k+!#b*M+Z9zFce)f-7;?Km5O=LHV9f9_87; zF7%R2B+$?@sH&&-$@tzaPYkw0;=i|;vWdI|Wl3q_Zu>l;XdIw2FjV=;Mq5t1Q0|f< zs08j54Bp`3RzqE=2enlkZxmX6OF+@|2<)A^RNQpBd6o@OXl+i)zO%D4iGiQNuXd+zIR{_lb96{lc~bxsBveIw6umhShTX+3@ZJ=YHh@ zWY3(d0azg;7oHn>H<>?4@*RQbi>SmM=JrHvIG(~BrvI)#W(EAeO6fS+}mxxcc+X~W6&YVl86W9WFSS}Vz-f9vS?XUDBk)3TcF z8V?$4Q)`uKFq>xT=)Y9mMFVTUk*NIA!0$?RP6Ig0TBmUFrq*Q-Agq~DzxjStQyJ({ zBeZ;o5qUUKg=4Hypm|}>>L=XKsZ!F$yNTDO)jt4H0gdQ5$f|d&bnVCMMXhNh)~mN z@_UV6D7MVlsWz+zM+inZZp&P4fj=tm6fX)SG5H>OsQf_I8c~uGCig$GzuwViK54bcgL;VN|FnyQl>Ed7(@>=8$a_UKIz|V6CeVSd2(P z0Uu>A8A+muM%HLFJQ9UZ5c)BSAv_zH#1f02x?h9C}@pN@6{>UiAp>({Fn(T9Q8B z^`zB;kJ5b`>%dLm+Ol}ty!3;8f1XDSVX0AUe5P#@I+FQ-`$(a;zNgz)4x5hz$Hfbg z!Q(z26wHLXko(1`;(BAOg_wShpX0ixfWq3ponndY+u%1gyX)_h=v1zR#V}#q{au6; z!3K=7fQwnRfg6FXtNQmP>`<;!N137paFS%y?;lb1@BEdbvQHYC{976l`cLqn;b8lp zIDY>~m{gDj(wfnK!lpW6pli)HyLEiUrNc%eXTil|F2s(AY+LW5hkKb>TQ3|Q4S9rr zpDs4uK_co6XPsn_z$LeS{K4jFF`2>U`tbgKdyDne`xmR<@6AA+_hPNKCOR-Zqv;xk zu5!HsBUb^!4uJ7v0RuH-7?l?}b=w5lzzXJ~gZcxRKOovSk@|#V+MuX%Y+=;14i*%{)_gSW9(#4%)AV#3__kac1|qUy!uyP{>?U#5wYNq}y$S9pCc zFc~4mgSC*G~j0u#qqp9 z${>3HV~@->GqEhr_Xwoxq?Hjn#=s2;i~g^&Hn|aDKpA>Oc%HlW(KA1?BXqpxB;Ydx)w;2z^MpjJ(Qi(X!$5RC z*P{~%JGDQqojV>2JbEeCE*OEu!$XJ>bWA9Oa_Hd;y)F%MhBRi*LPcdqR8X`NQ&1L# z5#9L*@qxrx8n}LfeB^J{%-?SU{FCwiWyHp682F+|pa+CQa3ZLzBqN1{)h4d6+vBbV zC#NEbQLC;}me3eeYnOG*nXOJZEU$xLZ1<1Y=7r0(-U0P6-AqwMAM`a(Ed#7vJkn6plb4eI4?2y3yOTGmmDQ!z9`wzbf z_OY#0@5=bnep;MV0X_;;SJJWEf^E6Bd^tVJ9znWx&Ks8t*B>AM@?;D4oWUGc z!H*`6d7Cxo6VuyS4Eye&L1ZRhrRmN6Lr`{NL(wDbif|y&z)JN>Fl5#Wi&mMIr5i;x zBx}3YfF>>8EC(fYnmpu~)CYHuHCyr5*`ECap%t@y=jD>!_%3iiE|LN$mK9>- zHdtpy8fGZtkZF?%TW~29JIAfi2jZT8>OA7=h;8T{{k?c2`nCEx9$r zS+*&vt~2o^^J+}RDG@+9&M^K*z4p{5#IEVbz`1%`m5c2};aGt=V?~vIM}ZdPECDI)47|CWBCfDWUbxBCnmYivQ*0Nu_xb*C>~C9(VjHM zxe<*D<#dQ8TlpMX2c@M<9$w!RP$hpG4cs%AI){jp*Sj|*`m)5(Bw*A0$*i-(CA5#%>a)$+jI2C9r6|(>J8InryENI z$NohnxDUB;wAYDwrb*!N3noBTKPpPN}~09SEL18tkG zxgz(RYU_;DPT{l?Q$+eaZaxnsWCA^ds^0PVRkIM%bOd|G2IEBBiz{&^JtNsODs;5z zICt_Zj8wo^KT$7Bg4H+y!Df#3mbl%%?|EXe!&(Vmac1DJ*y~3+kRKAD=Ovde4^^%~ zw<9av18HLyrf*_>Slp;^i`Uy~`mvBjZ|?Ad63yQa#YK`4+c6;pW4?XIY9G1(Xh9WO8{F-Aju+nS9Vmv=$Ac0ienZ+p9*O%NG zMZKy5?%Z6TAJTE?o5vEr0r>f>hb#2w2U3DL64*au_@P!J!TL`oH2r*{>ffu6|A7tv zL4juf$DZ1MW5ZPsG!5)`k8d8c$J$o;%EIL0va9&GzWvkS%ZsGb#S(?{!UFOZ9<$a| zY|a+5kmD5N&{vRqkgY>aHsBT&`rg|&kezoD)gP0fsNYHsO#TRc_$n6Lf1Z{?+DLziXlHrq4sf(!>O{?Tj;Eh@%)+nRE_2VxbN&&%%caU#JDU%vL3}Cb zsb4AazPI{>8H&d=jUaZDS$-0^AxE@utGs;-Ez_F(qC9T=UZX=>ok2k2 ziTn{K?y~a5reD2A)P${NoI^>JXn>`IeArow(41c-Wm~)wiryEP(OS{YXWi7;%dG9v zI?mwu1MxD{yp_rrk!j^cKM)dc4@p4Ezyo%lRN|XyD}}>v=Xoib0gOcdXrQ^*61HNj z=NP|pd>@yfvr-=m{8$3A8TQGMTE7g=z!%yt`8`Bk-0MMwW~h^++;qyUP!J~ykh1GO z(FZ59xuFR$(WE;F@UUyE@Sp>`aVNjyj=Ty>_Vo}xf`e7`F;j-IgL5`1~-#70$9_=uBMq!2&1l zomRgpD58@)YYfvLtPW}{C5B35R;ZVvB<<#)x%srmc_S=A7F@DW8>QOEGwD6suhwCg z>Pa+YyULhmw%BA*4yjDp|2{!T98~<6Yfd(wo1mQ!KWwq0eg+6)o1>W~f~kL<-S+P@$wx*zeI|1t7z#Sxr5 zt6w+;YblPQNplq4Z#T$GLX#j6yldXAqj>4gAnnWtBICUnA&-dtnlh=t0Ho_vEKwV` z)DlJi#!@nkYV#$!)@>udAU*hF?V`2$Hf=V&6PP_|r#Iv*J$9)pF@X3`k;5})9^o4y z&)~?EjX5yX12O(BsFy-l6}nYeuKkiq`u9145&3Ssg^y{5G3Pse z9w(YVa0)N-fLaBq1`P!_#>SS(8fh_5!f{UrgZ~uEdeMJIz7DzI5!NHHqQtm~#CPij z?=N|J>nPR6_sL7!f4hD_|KH`vf8(Wpnj-(gPWH+ZvID}%?~68SwhPTC3u1_cB`otq z)U?6qo!ZLi5b>*KnYHWW=3F!p%h1;h{L&(Q&{qY6)_qxNfbP6E3yYpW!EO+IW3?@J z);4>g4gnl^8klu7uA>eGF6rIGSynacogr)KUwE_R4E5Xzi*Qir@b-jy55-JPC8c~( zo!W8y9OGZ&`xmc8;=4-U9=h{vCqfCNzYirONmGbRQlR`WWlgnY+1wCXbMz&NT~9*| z6@FrzP!LX&{no2!Ln_3|I==_4`@}V?4a;YZKTdw;vT<+K+z=uWbW(&bXEaWJ^W8Td z-3&1bY^Z*oM<=M}LVt>_j+p=2Iu7pZmbXrhQ_k)ysE9yXKygFNw$5hwDn(M>H+e1&9BM5!|81vd%r%vEm zqxY3?F@fb6O#5UunwgAHR9jp_W2zZ}NGp2%mTW@(hz7$^+a`A?mb8|_G*GNMJ) zjqegXQio=i@AINre&%ofexAr95aop5C+0MZ0m-l=MeO8m3epm7U%vZB8+I+C*iNFM z#T3l`gknX;D$-`2XT^Cg*vrv=RH+P;_dfF++cP?B_msQI4j+lt&rX2)3GaJx%W*Nn zkML%D{z5tpHH=dksQ*gzc|}gzW;lwAbxoR07VNgS*-c3d&8J|;@3t^ zVUz*J*&r7DFRuFVDCJDK8V9NN5hvpgGjwx+5n)qa;YCKe8TKtdnh{I7NU9BCN!0dq zczrBk8pE{{@vJa9ywR@mq*J=v+PG;?fwqlJVhijG!3VmIKs>9T6r7MJpC)m!Tc#>g zMtVsU>wbwFJEfwZ{vB|ZlttNe83)$iz`~#8UJ^r)lJ@HA&G#}W&ZH*;k{=TavpjWE z7hdyLZPf*X%Gm}i`Y{OGeeu^~nB8=`{r#TUrM-`;1cBvEd#d!kPqIgYySYhN-*1;L z^byj%Yi}Gx)Wnkosi337BKs}+5H5dth1JA{Ir-JKN$7zC)*}hqeoD(WfaUDPT>0`- z(6sa0AoIqASwF`>hP}^|)a_j2s^PQn*qVC{Q}htR z5-)duBFXT_V56-+UohKXlq~^6uf!6sA#ttk1o~*QEy_Y-S$gAvq47J9Vtk$5oA$Ct zYhYJ@8{hsC^98${!#Ho?4y5MCa7iGnfz}b9jE~h%EAAv~Qxu)_rAV;^cygV~5r_~?l=B`zObj7S=H=~$W zPtI_m%g$`kL_fVUk9J@>EiBH zOO&jtn~&`hIFMS5S`g8w94R4H40mdNUH4W@@XQk1sr17b{@y|JB*G9z1|CrQjd+GX z6+KyURG3;!*BQrentw{B2R&@2&`2}n(z-2&X7#r!{yg@Soy}cRD~j zj9@UBW+N|4HW4AWapy4wfUI- zZ`gSL6DUlgj*f1hSOGXG0IVH8HxK?o2|3HZ;KW{K+yPAlxtb)NV_2AwJm|E)FRs&& z=c^e7bvUsztY|+f^k7NXs$o1EUq>cR7C0$UKi6IooHWlK_#?IWDkvywnzg&ThWo^? z2O_N{5X39#?eV9l)xI(>@!vSB{DLt*oY!K1R8}_?%+0^C{d9a%N4 zoxHVT1&Lm|uDX%$QrBun5e-F`HJ^T$ zmzv)p@4ZHd_w9!%Hf9UYNvGCw2TTTbrj9pl+T9%-_-}L(tES>Or-}Z4F*{##n3~L~TuxjirGuIY#H7{%$E${?p{Q01 zi6T`n;rbK1yIB9jmQNycD~yZq&mbIsFWHo|ZAChSFPQa<(%d8mGw*V3fh|yFoxOOiWJd(qvVb!Z$b88cg->N=qO*4k~6;R==|9ihg&riu#P~s4Oap9O7f%crSr^rljeIfXDEg>wi)&v*a%7zpz<9w z*r!3q9J|390x`Zk;g$&OeN&ctp)VKRpDSV@kU2Q>jtok($Y-*x8_$2piTxun81@vt z!Vj?COa0fg2RPXMSIo26T=~0d`{oGP*eV+$!0I<(4azk&Vj3SiG=Q!6mX0p$z7I}; z9BJUFgT-K9MQQ-0@Z=^7R<{bn2Fm48endsSs`V7_@%8?Bxkqv>BDoVcj?K#dV#uUP zL1ND~?D-|VGKe3Rw_7-Idpht>H6XRLh*U7epS6byiGvJpr%d}XwfusjH9g;Z98H`x zyde%%5mhGOiL4wljCaWCk-&uE4_OOccb9c!ZaWt4B(wYl!?vyzl%7n~QepN&eFUrw zFIOl9c({``6~QD+43*_tzP{f2x41h(?b43^y6=iwyB)2os5hBE!@YUS5?N_tXd=h( z)WE286Fbd>R4M^P{!G)f;h<3Q>Fipuy+d2q-)!RyTgt;wr$(?9ox3;q+{E*ZQHhOn;lM`cjnu9 zXa48ks-v(~b*;MAI<>YZH(^NV8vjb34beE<_cwKlJoR;k6lJNSP6v}uiyRD?|0w+X@o1ONrH8a$fCxXpf? z?$DL0)7|X}Oc%h^zrMKWc-NS9I0Utu@>*j}b@tJ=ixQSJ={4@854wzW@E>VSL+Y{i z#0b=WpbCZS>kUCO_iQz)LoE>P5LIG-hv9E+oG}DtlIDF>$tJ1aw9^LuhLEHt?BCj& z(O4I8v1s#HUi5A>nIS-JK{v!7dJx)^Yg%XjNmlkWAq2*cv#tHgz`Y(bETc6CuO1VkN^L-L3j_x<4NqYb5rzrLC-7uOv z!5e`GZt%B782C5-fGnn*GhDF$%(qP<74Z}3xx+{$4cYKy2ikxI7B2N+2r07DN;|-T->nU&!=Cm#rZt%O_5c&1Z%nlWq3TKAW0w zQqemZw_ue--2uKQsx+niCUou?HjD`xhEjjQd3%rrBi82crq*~#uA4+>vR<_S{~5ce z-2EIl?~s z1=GVL{NxP1N3%=AOaC}j_Fv=ur&THz zyO!d9kHq|c73kpq`$+t+8Bw7MgeR5~`d7ChYyGCBWSteTB>8WAU(NPYt2Dk`@#+}= zI4SvLlyk#pBgVigEe`?NG*vl7V6m+<}%FwPV=~PvvA)=#ths==DRTDEYh4V5}Cf$z@#;< zyWfLY_5sP$gc3LLl2x+Ii)#b2nhNXJ{R~vk`s5U7Nyu^3yFg&D%Txwj6QezMX`V(x z=C`{76*mNb!qHHs)#GgGZ_7|vkt9izl_&PBrsu@}L`X{95-2jf99K)0=*N)VxBX2q z((vkpP2RneSIiIUEnGb?VqbMb=Zia+rF~+iqslydE34cSLJ&BJW^3knX@M;t*b=EA zNvGzv41Ld_T+WT#XjDB840vovUU^FtN_)G}7v)1lPetgpEK9YS^OWFkPoE{ovj^=@ zO9N$S=G$1ecndT_=5ehth2Lmd1II-PuT~C9`XVePw$y8J#dpZ?Tss<6wtVglm(Ok7 z3?^oi@pPio6l&!z8JY(pJvG=*pI?GIOu}e^EB6QYk$#FJQ%^AIK$I4epJ+9t?KjqA+bkj&PQ*|vLttme+`9G=L% ziadyMw_7-M)hS(3E$QGNCu|o23|%O+VN7;Qggp?PB3K-iSeBa2b}V4_wY`G1Jsfz4 z9|SdB^;|I8E8gWqHKx!vj_@SMY^hLEIbSMCuE?WKq=c2mJK z8LoG-pnY!uhqFv&L?yEuxo{dpMTsmCn)95xanqBrNPTgXP((H$9N${Ow~Is-FBg%h z53;|Y5$MUN)9W2HBe2TD`ct^LHI<(xWrw}$qSoei?}s)&w$;&!14w6B6>Yr6Y8b)S z0r71`WmAvJJ`1h&poLftLUS6Ir zC$bG9!Im_4Zjse)#K=oJM9mHW1{%l8sz$1o?ltdKlLTxWWPB>Vk22czVt|1%^wnN@*!l)}?EgtvhC>vlHm^t+ogpgHI1_$1ox9e;>0!+b(tBrmXRB`PY1vp-R**8N7 zGP|QqI$m(Rdu#=(?!(N}G9QhQ%o!aXE=aN{&wtGP8|_qh+7a_j_sU5|J^)vxq;# zjvzLn%_QPHZZIWu1&mRAj;Sa_97p_lLq_{~j!M9N^1yp3U_SxRqK&JnR%6VI#^E12 z>CdOVI^_9aPK2eZ4h&^{pQs}xsijXgFYRIxJ~N7&BB9jUR1fm!(xl)mvy|3e6-B3j zJn#ajL;bFTYJ2+Q)tDjx=3IklO@Q+FFM}6UJr6km7hj7th9n_&JR7fnqC!hTZoM~T zBeaVFp%)0cbPhejX<8pf5HyRUj2>aXnXBqDJe73~J%P(2C?-RT{c3NjE`)om! zl$uewSgWkE66$Kb34+QZZvRn`fob~Cl9=cRk@Es}KQm=?E~CE%spXaMO6YmrMl%9Q zlA3Q$3|L1QJ4?->UjT&CBd!~ru{Ih^in&JXO=|<6J!&qp zRe*OZ*cj5bHYlz!!~iEKcuE|;U4vN1rk$xq6>bUWD*u(V@8sG^7>kVuo(QL@Ki;yL zWC!FT(q{E8#on>%1iAS0HMZDJg{Z{^!De(vSIq&;1$+b)oRMwA3nc3mdTSG#3uYO_ z>+x;7p4I;uHz?ZB>dA-BKl+t-3IB!jBRgdvAbW!aJ(Q{aT>+iz?91`C-xbe)IBoND z9_Xth{6?(y3rddwY$GD65IT#f3<(0o#`di{sh2gm{dw*#-Vnc3r=4==&PU^hCv$qd zjw;>i&?L*Wq#TxG$mFIUf>eK+170KG;~+o&1;Tom9}}mKo23KwdEM6UonXgc z!6N(@k8q@HPw{O8O!lAyi{rZv|DpgfU{py+j(X_cwpKqcalcqKIr0kM^%Br3SdeD> zHSKV94Yxw;pjzDHo!Q?8^0bb%L|wC;4U^9I#pd5O&eexX+Im{ z?jKnCcsE|H?{uGMqVie_C~w7GX)kYGWAg%-?8|N_1#W-|4F)3YTDC+QSq1s!DnOML3@d`mG%o2YbYd#jww|jD$gotpa)kntakp#K;+yo-_ZF9qrNZw<%#C zuPE@#3RocLgPyiBZ+R_-FJ_$xP!RzWm|aN)S+{$LY9vvN+IW~Kf3TsEIvP+B9Mtm! zpfNNxObWQpLoaO&cJh5>%slZnHl_Q~(-Tfh!DMz(dTWld@LG1VRF`9`DYKhyNv z2pU|UZ$#_yUx_B_|MxUq^glT}O5Xt(Vm4Mr02><%C)@v;vPb@pT$*yzJ4aPc_FZ3z z3}PLoMBIM>q_9U2rl^sGhk1VUJ89=*?7|v`{!Z{6bqFMq(mYiA?%KbsI~JwuqVA9$H5vDE+VocjX+G^%bieqx->s;XWlKcuv(s%y%D5Xbc9+ zc(_2nYS1&^yL*ey664&4`IoOeDIig}y-E~_GS?m;D!xv5-xwz+G`5l6V+}CpeJDi^ z%4ed$qowm88=iYG+(`ld5Uh&>Dgs4uPHSJ^TngXP_V6fPyl~>2bhi20QB%lSd#yYn zO05?KT1z@?^-bqO8Cg`;ft>ilejsw@2%RR7;`$Vs;FmO(Yr3Fp`pHGr@P2hC%QcA|X&N2Dn zYf`MqXdHi%cGR@%y7Rg7?d3?an){s$zA{!H;Ie5exE#c~@NhQUFG8V=SQh%UxUeiV zd7#UcYqD=lk-}sEwlpu&H^T_V0{#G?lZMxL7ih_&{(g)MWBnCZxtXg znr#}>U^6!jA%e}@Gj49LWG@*&t0V>Cxc3?oO7LSG%~)Y5}f7vqUUnQ;STjdDU}P9IF9d9<$;=QaXc zL1^X7>fa^jHBu_}9}J~#-oz3Oq^JmGR#?GO7b9a(=R@fw@}Q{{@`Wy1vIQ#Bw?>@X z-_RGG@wt|%u`XUc%W{J z>iSeiz8C3H7@St3mOr_mU+&bL#Uif;+Xw-aZdNYUpdf>Rvu0i0t6k*}vwU`XNO2he z%miH|1tQ8~ZK!zmL&wa3E;l?!!XzgV#%PMVU!0xrDsNNZUWKlbiOjzH-1Uoxm8E#r`#2Sz;-o&qcqB zC-O_R{QGuynW14@)7&@yw1U}uP(1cov)twxeLus0s|7ayrtT8c#`&2~Fiu2=R;1_4bCaD=*E@cYI>7YSnt)nQc zohw5CsK%m?8Ack)qNx`W0_v$5S}nO|(V|RZKBD+btO?JXe|~^Qqur%@eO~<8-L^9d z=GA3-V14ng9L29~XJ>a5k~xT2152zLhM*@zlp2P5Eu}bywkcqR;ISbas&#T#;HZSf z2m69qTV(V@EkY(1Dk3`}j)JMo%ZVJ*5eB zYOjIisi+igK0#yW*gBGj?@I{~mUOvRFQR^pJbEbzFxTubnrw(Muk%}jI+vXmJ;{Q6 zrSobKD>T%}jV4Ub?L1+MGOD~0Ir%-`iTnWZN^~YPrcP5y3VMAzQ+&en^VzKEb$K!Q z<7Dbg&DNXuow*eD5yMr+#08nF!;%4vGrJI++5HdCFcGLfMW!KS*Oi@=7hFwDG!h2< zPunUEAF+HncQkbfFj&pbzp|MU*~60Z(|Ik%Tn{BXMN!hZOosNIseT?R;A`W?=d?5X zK(FB=9mZusYahp|K-wyb={rOpdn=@;4YI2W0EcbMKyo~-#^?h`BA9~o285%oY zfifCh5Lk$SY@|2A@a!T2V+{^!psQkx4?x0HSV`(w9{l75QxMk!)U52Lbhn{8ol?S) zCKo*7R(z!uk<6*qO=wh!Pul{(qq6g6xW;X68GI_CXp`XwO zxuSgPRAtM8K7}5E#-GM!*ydOOG_{A{)hkCII<|2=ma*71ci_-}VPARm3crFQjLYV! z9zbz82$|l01mv`$WahE2$=fAGWkd^X2kY(J7iz}WGS z@%MyBEO=A?HB9=^?nX`@nh;7;laAjs+fbo!|K^mE!tOB>$2a_O0y-*uaIn8k^6Y zSbuv;5~##*4Y~+y7Z5O*3w4qgI5V^17u*ZeupVGH^nM&$qmAk|anf*>r zWc5CV;-JY-Z@Uq1Irpb^O`L_7AGiqd*YpGUShb==os$uN3yYvb`wm6d=?T*it&pDk zo`vhw)RZX|91^^Wa_ti2zBFyWy4cJu#g)_S6~jT}CC{DJ_kKpT`$oAL%b^!2M;JgT zM3ZNbUB?}kP(*YYvXDIH8^7LUxz5oE%kMhF!rnPqv!GiY0o}NR$OD=ITDo9r%4E>E0Y^R(rS^~XjWyVI6 zMOR5rPXhTp*G*M&X#NTL`Hu*R+u*QNoiOKg4CtNPrjgH>c?Hi4MUG#I917fx**+pJfOo!zFM&*da&G_x)L(`k&TPI*t3e^{crd zX<4I$5nBQ8Ax_lmNRa~E*zS-R0sxkz`|>7q_?*e%7bxqNm3_eRG#1ae3gtV9!fQpY z+!^a38o4ZGy9!J5sylDxZTx$JmG!wg7;>&5H1)>f4dXj;B+@6tMlL=)cLl={jLMxY zbbf1ax3S4>bwB9-$;SN2?+GULu;UA-35;VY*^9Blx)Jwyb$=U!D>HhB&=jSsd^6yw zL)?a|>GxU!W}ocTC(?-%z3!IUhw^uzc`Vz_g>-tv)(XA#JK^)ZnC|l1`@CdX1@|!| z_9gQ)7uOf?cR@KDp97*>6X|;t@Y`k_N@)aH7gY27)COv^P3ya9I{4z~vUjLR9~z1Z z5=G{mVtKH*&$*t0@}-i_v|3B$AHHYale7>E+jP`ClqG%L{u;*ff_h@)al?RuL7tOO z->;I}>%WI{;vbLP3VIQ^iA$4wl6@0sDj|~112Y4OFjMs`13!$JGkp%b&E8QzJw_L5 zOnw9joc0^;O%OpF$Qp)W1HI!$4BaXX84`%@#^dk^hFp^pQ@rx4g(8Xjy#!X%+X5Jd@fs3amGT`}mhq#L97R>OwT5-m|h#yT_-v@(k$q7P*9X~T*3)LTdzP!*B} z+SldbVWrrwQo9wX*%FyK+sRXTa@O?WM^FGWOE?S`R(0P{<6p#f?0NJvnBia?k^fX2 zNQs7K-?EijgHJY}&zsr;qJ<*PCZUd*x|dD=IQPUK_nn)@X4KWtqoJNHkT?ZWL_hF? zS8lp2(q>;RXR|F;1O}EE#}gCrY~#n^O`_I&?&z5~7N;zL0)3Tup`%)oHMK-^r$NT% zbFg|o?b9w(q@)6w5V%si<$!U<#}s#x@0aX-hP>zwS#9*75VXA4K*%gUc>+yzupTDBOKH8WR4V0pM(HrfbQ&eJ79>HdCvE=F z|J>s;;iDLB^3(9}?biKbxf1$lI!*Z%*0&8UUq}wMyPs_hclyQQi4;NUY+x2qy|0J; zhn8;5)4ED1oHwg+VZF|80<4MrL97tGGXc5Sw$wAI#|2*cvQ=jB5+{AjMiDHmhUC*a zlmiZ`LAuAn_}hftXh;`Kq0zblDk8?O-`tnilIh|;3lZp@F_osJUV9`*R29M?7H{Fy z`nfVEIDIWXmU&YW;NjU8)EJpXhxe5t+scf|VXM!^bBlwNh)~7|3?fWwo_~ZFk(22% zTMesYw+LNx3J-_|DM~`v93yXe=jPD{q;li;5PD?Dyk+b? zo21|XpT@)$BM$%F=P9J19Vi&1#{jM3!^Y&fr&_`toi`XB1!n>sbL%U9I5<7!@?t)~ z;&H%z>bAaQ4f$wIzkjH70;<8tpUoxzKrPhn#IQfS%9l5=Iu))^XC<58D!-O z{B+o5R^Z21H0T9JQ5gNJnqh#qH^na|z92=hONIM~@_iuOi|F>jBh-?aA20}Qx~EpDGElELNn~|7WRXRFnw+Wdo`|# zBpU=Cz3z%cUJ0mx_1($X<40XEIYz(`noWeO+x#yb_pwj6)R(__%@_Cf>txOQ74wSJ z0#F3(zWWaR-jMEY$7C*3HJrohc79>MCUu26mfYN)f4M~4gD`}EX4e}A!U}QV8!S47 z6y-U-%+h`1n`*pQuKE%Av0@)+wBZr9mH}@vH@i{v(m-6QK7Ncf17x_D=)32`FOjjo zg|^VPf5c6-!FxN{25dvVh#fog=NNpXz zfB$o+0jbRkHH{!TKhE709f+jI^$3#v1Nmf80w`@7-5$1Iv_`)W^px8P-({xwb;D0y z7LKDAHgX<84?l!I*Dvi2#D@oAE^J|g$3!)x1Ua;_;<@#l1fD}lqU2_tS^6Ht$1Wl} zBESo7o^)9-Tjuz$8YQSGhfs{BQV6zW7dA?0b(Dbt=UnQs&4zHfe_sj{RJ4uS-vQpC zX;Bbsuju4%!o8?&m4UZU@~ZZjeFF6ex2ss5_60_JS_|iNc+R0GIjH1@Z z=rLT9%B|WWgOrR7IiIwr2=T;Ne?30M!@{%Qf8o`!>=s<2CBpCK_TWc(DX51>e^xh8 z&@$^b6CgOd7KXQV&Y4%}_#uN*mbanXq(2=Nj`L7H7*k(6F8s6{FOw@(DzU`4-*77{ zF+dxpv}%mFpYK?>N_2*#Y?oB*qEKB}VoQ@bzm>ptmVS_EC(#}Lxxx730trt0G)#$b zE=wVvtqOct1%*9}U{q<)2?{+0TzZzP0jgf9*)arV)*e!f`|jgT{7_9iS@e)recI#z zbzolURQ+TOzE!ymqvBY7+5NnAbWxvMLsLTwEbFqW=CPyCsmJ}P1^V30|D5E|p3BC5 z)3|qgw@ra7aXb-wsa|l^in~1_fm{7bS9jhVRkYVO#U{qMp z)Wce+|DJ}4<2gp8r0_xfZpMo#{Hl2MfjLcZdRB9(B(A(f;+4s*FxV{1F|4d`*sRNd zp4#@sEY|?^FIJ;tmH{@keZ$P(sLh5IdOk@k^0uB^BWr@pk6mHy$qf&~rI>P*a;h0C{%oA*i!VjWn&D~O#MxN&f@1Po# zKN+ zrGrkSjcr?^R#nGl<#Q722^wbYcgW@{+6CBS<1@%dPA8HC!~a`jTz<`g_l5N1M@9wn9GOAZ>nqNgq!yOCbZ@1z`U_N`Z>}+1HIZxk*5RDc&rd5{3qjRh8QmT$VyS;jK z;AF+r6XnnCp=wQYoG|rT2@8&IvKq*IB_WvS%nt%e{MCFm`&W*#LXc|HrD?nVBo=(8*=Aq?u$sDA_sC_RPDUiQ+wnIJET8vx$&fxkW~kP9qXKt zozR)@xGC!P)CTkjeWvXW5&@2?)qt)jiYWWBU?AUtzAN}{JE1I)dfz~7$;}~BmQF`k zpn11qmObXwRB8&rnEG*#4Xax3XBkKlw(;tb?Np^i+H8m(Wyz9k{~ogba@laiEk;2! zV*QV^6g6(QG%vX5Um#^sT&_e`B1pBW5yVth~xUs#0}nv?~C#l?W+9Lsb_5)!71rirGvY zTIJ$OPOY516Y|_014sNv+Z8cc5t_V=i>lWV=vNu#!58y9Zl&GsMEW#pPYPYGHQ|;vFvd*9eM==$_=vc7xnyz0~ zY}r??$<`wAO?JQk@?RGvkWVJlq2dk9vB(yV^vm{=NVI8dhsX<)O(#nr9YD?I?(VmQ z^r7VfUBn<~p3()8yOBjm$#KWx!5hRW)5Jl7wY@ky9lNM^jaT##8QGVsYeaVywmpv>X|Xj7gWE1Ezai&wVLt3p)k4w~yrskT-!PR!kiyQlaxl(( zXhF%Q9x}1TMt3~u@|#wWm-Vq?ZerK={8@~&@9r5JW}r#45#rWii};t`{5#&3$W)|@ zbAf2yDNe0q}NEUvq_Quq3cTjcw z@H_;$hu&xllCI9CFDLuScEMg|x{S7GdV8<&Mq=ezDnRZAyX-8gv97YTm0bg=d)(>N z+B2FcqvI9>jGtnK%eO%y zoBPkJTk%y`8TLf4)IXPBn`U|9>O~WL2C~C$z~9|0m*YH<-vg2CD^SX#&)B4ngOSG$ zV^wmy_iQk>dfN@Pv(ckfy&#ak@MLC7&Q6Ro#!ezM*VEh`+b3Jt%m(^T&p&WJ2Oqvj zs-4nq0TW6cv~(YI$n0UkfwN}kg3_fp?(ijSV#tR9L0}l2qjc7W?i*q01=St0eZ=4h zyGQbEw`9OEH>NMuIe)hVwYHsGERWOD;JxEiO7cQv%pFCeR+IyhwQ|y@&^24k+|8fD zLiOWFNJ2&vu2&`Jv96_z-Cd5RLgmeY3*4rDOQo?Jm`;I_(+ejsPM03!ly!*Cu}Cco zrQSrEDHNyzT(D5s1rZq!8#?f6@v6dB7a-aWs(Qk>N?UGAo{gytlh$%_IhyL7h?DLXDGx zgxGEBQoCAWo-$LRvM=F5MTle`M})t3vVv;2j0HZY&G z22^iGhV@uaJh(XyyY%} zd4iH_UfdV#T=3n}(Lj^|n;O4|$;xhu*8T3hR1mc_A}fK}jfZ7LX~*n5+`8N2q#rI$ z@<_2VANlYF$vIH$ zl<)+*tIWW78IIINA7Rr7i{<;#^yzxoLNkXL)eSs=%|P>$YQIh+ea_3k z_s7r4%j7%&*NHSl?R4k%1>Z=M9o#zxY!n8sL5>BO-ZP;T3Gut>iLS@U%IBrX6BA3k z)&@q}V8a{X<5B}K5s(c(LQ=%v1ocr`t$EqqY0EqVjr65usa=0bkf|O#ky{j3)WBR(((L^wmyHRzoWuL2~WTC=`yZ zn%VX`L=|Ok0v7?s>IHg?yArBcync5rG#^+u)>a%qjES%dRZoIyA8gQ;StH z1Ao7{<&}6U=5}4v<)1T7t!J_CL%U}CKNs-0xWoTTeqj{5{?Be$L0_tk>M9o8 zo371}S#30rKZFM{`H_(L`EM9DGp+Mifk&IP|C2Zu_)Ghr4Qtpmkm1osCf@%Z$%t+7 zYH$Cr)Ro@3-QDeQJ8m+x6%;?YYT;k6Z0E-?kr>x33`H%*ueBD7Zx~3&HtWn0?2Wt} zTG}*|v?{$ajzt}xPzV%lL1t-URi8*Zn)YljXNGDb>;!905Td|mpa@mHjIH%VIiGx- zd@MqhpYFu4_?y5N4xiHn3vX&|e6r~Xt> zZG`aGq|yTNjv;9E+Txuoa@A(9V7g?1_T5FzRI;!=NP1Kqou1z5?%X~Wwb{trRfd>i z8&y^H)8YnKyA_Fyx>}RNmQIczT?w2J4SNvI{5J&}Wto|8FR(W;Qw#b1G<1%#tmYzQ zQ2mZA-PAdi%RQOhkHy9Ea#TPSw?WxwL@H@cbkZwIq0B!@ns}niALidmn&W?!Vd4Gj zO7FiuV4*6Mr^2xlFSvM;Cp_#r8UaqIzHJQg_z^rEJw&OMm_8NGAY2)rKvki|o1bH~ z$2IbfVeY2L(^*rMRU1lM5Y_sgrDS`Z??nR2lX;zyR=c%UyGb*%TC-Dil?SihkjrQy~TMv6;BMs7P8il`H7DmpVm@rJ;b)hW)BL)GjS154b*xq-NXq2cwE z^;VP7ua2pxvCmxrnqUYQMH%a%nHmwmI33nJM(>4LznvY*k&C0{8f*%?zggpDgkuz&JBx{9mfb@wegEl2v!=}Sq2Gaty0<)UrOT0{MZtZ~j5y&w zXlYa_jY)I_+VA-^#mEox#+G>UgvM!Ac8zI<%JRXM_73Q!#i3O|)lOP*qBeJG#BST0 zqohi)O!|$|2SeJQo(w6w7%*92S})XfnhrH_Z8qe!G5>CglP=nI7JAOW?(Z29;pXJ9 zR9`KzQ=WEhy*)WH>$;7Cdz|>*i>=##0bB)oU0OR>>N<21e4rMCHDemNi2LD>Nc$;& zQRFthpWniC1J6@Zh~iJCoLOxN`oCKD5Q4r%ynwgUKPlIEd#?QViIqovY|czyK8>6B zSP%{2-<;%;1`#0mG^B(8KbtXF;Nf>K#Di72UWE4gQ%(_26Koiad)q$xRL~?pN71ZZ zujaaCx~jXjygw;rI!WB=xrOJO6HJ!!w}7eiivtCg5K|F6$EXa)=xUC za^JXSX98W`7g-tm@uo|BKj39Dl;sg5ta;4qjo^pCh~{-HdLl6qI9Ix6f$+qiZ$}s= zNguKrU;u+T@ko(Vr1>)Q%h$?UKXCY>3se%&;h2osl2D zE4A9bd7_|^njDd)6cI*FupHpE3){4NQ*$k*cOWZ_?CZ>Z4_fl@n(mMnYK62Q1d@+I zr&O))G4hMihgBqRIAJkLdk(p(D~X{-oBUA+If@B}j& zsHbeJ3RzTq96lB7d($h$xTeZ^gP0c{t!Y0c)aQE;$FY2!mACg!GDEMKXFOPI^)nHZ z`aSPJpvV0|bbrzhWWkuPURlDeN%VT8tndV8?d)eN*i4I@u zVKl^6{?}A?P)Fsy?3oi#clf}L18t;TjNI2>eI&(ezDK7RyqFxcv%>?oxUlonv(px) z$vnPzRH`y5A(x!yOIfL0bmgeMQB$H5wenx~!ujQK*nUBW;@Em&6Xv2%s(~H5WcU2R z;%Nw<$tI)a`Ve!>x+qegJnQsN2N7HaKzrFqM>`6R*gvh%O*-%THt zrB$Nk;lE;z{s{r^PPm5qz(&lM{sO*g+W{sK+m3M_z=4=&CC>T`{X}1Vg2PEfSj2x_ zmT*(x;ov%3F?qoEeeM>dUn$a*?SIGyO8m806J1W1o+4HRhc2`9$s6hM#qAm zChQ87b~GEw{ADfs+5}FJ8+|bIlIv(jT$Ap#hSHoXdd9#w<#cA<1Rkq^*EEkknUd4& zoIWIY)sAswy6fSERVm&!SO~#iN$OgOX*{9@_BWFyJTvC%S++ilSfCrO(?u=Dc?CXZ zzCG&0yVR{Z`|ZF0eEApWEo#s9osV>F{uK{QA@BES#&;#KsScf>y zvs?vIbI>VrT<*!;XmQS=bhq%46-aambZ(8KU-wOO2=en~D}MCToB_u;Yz{)1ySrPZ z@=$}EvjTdzTWU7c0ZI6L8=yP+YRD_eMMos}b5vY^S*~VZysrkq<`cK3>>v%uy7jgq z0ilW9KjVDHLv0b<1K_`1IkbTOINs0=m-22c%M~l=^S}%hbli-3?BnNq?b`hx^HX2J zIe6ECljRL0uBWb`%{EA=%!i^4sMcj+U_TaTZRb+~GOk z^ZW!nky0n*Wb*r+Q|9H@ml@Z5gU&W`(z4-j!OzC1wOke`TRAYGZVl$PmQ16{3196( zO*?`--I}Qf(2HIwb2&1FB^!faPA2=sLg(@6P4mN)>Dc3i(B0;@O-y2;lM4akD>@^v z=u>*|!s&9zem70g7zfw9FXl1bpJW(C#5w#uy5!V?Q(U35A~$dR%LDVnq@}kQm13{} zd53q3N(s$Eu{R}k2esbftfjfOITCL;jWa$}(mmm}d(&7JZ6d3%IABCapFFYjdEjdK z&4Edqf$G^MNAtL=uCDRs&Fu@FXRgX{*0<(@c3|PNHa>L%zvxWS={L8%qw`STm+=Rd zA}FLspESSIpE_^41~#5yI2bJ=9`oc;GIL!JuW&7YetZ?0H}$$%8rW@*J37L-~Rsx!)8($nI4 zZhcZ2^=Y+p4YPl%j!nFJA|*M^gc(0o$i3nlphe+~-_m}jVkRN{spFs(o0ajW@f3K{ zDV!#BwL322CET$}Y}^0ixYj2w>&Xh12|R8&yEw|wLDvF!lZ#dOTHM9pK6@Nm-@9Lnng4ZHBgBSrr7KI8YCC9DX5Kg|`HsiwJHg2(7#nS;A{b3tVO?Z% za{m5b3rFV6EpX;=;n#wltDv1LE*|g5pQ+OY&*6qCJZc5oDS6Z6JD#6F)bWxZSF@q% z+1WV;m!lRB!n^PC>RgQCI#D1br_o^#iPk>;K2hB~0^<~)?p}LG%kigm@moD#q3PE+ zA^Qca)(xnqw6x>XFhV6ku9r$E>bWNrVH9fum0?4s?Rn2LG{Vm_+QJHse6xa%nzQ?k zKug4PW~#Gtb;#5+9!QBgyB@q=sk9=$S{4T>wjFICStOM?__fr+Kei1 z3j~xPqW;W@YkiUM;HngG!;>@AITg}vAE`M2Pj9Irl4w1fo4w<|Bu!%rh%a(Ai^Zhi zs92>v5;@Y(Zi#RI*ua*h`d_7;byQSa*v9E{2x$<-_=5Z<7{%)}4XExANcz@rK69T0x3%H<@frW>RA8^swA+^a(FxK| zFl3LD*ImHN=XDUkrRhp6RY5$rQ{bRgSO*(vEHYV)3Mo6Jy3puiLmU&g82p{qr0F?ohmbz)f2r{X2|T2 z$4fdQ=>0BeKbiVM!e-lIIs8wVTuC_m7}y4A_%ikI;Wm5$9j(^Y z(cD%U%k)X>_>9~t8;pGzL6L-fmQO@K; zo&vQzMlgY95;1BSkngY)e{`n0!NfVgf}2mB3t}D9@*N;FQ{HZ3Pb%BK6;5#-O|WI( zb6h@qTLU~AbVW#_6?c!?Dj65Now7*pU{h!1+eCV^KCuPAGs28~3k@ueL5+u|Z-7}t z9|lskE`4B7W8wMs@xJa{#bsCGDFoRSNSnmNYB&U7 zVGKWe%+kFB6kb)e;TyHfqtU6~fRg)f|>=5(N36)0+C z`hv65J<$B}WUc!wFAb^QtY31yNleq4dzmG`1wHTj=c*=hay9iD071Hc?oYoUk|M*_ zU1GihAMBsM@5rUJ(qS?9ZYJ6@{bNqJ`2Mr+5#hKf?doa?F|+^IR!8lq9)wS3tF_9n zW_?hm)G(M+MYb?V9YoX^_mu5h-LP^TL^!Q9Z7|@sO(rg_4+@=PdI)WL(B7`!K^ND- z-uIuVDCVEdH_C@c71YGYT^_Scf_dhB8Z2Xy6vGtBSlYud9vggOqv^L~F{BraSE_t} zIkP+Hp2&nH^-MNEs}^`oMLy11`PQW$T|K(`Bu*(f@)mv1-qY(_YG&J2M2<7k;;RK~ zL{Fqj9yCz8(S{}@c)S!65aF<=&eLI{hAMErCx&>i7OeDN>okvegO87OaG{Jmi<|}D zaT@b|0X{d@OIJ7zvT>r+eTzgLq~|Dpu)Z&db-P4z*`M$UL51lf>FLlq6rfG)%doyp z)3kk_YIM!03eQ8Vu_2fg{+osaEJPtJ-s36R+5_AEG12`NG)IQ#TF9c@$99%0iye+ zUzZ57=m2)$D(5Nx!n)=5Au&O0BBgwxIBaeI(mro$#&UGCr<;C{UjJVAbVi%|+WP(a zL$U@TYCxJ=1{Z~}rnW;7UVb7+ZnzgmrogDxhjLGo>c~MiJAWs&&;AGg@%U?Y^0JhL ze(x6Z74JG6FlOFK(T}SXQfhr}RIFl@QXKnIcXYF)5|V~e-}suHILKT-k|<*~Ij|VF zC;t@=uj=hot~*!C68G8hTA%8SzOfETOXQ|3FSaIEjvBJp(A)7SWUi5!Eu#yWgY+;n zlm<$+UDou*V+246_o#V4kMdto8hF%%Lki#zPh}KYXmMf?hrN0;>Mv%`@{0Qn`Ujp) z=lZe+13>^Q!9zT);H<(#bIeRWz%#*}sgUX9P|9($kexOyKIOc`dLux}c$7It4u|Rl z6SSkY*V~g_B-hMPo_ak>>z@AVQ(_N)VY2kB3IZ0G(iDUYw+2d7W^~(Jq}KY=JnWS( z#rzEa&0uNhJ>QE8iiyz;n2H|SV#Og+wEZv=f2%1ELX!SX-(d3tEj$5$1}70Mp<&eI zCkfbByL7af=qQE@5vDVxx1}FSGt_a1DoE3SDI+G)mBAna)KBG4p8Epxl9QZ4BfdAN zFnF|Y(umr;gRgG6NLQ$?ZWgllEeeq~z^ZS7L?<(~O&$5|y)Al^iMKy}&W+eMm1W z7EMU)u^ke(A1#XCV>CZ71}P}0x)4wtHO8#JRG3MA-6g=`ZM!FcICCZ{IEw8Dm2&LQ z1|r)BUG^0GzI6f946RrBlfB1Vs)~8toZf~7)+G;pv&XiUO(%5bm)pl=p>nV^o*;&T z;}@oZSibzto$arQgfkp|z4Z($P>dTXE{4O=vY0!)kDO* zGF8a4wq#VaFpLfK!iELy@?-SeRrdz%F*}hjKcA*y@mj~VD3!it9lhRhX}5YOaR9$} z3mS%$2Be7{l(+MVx3 z(4?h;P!jnRmX9J9sYN#7i=iyj_5q7n#X(!cdqI2lnr8T$IfOW<_v`eB!d9xY1P=2q&WtOXY=D9QYteP)De?S4}FK6#6Ma z=E*V+#s8>L;8aVroK^6iKo=MH{4yEZ_>N-N z`(|;aOATba1^asjxlILk<4}f~`39dBFlxj>Dw(hMYKPO3EEt1@S`1lxFNM+J@uB7T zZ8WKjz7HF1-5&2=l=fqF-*@>n5J}jIxdDwpT?oKM3s8Nr`x8JnN-kCE?~aM1H!hAE z%%w(3kHfGwMnMmNj(SU(w42OrC-euI>Dsjk&jz3ts}WHqmMpzQ3vZrsXrZ|}+MHA7 z068obeXZTsO*6RS@o3x80E4ok``rV^Y3hr&C1;|ZZ0|*EKO`$lECUYG2gVFtUTw)R z4Um<0ZzlON`zTdvVdL#KFoMFQX*a5wM0Czp%wTtfK4Sjs)P**RW&?lP$(<}q%r68Z zS53Y!d@&~ne9O)A^tNrXHhXBkj~$8j%pT1%%mypa9AW5E&s9)rjF4@O3ytH{0z6riz|@< zB~UPh*wRFg2^7EbQrHf0y?E~dHlkOxof_a?M{LqQ^C!i2dawHTPYUE=X@2(3<=OOxs8qn_(y>pU>u^}3y&df{JarR0@VJn0f+U%UiF=$Wyq zQvnVHESil@d|8&R<%}uidGh7@u^(%?$#|&J$pvFC-n8&A>utA=n3#)yMkz+qnG3wd zP7xCnF|$9Dif@N~L)Vde3hW8W!UY0BgT2v(wzp;tlLmyk2%N|0jfG$%<;A&IVrOI< z!L)o>j>;dFaqA3pL}b-Je(bB@VJ4%!JeX@3x!i{yIeIso^=n?fDX`3bU=eG7sTc%g%ye8$v8P@yKE^XD=NYxTb zbf!Mk=h|otpqjFaA-vs5YOF-*GwWPc7VbaOW&stlANnCN8iftFMMrUdYNJ_Bnn5Vt zxfz@Ah|+4&P;reZxp;MmEI7C|FOv8NKUm8njF7Wb6Gi7DeODLl&G~}G4be&*Hi0Qw z5}77vL0P+7-B%UL@3n1&JPxW^d@vVwp?u#gVcJqY9#@-3X{ok#UfW3<1fb%FT`|)V~ggq z(3AUoUS-;7)^hCjdT0Kf{i}h)mBg4qhtHHBti=~h^n^OTH5U*XMgDLIR@sre`AaB$ zg)IGBET_4??m@cx&c~bA80O7B8CHR7(LX7%HThkeC*@vi{-pL%e)yXp!B2InafbDF zjPXf1mko3h59{lT6EEbxKO1Z5GF71)WwowO6kY|6tjSVSWdQ}NsK2x{>i|MKZK8%Q zfu&_0D;CO-Jg0#YmyfctyJ!mRJp)e#@O0mYdp|8x;G1%OZQ3Q847YWTyy|%^cpA;m zze0(5p{tMu^lDkpe?HynyO?a1$_LJl2L&mpeKu%8YvgRNr=%2z${%WThHG=vrWY@4 zsA`OP#O&)TetZ>s%h!=+CE15lOOls&nvC~$Qz0Ph7tHiP;O$i|eDwpT{cp>+)0-|; zY$|bB+Gbel>5aRN3>c0x)4U=|X+z+{ zn*_p*EQoquRL+=+p;=lm`d71&1NqBz&_ph)MXu(Nv6&XE7(RsS)^MGj5Q?Fwude-(sq zjJ>aOq!7!EN>@(fK7EE#;i_BGvli`5U;r!YA{JRodLBc6-`n8K+Fjgwb%sX;j=qHQ z7&Tr!)!{HXoO<2BQrV9Sw?JRaLXV8HrsNevvnf>Y-6|{T!pYLl7jp$-nEE z#X!4G4L#K0qG_4Z;Cj6=;b|Be$hi4JvMH!-voxqx^@8cXp`B??eFBz2lLD8RRaRGh zn7kUfy!YV~p(R|p7iC1Rdgt$_24i0cd-S8HpG|`@my70g^y`gu%#Tf_L21-k?sRRZHK&at(*ED0P8iw{7?R$9~OF$Ko;Iu5)ur5<->x!m93Eb zFYpIx60s=Wxxw=`$aS-O&dCO_9?b1yKiPCQmSQb>T)963`*U+Ydj5kI(B(B?HNP8r z*bfSBpSu)w(Z3j7HQoRjUG(+d=IaE~tv}y14zHHs|0UcN52fT8V_<@2ep_ee{QgZG zmgp8iv4V{k;~8@I%M3<#B;2R>Ef(Gg_cQM7%}0s*^)SK6!Ym+~P^58*wnwV1BW@eG z4sZLqsUvBbFsr#8u7S1r4teQ;t)Y@jnn_m5jS$CsW1um!p&PqAcc8!zyiXHVta9QC zY~wCwCF0U%xiQPD_INKtTb;A|Zf29(mu9NI;E zc-e>*1%(LSXB`g}kd`#}O;veb<(sk~RWL|f3ljxCnEZDdNSTDV6#Td({6l&y4IjKF z^}lIUq*ZUqgTPumD)RrCN{M^jhY>E~1pn|KOZ5((%F)G|*ZQ|r4zIbrEiV%42hJV8 z3xS)=!X1+=olbdGJ=yZil?oXLct8FM{(6ikLL3E%=q#O6(H$p~gQu6T8N!plf!96| z&Q3=`L~>U0zZh;z(pGR2^S^{#PrPxTRHD1RQOON&f)Siaf`GLj#UOk&(|@0?zm;Sx ztsGt8=29-MZs5CSf1l1jNFtNt5rFNZxJPvkNu~2}7*9468TWm>nN9TP&^!;J{-h)_ z7WsHH9|F%I`Pb!>KAS3jQWKfGivTVkMJLO-HUGM_a4UQ_%RgL6WZvrW+Z4ujZn;y@ zz9$=oO!7qVTaQAA^BhX&ZxS*|5dj803M=k&2%QrXda`-Q#IoZL6E(g+tN!6CA!CP* zCpWtCujIea)ENl0liwVfj)Nc<9mV%+e@=d`haoZ*`B7+PNjEbXBkv=B+Pi^~L#EO$D$ZqTiD8f<5$eyb54-(=3 zh)6i8i|jp(@OnRrY5B8t|LFXFQVQ895n*P16cEKTrT*~yLH6Z4e*bZ5otpRDri&+A zfNbK1D5@O=sm`fN=WzWyse!za5n%^+6dHPGX#8DyIK>?9qyX}2XvBWVqbP%%D)7$= z=#$WulZlZR<{m#gU7lwqK4WS1Ne$#_P{b17qe$~UOXCl>5b|6WVh;5vVnR<%d+Lnp z$uEmML38}U4vaW8>shm6CzB(Wei3s#NAWE3)a2)z@i{4jTn;;aQS)O@l{rUM`J@K& l00vQ5JBs~;vo!vr%%-k{2_Fq1Mn4QF81S)AQ99zk{{c4yR+0b! literal 60756 zcmb5WV{~QRw(p$^Dz@00IL3?^hro$gg*4VI_WAaTyVM5Foj~O|-84 z$;06hMwt*rV;^8iB z1~&0XWpYJmG?Ts^K9PC62H*`G}xom%S%yq|xvG~FIfP=9*f zZoDRJBm*Y0aId=qJ?7dyb)6)JGWGwe)MHeNSzhi)Ko6J<-m@v=a%NsP537lHe0R* z`If4$aaBA#S=w!2z&m>{lpTy^Lm^mg*3?M&7HFv}7K6x*cukLIGX;bQG|QWdn{%_6 zHnwBKr84#B7Z+AnBXa16a?or^R?+>$4`}{*a_>IhbjvyTtWkHw)|ay)ahWUd-qq$~ zMbh6roVsj;_qnC-R{G+Cy6bApVOinSU-;(DxUEl!i2)1EeQ9`hrfqj(nKI7?Z>Xur zoJz-a`PxkYit1HEbv|jy%~DO^13J-ut986EEG=66S}D3!L}Efp;Bez~7tNq{QsUMm zh9~(HYg1pA*=37C0}n4g&bFbQ+?-h-W}onYeE{q;cIy%eZK9wZjSwGvT+&Cgv z?~{9p(;bY_1+k|wkt_|N!@J~aoY@|U_RGoWX<;p{Nu*D*&_phw`8jYkMNpRTWx1H* z>J-Mi_!`M468#5Aix$$u1M@rJEIOc?k^QBc?T(#=n&*5eS#u*Y)?L8Ha$9wRWdH^3D4|Ps)Y?m0q~SiKiSfEkJ!=^`lJ(%W3o|CZ zSrZL-Xxc{OrmsQD&s~zPfNJOpSZUl%V8tdG%ei}lQkM+z@-4etFPR>GOH9+Y_F<3=~SXln9Kb-o~f>2a6Xz@AS3cn^;c_>lUwlK(n>z?A>NbC z`Ud8^aQy>wy=$)w;JZzA)_*Y$Z5hU=KAG&htLw1Uh00yE!|Nu{EZkch zY9O6x7Y??>!7pUNME*d!=R#s)ghr|R#41l!c?~=3CS8&zr6*aA7n9*)*PWBV2w+&I zpW1-9fr3j{VTcls1>ua}F*bbju_Xq%^v;-W~paSqlf zolj*dt`BBjHI)H9{zrkBo=B%>8}4jeBO~kWqO!~Thi!I1H(in=n^fS%nuL=X2+s!p}HfTU#NBGiwEBF^^tKU zbhhv+0dE-sbK$>J#t-J!B$TMgN@Wh5wTtK2BG}4BGfsZOoRUS#G8Cxv|6EI*n&Xxq zt{&OxCC+BNqz$9b0WM7_PyBJEVObHFh%%`~!@MNZlo*oXDCwDcFwT~Rls!aApL<)^ zbBftGKKBRhB!{?fX@l2_y~%ygNFfF(XJzHh#?`WlSL{1lKT*gJM zs>bd^H9NCxqxn(IOky5k-wALFowQr(gw%|`0991u#9jXQh?4l|l>pd6a&rx|v=fPJ z1mutj{YzpJ_gsClbWFk(G}bSlFi-6@mwoQh-XeD*j@~huW4(8ub%^I|azA)h2t#yG z7e_V_<4jlM3D(I+qX}yEtqj)cpzN*oCdYHa!nm%0t^wHm)EmFP*|FMw!tb@&`G-u~ zK)=Sf6z+BiTAI}}i{*_Ac$ffr*Wrv$F7_0gJkjx;@)XjYSh`RjAgrCck`x!zP>Ifu z&%he4P|S)H*(9oB4uvH67^0}I-_ye_!w)u3v2+EY>eD3#8QR24<;7?*hj8k~rS)~7 zSXs5ww)T(0eHSp$hEIBnW|Iun<_i`}VE0Nc$|-R}wlSIs5pV{g_Dar(Zz<4X3`W?K z6&CAIl4U(Qk-tTcK{|zYF6QG5ArrEB!;5s?tW7 zrE3hcFY&k)+)e{+YOJ0X2uDE_hd2{|m_dC}kgEKqiE9Q^A-+>2UonB+L@v3$9?AYw zVQv?X*pK;X4Ovc6Ev5Gbg{{Eu*7{N3#0@9oMI~}KnObQE#Y{&3mM4`w%wN+xrKYgD zB-ay0Q}m{QI;iY`s1Z^NqIkjrTlf`B)B#MajZ#9u41oRBC1oM1vq0i|F59> z#StM@bHt|#`2)cpl_rWB($DNJ3Lap}QM-+A$3pe}NyP(@+i1>o^fe-oxX#Bt`mcQc zb?pD4W%#ep|3%CHAYnr*^M6Czg>~L4?l16H1OozM{P*en298b+`i4$|w$|4AHbzqB zHpYUsHZET$Z0ztC;U+0*+amF!@PI%^oUIZy{`L{%O^i{Xk}X0&nl)n~tVEpcAJSJ} zverw15zP1P-O8h9nd!&hj$zuwjg?DoxYIw{jWM zW5_pj+wFy8Tsa9g<7Qa21WaV&;ejoYflRKcz?#fSH_)@*QVlN2l4(QNk| z4aPnv&mrS&0|6NHq05XQw$J^RR9T{3SOcMKCXIR1iSf+xJ0E_Wv?jEc*I#ZPzyJN2 zUG0UOXHl+PikM*&g$U@g+KbG-RY>uaIl&DEtw_Q=FYq?etc!;hEC_}UX{eyh%dw2V zTTSlap&5>PY{6I#(6`j-9`D&I#|YPP8a;(sOzgeKDWsLa!i-$frD>zr-oid!Hf&yS z!i^cr&7tN}OOGmX2)`8k?Tn!!4=tz~3hCTq_9CdiV!NIblUDxHh(FJ$zs)B2(t5@u z-`^RA1ShrLCkg0)OhfoM;4Z{&oZmAec$qV@ zGQ(7(!CBk<5;Ar%DLJ0p0!ResC#U<+3i<|vib1?{5gCebG7$F7URKZXuX-2WgF>YJ^i zMhHDBsh9PDU8dlZ$yJKtc6JA#y!y$57%sE>4Nt+wF1lfNIWyA`=hF=9Gj%sRwi@vd z%2eVV3y&dvAgyuJ=eNJR+*080dbO_t@BFJO<@&#yqTK&+xc|FRR;p;KVk@J3$S{p` zGaMj6isho#%m)?pOG^G0mzOAw0z?!AEMsv=0T>WWcE>??WS=fII$t$(^PDPMU(P>o z_*0s^W#|x)%tx8jIgZY~A2yG;US0m2ZOQt6yJqW@XNY_>_R7(Nxb8Ged6BdYW6{prd!|zuX$@Q2o6Ona8zzYC1u!+2!Y$Jc9a;wy+pXt}o6~Bu1oF1c zp7Y|SBTNi@=I(K%A60PMjM#sfH$y*c{xUgeSpi#HB`?|`!Tb&-qJ3;vxS!TIzuTZs-&%#bAkAyw9m4PJgvey zM5?up*b}eDEY+#@tKec)-c(#QF0P?MRlD1+7%Yk*jW;)`f;0a-ZJ6CQA?E%>i2Dt7T9?s|9ZF|KP4;CNWvaVKZ+Qeut;Jith_y{v*Ny6Co6!8MZx;Wgo z=qAi%&S;8J{iyD&>3CLCQdTX*$+Rx1AwA*D_J^0>suTgBMBb=*hefV+Ars#mmr+YsI3#!F@Xc1t4F-gB@6aoyT+5O(qMz*zG<9Qq*f0w^V!03rpr*-WLH}; zfM{xSPJeu6D(%8HU%0GEa%waFHE$G?FH^kMS-&I3)ycx|iv{T6Wx}9$$D&6{%1N_8 z_CLw)_9+O4&u94##vI9b-HHm_95m)fa??q07`DniVjAy`t7;)4NpeyAY(aAk(+T_O z1om+b5K2g_B&b2DCTK<>SE$Ode1DopAi)xaJjU>**AJK3hZrnhEQ9E`2=|HHe<^tv z63e(bn#fMWuz>4erc47}!J>U58%<&N<6AOAewyzNTqi7hJc|X{782&cM zHZYclNbBwU6673=!ClmxMfkC$(CykGR@10F!zN1Se83LR&a~$Ht&>~43OX22mt7tcZUpa;9@q}KDX3O&Ugp6< zLZLfIMO5;pTee1vNyVC$FGxzK2f>0Z-6hM82zKg44nWo|n}$Zk6&;5ry3`(JFEX$q zK&KivAe${e^5ZGc3a9hOt|!UOE&OocpVryE$Y4sPcs4rJ>>Kbi2_subQ9($2VN(3o zb~tEzMsHaBmBtaHAyES+d3A(qURgiskSSwUc9CfJ@99&MKp2sooSYZu+-0t0+L*!I zYagjOlPgx|lep9tiU%ts&McF6b0VE57%E0Ho%2oi?=Ks+5%aj#au^OBwNwhec zta6QAeQI^V!dF1C)>RHAmB`HnxyqWx?td@4sd15zPd*Fc9hpDXP23kbBenBxGeD$k z;%0VBQEJ-C)&dTAw_yW@k0u?IUk*NrkJ)(XEeI z9Y>6Vel>#s_v@=@0<{4A{pl=9cQ&Iah0iD0H`q)7NeCIRz8zx;! z^OO;1+IqoQNak&pV`qKW+K0^Hqp!~gSohcyS)?^P`JNZXw@gc6{A3OLZ?@1Uc^I2v z+X!^R*HCm3{7JPq{8*Tn>5;B|X7n4QQ0Bs79uTU%nbqOJh`nX(BVj!#f;#J+WZxx4 z_yM&1Y`2XzhfqkIMO7tB3raJKQS+H5F%o83bM+hxbQ zeeJm=Dvix$2j|b4?mDacb67v-1^lTp${z=jc1=j~QD>7c*@+1?py>%Kj%Ejp7Y-!? z8iYRUlGVrQPandAaxFfks53@2EC#0)%mrnmGRn&>=$H$S8q|kE_iWko4`^vCS2aWg z#!`RHUGyOt*k?bBYu3*j3u0gB#v(3tsije zgIuNNWNtrOkx@Pzs;A9un+2LX!zw+p3_NX^Sh09HZAf>m8l@O*rXy_82aWT$Q>iyy zqO7Of)D=wcSn!0+467&!Hl))eff=$aneB?R!YykdKW@k^_uR!+Q1tR)+IJb`-6=jj zymzA>Sv4>Z&g&WWu#|~GcP7qP&m*w-S$)7Xr;(duqCTe7p8H3k5>Y-n8438+%^9~K z3r^LIT_K{i7DgEJjIocw_6d0!<;wKT`X;&vv+&msmhAAnIe!OTdybPctzcEzBy88_ zWO{6i4YT%e4^WQZB)KHCvA(0tS zHu_Bg+6Ko%a9~$EjRB90`P(2~6uI@SFibxct{H#o&y40MdiXblu@VFXbhz>Nko;7R z70Ntmm-FePqhb%9gL+7U8@(ch|JfH5Fm)5${8|`Lef>LttM_iww6LW2X61ldBmG0z zax3y)njFe>j*T{i0s8D4=L>X^j0)({R5lMGVS#7(2C9@AxL&C-lZQx~czI7Iv+{%1 z2hEG>RzX4S8x3v#9sgGAnPzptM)g&LB}@%E>fy0vGSa(&q0ch|=ncKjNrK z`jA~jObJhrJ^ri|-)J^HUyeZXz~XkBp$VhcTEcTdc#a2EUOGVX?@mYx#Vy*!qO$Jv zQ4rgOJ~M*o-_Wptam=~krnmG*p^j!JAqoQ%+YsDFW7Cc9M%YPiBOrVcD^RY>m9Pd< zu}#9M?K{+;UIO!D9qOpq9yxUquQRmQNMo0pT`@$pVt=rMvyX)ph(-CCJLvUJy71DI zBk7oc7)-%ngdj~s@76Yse3L^gV0 z2==qfp&Q~L(+%RHP0n}+xH#k(hPRx(!AdBM$JCfJ5*C=K3ts>P?@@SZ_+{U2qFZb>4kZ{Go37{# zSQc+-dq*a-Vy4?taS&{Ht|MLRiS)Sn14JOONyXqPNnpq&2y~)6wEG0oNy>qvod$FF z`9o&?&6uZjhZ4_*5qWVrEfu(>_n2Xi2{@Gz9MZ8!YmjYvIMasE9yVQL10NBrTCczq zcTY1q^PF2l!Eraguf{+PtHV3=2A?Cu&NN&a8V(y;q(^_mFc6)%Yfn&X&~Pq zU1?qCj^LF(EQB1F`8NxNjyV%fde}dEa(Hx=r7$~ts2dzDwyi6ByBAIx$NllB4%K=O z$AHz1<2bTUb>(MCVPpK(E9wlLElo(aSd(Os)^Raum`d(g9Vd_+Bf&V;l=@mM=cC>) z)9b0enb)u_7V!!E_bl>u5nf&Rl|2r=2F3rHMdb7y9E}}F82^$Rf+P8%dKnOeKh1vs zhH^P*4Ydr^$)$h@4KVzxrHyy#cKmWEa9P5DJ|- zG;!Qi35Tp7XNj60=$!S6U#!(${6hyh7d4q=pF{`0t|N^|L^d8pD{O9@tF~W;#Je*P z&ah%W!KOIN;SyAEhAeTafJ4uEL`(RtnovM+cb(O#>xQnk?dzAjG^~4$dFn^<@-Na3 z395;wBnS{t*H;Jef2eE!2}u5Ns{AHj>WYZDgQJt8v%x?9{MXqJsGP|l%OiZqQ1aB! z%E=*Ig`(!tHh>}4_z5IMpg{49UvD*Pp9!pxt_gdAW%sIf3k6CTycOT1McPl=_#0?8 zVjz8Hj*Vy9c5-krd-{BQ{6Xy|P$6LJvMuX$* zA+@I_66_ET5l2&gk9n4$1M3LN8(yEViRx&mtd#LD}AqEs?RW=xKC(OCWH;~>(X6h!uDxXIPH06xh z*`F4cVlbDP`A)-fzf>MuScYsmq&1LUMGaQ3bRm6i7OsJ|%uhTDT zlvZA1M}nz*SalJWNT|`dBm1$xlaA>CCiQ zK`xD-RuEn>-`Z?M{1%@wewf#8?F|(@1e0+T4>nmlSRrNK5f)BJ2H*$q(H>zGD0>eL zQ!tl_Wk)k*e6v^m*{~A;@6+JGeWU-q9>?+L_#UNT%G?4&BnOgvm9@o7l?ov~XL+et zbGT)|G7)KAeqb=wHSPk+J1bdg7N3$vp(ekjI1D9V$G5Cj!=R2w=3*4!z*J-r-cyeb zd(i2KmX!|Lhey!snRw z?#$Gu%S^SQEKt&kep)up#j&9}e+3=JJBS(s>MH+|=R(`8xK{mmndWo_r`-w1#SeRD&YtAJ#GiVI*TkQZ}&aq<+bU2+coU3!jCI6E+Ad_xFW*ghnZ$q zAoF*i&3n1j#?B8x;kjSJD${1jdRB;)R*)Ao!9bd|C7{;iqDo|T&>KSh6*hCD!rwv= zyK#F@2+cv3=|S1Kef(E6Niv8kyLVLX&e=U;{0x{$tDfShqkjUME>f8d(5nzSkY6@! z^-0>DM)wa&%m#UF1F?zR`8Y3X#tA!*7Q$P3lZJ%*KNlrk_uaPkxw~ zxZ1qlE;Zo;nb@!SMazSjM>;34ROOoygo%SF);LL>rRonWwR>bmSd1XD^~sGSu$Gg# zFZ`|yKU0%!v07dz^v(tY%;So(e`o{ZYTX`hm;@b0%8|H>VW`*cr8R%3n|ehw2`(9B+V72`>SY}9^8oh$En80mZK9T4abVG*to;E z1_S6bgDOW?!Oy1LwYy=w3q~KKdbNtyH#d24PFjX)KYMY93{3-mPP-H>@M-_>N~DDu zENh~reh?JBAK=TFN-SfDfT^=+{w4ea2KNWXq2Y<;?(gf(FgVp8Zp-oEjKzB%2Iqj;48GmY3h=bcdYJ}~&4tS`Q1sb=^emaW$IC$|R+r-8V- zf0$gGE(CS_n4s>oicVk)MfvVg#I>iDvf~Ov8bk}sSxluG!6#^Z_zhB&U^`eIi1@j( z^CK$z^stBHtaDDHxn+R;3u+>Lil^}fj?7eaGB z&5nl^STqcaBxI@v>%zG|j))G(rVa4aY=B@^2{TFkW~YP!8!9TG#(-nOf^^X-%m9{Z zCC?iC`G-^RcBSCuk=Z`(FaUUe?hf3{0C>>$?Vs z`2Uud9M+T&KB6o4o9kvdi^Q=Bw!asPdxbe#W-Oaa#_NP(qpyF@bVxv5D5))srkU#m zj_KA+#7sqDn*Ipf!F5Byco4HOSd!Ui$l94|IbW%Ny(s1>f4|Mv^#NfB31N~kya9!k zWCGL-$0ZQztBate^fd>R!hXY_N9ZjYp3V~4_V z#eB)Kjr8yW=+oG)BuNdZG?jaZlw+l_ma8aET(s+-x+=F-t#Qoiuu1i`^x8Sj>b^U} zs^z<()YMFP7CmjUC@M=&lA5W7t&cxTlzJAts*%PBDAPuqcV5o7HEnqjif_7xGt)F% zGx2b4w{@!tE)$p=l3&?Bf#`+!-RLOleeRk3 z7#pF|w@6_sBmn1nECqdunmG^}pr5(ZJQVvAt$6p3H(16~;vO>?sTE`Y+mq5YP&PBo zvq!7#W$Gewy`;%6o^!Dtjz~x)T}Bdk*BS#=EY=ODD&B=V6TD2z^hj1m5^d6s)D*wk zu$z~D7QuZ2b?5`p)E8e2_L38v3WE{V`bVk;6fl#o2`) z99JsWhh?$oVRn@$S#)uK&8DL8>An0&S<%V8hnGD7Z^;Y(%6;^9!7kDQ5bjR_V+~wp zfx4m3z6CWmmZ<8gDGUyg3>t8wgJ5NkkiEm^(sedCicP^&3D%}6LtIUq>mXCAt{9eF zNXL$kGcoUTf_Lhm`t;hD-SE)m=iBnxRU(NyL}f6~1uH)`K!hmYZjLI%H}AmEF5RZt z06$wn63GHnApHXZZJ}s^s)j9(BM6e*7IBK6Bq(!)d~zR#rbxK9NVIlgquoMq z=eGZ9NR!SEqP6=9UQg#@!rtbbSBUM#ynF);zKX+|!Zm}*{H z+j=d?aZ2!?@EL7C~%B?6ouCKLnO$uWn;Y6Xz zX8dSwj732u(o*U3F$F=7xwxm>E-B+SVZH;O-4XPuPkLSt_?S0)lb7EEg)Mglk0#eS z9@jl(OnH4juMxY+*r03VDfPx_IM!Lmc(5hOI;`?d37f>jPP$?9jQQIQU@i4vuG6MagEoJrQ=RD7xt@8E;c zeGV*+Pt+t$@pt!|McETOE$9k=_C!70uhwRS9X#b%ZK z%q(TIUXSS^F0`4Cx?Rk07C6wI4!UVPeI~-fxY6`YH$kABdOuiRtl73MqG|~AzZ@iL&^s?24iS;RK_pdlWkhcF z@Wv-Om(Aealfg)D^adlXh9Nvf~Uf@y;g3Y)i(YP zEXDnb1V}1pJT5ZWyw=1i+0fni9yINurD=EqH^ciOwLUGi)C%Da)tyt=zq2P7pV5-G zR7!oq28-Fgn5pW|nlu^b!S1Z#r7!Wtr{5J5PQ>pd+2P7RSD?>(U7-|Y z7ZQ5lhYIl_IF<9?T9^IPK<(Hp;l5bl5tF9>X-zG14_7PfsA>6<$~A338iYRT{a@r_ zuXBaT=`T5x3=s&3=RYx6NgG>No4?5KFBVjE(swfcivcIpPQFx5l+O;fiGsOrl5teR z_Cm+;PW}O0Dwe_(4Z@XZ)O0W-v2X><&L*<~*q3dg;bQW3g7)a#3KiQP>+qj|qo*Hk z?57>f2?f@`=Fj^nkDKeRkN2d$Z@2eNKpHo}ksj-$`QKb6n?*$^*%Fb3_Kbf1(*W9K>{L$mud2WHJ=j0^=g30Xhg8$#g^?36`p1fm;;1@0Lrx+8t`?vN0ZorM zSW?rhjCE8$C|@p^sXdx z|NOHHg+fL;HIlqyLp~SSdIF`TnSHehNCU9t89yr@)FY<~hu+X`tjg(aSVae$wDG*C zq$nY(Y494R)hD!i1|IIyP*&PD_c2FPgeY)&mX1qujB1VHPG9`yFQpLFVQ0>EKS@Bp zAfP5`C(sWGLI?AC{XEjLKR4FVNw(4+9b?kba95ukgR1H?w<8F7)G+6&(zUhIE5Ef% z=fFkL3QKA~M@h{nzjRq!Y_t!%U66#L8!(2-GgFxkD1=JRRqk=n%G(yHKn%^&$dW>; zSjAcjETMz1%205se$iH_)ZCpfg_LwvnsZQAUCS#^FExp8O4CrJb6>JquNV@qPq~3A zZ<6dOU#6|8+fcgiA#~MDmcpIEaUO02L5#T$HV0$EMD94HT_eXLZ2Zi&(! z&5E>%&|FZ`)CN10tM%tLSPD*~r#--K(H-CZqIOb99_;m|D5wdgJ<1iOJz@h2Zkq?} z%8_KXb&hf=2Wza(Wgc;3v3TN*;HTU*q2?#z&tLn_U0Nt!y>Oo>+2T)He6%XuP;fgn z-G!#h$Y2`9>Jtf}hbVrm6D70|ERzLAU>3zoWhJmjWfgM^))T+2u$~5>HF9jQDkrXR z=IzX36)V75PrFjkQ%TO+iqKGCQ-DDXbaE;C#}!-CoWQx&v*vHfyI>$HNRbpvm<`O( zlx9NBWD6_e&J%Ous4yp~s6)Ghni!I6)0W;9(9$y1wWu`$gs<$9Mcf$L*piP zPR0Av*2%ul`W;?-1_-5Zy0~}?`e@Y5A&0H!^ApyVTT}BiOm4GeFo$_oPlDEyeGBbh z1h3q&Dx~GmUS|3@4V36&$2uO8!Yp&^pD7J5&TN{?xphf*-js1fP?B|`>p_K>lh{ij zP(?H%e}AIP?_i^f&Li=FDSQ`2_NWxL+BB=nQr=$ zHojMlXNGauvvwPU>ZLq!`bX-5F4jBJ&So{kE5+ms9UEYD{66!|k~3vsP+mE}x!>%P za98bAU0!h0&ka4EoiDvBM#CP#dRNdXJcb*(%=<(g+M@<)DZ!@v1V>;54En?igcHR2 zhubQMq}VSOK)onqHfczM7YA@s=9*ow;k;8)&?J3@0JiGcP! zP#00KZ1t)GyZeRJ=f0^gc+58lc4Qh*S7RqPIC6GugG1gXe$LIQMRCo8cHf^qXgAa2 z`}t>u2Cq1CbSEpLr~E=c7~=Qkc9-vLE%(v9N*&HF`(d~(0`iukl5aQ9u4rUvc8%m) zr2GwZN4!s;{SB87lJB;veebPmqE}tSpT>+`t?<457Q9iV$th%i__Z1kOMAswFldD6 ztbOvO337S5o#ZZgN2G99_AVqPv!?Gmt3pzgD+Hp3QPQ`9qJ(g=kjvD+fUSS3upJn! zqoG7acIKEFRX~S}3|{EWT$kdz#zrDlJU(rPkxjws_iyLKU8+v|*oS_W*-guAb&Pj1 z35Z`3z<&Jb@2Mwz=KXucNYdY#SNO$tcVFr9KdKm|%^e-TXzs6M`PBper%ajkrIyUe zp$vVxVs9*>Vp4_1NC~Zg)WOCPmOxI1V34QlG4!aSFOH{QqSVq1^1)- z0P!Z?tT&E-ll(pwf0?=F=yOzik=@nh1Clxr9}Vij89z)ePDSCYAqw?lVI?v?+&*zH z)p$CScFI8rrwId~`}9YWPFu0cW1Sf@vRELs&cbntRU6QfPK-SO*mqu|u~}8AJ!Q$z znzu}50O=YbjwKCuSVBs6&CZR#0FTu)3{}qJJYX(>QPr4$RqWiwX3NT~;>cLn*_&1H zaKpIW)JVJ>b{uo2oq>oQt3y=zJjb%fU@wLqM{SyaC6x2snMx-}ivfU<1- znu1Lh;i$3Tf$Kh5Uk))G!D1UhE8pvx&nO~w^fG)BC&L!_hQk%^p`Kp@F{cz>80W&T ziOK=Sq3fdRu*V0=S53rcIfWFazI}Twj63CG(jOB;$*b`*#B9uEnBM`hDk*EwSRdwP8?5T?xGUKs=5N83XsR*)a4|ijz|c{4tIU+4j^A5C<#5 z*$c_d=5ml~%pGxw#?*q9N7aRwPux5EyqHVkdJO=5J>84!X6P>DS8PTTz>7C#FO?k#edkntG+fJk8ZMn?pmJSO@`x-QHq;7^h6GEXLXo1TCNhH z8ZDH{*NLAjo3WM`xeb=X{((uv3H(8&r8fJJg_uSs_%hOH%JDD?hu*2NvWGYD+j)&` zz#_1%O1wF^o5ryt?O0n;`lHbzp0wQ?rcbW(F1+h7_EZZ9{>rePvLAPVZ_R|n@;b$;UchU=0j<6k8G9QuQf@76oiE*4 zXOLQ&n3$NR#p4<5NJMVC*S);5x2)eRbaAM%VxWu9ohlT;pGEk7;002enCbQ>2r-us z3#bpXP9g|mE`65VrN`+3mC)M(eMj~~eOf)do<@l+fMiTR)XO}422*1SL{wyY(%oMpBgJagtiDf zz>O6(m;};>Hi=t8o{DVC@YigqS(Qh+ix3Rwa9aliH}a}IlOCW1@?%h_bRbq-W{KHF z%Vo?-j@{Xi@=~Lz5uZP27==UGE15|g^0gzD|3x)SCEXrx`*MP^FDLl%pOi~~Il;dc z^hrwp9sYeT7iZ)-ajKy@{a`kr0-5*_!XfBpXwEcFGJ;%kV$0Nx;apKrur zJN2J~CAv{Zjj%FolyurtW8RaFmpn&zKJWL>(0;;+q(%(Hx!GMW4AcfP0YJ*Vz!F4g z!ZhMyj$BdXL@MlF%KeInmPCt~9&A!;cRw)W!Hi@0DY(GD_f?jeV{=s=cJ6e}JktJw zQORnxxj3mBxfrH=x{`_^Z1ddDh}L#V7i}$njUFRVwOX?qOTKjfPMBO4y(WiU<)epb zvB9L=%jW#*SL|Nd_G?E*_h1^M-$PG6Pc_&QqF0O-FIOpa4)PAEPsyvB)GKasmBoEt z?_Q2~QCYGH+hW31x-B=@5_AN870vY#KB~3a*&{I=f);3Kv7q4Q7s)0)gVYx2#Iz9g(F2;=+Iy4 z6KI^8GJ6D@%tpS^8boU}zpi=+(5GfIR)35PzrbuXeL1Y1N%JK7PG|^2k3qIqHfX;G zQ}~JZ-UWx|60P5?d1e;AHx!_;#PG%d=^X(AR%i`l0jSpYOpXoKFW~7ip7|xvN;2^? zsYC9fanpO7rO=V7+KXqVc;Q5z%Bj})xHVrgoR04sA2 zl~DAwv=!(()DvH*=lyhIlU^hBkA0$e*7&fJpB0|oB7)rqGK#5##2T`@_I^|O2x4GO z;xh6ROcV<9>?e0)MI(y++$-ksV;G;Xe`lh76T#Htuia+(UrIXrf9?

L(tZ$0BqX1>24?V$S+&kLZ`AodQ4_)P#Q3*4xg8}lMV-FLwC*cN$< zt65Rf%7z41u^i=P*qO8>JqXPrinQFapR7qHAtp~&RZ85$>ob|Js;GS^y;S{XnGiBc zGa4IGvDl?x%gY`vNhv8wgZnP#UYI-w*^4YCZnxkF85@ldepk$&$#3EAhrJY0U)lR{F6sM3SONV^+$;Zx8BD&Eku3K zKNLZyBni3)pGzU0;n(X@1fX8wYGKYMpLmCu{N5-}epPDxClPFK#A@02WM3!myN%bkF z|GJ4GZ}3sL{3{qXemy+#Uk{4>Kf8v11;f8I&c76+B&AQ8udd<8gU7+BeWC`akUU~U zgXoxie>MS@rBoyY8O8Tc&8id!w+_ooxcr!1?#rc$-|SBBtH6S?)1e#P#S?jFZ8u-Bs&k`yLqW|{j+%c#A4AQ>+tj$Y z^CZajspu$F%73E68Lw5q7IVREED9r1Ijsg#@DzH>wKseye>hjsk^{n0g?3+gs@7`i zHx+-!sjLx^fS;fY!ERBU+Q zVJ!e0hJH%P)z!y%1^ZyG0>PN@5W~SV%f>}c?$H8r;Sy-ui>aruVTY=bHe}$e zi&Q4&XK!qT7-XjCrDaufT@>ieQ&4G(SShUob0Q>Gznep9fR783jGuUynAqc6$pYX; z7*O@@JW>O6lKIk0G00xsm|=*UVTQBB`u1f=6wGAj%nHK_;Aqmfa!eAykDmi-@u%6~ z;*c!pS1@V8r@IX9j&rW&d*}wpNs96O2Ute>%yt{yv>k!6zfT6pru{F1M3P z2WN1JDYqoTB#(`kE{H676QOoX`cnqHl1Yaru)>8Ky~VU{)r#{&s86Vz5X)v15ULHA zAZDb{99+s~qI6;-dQ5DBjHJP@GYTwn;Dv&9kE<0R!d z8tf1oq$kO`_sV(NHOSbMwr=To4r^X$`sBW4$gWUov|WY?xccQJN}1DOL|GEaD_!@& z15p?Pj+>7d`@LvNIu9*^hPN)pwcv|akvYYq)ks%`G>!+!pW{-iXPZsRp8 z35LR;DhseQKWYSD`%gO&k$Dj6_6q#vjWA}rZcWtQr=Xn*)kJ9kacA=esi*I<)1>w^ zO_+E>QvjP)qiSZg9M|GNeLtO2D7xT6vsj`88sd!94j^AqxFLi}@w9!Y*?nwWARE0P znuI_7A-saQ+%?MFA$gttMV-NAR^#tjl_e{R$N8t2NbOlX373>e7Ox=l=;y#;M7asp zRCz*CLnrm$esvSb5{T<$6CjY zmZ(i{Rs_<#pWW>(HPaaYj`%YqBra=Ey3R21O7vUbzOkJJO?V`4-D*u4$Me0Bx$K(lYo`JO}gnC zx`V}a7m-hLU9Xvb@K2ymioF)vj12<*^oAqRuG_4u%(ah?+go%$kOpfb`T96P+L$4> zQ#S+sA%VbH&mD1k5Ak7^^dZoC>`1L%i>ZXmooA!%GI)b+$D&ziKrb)a=-ds9xk#~& z7)3iem6I|r5+ZrTRe_W861x8JpD`DDIYZNm{$baw+$)X^Jtjnl0xlBgdnNY}x%5za zkQ8E6T<^$sKBPtL4(1zi_Rd(tVth*3Xs!ulflX+70?gb&jRTnI8l+*Aj9{|d%qLZ+ z>~V9Z;)`8-lds*Zgs~z1?Fg?Po7|FDl(Ce<*c^2=lFQ~ahwh6rqSjtM5+$GT>3WZW zj;u~w9xwAhOc<kF}~`CJ68 z?(S5vNJa;kriPlim33{N5`C{9?NWhzsna_~^|K2k4xz1`xcui*LXL-1#Y}Hi9`Oo!zQ>x-kgAX4LrPz63uZ+?uG*84@PKq-KgQlMNRwz=6Yes) zY}>YN+qP}nwr$(CZQFjUOI=-6J$2^XGvC~EZ+vrqWaOXB$k?%Suf5k=4>AveC1aJ! ziaW4IS%F$_Babi)kA8Y&u4F7E%99OPtm=vzw$$ zEz#9rvn`Iot_z-r3MtV>k)YvErZ<^Oa${`2>MYYODSr6?QZu+be-~MBjwPGdMvGd!b!elsdi4% z`37W*8+OGulab8YM?`KjJ8e+jM(tqLKSS@=jimq3)Ea2EB%88L8CaM+aG7;27b?5` z4zuUWBr)f)k2o&xg{iZ$IQkJ+SK>lpq4GEacu~eOW4yNFLU!Kgc{w4&D$4ecm0f}~ zTTzquRW@`f0}|IILl`!1P+;69g^upiPA6F{)U8)muWHzexRenBU$E^9X-uIY2%&1w z_=#5*(nmxJ9zF%styBwivi)?#KMG96-H@hD-H_&EZiRNsfk7mjBq{L%!E;Sqn!mVX*}kXhwH6eh;b42eD!*~upVG@ z#smUqz$ICm!Y8wY53gJeS|Iuard0=;k5i5Z_hSIs6tr)R4n*r*rE`>38Pw&lkv{_r!jNN=;#?WbMj|l>cU(9trCq; z%nN~r^y7!kH^GPOf3R}?dDhO=v^3BeP5hF|%4GNQYBSwz;x({21i4OQY->1G=KFyu z&6d`f2tT9Yl_Z8YACZaJ#v#-(gcyeqXMhYGXb=t>)M@fFa8tHp2x;ODX=Ap@a5I=U z0G80^$N0G4=U(>W%mrrThl0DjyQ-_I>+1Tdd_AuB3qpYAqY54upwa3}owa|x5iQ^1 zEf|iTZxKNGRpI>34EwkIQ2zHDEZ=(J@lRaOH>F|2Z%V_t56Km$PUYu^xA5#5Uj4I4RGqHD56xT%H{+P8Ag>e_3pN$4m8n>i%OyJFPNWaEnJ4McUZPa1QmOh?t8~n& z&RulPCors8wUaqMHECG=IhB(-tU2XvHP6#NrLVyKG%Ee*mQ5Ps%wW?mcnriTVRc4J`2YVM>$ixSF2Xi+Wn(RUZnV?mJ?GRdw%lhZ+t&3s7g!~g{%m&i<6 z5{ib-<==DYG93I(yhyv4jp*y3#*WNuDUf6`vTM%c&hiayf(%=x@4$kJ!W4MtYcE#1 zHM?3xw63;L%x3drtd?jot!8u3qeqctceX3m;tWetK+>~q7Be$h>n6riK(5@ujLgRS zvOym)k+VAtyV^mF)$29Y`nw&ijdg~jYpkx%*^ z8dz`C*g=I?;clyi5|!27e2AuSa$&%UyR(J3W!A=ZgHF9OuKA34I-1U~pyD!KuRkjA zbkN!?MfQOeN>DUPBxoy5IX}@vw`EEB->q!)8fRl_mqUVuRu|C@KD-;yl=yKc=ZT0% zB$fMwcC|HE*0f8+PVlWHi>M`zfsA(NQFET?LrM^pPcw`cK+Mo0%8*x8@65=CS_^$cG{GZQ#xv($7J z??R$P)nPLodI;P!IC3eEYEHh7TV@opr#*)6A-;EU2XuogHvC;;k1aI8asq7ovoP!* z?x%UoPrZjj<&&aWpsbr>J$Er-7!E(BmOyEv!-mbGQGeJm-U2J>74>o5x`1l;)+P&~ z>}f^=Rx(ZQ2bm+YE0u=ZYrAV@apyt=v1wb?R@`i_g64YyAwcOUl=C!i>=Lzb$`tjv zOO-P#A+)t-JbbotGMT}arNhJmmGl-lyUpMn=2UacVZxmiG!s!6H39@~&uVokS zG=5qWhfW-WOI9g4!R$n7!|ViL!|v3G?GN6HR0Pt_L5*>D#FEj5wM1DScz4Jv@Sxnl zB@MPPmdI{(2D?;*wd>3#tjAirmUnQoZrVv`xM3hARuJksF(Q)wd4P$88fGYOT1p6U z`AHSN!`St}}UMBT9o7i|G`r$ zrB=s$qV3d6$W9@?L!pl0lf%)xs%1ko^=QY$ty-57=55PvP(^6E7cc zGJ*>m2=;fOj?F~yBf@K@9qwX0hA803Xw+b0m}+#a(>RyR8}*Y<4b+kpp|OS+!whP( zH`v{%s>jsQI9rd$*vm)EkwOm#W_-rLTHcZRek)>AtF+~<(did)*oR1|&~1|e36d-d zgtm5cv1O0oqgWC%Et@P4Vhm}Ndl(Y#C^MD03g#PH-TFy+7!Osv1z^UWS9@%JhswEq~6kSr2DITo59+; ze=ZC}i2Q?CJ~Iyu?vn|=9iKV>4j8KbxhE4&!@SQ^dVa-gK@YfS9xT(0kpW*EDjYUkoj! zE49{7H&E}k%5(>sM4uGY)Q*&3>{aitqdNnRJkbOmD5Mp5rv-hxzOn80QsG=HJ_atI-EaP69cacR)Uvh{G5dTpYG7d zbtmRMq@Sexey)||UpnZ?;g_KMZq4IDCy5}@u!5&B^-=6yyY{}e4Hh3ee!ZWtL*s?G zxG(A!<9o!CL+q?u_utltPMk+hn?N2@?}xU0KlYg?Jco{Yf@|mSGC<(Zj^yHCvhmyx z?OxOYoxbptDK()tsJ42VzXdINAMWL$0Gcw?G(g8TMB)Khw_|v9`_ql#pRd2i*?CZl z7k1b!jQB=9-V@h%;Cnl7EKi;Y^&NhU0mWEcj8B|3L30Ku#-9389Q+(Yet0r$F=+3p z6AKOMAIi|OHyzlHZtOm73}|ntKtFaXF2Fy|M!gOh^L4^62kGUoWS1i{9gsds_GWBc zLw|TaLP64z3z9?=R2|T6Xh2W4_F*$cq>MtXMOy&=IPIJ`;!Tw?PqvI2b*U1)25^<2 zU_ZPoxg_V0tngA0J+mm?3;OYw{i2Zb4x}NedZug!>EoN3DC{1i)Z{Z4m*(y{ov2%- zk(w>+scOO}MN!exSc`TN)!B=NUX`zThWO~M*ohqq;J2hx9h9}|s#?@eR!=F{QTrq~ zTcY|>azkCe$|Q0XFUdpFT=lTcyW##i;-e{}ORB4D?t@SfqGo_cS z->?^rh$<&n9DL!CF+h?LMZRi)qju!meugvxX*&jfD!^1XB3?E?HnwHP8$;uX{Rvp# zh|)hM>XDv$ZGg=$1{+_bA~u-vXqlw6NH=nkpyWE0u}LQjF-3NhATL@9rRxMnpO%f7 z)EhZf{PF|mKIMFxnC?*78(}{Y)}iztV12}_OXffJ;ta!fcFIVjdchyHxH=t%ci`Xd zX2AUB?%?poD6Zv*&BA!6c5S#|xn~DK01#XvjT!w!;&`lDXSJT4_j$}!qSPrb37vc{ z9^NfC%QvPu@vlxaZ;mIbn-VHA6miwi8qJ~V;pTZkKqqOii<1Cs}0i?uUIss;hM4dKq^1O35y?Yp=l4i zf{M!@QHH~rJ&X~8uATV><23zZUbs-J^3}$IvV_ANLS08>k`Td7aU_S1sLsfi*C-m1 z-e#S%UGs4E!;CeBT@9}aaI)qR-6NU@kvS#0r`g&UWg?fC7|b^_HyCE!8}nyh^~o@< zpm7PDFs9yxp+byMS(JWm$NeL?DNrMCNE!I^ko-*csB+dsf4GAq{=6sfyf4wb>?v1v zmb`F*bN1KUx-`ra1+TJ37bXNP%`-Fd`vVQFTwWpX@;s(%nDQa#oWhgk#mYlY*!d>( zE&!|ySF!mIyfING+#%RDY3IBH_fW$}6~1%!G`suHub1kP@&DoAd5~7J55;5_noPI6eLf{t;@9Kf<{aO0`1WNKd?<)C-|?C?)3s z>wEq@8=I$Wc~Mt$o;g++5qR+(6wt9GI~pyrDJ%c?gPZe)owvy^J2S=+M^ z&WhIE`g;;J^xQLVeCtf7b%Dg#Z2gq9hp_%g)-%_`y*zb; zn9`f`mUPN-Ts&fFo(aNTsXPA|J!TJ{0hZp0^;MYHLOcD=r_~~^ymS8KLCSeU3;^QzJNqS z5{5rEAv#l(X?bvwxpU;2%pQftF`YFgrD1jt2^~Mt^~G>T*}A$yZc@(k9orlCGv&|1 zWWvVgiJsCAtamuAYT~nzs?TQFt<1LSEx!@e0~@yd6$b5!Zm(FpBl;(Cn>2vF?k zOm#TTjFwd2D-CyA!mqR^?#Uwm{NBemP>(pHmM}9;;8`c&+_o3#E5m)JzfwN?(f-a4 zyd%xZc^oQx3XT?vcCqCX&Qrk~nu;fxs@JUoyVoi5fqpi&bUhQ2y!Ok2pzsFR(M(|U zw3E+kH_zmTRQ9dUMZWRE%Zakiwc+lgv7Z%|YO9YxAy`y28`Aw;WU6HXBgU7fl@dnt z-fFBV)}H-gqP!1;V@Je$WcbYre|dRdp{xt!7sL3Eoa%IA`5CAA%;Wq8PktwPdULo! z8!sB}Qt8#jH9Sh}QiUtEPZ6H0b*7qEKGJ%ITZ|vH)5Q^2m<7o3#Z>AKc%z7_u`rXA zqrCy{-{8;9>dfllLu$^M5L z-hXs))h*qz%~ActwkIA(qOVBZl2v4lwbM>9l70Y`+T*elINFqt#>OaVWoja8RMsep z6Or3f=oBnA3vDbn*+HNZP?8LsH2MY)x%c13@(XfuGR}R?Nu<|07{$+Lc3$Uv^I!MQ z>6qWgd-=aG2Y^24g4{Bw9ueOR)(9h`scImD=86dD+MnSN4$6 z^U*o_mE-6Rk~Dp!ANp#5RE9n*LG(Vg`1)g6!(XtDzsov$Dvz|Gv1WU68J$CkshQhS zCrc|cdkW~UK}5NeaWj^F4MSgFM+@fJd{|LLM)}_O<{rj z+?*Lm?owq?IzC%U%9EBga~h-cJbIu=#C}XuWN>OLrc%M@Gu~kFEYUi4EC6l#PR2JS zQUkGKrrS#6H7}2l0F@S11DP`@pih0WRkRJl#F;u{c&ZC{^$Z+_*lB)r)-bPgRFE;* zl)@hK4`tEP=P=il02x7-C7p%l=B`vkYjw?YhdJU9!P!jcmY$OtC^12w?vy3<<=tlY zUwHJ_0lgWN9vf>1%WACBD{UT)1qHQSE2%z|JHvP{#INr13jM}oYv_5#xsnv9`)UAO zuwgyV4YZ;O)eSc3(mka6=aRohi!HH@I#xq7kng?Acdg7S4vDJb6cI5fw?2z%3yR+| zU5v@Hm}vy;${cBp&@D=HQ9j7NcFaOYL zj-wV=eYF{|XTkFNM2uz&T8uH~;)^Zo!=KP)EVyH6s9l1~4m}N%XzPpduPg|h-&lL` zAXspR0YMOKd2yO)eMFFJ4?sQ&!`dF&!|niH*!^*Ml##o0M(0*uK9&yzekFi$+mP9s z>W9d%Jb)PtVi&-Ha!o~Iyh@KRuKpQ@)I~L*d`{O8!kRObjO7=n+Gp36fe!66neh+7 zW*l^0tTKjLLzr`x4`_8&on?mjW-PzheTNox8Hg7Nt@*SbE-%kP2hWYmHu#Fn@Q^J(SsPUz*|EgOoZ6byg3ew88UGdZ>9B2Tq=jF72ZaR=4u%1A6Vm{O#?@dD!(#tmR;eP(Fu z{$0O%=Vmua7=Gjr8nY%>ul?w=FJ76O2js&17W_iq2*tb!i{pt#`qZB#im9Rl>?t?0c zicIC}et_4d+CpVPx)i4~$u6N-QX3H77ez z?ZdvXifFk|*F8~L(W$OWM~r`pSk5}#F?j_5u$Obu9lDWIknO^AGu+Blk7!9Sb;NjS zncZA?qtASdNtzQ>z7N871IsPAk^CC?iIL}+{K|F@BuG2>qQ;_RUYV#>hHO(HUPpk@ z(bn~4|F_jiZi}Sad;_7`#4}EmD<1EiIxa48QjUuR?rC}^HRocq`OQPM@aHVKP9E#q zy%6bmHygCpIddPjE}q_DPC`VH_2m;Eey&ZH)E6xGeStOK7H)#+9y!%-Hm|QF6w#A( zIC0Yw%9j$s-#odxG~C*^MZ?M<+&WJ+@?B_QPUyTg9DJGtQN#NIC&-XddRsf3n^AL6 zT@P|H;PvN;ZpL0iv$bRb7|J{0o!Hq+S>_NrH4@coZtBJu#g8#CbR7|#?6uxi8d+$g z87apN>EciJZ`%Zv2**_uiET9Vk{pny&My;+WfGDw4EVL#B!Wiw&M|A8f1A@ z(yFQS6jfbH{b8Z-S7D2?Ixl`j0{+ZnpT=;KzVMLW{B$`N?Gw^Fl0H6lT61%T2AU**!sX0u?|I(yoy&Xveg7XBL&+>n6jd1##6d>TxE*Vj=8lWiG$4=u{1UbAa5QD>5_ z;Te^42v7K6Mmu4IWT6Rnm>oxrl~b<~^e3vbj-GCdHLIB_>59}Ya+~OF68NiH=?}2o zP(X7EN=quQn&)fK>M&kqF|<_*H`}c zk=+x)GU>{Af#vx&s?`UKUsz})g^Pc&?Ka@t5$n$bqf6{r1>#mWx6Ep>9|A}VmWRnowVo`OyCr^fHsf# zQjQ3Ttp7y#iQY8l`zEUW)(@gGQdt(~rkxlkefskT(t%@i8=|p1Y9Dc5bc+z#n$s13 zGJk|V0+&Ekh(F};PJzQKKo+FG@KV8a<$gmNSD;7rd_nRdc%?9)p!|B-@P~kxQG}~B zi|{0}@}zKC(rlFUYp*dO1RuvPC^DQOkX4<+EwvBAC{IZQdYxoq1Za!MW7%p7gGr=j zzWnAq%)^O2$eItftC#TTSArUyL$U54-O7e|)4_7%Q^2tZ^0-d&3J1}qCzR4dWX!)4 zzIEKjgnYgMus^>6uw4Jm8ga6>GBtMjpNRJ6CP~W=37~||gMo_p@GA@#-3)+cVYnU> zE5=Y4kzl+EbEh%dhQokB{gqNDqx%5*qBusWV%!iprn$S!;oN_6E3?0+umADVs4ako z?P+t?m?};gev9JXQ#Q&KBpzkHPde_CGu-y z<{}RRAx=xlv#mVi+Ibrgx~ujW$h{?zPfhz)Kp7kmYS&_|97b&H&1;J-mzrBWAvY} zh8-I8hl_RK2+nnf&}!W0P+>5?#?7>npshe<1~&l_xqKd0_>dl_^RMRq@-Myz&|TKZBj1=Q()) zF{dBjv5)h=&Z)Aevx}+i|7=R9rG^Di!sa)sZCl&ctX4&LScQ-kMncgO(9o6W6)yd< z@Rk!vkja*X_N3H=BavGoR0@u0<}m-7|2v!0+2h~S2Q&a=lTH91OJsvms2MT~ zY=c@LO5i`mLpBd(vh|)I&^A3TQLtr>w=zoyzTd=^f@TPu&+*2MtqE$Avf>l>}V|3-8Fp2hzo3y<)hr_|NO(&oSD z!vEjTWBxbKTiShVl-U{n*B3#)3a8$`{~Pk}J@elZ=>Pqp|MQ}jrGv7KrNcjW%TN_< zZz8kG{#}XoeWf7qY?D)L)8?Q-b@Na&>i=)(@uNo zr;cH98T3$Iau8Hn*@vXi{A@YehxDE2zX~o+RY`)6-X{8~hMpc#C`|8y> zU8Mnv5A0dNCf{Ims*|l-^ z(MRp{qoGohB34|ggDI*p!Aw|MFyJ|v+<+E3brfrI)|+l3W~CQLPbnF@G0)P~Ly!1TJLp}xh8uW`Q+RB-v`MRYZ9Gam3cM%{ zb4Cb*f)0deR~wtNb*8w-LlIF>kc7DAv>T0D(a3@l`k4TFnrO+g9XH7;nYOHxjc4lq zMmaW6qpgAgy)MckYMhl?>sq;-1E)-1llUneeA!ya9KM$)DaNGu57Z5aE>=VST$#vb zFo=uRHr$0M{-ha>h(D_boS4zId;3B|Tpqo|?B?Z@I?G(?&Iei+-{9L_A9=h=Qfn-U z1wIUnQe9!z%_j$F_{rf&`ZFSott09gY~qrf@g3O=Y>vzAnXCyL!@(BqWa)Zqt!#_k zfZHuwS52|&&)aK;CHq9V-t9qt0au{$#6c*R#e5n3rje0hic7c7m{kW$p(_`wB=Gw7 z4k`1Hi;Mc@yA7dp@r~?@rfw)TkjAW++|pkfOG}0N|2guek}j8Zen(!+@7?qt_7ndX zB=BG6WJ31#F3#Vk3=aQr8T)3`{=p9nBHlKzE0I@v`{vJ}h8pd6vby&VgFhzH|q;=aonunAXL6G2y(X^CtAhWr*jI zGjpY@raZDQkg*aMq}Ni6cRF z{oWv}5`nhSAv>usX}m^GHt`f(t8@zHc?K|y5Zi=4G*UG1Sza{$Dpj%X8 zzEXaKT5N6F5j4J|w#qlZP!zS7BT)9b+!ZSJdToqJts1c!)fwih4d31vfb{}W)EgcA zH2pZ^8_k$9+WD2n`6q5XbOy8>3pcYH9 z07eUB+p}YD@AH!}p!iKv><2QF-Y^&xx^PAc1F13A{nUeCDg&{hnix#FiO!fe(^&%Qcux!h znu*S!s$&nnkeotYsDthh1dq(iQrE|#f_=xVgfiiL&-5eAcC-> z5L0l|DVEM$#ulf{bj+Y~7iD)j<~O8CYM8GW)dQGq)!mck)FqoL^X zwNdZb3->hFrbHFm?hLvut-*uK?zXn3q1z|UX{RZ;-WiLoOjnle!xs+W0-8D)kjU#R z+S|A^HkRg$Ij%N4v~k`jyHffKaC~=wg=9)V5h=|kLQ@;^W!o2^K+xG&2n`XCd>OY5Ydi= zgHH=lgy++erK8&+YeTl7VNyVm9-GfONlSlVb3)V9NW5tT!cJ8d7X)!b-$fb!s76{t z@d=Vg-5K_sqHA@Zx-L_}wVnc@L@GL9_K~Zl(h5@AR#FAiKad8~KeWCo@mgXIQ#~u{ zgYFwNz}2b6Vu@CP0XoqJ+dm8px(5W5-Jpis97F`+KM)TuP*X8H@zwiVKDKGVp59pI zifNHZr|B+PG|7|Y<*tqap0CvG7tbR1R>jn70t1X`XJixiMVcHf%Ez*=xm1(CrTSDt z0cle!+{8*Ja&EOZ4@$qhBuKQ$U95Q%rc7tg$VRhk?3=pE&n+T3upZg^ZJc9~c2es% zh7>+|mrmA-p&v}|OtxqmHIBgUxL~^0+cpfkSK2mhh+4b=^F1Xgd2)}U*Yp+H?ls#z zrLxWg_hm}AfK2XYWr!rzW4g;+^^&bW%LmbtRai9f3PjU${r@n`JThy-cphbcwn)rq9{A$Ht`lmYKxOacy z6v2R(?gHhD5@&kB-Eg?4!hAoD7~(h>(R!s1c1Hx#s9vGPePUR|of32bS`J5U5w{F) z>0<^ktO2UHg<0{oxkdOQ;}coZDQph8p6ruj*_?uqURCMTac;>T#v+l1Tc~%^k-Vd@ zkc5y35jVNc49vZpZx;gG$h{%yslDI%Lqga1&&;mN{Ush1c7p>7e-(zp}6E7f-XmJb4nhk zb8zS+{IVbL$QVF8pf8}~kQ|dHJAEATmmnrb_wLG}-yHe>W|A&Y|;muy-d^t^<&)g5SJfaTH@P1%euONny=mxo+C z4N&w#biWY41r8k~468tvuYVh&XN&d#%QtIf9;iVXfWY)#j=l`&B~lqDT@28+Y!0E+MkfC}}H*#(WKKdJJq=O$vNYCb(ZG@p{fJgu;h z21oHQ(14?LeT>n5)s;uD@5&ohU!@wX8w*lB6i@GEH0pM>YTG+RAIWZD;4#F1&F%Jp zXZUml2sH0!lYJT?&sA!qwez6cXzJEd(1ZC~kT5kZSp7(@=H2$Azb_*W&6aA|9iwCL zdX7Q=42;@dspHDwYE?miGX#L^3xD&%BI&fN9^;`v4OjQXPBaBmOF1;#C)8XA(WFlH zycro;DS2?(G&6wkr6rqC>rqDv3nfGw3hmN_9Al>TgvmGsL8_hXx09};l9Ow@)F5@y z#VH5WigLDwZE4nh^7&@g{1FV^UZ%_LJ-s<{HN*2R$OPg@R~Z`c-ET*2}XB@9xvAjrK&hS=f|R8Gr9 zr|0TGOsI7RD+4+2{ZiwdVD@2zmg~g@^D--YL;6UYGSM8i$NbQr4!c7T9rg!8;TM0E zT#@?&S=t>GQm)*ua|?TLT2ktj#`|R<_*FAkOu2Pz$wEc%-=Y9V*$&dg+wIei3b*O8 z2|m$!jJG!J!ZGbbIa!(Af~oSyZV+~M1qGvelMzPNE_%5?c2>;MeeG2^N?JDKjFYCy z7SbPWH-$cWF9~fX%9~v99L!G(wi!PFp>rB!9xj7=Cv|F+7CsGNwY0Q_J%FID%C^CBZQfJ9K(HK%k31j~e#&?hQ zNuD6gRkVckU)v+53-fc} z7ZCzYN-5RG4H7;>>Hg?LU9&5_aua?A0)0dpew1#MMlu)LHe(M;OHjHIUl7|%%)YPo z0cBk;AOY00%Fe6heoN*$(b<)Cd#^8Iu;-2v@>cE-OB$icUF9EEoaC&q8z9}jMTT2I z8`9;jT%z0;dy4!8U;GW{i`)3!c6&oWY`J3669C!tM<5nQFFrFRglU8f)5Op$GtR-3 zn!+SPCw|04sv?%YZ(a7#L?vsdr7ss@WKAw&A*}-1S|9~cL%uA+E~>N6QklFE>8W|% zyX-qAUGTY1hQ-+um`2|&ji0cY*(qN!zp{YpDO-r>jPk*yuVSay<)cUt`t@&FPF_&$ zcHwu1(SQ`I-l8~vYyUxm@D1UEdFJ$f5Sw^HPH7b!9 zzYT3gKMF((N(v0#4f_jPfVZ=ApN^jQJe-X$`A?X+vWjLn_%31KXE*}5_}d8 zw_B1+a#6T1?>M{ronLbHIlEsMf93muJ7AH5h%;i99<~JX^;EAgEB1uHralD*!aJ@F zV2ruuFe9i2Q1C?^^kmVy921eb=tLDD43@-AgL^rQ3IO9%+vi_&R2^dpr}x{bCVPej z7G0-0o64uyWNtr*loIvslyo0%)KSDDKjfThe0hcqs)(C-MH1>bNGBDRTW~scy_{w} zp^aq8Qb!h9Lwielq%C1b8=?Z=&U)ST&PHbS)8Xzjh2DF?d{iAv)Eh)wsUnf>UtXN( zL7=$%YrZ#|^c{MYmhn!zV#t*(jdmYdCpwqpZ{v&L8KIuKn`@IIZfp!uo}c;7J57N` zAxyZ-uA4=Gzl~Ovycz%MW9ZL7N+nRo&1cfNn9(1H5eM;V_4Z_qVann7F>5f>%{rf= zPBZFaV@_Sobl?Fy&KXyzFDV*FIdhS5`Uc~S^Gjo)aiTHgn#<0C=9o-a-}@}xDor;D zZyZ|fvf;+=3MZd>SR1F^F`RJEZo+|MdyJYQAEauKu%WDol~ayrGU3zzbHKsnHKZ*z zFiwUkL@DZ>!*x05ql&EBq@_Vqv83&?@~q5?lVmffQZ+V-=qL+!u4Xs2Z2zdCQ3U7B&QR9_Iggy} z(om{Y9eU;IPe`+p1ifLx-XWh?wI)xU9ik+m#g&pGdB5Bi<`PR*?92lE0+TkRuXI)z z5LP!N2+tTc%cB6B1F-!fj#}>S!vnpgVU~3!*U1ej^)vjUH4s-bd^%B=ItQqDCGbrEzNQi(dJ`J}-U=2{7-d zK8k^Rlq2N#0G?9&1?HSle2vlkj^KWSBYTwx`2?9TU_DX#J+f+qLiZCqY1TXHFxXZqYMuD@RU$TgcnCC{_(vwZ-*uX)~go#%PK z@}2Km_5aQ~(<3cXeJN6|F8X_1@L%@xTzs}$_*E|a^_URF_qcF;Pfhoe?FTFwvjm1o z8onf@OY@jC2tVcMaZS;|T!Ks(wOgPpRzRnFS-^RZ4E!9dsnj9sFt609a|jJbb1Dt@ z<=Gal2jDEupxUSwWu6zp<<&RnAA;d&4gKVG0iu6g(DsST(4)z6R)zDpfaQ}v{5ARt zyhwvMtF%b-YazR5XLz+oh=mn;y-Mf2a8>7?2v8qX;19y?b>Z5laGHvzH;Nu9S`B8} zI)qN$GbXIQ1VL3lnof^6TS~rvPVg4V?Dl2Bb*K2z4E{5vy<(@@K_cN@U>R!>aUIRnb zL*)=787*cs#zb31zBC49x$`=fkQbMAef)L2$dR{)6BAz!t5U_B#1zZG`^neKSS22oJ#5B=gl%U=WeqL9REF2g zZnfCb0?quf?Ztj$VXvDSWoK`0L=Zxem2q}!XWLoT-kYMOx)!7fcgT35uC~0pySEme z`{wGWTkGr7>+Kb^n;W?BZH6ZP(9tQX%-7zF>vc2}LuWDI(9kh1G#7B99r4x6;_-V+k&c{nPUrR zAXJGRiMe~aup{0qzmLNjS_BC4cB#sXjckx{%_c&^xy{M61xEb>KW_AG5VFXUOjAG4 z^>Qlm9A#1N{4snY=(AmWzatb!ngqiqPbBZ7>Uhb3)dTkSGcL#&SH>iMO-IJBPua`u zo)LWZ>=NZLr758j{%(|uQuZ)pXq_4c!!>s|aDM9#`~1bzK3J1^^D#<2bNCccH7~-X}Ggi!pIIF>uFx%aPARGQsnC8ZQc8lrQ5o~smqOg>Ti^GNme94*w z)JZy{_{#$jxGQ&`M z!OMvZMHR>8*^>eS%o*6hJwn!l8VOOjZQJvh)@tnHVW&*GYPuxqXw}%M!(f-SQf`=L z5;=5w2;%82VMH6Xi&-K3W)o&K^+vJCepWZ-rW%+Dc6X3(){z$@4zjYxQ|}8UIojeC zYZpQ1dU{fy=oTr<4VX?$q)LP}IUmpiez^O&N3E_qPpchGTi5ZM6-2ScWlQq%V&R2Euz zO|Q0Hx>lY1Q1cW5xHv5!0OGU~PVEqSuy#fD72d#O`N!C;o=m+YioGu-wH2k6!t<~K zSr`E=W9)!g==~x9VV~-8{4ZN9{~-A9zJpRe%NGg$+MDuI-dH|b@BD)~>pPCGUNNzY zMDg||0@XGQgw`YCt5C&A{_+J}mvV9Wg{6V%2n#YSRN{AP#PY?1FF1#|vO_%e+#`|2*~wGAJaeRX6=IzFNeWhz6gJc8+(03Ph4y6ELAm=AkN7TOgMUEw*N{= z_)EIDQx5q22oUR+_b*tazu9+pX|n1c*IB-}{DqIj z-?E|ks{o3AGRNb;+iKcHkZvYJvFsW&83RAPs1Oh@IWy%l#5x2oUP6ZCtv+b|q>jsf zZ_9XO;V!>n`UxH1LvH8)L4?8raIvasEhkpQoJ`%!5rBs!0Tu(s_D{`4opB;57)pkX z4$A^8CsD3U5*!|bHIEqsn~{q+Ddj$ME@Gq4JXtgVz&7l{Ok!@?EA{B3P~NAqb9)4? zkQo30A^EbHfQ@87G5&EQTd`frrwL)&Yw?%-W@uy^Gn23%j?Y!Iea2xw<-f;esq zf%w5WN@E1}zyXtYv}}`U^B>W`>XPmdLj%4{P298|SisrE;7HvXX;A}Ffi8B#3Lr;1 zHt6zVb`8{#+e$*k?w8|O{Uh|&AG}|DG1PFo1i?Y*cQm$ZwtGcVgMwtBUDa{~L1KT-{jET4w60>{KZ27vXrHJ;fW{6| z=|Y4!&UX020wU1>1iRgB@Q#m~1^Z^9CG1LqDhYBrnx%IEdIty z!46iOoKlKs)c}newDG)rWUikD%j`)p z_w9Ph&e40=(2eBy;T!}*1p1f1SAUDP9iWy^u^Ubdj21Kn{46;GR+hwLO=4D11@c~V zI8x&(D({K~Df2E)Nx_yQvYfh4;MbMJ@Z}=Dt3_>iim~QZ*hZIlEs0mEb z_54+&*?wMD`2#vsQRN3KvoT>hWofI_Vf(^C1ff-Ike@h@saEf7g}<9T`W;HAne-Nd z>RR+&SP35w)xKn8^U$7))PsM!jKwYZ*RzEcG-OlTrX3}9a{q%#Un5E5W{{hp>w~;` zGky+3(vJvQyGwBo`tCpmo0mo((?nM8vf9aXrrY1Ve}~TuVkB(zeds^jEfI}xGBCM2 zL1|#tycSaWCurP+0MiActG3LCas@_@tao@(R1ANlwB$4K53egNE_;!&(%@Qo$>h`^1S_!hN6 z)vZtG$8fN!|BXBJ=SI>e(LAU(y(i*PHvgQ2llulxS8>qsimv7yL}0q_E5WiAz7)(f zC(ahFvG8&HN9+6^jGyLHM~$)7auppeWh_^zKk&C_MQ~8;N??OlyH~azgz5fe^>~7F zl3HnPN3z-kN)I$4@`CLCMQx3sG~V8hPS^}XDXZrQA>}mQPw%7&!sd(Pp^P=tgp-s^ zjl}1-KRPNWXgV_K^HkP__SR`S-|OF0bR-N5>I%ODj&1JUeAQ3$9i;B~$S6}*^tK?= z**%aCiH7y?xdY?{LgVP}S0HOh%0%LI$wRx;$T|~Y8R)Vdwa}kGWv8?SJVm^>r6+%I z#lj1aR94{@MP;t-scEYQWc#xFA30^}?|BeX*W#9OL;Q9#WqaaM546j5j29((^_8Nu z4uq}ESLr~r*O7E7$D{!k9W>`!SLoyA53i9QwRB{!pHe8um|aDE`Cg0O*{jmor)^t)3`>V>SWN-2VJcFmj^1?~tT=JrP`fVh*t zXHarp=8HEcR#vFe+1a%XXuK+)oFs`GDD}#Z+TJ}Ri`FvKO@ek2ayn}yaOi%(8p%2$ zpEu)v0Jym@f}U|-;}CbR=9{#<^z28PzkkTNvyKvJDZe+^VS2bES3N@Jq!-*}{oQlz z@8bgC_KnDnT4}d#&Cpr!%Yb?E!brx0!eVOw~;lLwUoz#Np%d$o%9scc3&zPm`%G((Le|6o1 zM(VhOw)!f84zG^)tZ1?Egv)d8cdNi+T${=5kV+j;Wf%2{3g@FHp^Gf*qO0q!u$=m9 zCaY`4mRqJ;FTH5`a$affE5dJrk~k`HTP_7nGTY@B9o9vvnbytaID;^b=Tzp7Q#DmD zC(XEN)Ktn39z5|G!wsVNnHi) z%^q94!lL|hF`IijA^9NR0F$@h7k5R^ljOW(;Td9grRN0Mb)l_l7##{2nPQ@?;VjXv zaLZG}yuf$r$<79rVPpXg?6iiieX|r#&`p#Con2i%S8*8F}(E) zI5E6c3tG*<;m~6>!&H!GJ6zEuhH7mkAzovdhLy;)q z{H2*8I^Pb}xC4s^6Y}6bJvMu=8>g&I)7!N!5QG$xseeU#CC?ZM-TbjsHwHgDGrsD= z{%f;@Sod+Ch66Ko2WF~;Ty)v>&x^aovCbCbD7>qF*!?BXmOV3(s|nxsb*Lx_2lpB7 zokUnzrk;P=T-&kUHO}td+Zdj!3n&NR?K~cRU zAXU!DCp?51{J4w^`cV#ye}(`SQhGQkkMu}O3M*BWt4UsC^jCFUy;wTINYmhD$AT;4 z?Xd{HaJjP`raZ39qAm;%beDbrLpbRf(mkKbANan7XsL>_pE2oo^$TgdidjRP!5-`% zv0d!|iKN$c0(T|L0C~XD0aS8t{*&#LnhE;1Kb<9&=c2B+9JeLvJr*AyyRh%@jHej=AetOMSlz^=!kxX>>B{2B1uIrQyfd8KjJ+DBy!h)~*(!|&L4^Q_07SQ~E zcemVP`{9CwFvPFu7pyVGCLhH?LhEVb2{7U+Z_>o25#+3<|8%1T^5dh}*4(kfJGry} zm%r#hU+__Z;;*4fMrX=Bkc@7|v^*B;HAl0((IBPPii%X9+u3DDF6%bI&6?Eu$8&aWVqHIM7mK6?Uvq$1|(-T|)IV<>e?!(rY zqkmO1MRaLeTR=)io(0GVtQT@s6rN%C6;nS3@eu;P#ry4q;^O@1ZKCJyp_Jo)Ty^QW z+vweTx_DLm{P-XSBj~Sl<%_b^$=}odJ!S2wAcxenmzFGX1t&Qp8Vxz2VT`uQsQYtdn&_0xVivIcxZ_hnrRtwq4cZSj1c-SG9 z7vHBCA=fd0O1<4*=lu$6pn~_pVKyL@ztw1swbZi0B?spLo56ZKu5;7ZeUml1Ws1?u zqMf1p{5myAzeX$lAi{jIUqo1g4!zWLMm9cfWcnw`k6*BR^?$2(&yW?>w;G$EmTA@a z6?y#K$C~ZT8+v{87n5Dm&H6Pb_EQ@V0IWmG9cG=O;(;5aMWWrIPzz4Q`mhK;qQp~a z+BbQrEQ+w{SeiuG-~Po5f=^EvlouB@_|4xQXH@A~KgpFHrwu%dwuCR)=B&C(y6J4J zvoGk9;lLs9%iA-IJGU#RgnZZR+@{5lYl8(e1h6&>Vc_mvg0d@);X zji4T|n#lB!>pfL|8tQYkw?U2bD`W{na&;*|znjmalA&f;*U++_aBYerq;&C8Kw7mI z7tsG*?7*5j&dU)Lje;^{D_h`%(dK|pB*A*1(Jj)w^mZ9HB|vGLkF1GEFhu&rH=r=8 zMxO42e{Si6$m+Zj`_mXb&w5Q(i|Yxyg?juUrY}78uo@~3v84|8dfgbPd0iQJRdMj< zncCNGdMEcsxu#o#B5+XD{tsg*;j-eF8`mp~K8O1J!Z0+>0=7O=4M}E?)H)ENE;P*F z$Ox?ril_^p0g7xhDUf(q652l|562VFlC8^r8?lQv;TMvn+*8I}&+hIQYh2 z1}uQQaag&!-+DZ@|C+C$bN6W;S-Z@)d1|en+XGvjbOxCa-qAF*LA=6s(Jg+g;82f$ z(Vb)8I)AH@cdjGFAR5Rqd0wiNCu!xtqWbcTx&5kslzTb^7A78~Xzw1($UV6S^VWiP zFd{Rimd-0CZC_Bu(WxBFW7+k{cOW7DxBBkJdJ;VsJ4Z@lERQr%3eVv&$%)b%<~ zCl^Y4NgO}js@u{|o~KTgH}>!* z_iDNqX2(As7T0xivMH|3SC1ivm8Q}6Ffcd7owUKN5lHAtzMM4<0v+ykUT!QiowO;`@%JGv+K$bBx@*S7C8GJVqQ_K>12}M`f_Ys=S zKFh}HM9#6Izb$Y{wYzItTy+l5U2oL%boCJn?R3?jP@n$zSIwlmyGq30Cw4QBO|14` zW5c);AN*J3&eMFAk$SR~2k|&+&Bc$e>s%c{`?d~85S-UWjA>DS5+;UKZ}5oVa5O(N zqqc@>)nee)+4MUjH?FGv%hm2{IlIF-QX}ym-7ok4Z9{V+ZHVZQl$A*x!(q%<2~iVv znUa+BX35&lCb#9VE-~Y^W_f;Xhl%vgjwdjzMy$FsSIj&ok}L+X`4>J=9BkN&nu^E*gbhj3(+D>C4E z@Fwq_=N)^bKFSHTzZk?-gNU$@l}r}dwGyh_fNi=9b|n}J>&;G!lzilbWF4B}BBq4f zYIOl?b)PSh#XTPp4IS5ZR_2C!E)Z`zH0OW%4;&~z7UAyA-X|sh9@~>cQW^COA9hV4 zXcA6qUo9P{bW1_2`eo6%hgbN%(G-F1xTvq!sc?4wN6Q4`e9Hku zFwvlAcRY?6h^Fj$R8zCNEDq8`=uZB8D-xn)tA<^bFFy}4$vA}Xq0jAsv1&5!h!yRA zU()KLJya5MQ`q&LKdH#fwq&(bNFS{sKlEh_{N%{XCGO+po#(+WCLmKW6&5iOHny>g z3*VFN?mx!16V5{zyuMWDVP8U*|BGT$(%IO|)?EF|OI*sq&RovH!N%=>i_c?K*A>>k zyg1+~++zY4Q)J;VWN0axhoIKx;l&G$gvj(#go^pZskEVj8^}is3Jw26LzYYVos0HX zRPvmK$dVxM8(Tc?pHFe0Z3uq){{#OK3i-ra#@+;*=ui8)y6hsRv z4Fxx1c1+fr!VI{L3DFMwXKrfl#Q8hfP@ajgEau&QMCxd{g#!T^;ATXW)nUg&$-n25 zruy3V!!;{?OTobo|0GAxe`Acn3GV@W=&n;~&9 zQM>NWW~R@OYORkJAo+eq1!4vzmf9K%plR4(tB@TR&FSbDoRgJ8qVcH#;7lQub*nq&?Z>7WM=oeEVjkaG zT#f)=o!M2DO5hLR+op>t0CixJCIeXH*+z{-XS|%jx)y(j&}Wo|3!l7{o)HU3m7LYyhv*xF&tq z%IN7N;D4raue&&hm0xM=`qv`+TK@;_xAcGKuK(2|75~ar2Yw)geNLSmVxV@x89bQu zpViVKKnlkwjS&&c|-X6`~xdnh}Ps)Hs z4VbUL^{XNLf7_|Oi>tA%?SG5zax}esF*FH3d(JH^Gvr7Rp*n=t7frH!U;!y1gJB^i zY_M$KL_}mW&XKaDEi9K-wZR|q*L32&m+2n_8lq$xRznJ7p8}V>w+d@?uB!eS3#u<} zIaqi!b!w}a2;_BfUUhGMy#4dPx>)_>yZ`ai?Rk`}d0>~ce-PfY-b?Csd(28yX22L% zI7XI>OjIHYTk_@Xk;Gu^F52^Gn6E1&+?4MxDS2G_#PQ&yXPXP^<-p|2nLTb@AAQEY zI*UQ9Pmm{Kat}wuazpjSyXCdnrD&|C1c5DIb1TnzF}f4KIV6D)CJ!?&l&{T)e4U%3HTSYqsQ zo@zWB1o}ceQSV)<4G<)jM|@@YpL+XHuWsr5AYh^Q{K=wSV99D~4RRU52FufmMBMmd z_H}L#qe(}|I9ZyPRD6kT>Ivj&2Y?qVZq<4bG_co_DP`sE*_Xw8D;+7QR$Uq(rr+u> z8bHUWbV19i#)@@G4bCco@Xb<8u~wVDz9S`#k@ciJtlu@uP1U0X?yov8v9U3VOig2t zL9?n$P3=1U_Emi$#slR>N5wH-=J&T=EdUHA}_Z zZIl3nvMP*AZS9{cDqFanrA~S5BqxtNm9tlu;^`)3X&V4tMAkJ4gEIPl= zoV!Gyx0N{3DpD@)pv^iS*dl2FwANu;1;%EDl}JQ7MbxLMAp>)UwNwe{=V}O-5C*>F zu?Ny+F64jZn<+fKjF01}8h5H_3pey|;%bI;SFg$w8;IC<8l|3#Lz2;mNNik6sVTG3 z+Su^rIE#40C4a-587$U~%KedEEw1%r6wdvoMwpmlXH$xPnNQN#f%Z7|p)nC>WsuO= z4zyqapLS<8(UJ~Qi9d|dQijb_xhA2)v>la)<1md5s^R1N&PiuA$^k|A<+2C?OiHbj z>Bn$~t)>Y(Zb`8hW7q9xQ=s>Rv81V+UiuZJc<23HplI88isqRCId89fb`Kt|CxVIg znWcwprwXnotO>3s&Oypkte^9yJjlUVVxSe%_xlzmje|mYOVPH^vjA=?6xd0vaj0Oz zwJ4OJNiFdnHJX3rw&inskjryukl`*fRQ#SMod5J|KroJRsVXa5_$q7whSQ{gOi*s0 z1LeCy|JBWRsDPn7jCb4s(p|JZiZ8+*ExC@Vj)MF|*Vp{B(ziccSn`G1Br9bV(v!C2 z6#?eqpJBc9o@lJ#^p-`-=`4i&wFe>2)nlPK1p9yPFzJCzBQbpkcR>={YtamIw)3nt z(QEF;+)4`>8^_LU)_Q3 zC5_7lgi_6y>U%m)m@}Ku4C}=l^J=<<7c;99ec3p{aR+v=diuJR7uZi%aQv$oP?dn?@6Yu_+*^>T0ptf(oobdL;6)N-I!TO`zg^Xbv3#L0I~sn@WGk-^SmPh5>W+LB<+1PU}AKa?FCWF|qMNELOgdxR{ zbqE7@jVe+FklzdcD$!(A$&}}H*HQFTJ+AOrJYnhh}Yvta(B zQ_bW4Rr;R~&6PAKwgLWXS{Bnln(vUI+~g#kl{r+_zbngT`Y3`^Qf=!PxN4IYX#iW4 zucW7@LLJA9Zh3(rj~&SyN_pjO8H&)|(v%!BnMWySBJV=eSkB3YSTCyIeJ{i;(oc%_hk{$_l;v>nWSB)oVeg+blh=HB5JSlG_r7@P z3q;aFoZjD_qS@zygYqCn=;Zxjo!?NK!%J$ z52lOP`8G3feEj+HTp@Tnn9X~nG=;tS+z}u{mQX_J0kxtr)O30YD%oo)L@wy`jpQYM z@M>Me=95k1p*FW~rHiV1CIfVc{K8r|#Kt(ApkXKsDG$_>76UGNhHExFCw#Ky9*B-z zNq2ga*xax!HMf_|Vp-86r{;~YgQKqu7%szk8$hpvi_2I`OVbG1doP(`gn}=W<8%Gn z%81#&WjkH4GV;4u43EtSW>K_Ta3Zj!XF?;SO3V#q=<=>Tc^@?A`i;&`-cYj|;^ zEo#Jl5zSr~_V-4}y8pnufXLa80vZY4z2ko7fj>DR)#z=wWuS1$$W!L?(y}YC+yQ|G z@L&`2upy3f>~*IquAjkVNU>}c10(fq#HdbK$~Q3l6|=@-eBbo>B9(6xV`*)sae58*f zym~RRVx;xoCG3`JV`xo z!lFw)=t2Hy)e!IFs?0~7osWk(d%^wxq&>_XD4+U#y&-VF%4z?XH^i4w`TxpF{`XhZ z%G}iEzf!T(l>g;W9<~K+)$g!{UvhW{E0Lis(S^%I8OF&%kr!gJ&fMOpM=&=Aj@wuL zBX?*6i51Qb$uhkwkFYkaD_UDE+)rh1c;(&Y=B$3)J&iJfQSx!1NGgPtK!$c9OtJuu zX(pV$bfuJpRR|K(dp@^j}i&HeJOh@|7lWo8^$*o~Xqo z5Sb+!EtJ&e@6F+h&+_1ETbg7LfP5GZjvIUIN3ibCOldAv z)>YdO|NH$x7AC8dr=<2ekiY1%fN*r~e5h6Yaw<{XIErujKV~tiyrvV_DV0AzEknC- zR^xKM3i<1UkvqBj3C{wDvytOd+YtDSGu!gEMg+!&|8BQrT*|p)(dwQLEy+ zMtMzij3zo40)CA!BKZF~yWg?#lWhqD3@qR)gh~D{uZaJO;{OWV8XZ_)J@r3=)T|kt zUS1pXr6-`!Z}w2QR7nP%d?ecf90;K_7C3d!UZ`N(TZoWNN^Q~RjVhQG{Y<%E1PpV^4 z-m-K+$A~-+VDABs^Q@U*)YvhY4Znn2^w>732H?NRK(5QSS$V@D7yz2BVX4)f5A04~$WbxGOam22>t&uD)JB8-~yiQW6ik;FGblY_I>SvB_z2?PS z*Qm&qbKI{H1V@YGWzpx`!v)WeLT02};JJo*#f$a*FH?IIad-^(;9XC#YTWN6;Z6+S zm4O1KH=#V@FJw7Pha0!9Vb%ZIM$)a`VRMoiN&C|$YA3~ZC*8ayZRY^fyuP6$n%2IU z$#XceYZeqLTXw(m$_z|33I$B4k~NZO>pP6)H_}R{E$i%USGy{l{-jOE;%CloYPEU+ zRFxOn4;7lIOh!7abb23YKD+_-?O z0FP9otcAh+oSj;=f#$&*ExUHpd&e#bSF%#8*&ItcL2H$Sa)?pt0Xtf+t)z$_u^wZi z44oE}r4kIZGy3!Mc8q$B&6JqtnHZ>Znn!Zh@6rgIu|yU+zG8q`q9%B18|T|oN3zMq z`l&D;U!OL~%>vo&q0>Y==~zLiCZk4v%s_7!9DxQ~id1LLE93gf*gg&2$|hB#j8;?3 z5v4S;oM6rT{Y;I+#FdmNw z){d%tNM<<#GN%n9ox7B=3#;u7unZ~tLB_vRZ52a&2=IM)2VkXm=L+Iqq~uk#Dug|x z>S84e+A7EiOY5lj*!q?6HDkNh~0g;0Jy(al!ZHHDtur9T$y-~)94HelX1NHjXWIM7UAe}$?jiz z9?P4`I0JM=G5K{3_%2jPLC^_Mlw?-kYYgb7`qGa3@dn|^1fRMwiyM@Ch z;CB&o7&&?c5e>h`IM;Wnha0QKnEp=$hA8TJgR-07N~U5(>9vJzeoFsSRBkDq=x(YgEMpb=l4TDD`2 zwVJpWGTA_u7}?ecW7s6%rUs&NXD3+n;jB86`X?8(l3MBo6)PdakI6V6a}22{)8ilT zM~T*mU}__xSy|6XSrJ^%lDAR3Lft%+yxC|ZUvSO_nqMX!_ul3;R#*{~4DA=h$bP)%8Yv9X zyp><|e8=_ttI}ZAwOd#dlnSjck#6%273{E$kJuCGu=I@O)&6ID{nWF5@gLb16sj|&Sb~+du4e4O_%_o`Ix4NRrAsyr1_}MuP94s>de8cH-OUkVPk3+K z&jW)It9QiU-ti~AuJkL`XMca8Oh4$SyJ=`-5WU<{cIh+XVH#e4d&zive_UHC!pN>W z3TB;Mn5i)9Qn)#6@lo4QpI3jFYc0~+jS)4AFz8fVC;lD^+idw^S~Qhq>Tg(!3$yLD zzktzoFrU@6s4wwCMz}edpF5i5Q1IMmEJQHzp(LAt)pgN3&O!&d?3W@6U4)I^2V{;- z6A(?zd93hS*uQmnh4T)nHnE{wVhh(=MMD(h(P4+^p83Om6t<*cUW>l(qJzr%5vp@K zN27ka(L{JX=1~e2^)F^i=TYj&;<7jyUUR2Bek^A8+3Up*&Xwc{)1nRR5CT8vG>ExV zHnF3UqXJOAno_?bnhCX-&kwI~Ti8t4`n0%Up>!U`ZvK^w2+0Cs-b9%w%4`$+To|k= zKtgc&l}P`*8IS>8DOe?EB84^kx4BQp3<7P{Pq}&p%xF_81pg!l2|u=&I{AuUgmF5n zJQCTLv}%}xbFGYtKfbba{CBo)lWW%Z>i(_NvLhoQZ*5-@2l&x>e+I~0Nld3UI9tdL zRzu8}i;X!h8LHVvN?C+|M81e>Jr38%&*9LYQec9Ax>?NN+9(_>XSRv&6hlCYB`>Qm z1&ygi{Y()OU4@D_jd_-7vDILR{>o|7-k)Sjdxkjgvi{@S>6GqiF|o`*Otr;P)kLHN zZkpts;0zw_6;?f(@4S1FN=m!4^mv~W+lJA`&7RH%2$)49z0A+8@0BCHtj|yH--AEL z0tW6G%X-+J+5a{5*WKaM0QDznf;V?L5&uQw+yegDNDP`hA;0XPYc6e0;Xv6|i|^F2WB)Z$LR|HR4 zTQsRAby9(^Z@yATyOgcfQw7cKyr^3Tz7lc7+JEwwzA7)|2x+PtEb>nD(tpxJQm)Kn zW9K_*r!L%~N*vS8<5T=iv|o!zTe9k_2jC_j*7ik^M_ zaf%k{WX{-;0*`t`G!&`eW;gChVXnJ-Rn)To8vW-?>>a%QU1v`ZC=U)f8iA@%JG0mZ zDqH;~mgBnrCP~1II<=V9;EBL)J+xzCoiRBaeH&J6rL!{4zIY8tZka?_FBeQeNO3q6 zyG_alW54Ba&wQf{&F1v-r1R6ID)PTsqjIBc+5MHkcW5Fnvi~{-FjKe)t1bl}Y;z@< z=!%zvpRua>>t_x}^}z0<7MI!H2v6|XAyR9!t50q-A)xk0nflgF4*OQlCGK==4S|wc zRMsSscNhRzHMBU8TdcHN!q^I}x0iXJ%uehac|Zs_B$p@CnF)HeXPpB_Za}F{<@6-4 zl%kml@}kHQ(ypD8FsPJ2=14xXJE|b20RUIgs!2|R3>LUMGF6X*B_I|$`Qg=;zm7C z{mEDy9dTmPbued7mlO@phdmAmJ7p@GR1bjCkMw6*G7#4+`k>fk1czdJUB!e@Q(~6# zwo%@p@V5RL0ABU2LH7Asq^quDUho@H>eTZH9f*no9fY0T zD_-9px3e}A!>>kv5wk91%C9R1J_Nh!*&Kk$J3KNxC}c_@zlgpJZ+5L)Nw|^p=2ue}CJtm;uj*Iqr)K})kA$xtNUEvX;4!Px*^&9T_`IN{D z{6~QY=Nau6EzpvufB^hflc#XIsSq0Y9(nf$d~6ZwK}fal92)fr%T3=q{0mP-EyP_G z)UR5h@IX}3Qll2b0oCAcBF>b*@Etu*aTLPU<%C>KoOrk=x?pN!#f_Og-w+;xbFgjQ zXp`et%lDBBh~OcFnMKMUoox0YwBNy`N0q~bSPh@+enQ=4RUw1) zpovN`QoV>vZ#5LvC;cl|6jPr}O5tu!Ipoyib8iXqy}TeJ;4+_7r<1kV0v5?Kv>fYp zg>9L`;XwXa&W7-jf|9~uP2iyF5`5AJ`Q~p4eBU$MCC00`rcSF>`&0fbd^_eqR+}mK z4n*PMMa&FOcc)vTUR zlDUAn-mh`ahi_`f`=39JYTNVjsTa_Y3b1GOIi)6dY)D}xeshB0T8Eov5%UhWd1)u}kjEQ|LDo{tqKKrYIfVz~@dp!! zMOnah@vp)%_-jDTUG09l+;{CkDCH|Q{NqX*uHa1YxFShy*1+;J`gywKaz|2Q{lG8x zP?KBur`}r`!WLKXY_K;C8$EWG>jY3UIh{+BLv0=2)KH%P}6xE2kg)%(-uA6lC?u8}{K(#P*c zE9C8t*u%j2r_{;Rpe1A{9nNXU;b_N0vNgyK!EZVut~}+R2rcbsHilqsOviYh-pYX= zHw@53nlmwYI5W5KP>&`dBZe0Jn?nAdC^HY1wlR6$u^PbpB#AS&5L6zqrXN&7*N2Q` z+Rae1EwS)H=aVSIkr8Ek^1jy2iS2o7mqm~Mr&g5=jjt7VxwglQ^`h#Mx+x2v|9ZAwE$i_9918MjJxTMr?n!bZ6n$}y11u8I9COTU`Z$Fi z!AeAQLMw^gp_{+0QTEJrhL424pVDp%wpku~XRlD3iv{vQ!lAf!_jyqd_h}+Tr1XG| z`*FT*NbPqvHCUsYAkFnM`@l4u_QH&bszpUK#M~XLJt{%?00GXY?u_{gj3Hvs!=N(I z(=AuWPijyoU!r?aFTsa8pLB&cx}$*%;K$e*XqF{~*rA-qn)h^!(-;e}O#B$|S~c+U zN4vyOK0vmtx$5K!?g*+J@G1NmlEI=pyZXZ69tAv=@`t%ag_Hk{LP~OH9iE)I= zaJ69b4kuCkV0V zo(M0#>phpQ_)@j;h%m{-a*LGi(72TP)ws2w*@4|C-3+;=5DmC4s7Lp95%n%@Ko zfdr3-a7m*dys9iIci$A=4NPJ`HfJ;hujLgU)ZRuJI`n;Pw|yksu!#LQnJ#dJysgNb z@@qwR^wrk(jbq4H?d!lNyy72~Dnn87KxsgQ!)|*m(DRM+eC$wh7KnS-mho3|KE)7h zK3k;qZ;K1Lj6uEXLYUYi)1FN}F@-xJ z@@3Hb84sl|j{4$3J}aTY@cbX@pzB_qM~APljrjju6P0tY{C@ zpUCOz_NFmALMv1*blCcwUD3?U6tYs+N%cmJ98D%3)%)Xu^uvzF zS5O!sc#X6?EwsYkvPo6A%O8&y8sCCQH<%f2togVwW&{M;PR!a(ZT_A+jVAbf{@5kL zB@Z(hb$3U{T_}SKA_CoQVU-;j>2J=L#lZ~aQCFg-d<9rzs$_gO&d5N6eFSc z1ml8)P*FSi+k@!^M9nDWR5e@ATD8oxtDu=36Iv2!;dZzidIS(PCtEuXAtlBb1;H%Z zwnC^Ek*D)EX4#Q>R$$WA2sxC_t(!!6Tr?C#@{3}n{<^o;9id1RA&-Pig1e-2B1XpG zliNjgmd3c&%A}s>qf{_j#!Z`fu0xIwm4L0)OF=u(OEmp;bLCIaZX$&J_^Z%4Sq4GZ zPn6sV_#+6pJmDN_lx@1;Zw6Md_p0w9h6mHtzpuIEwNn>OnuRSC2=>fP^Hqgc)xu^4 z<3!s`cORHJh#?!nKI`Et7{3C27+EuH)Gw1f)aoP|B3y?fuVfvpYYmmukx0ya-)TQX zR{ggy5cNf4X|g)nl#jC9p>7|09_S7>1D2GTRBUTW zAkQ=JMRogZqG#v;^=11O6@rPPwvJkr{bW-Qg8`q8GoD#K`&Y+S#%&B>SGRL>;ZunM@49!}Uy zN|bBCJ%sO;@3wl0>0gbl3L@1^O60ONObz8ZI7nder>(udj-jt`;yj^nTQ$L9`OU9W zX4alF#$|GiR47%x@s&LV>2Sz2R6?;2R~5k6V>)nz!o_*1Y!$p>BC5&?hJg_MiE6UBy>RkVZj`9UWbRkN-Hk!S`=BS3t3uyX6)7SF#)71*}`~Ogz z1rap5H6~dhBJ83;q-Y<5V35C2&F^JI-it(=5D#v!fAi9p#UwV~2tZQI+W(Dv?1t9? zfh*xpxxO{-(VGB>!Q&0%^YW_F!@aZS#ucP|YaD#>wd1Fv&Z*SR&mc;asi}1G) z_H>`!akh-Zxq9#io(7%;a$)w+{QH)Y$?UK1Dt^4)up!Szcxnu}kn$0afcfJL#IL+S z5gF_Y30j;{lNrG6m~$Ay?)*V9fZuU@3=kd40=LhazjFrau>(Y>SJNtOz>8x_X-BlA zIpl{i>OarVGj1v(4?^1`R}aQB&WCRQzS~;7R{tDZG=HhgrW@B`W|#cdyj%YBky)P= zpxuOZkW>S6%q7U{VsB#G(^FMsH5QuGXhb(sY+!-R8Bmv6Sx3WzSW<1MPPN1!&PurYky(@`bP9tz z52}LH9Q?+FF5jR6-;|+GVdRA!qtd;}*-h&iIw3Tq3qF9sDIb1FFxGbo&fbG5n8$3F zyY&PWL{ys^dTO}oZ#@sIX^BKW*bon=;te9j5k+T%wJ zNJtoN1~YVj4~YRrlZl)b&kJqp+Z`DqT!la$x&&IxgOQw#yZd-nBP3!7FijBXD|IsU8Zl^ zc6?MKpJQ+7ka|tZQLfchD$PD|;K(9FiLE|eUZX#EZxhG!S-63C$jWX1Yd!6-Yxi-u zjULIr|0-Q%D9jz}IF~S%>0(jOqZ(Ln<$9PxiySr&2Oic7vb<8q=46)Ln%Z|<*z5&> z3f~Zw@m;vR(bESB<=Jqkxn(=#hQw42l(7)h`vMQQTttz9XW6^|^8EK7qhju4r_c*b zJIi`)MB$w@9epwdIfnEBR+?~);yd6C(LeMC& zn&&N*?-g&BBJcV;8&UoZi4Lmxcj16ojlxR~zMrf=O_^i1wGb9X-0@6_rpjPYemIin zmJb+;lHe;Yp=8G)Q(L1bzH*}I>}uAqhj4;g)PlvD9_e_ScR{Ipq|$8NvAvLD8MYr}xl=bU~)f%B3E>r3Bu9_t|ThF3C5~BdOve zEbk^r&r#PT&?^V1cb{72yEWH}TXEE}w>t!cY~rA+hNOTK8FAtIEoszp!qqptS&;r$ zaYV-NX96-h$6aR@1xz6_E0^N49mU)-v#bwtGJm)ibygzJ8!7|WIrcb`$XH~^!a#s& z{Db-0IOTFq#9!^j!n_F}#Z_nX{YzBK8XLPVmc&X`fT7!@$U-@2KM9soGbmOSAmqV z{nr$L^MBo_u^Joyf0E^=eo{Rt0{{e$IFA(#*kP@SQd6lWT2-#>` zP1)7_@IO!9lk>Zt?#CU?cuhiLF&)+XEM9B)cS(gvQT!X3`wL*{fArTS;Ak`J<84du zALKPz4}3nlG8Fo^MH0L|oK2-4xIY!~Oux~1sw!+It)&D3p;+N8AgqKI`ld6v71wy8I!eP0o~=RVcFQR2Gr(eP_JbSytoQ$Yt}l*4r@A8Me94y z8cTDWhqlq^qoAhbOzGBXv^Wa4vUz$(7B!mX`T=x_ueKRRDfg&Uc-e1+z4x$jyW_Pm zp?U;-R#xt^Z8Ev~`m`iL4*c#65Nn)q#=Y0l1AuD&+{|8-Gsij3LUZXpM0Bx0u7WWm zH|%yE@-#XEph2}-$-thl+S;__ciBxSSzHveP%~v}5I%u!z_l_KoW{KRx2=eB33umE zIYFtu^5=wGU`Jab8#}cnYry@9p5UE#U|VVvx_4l49JQ;jQdp(uw=$^A$EA$LM%vmE zvdEOaIcp5qX8wX{mYf0;#51~imYYPn4=k&#DsKTxo{_Mg*;S495?OBY?#gv=edYC* z^O@-sd-qa+U24xvcbL0@C7_6o!$`)sVr-jSJE4XQUQ$?L7}2(}Eixqv;L8AdJAVqc zq}RPgpnDb@E_;?6K58r3h4-!4rT4Ab#rLHLX?eMOfluJk=3i1@Gt1i#iA=O`M0@x! z(HtJP9BMHXEzuD93m|B&woj0g6T?f#^)>J>|I4C5?Gam>n9!8CT%~aT;=oco5d6U8 zMXl(=W;$ND_8+DD*?|5bJ!;8ebESXMUKBAf7YBwNVJibGaJ*(2G`F%wx)grqVPjudiaq^Kl&g$8A2 zWMxMr@_$c}d+;_B`#kUX-t|4VKH&_f^^EP0&=DPLW)H)UzBG%%Tra*5 z%$kyZe3I&S#gfie^z5)!twG={3Cuh)FdeA!Kj<-9** zvT*5%Tb`|QbE!iW-XcOuy39>D3oe6x{>&<#E$o8Ac|j)wq#kQzz|ATd=Z0K!p2$QE zPu?jL8Lb^y3_CQE{*}sTDe!2!dtlFjq&YLY@2#4>XS`}v#PLrpvc4*@q^O{mmnr5D zmyJq~t?8>FWU5vZdE(%4cuZuao0GNjp3~Dt*SLaxI#g_u>hu@k&9Ho*#CZP~lFJHj z(e!SYlLigyc?&5-YxlE{uuk$9b&l6d`uIlpg_z15dPo*iU&|Khx2*A5Fp;8iK_bdP z?T6|^7@lcx2j0T@x>X7|kuuBSB7<^zeY~R~4McconTxA2flHC0_jFxmSTv-~?zVT| zG_|yDqa9lkF*B6_{j=T>=M8r<0s;@z#h)3BQ4NLl@`Xr__o7;~M&dL3J8fP&zLfDfy z);ckcTev{@OUlZ`bCo(-3? z1u1xD`PKgSg?RqeVVsF<1SLF;XYA@Bsa&cY!I48ZJn1V<3d!?s=St?TLo zC0cNr`qD*M#s6f~X>SCNVkva^9A2ZP>CoJ9bvgXe_c}WdX-)pHM5m7O zrHt#g$F0AO+nGA;7dSJ?)|Mo~cf{z2L)Rz!`fpi73Zv)H=a5K)*$5sf_IZypi($P5 zsPwUc4~P-J1@^3C6-r9{V-u0Z&Sl7vNfmuMY4yy*cL>_)BmQF!8Om9Dej%cHxbIzA zhtV0d{=%cr?;bpBPjt@4w=#<>k5ee=TiWAXM2~tUGfm z$s&!Dm0R^V$}fOR*B^kGaipi~rx~A2cS0;t&khV1a4u38*XRUP~f za!rZMtay8bsLt6yFYl@>-y^31(*P!L^^s@mslZy(SMsv9bVoX`O#yBgEcjCmGpyc* zeH$Dw6vB5P*;jor+JOX@;6K#+xc)Z9B8M=x2a@Wx-{snPGpRmOC$zpsqW*JCh@M2Y z#K+M(>=#d^>Of9C`))h<=Bsy)6zaMJ&x-t%&+UcpLjV`jo4R2025 zXaG8EA!0lQa)|dx-@{O)qP6`$rhCkoQqZ`^SW8g-kOwrwsK8 z3ms*AIcyj}-1x&A&vSq{r=QMyp3CHdWH35!sad#!Sm>^|-|afB+Q;|Iq@LFgqIp#Z zD1%H+3I?6RGnk&IFo|u+E0dCxXz4yI^1i!QTu7uvIEH>i3rR{srcST`LIRwdV1P;W z+%AN1NIf@xxvVLiSX`8ILA8MzNqE&7>%jMzGt9wm78bo9<;h*W84i29^w!>V>{N+S zd`5Zmz^G;f=icvoOZfK5#1ctx*~UwD=ab4DGQXehQ!XYnak*dee%YN$_ZPL%KZuz$ zD;$PpT;HM^$KwtQm@7uvT`i6>Hae1CoRVM2)NL<2-k2PiX=eAx+-6j#JI?M}(tuBW zkF%jjLR)O`gI2fcPBxF^HeI|DWwQWHVR!;;{BXXHskxh8F@BMDn`oEi-NHt;CLymW z=KSv5)3dyzec0T5B*`g-MQ<;gz=nIWKUi9ko<|4I(-E0k$QncH>E4l z**1w&#={&zv4Tvhgz#c29`m|;lU-jmaXFMC11 z*dlXDMEOG>VoLMc>!rApwOu2prKSi*!w%`yzGmS+k(zm*CsLK*wv{S_0WX^8A-rKy zbk^Gf_92^7iB_uUF)EE+ET4d|X|>d&mdN?x@vxKAQk`O+r4Qdu>XGy(a(19g;=jU} zFX{O*_NG>!$@jh!U369Lnc+D~qch3uT+_Amyi}*k#LAAwh}k8IPK5a-WZ81ufD>l> z$4cF}GSz>ce`3FAic}6W4Z7m9KGO?(eWqi@L|5Hq0@L|&2flN1PVl}XgQ2q*_n2s3 zt5KtowNkTYB5b;SVuoXA@i5irXO)A&%7?V`1@HGCB&)Wgk+l|^XXChq;u(nyPB}b3 zY>m5jkxpZgi)zfbgv&ec4Zqdvm+D<?Im*mXweS9H+V>)zF#Zp3)bhl$PbISY{5=_z!8&*Jv~NYtI-g!>fDs zmvL5O^U%!^VaKA9gvKw|5?-jk>~%CVGvctKmP$kpnpfN{D8@X*Aazi$txfa%vd-|E z>kYmV66W!lNekJPom29LdZ%(I+ZLZYTXzTg*to~m?7vp%{V<~>H+2}PQ?PPAq`36R z<%wR8v6UkS>Wt#hzGk#44W<%9S=nBfB);6clKwnxY}T*w21Qc3_?IJ@4gYzC7s;WP zVQNI(M=S=JT#xsZy7G`cR(BP9*je0bfeN8JN5~zY(DDs0t{LpHOIbN);?T-69Pf3R zSNe*&p2%AwXHL>__g+xd4Hlc_vu<25H?(`nafS%)3UPP7_4;gk-9ckt8SJRTv5v0M z_Hww`qPudL?ajIR&X*;$y-`<)6dxx1U~5eGS13CB!lX;3w7n&lDDiArbAhSycd}+b zya_3p@A`$kQy;|NJZ~s44Hqo7Hwt}X86NK=(ey>lgWTtGL6k@Gy;PbO!M%1~Wcn2k zUFP|*5d>t-X*RU8g%>|(wwj*~#l4z^Aatf^DWd1Wj#Q*AY0D^V@sC`M zjJc6qXu0I7Y*2;;gGu!plAFzG=J;1%eIOdn zQA>J&e05UN*7I5@yRhK|lbBSfJ+5Uq;!&HV@xfPZrgD}kE*1DSq^=%{o%|LChhl#0 zlMb<^a6ixzpd{kNZr|3jTGeEzuo}-eLT-)Q$#b{!vKx8Tg}swCni>{#%vDY$Ww$84 zew3c9BBovqb}_&BRo#^!G(1Eg((BScRZ}C)Oz?y`T5wOrv);)b^4XR8 zhJo7+<^7)qB>I;46!GySzdneZ>n_E1oWZY;kf94#)s)kWjuJN1c+wbVoNQcmnv}{> zN0pF+Sl3E}UQ$}slSZeLJrwT>Sr}#V(dVaezCQl2|4LN`7L7v&siYR|r7M(*JYfR$ zst3=YaDw$FSc{g}KHO&QiKxuhEzF{f%RJLKe3p*7=oo`WNP)M(9X1zIQPP0XHhY3c znrP{$4#Ol$A0s|4S7Gx2L23dv*Gv2o;h((XVn+9+$qvm}s%zi6nI-_s6?mG! zj{DV;qesJb&owKeEK?=J>UcAlYckA7Sl+I&IN=yasrZOkejir*kE@SN`fk<8Fgx*$ zy&fE6?}G)d_N`){P~U@1jRVA|2*69)KSe_}!~?+`Yb{Y=O~_+@!j<&oVQQMnhoIRU zA0CyF1OFfkK44n*JD~!2!SCPM;PRSk%1XL=0&rz00wxPs&-_eapJy#$h!eqY%nS0{ z!aGg58JIJPF3_ci%n)QSVpa2H`vIe$RD43;#IRfDV&Ibit z+?>HW4{2wOfC6Fw)}4x}i1maDxcE1qi@BS*qcxD2gE@h3#4cgU*D-&3z7D|tVZWt= z-Cy2+*Cm@P4GN_TPUtaVyVesbVDazF@)j8VJ4>XZv!f%}&eO1SvIgr}4`A*3#vat< z_MoByL(qW6L7SFZ#|Gc1fFN)L2PxY+{B8tJp+pxRyz*87)vXR}*=&ahXjBlQKguuf zX6x<<6fQulE^C*KH8~W%ptpaC0l?b=_{~*U4?5Vt;dgM4t_{&UZ1C2j?b>b+5}{IF_CUyvz-@QZPMlJ)r_tS$9kH%RPv#2_nMb zRLj5;chJ72*U`Z@Dqt4$@_+k$%|8m(HqLG!qT4P^DdfvGf&){gKnGCX#H0!;W=AGP zbA&Z`-__a)VTS}kKFjWGk z%|>yE?t*EJ!qeQ%dPk$;xIQ+P0;()PCBDgjJm6Buj{f^awNoVx+9<|lg3%-$G(*f) zll6oOkN|yamn1uyl2*N-lnqRI1cvs_JxLTeahEK=THV$Sz*gQhKNb*p0fNoda#-&F zB-qJgW^g}!TtM|0bS2QZekW7_tKu%GcJ!4?lObt0z_$mZ4rbQ0o=^curCs3bJK6sq z9fu-aW-l#>z~ca(B;4yv;2RZ?tGYAU)^)Kz{L|4oPj zdOf_?de|#yS)p2v8-N||+XL=O*%3+y)oI(HbM)Ds?q8~HPzIP(vs*G`iddbWq}! z(2!VjP&{Z1w+%eUq^ /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +131,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/gradlew.bat b/gradlew.bat index f127cfd..93e3f59 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% 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/settings.gradle b/settings.gradle index 48c039e..d94f73c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index efc80a5..827a6b5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,10 +7,10 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; - -import frc4388.utility.LEDPatterns; import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -23,78 +23,89 @@ import frc4388.utility.Gains; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 0.8; + public static final double MAX_ROT_SPEED = 3.5; + public static final double AUTO_MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final String CANBUS_NAME = "IDK"; public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.8; + public static final double SLOW_SPEED = 0.5; public static final double FAST_SPEED = 1.0; public static final double TURBO_SPEED = 4.0; + public static final class DefaultSwerveRotOffsets { + public static final double FRONT_LEFT_ROT_OFFSET = 220; + public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; + public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; + } + public static final class IDs { - public static final int LEFT_FRONT_WHEEL_ID = 2; - public static final int LEFT_FRONT_STEER_ID = 3; - public static final int LEFT_FRONT_ENCODER_ID = 10; - - public static final int RIGHT_FRONT_WHEEL_ID = 4; - public static final int RIGHT_FRONT_STEER_ID = 5; - public static final int RIGHT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; + public static final int RIGHT_FRONT_WHEEL_ID = 0; + public static final int RIGHT_FRONT_STEER_ID = 1; + public static final int RIGHT_FRONT_ENCODER_ID = 12; - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; + // public static final int LEFT_FRONT_WHEEL_ID = 4; + // public static final int LEFT_FRONT_STEER_ID = 5; + // public static final int LEFT_FRONT_ENCODER_ID = 11; + + // public static final int LEFT_BACK_WHEEL_ID = 6; + // public static final int LEFT_BACK_STEER_ID = 7; + // public static final int LEFT_BACK_ENCODER_ID = 12; + + // public static final int RIGHT_BACK_WHEEL_ID = 8; + // public static final int RIGHT_BACK_STEER_ID = 9; + // public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune + + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; - - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; + + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double WHEEL_DIAMETER_INCHES = 3.9; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + + public static final double TICK_TIME_TO_SECONDS = 10; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value } public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value @@ -111,30 +122,41 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } - public static final class VisionConstants { - public static final String NAME = "photonCamera"; + public static final class VisionConstants { + // public static final String NAME = "photonCamera"; - public static final int LIME_HIXELS = 640; - public static final int LIME_VIXELS = 480; + // public static final int LIME_HIXELS = 640; + // public static final int LIME_VIXELS = 480; - public static final double H_FOV = 59.6; - public static final double V_FOV = 45.7; + // public static final double H_FOV = 59.6; + // public static final double V_FOV = 45.7; - public static final double LIME_HEIGHT = 6.0; - public static final double LIME_ANGLE = 55.0; + // public static final double LIME_HEIGHT = 6.0; + // public static final double LIME_ANGLE = 55.0; - // public static final double HIGH_TARGET_HEIGHT = 46.0; - public static final double HIGH_TAPE_HEIGHT = 44.0; + // // public static final double HIGH_TARGET_HEIGHT = 46.0; + // public static final double HIGH_TAPE_HEIGHT = 44.0; - // public static final double MID_TARGET_HEIGHT = 34.0; - public static final double MID_TAPE_HEIGHT = 24.0; + // // public static final double MID_TARGET_HEIGHT = 34.0; + // public static final double MID_TAPE_HEIGHT = 24.0; - public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value - - } + // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); + public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); + + public static final double SpeakerBubbleDistance = 3; + public static final double targetPosDistance = 1.5; + } + + public static final class AutoAlignConstants { + public static final double MoveSpeed = 0.0; + public static final double RotSpeed = 0.0; + } + public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; + public static final int DRIVE_PIGEON_ID = 14; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } @@ -145,6 +167,34 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } + public static final class ShooterConstants { + public static final int LEFT_SHOOTER_ID = 15; // final + public static final int RIGHT_SHOOTER_ID = 16; // final + public static final double SHOOTER_SPEED = 0.50; // final + public static final double SHOOTER_IDLE = 0.20; // final + public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; + } + + public static final class IntakeConstants { + public static final int INTAKE_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 17; + public static final double INTAKE_SPEED = 0.75; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; + public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; + public static final double HANDOFF_SPEED = 0.4; + public static final double PIVOT_SPEED = 0.2; + + public static final class ArmPID { + public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); + } + } + + public static final class ClimbConstants { + public static final int CLIMB_MOTOR_ID = 19; + public static final double CLIMB_IN_SPEED = -1.0; + public static final double CLIMB_OUT_SPEED = 0.87; + } + public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e555b09..36c4330 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,12 +7,14 @@ package frc4388.robot; +import edu.wpi.first.cameraserver.CameraServer; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; - +//import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot @@ -22,16 +24,17 @@ import frc4388.utility.RobotTime; */ public class Robot extends TimedRobot { Command m_autonomousCommand; - + private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + //private LED mled = new LED(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ @Override public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -40,14 +43,16 @@ public class Robot extends TimedRobot { /** * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * autonomous, teleoperated and test.doubl * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ @Override - public void robotPeriodic() { + public void robotPeriodic() { m_robotTime.updateTimes(); + //System.out.println(m_robotContainer.limelight.isNearSpeaker()); + //mled.updateLED(); // 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 @@ -119,7 +124,7 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5934d2f..3888775 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,20 +7,50 @@ package frc4388.robot; +// Drive Systems +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.robot.Constants.OIConstants; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +// Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.*; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +// Autos +import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; -import frc4388.robot.subsystems.LED; +import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.Swerve.neoJoystickPlayback; +import frc4388.robot.commands.Swerve.neoJoystickRecorder; +// import frc4388.robot.commands.Intake.ArmIntakeIn; +//import frc4388.robot.commands.Autos.AutoAlign; + +// Subsystems +// import frc4388.robot.subsystems.LED; +// import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.LEDPatterns; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.IHandController; -import frc4388.utility.controller.XboxController; +// import frc4388.robot.subsystems.Shooter; +// import frc4388.robot.subsystems.Climber; +// import frc4388.robot.subsystems.Intake; + +// Utilites +import frc4388.utility.DeferredBlock; +import frc4388.utility.configurable.ConfigurableString; /** * This class is where the bulk of the robot should be declared. Since @@ -31,34 +61,160 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); - - /* Subsystems */ - private final LED m_robotLED = new LED(m_robotMap.LEDController); - - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - m_robotMap.gyro); + public final RobotMap m_robotMap = new RobotMap(); + /* Subsystems */ + // private final LED m_robotLED = new LED(); + + // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + // m_robotMap.rightFront, + // m_robotMap.leftBack, + // m_robotMap.rightBack, + + // m_robotMap.gyro); + /* Limelight */ + // private final Limelight limelight = new Limelight(); + + // private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + + // private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); /* Controllers */ - private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + // private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + + + /* Virtual Controllers */ + // private final VirtualController m_virtualDriver = new VirtualController(0); + // private final VirtualController m_virtualOperator = new VirtualController(1); + + // private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + // private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + + // private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.PIDIn()), + // new InstantCommand(() -> m_robotShooter.idle()) + // // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), + // // new InstantCommand(() -> m_robotShooter.spin()) + // ); + + + // ! Teleop Commands + + //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + + // private SequentialCommandGroup autoShoot = new SequentialCommandGroup( + // // MoveToSpeaker, + // //autoAlign, + // new InstantCommand(() -> m_robotShooter.spin()), + // new WaitCommand(3.0), + // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + // new WaitCommand(3.0), + // new InstantCommand(() -> m_robotShooter.idle()) + // // new InstantCommand(() -> autoAlign.reverse()), + // // autoAlign.asProxy() + // ); + + + // private SequentialCommandGroup i = new SequentialCommandGroup( + // intakeToShootStuff, intakeToShoot, + // new InstantCommand(() -> m_robotShooter.idle()) + // ); + + // private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + // new WaitCommand(0.75), + // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) + // ); + + // private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) + // // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + // ); + + // private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( + // interrupt, + // new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), + // new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + // ); + + // private SequentialCommandGroup ampShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.ampPosition()), + // new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed + // ); + + // ! /* Autos */ + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + // private String lastAutoName = "final_red_center_4note_taxi.auto"; + // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); + // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), // lastAutoName + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false); + + + // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + // .addOption("Taxi Auto", taxi.asProxy()) + // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) + // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) + // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) + // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) + // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) + // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) + // .buildDisplay(); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - configureButtonBindings(); + configureButtonBindings(); + configureVirtualButtonBindings(); + // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); + // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller + + // DriverStation.silenceJoystickConnectionWarning(true); + // // CameraServer.startAutomaticCapture(); + + // /* Default Commands */ + // // drives the robot with a two-axis input from the driver controller + // ! Swerve Drive Default Command (Regular Rotation) + + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); + // } + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRight(), + // true); + // }, m_robotSwerveDrive) + // .withName("SwerveDrive DefaultCommand")); + // m_robotSwerveDrive.setToSlow(); + + // ! Swerve Drive Default Command (Orientation Rotation) + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRightX(), + // getDeadbandedDriverController().getRightY(), + // true); + // }, m_robotSwerveDrive) + // .withName("SwerveDrive OrientationCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + } + // private void changeAuto() { + // autoPlayback.unloadAuto(); + // autoPlayback.loadAuto(); + // lastAutoName = autoplaybackName.get(); + // System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`"); + // } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -66,26 +222,191 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller + + // ? /* Driver Buttons */ + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + // * /* D-Pad Stuff */ + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // ! /* Auto Recording */ + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // () -> autoplaybackName.get())) + // .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) + // .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Taxi.txt")) + // .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .onFalse(new InstantCommand()); + // ! /* Speed */ + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .whileTrue(new InstantCommand(() -> + // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true), m_robotSwerveDrive)); + + + //? /* Operator Buttons */ + + // new Joystick(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotMap.rightFront.go(new Translation2d()))); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + // .onTrue(emergencyRetract); + + + // Override Intake Position encoder: out + // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); + + // // Override Intake Position encoder: in + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) + // .onFalse(turnOffShoot); + + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(i) + // .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); + + // //spins up shooter, no wind down + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + + // // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) + // // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); + + // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // .onTrue(emergencyRetract); + + // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + + } + + /** + * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

+ * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. + */ + private void configureVirtualButtonBindings() { + + // ? /* Driver Buttons */ + + /* Notice: the following buttons have not been replicated + * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback + * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. + * Auto Recording controls : We don't want an Null Ouroboros for an auto. + */ + + // ? /* Operator Buttons */ + + /* Notice: the following buttons have not been replicated + * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. + * We don't need it in an auto. + * Climbing controls : We don't need to climb in auto. + */ + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getVirtualOperatorController().getPOV() == 0) + // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); - /* Operator Buttons */ - // activates "Lit Mode" - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } /** @@ -94,8 +415,20 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); + //no auto + // return autoPlayback; + //return playbackChooser.getCommand(); + return new Command() {}; + } + + /** + * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller} + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); } /** @@ -108,4 +441,12 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } + + // public VirtualController getVirtualDriverController() { + // return m_virtualDriver; + // } + + // public VirtualController getVirtualOperatorController() { + // return m_virtualOperator; + // } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index b7f03c4..aa9fba8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,16 +7,20 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.WPI_Pigeon2; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.ClimbConstants; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -25,8 +29,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(m_pigeon2); + // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); public SwerveModule leftFront; public SwerveModule rightFront; @@ -39,58 +43,118 @@ public class RobotMap { } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + /* Swreve Drive Subsystem */ - public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + /* Shooter Subsystem */ + // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); + // public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); + + /* Intake Subsystem */ + // public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + // public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + + /* Climber Subsystem */ + // public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); void configureLEDMotorControllers() { } + void configureIntakeMotorControllers() { + // intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + // pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); + } + void configureDriveMotorControllers() { // config factory default - leftFrontWheel.configFactoryDefault(); - leftFrontSteer.configFactoryDefault(); + // leftFrontWheel.configFactoryDefault(); + // leftFrontSteer.configFactoryDefault(); - rightFrontWheel.configFactoryDefault(); - rightFrontSteer.configFactoryDefault(); + // rightFrontWheel.configFactoryDefault(); + // rightFrontSteer.configFactoryDefault(); - leftBackWheel.configFactoryDefault(); - leftBackSteer.configFactoryDefault(); + // leftBackWheel.configFactoryDefault(); + // leftBackSteer.configFactoryDefault(); - rightBackWheel.configFactoryDefault(); - rightBackSteer.configFactoryDefault(); + // rightBackWheel.configFactoryDefault(); + // rightBackSteer.configFactoryDefault(); - // set neutral mode - leftFrontWheel.setNeutralMode(NeutralMode.Brake); - rightFrontWheel.setNeutralMode(NeutralMode.Brake); - leftBackWheel.setNeutralMode(NeutralMode.Brake); - rightBackWheel.setNeutralMode(NeutralMode.Brake); + // // config open loop ramp + // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftFrontSteer.setNeutralMode(NeutralMode.Brake); - rightFrontSteer.setNeutralMode(NeutralMode.Brake); - leftBackSteer.setNeutralMode(NeutralMode.Brake); - rightBackSteer.setNeutralMode(NeutralMode.Brake); + // // config closed loop ramp + // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); + // // config neutral deadband + // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // // set neutral mode + // leftFrontWheel.setNeutralMode(NeutralMode.Brake); + // rightFrontWheel.setNeutralMode(NeutralMode.Brake); + // leftBackWheel.setNeutralMode(NeutralMode.Brake); + // rightBackWheel.setNeutralMode(NeutralMode.Brake); + + // leftFrontSteer.setNeutralMode(NeutralMode.Brake); + // rightFrontSteer.setNeutralMode(NeutralMode.Brake); + // leftBackSteer.setNeutralMode(NeutralMode.Brake); + // rightBackSteer.setNeutralMode(NeutralMode.Brake); + + // // initialize SwerveModules + // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); + // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } } diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java new file mode 100644 index 0000000..58eb3ed --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -0,0 +1,208 @@ +// package frc4388.robot.commands.Autos; +// import frc4388.robot.subsystems.Limelight; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.robot.Constants.AutoAlignConstants; +// import frc4388.robot.Constants.VisionConstants; +// import edu.wpi.first.wpilibj2.command.Command; + +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Rotation2d; + +// import java.util.Optional; + +// import org.opencv.core.RotatedRect; + +// import edu.wpi.first.math.geometry.Pose2d; + +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; + +// public class AutoAlign extends Command { +// private SwerveDrive swerve; +// private Limelight limelight; +// private Pose2d pose; +// private Translation2d targetPos; +// private Rotation2d targetRot; + +// private Optional alliance; +// private boolean isRed; + +// private boolean isFinished; +// private boolean isReverseFinished; + +// private boolean reverseAfterFinish; +// private Translation2d[] moveStickReplayArr; +// private Translation2d[] rotStickReplayArr; +// private int replayIndex; + +// // PID Stuff +// private double prevError; +// private double cumError; + +// public AutoAlign(SwerveDrive swerve, Limelight limelight) { +// this.swerve = swerve; +// this.limelight = limelight; +// } + +// // Calc the closest point on a circle, to the center of the speaker +// private Translation2d calcTargetPos(){ +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); + +// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point +// double vX = pX - cX; +// double vY = pY - cY; +// double magV = Math.sqrt(vX*vX + vY*vY); +// double aX = cX + vX / magV * R; +// double aY = cY + vY / magV * R; + +// return new Translation2d(aX, aY); +// } + +// // Calculate the angle facing the center, at the target rot +// private Rotation2d calcTargetRot() { +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); + +// final double dX = pX - cX; +// final double dY = pY - cY; + +// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); + +// return Rotation2d.fromDegrees(yaw); +// } + +// // Clamp to a circle, like an xbox controller +// private Translation2d clamp(double oldX, double oldY){ +// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley +// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; +// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); +// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); + +// final double newX = Math.max(-maxX, Math.min(maxX, oldX)); +// final double newY = Math.max(-maxY, Math.min(maxY, oldY)); + +// return new Translation2d(newX, newY); +// } + +// private Translation2d calcMoveStick(){ +// final double curX = pose.getX(); +// final double curY = pose.getY(); + +// // I think this might work, assuming the constants are good +// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; +// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; + +// return clamp(stickX, stickY); +// } + +// private Translation2d calcRotStick(){ +// double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); +// cumError += error * .02; // 20 ms +// double delta = error - prevError; + +// final double kP = 4; +// final double kI = 4; +// final double kD = 4; +// final double kF = 4; + +// double output = error * kP; +// output += cumError * kI; +// output += delta * kD; +// output += kF; + +// prevError = error; +// return clamp(output, 0); +// } + +// public void reverse() { +// this.reverseAfterFinish = true; +// } + +// // Called when the command is initially scheduled. +// @Override +// public final void initialize() { +// isRed = alliance.get() == DriverStation.Alliance.Red; +// if(reverseAfterFinish){ +// // isReverseFinished = false; +// replayIndex = 0; +// }else{ +// moveStickReplayArr = new Translation2d[]{}; +// rotStickReplayArr = new Translation2d[]{}; +// } +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// // Update limelight pos +// pose = limelight.getPose(); + +// // These must be 0, or it will error +// Translation2d moveStick = new Translation2d(0, 0); +// Translation2d rotStick = new Translation2d(0, 0); + +// // Regular replay +// if(!isFinished){ +// targetPos = calcTargetPos(); +// targetRot = calcTargetRot(); + +// moveStick = calcMoveStick(); +// rotStick = calcRotStick(); + +// // This is a way of appending... +// moveStickReplayArr[moveStickReplayArr.length] = moveStick; +// rotStickReplayArr[rotStickReplayArr.length] = rotStick; + +// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ +// // replayIndex +// // } +// isFinished = limelight.isNearSpeaker(); + +// // If reverseAfterFinish, then loop back over and replay in reverse +// }else if(reverseAfterFinish && !isReverseFinished){ +// // Get reverse direction +// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; +// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; + +// // Invert sticks +// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); +// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); + +// replayIndex++; + +// if(replayIndex >= moveStickReplayArr.length){ +// reverseAfterFinish = true; +// } +// } + +// // This would greatly benifit from having feild Relative implemented. +// swerve.driveWithInput(moveStick, rotStick, true); +// } + +// // Returns true when the command should end. +// @Override +// public final boolean isFinished() { +// return isFinished && (isReverseFinished || !reverseAfterFinish); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..f3d636d 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -42,6 +42,7 @@ public class PlaybackChooser { m_playback = m_choosers.get(0); nextChooser(); + // ! This does not work, why? Shuffleboard.getTab("Auto Chooser") .add("Add Sequence", new InstantCommand(() -> nextChooser())) .withPosition(4, 0); @@ -66,9 +67,15 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + String[] dirs = m_dir.list(); + + if(dirs != null){ // Fix funny error + for (String auto : dirs) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } } + + for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt new file mode 100644 index 0000000..a65aea9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt @@ -0,0 +1,20 @@ +AUTO file format + +HEADER static size 0x5 +0x00 BYTE NUM AXES: defualts to 6 +0x01 BYTE NUM POV: defualts to 1 +0x02 BYTE NUM CONTROLLERS: defualts to 2 +0x03 SHORT FRAMES: any value greator or equal than one. + +FRAME PER CONTROLLER: defualt size 0x34 +0x00 DOUBLE AXES[NUM AXES] +0x30 SHORT BUTTONS +0x32 SHORT POVs[NUM POV] + +FRAME: size varrys +FRAME PER CONTROLLER[NUM CONTROLLERS] +INT UNIXTIMESTAMP + +FILE: +HEADER +FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java new file mode 100644 index 0000000..86bc7b2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -0,0 +1,107 @@ +// package frc4388.robot.commands.Autos; + +// import java.io.File; +// import java.util.ArrayList; +// import java.util.HashMap; + +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import frc4388.robot.commands.Swerve.JoystickPlayback; +// import frc4388.robot.commands.Swerve.neoJoystickPlayback; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.utility.controller.VirtualController; + +// public class neoPlaybackChooser { +// private final SendableChooser m_teamChosser = new SendableChooser(); +// private final SendableChooser m_possitionChosser = new SendableChooser(); +// private final SendableChooser m_autoNameChosser = new SendableChooser(); + +// private final SwerveDrive m_swerve; +// private final VirtualController[] m_controllers; +// // private final ArrayList> m_choosers = new ArrayList<>(); +// // private SendableChooser m_playback = null; +// private final ArrayList m_widgets = new ArrayList<>(); +// // private final HashMap m_commandPool = new HashMap<>(); + +// // private final File m_dir = new File("/home/lvuser/autos/"); +// // private int m_cmdNum = 0; + +// // commands +// private Command m_noAuto = new InstantCommand(); + +// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { +// m_teamChosser.addOption("Red", "red"); +// m_teamChosser.setDefaultOption("Blue", "blue"); +// m_teamChosser.addOption("Nuetral", "nuetral"); +// m_possitionChosser.addOption("AMP", "amp"); +// m_possitionChosser.setDefaultOption("Center", "center"); +// m_possitionChosser.addOption("Source", "source"); +// m_swerve = swerve; +// m_controllers = controllers; +// } + +// public neoPlaybackChooser addOption(String name, String option) { +// m_autoNameChosser.addOption(name, option); +// return this; +// } + +// // public PlaybackChooser buildDisplay() { +// // for (int i = 0; i < 10; i++) { +// // appendCommand(); +// // } +// // m_playback = m_choosers.get(0); +// // nextChooser(); + +// // // ! This does not work, why? +// // Shuffleboard.getTab("Auto Chooser") +// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) +// // .withPosition(4, 0); +// // return this; +// // } + +// // This will be bound to a button for the time being +// public void render() { +// // var chooser = new SendableChooser(); +// // // chooser.setDefaultOption("No Auto", m_noAuto); + +// // m_choosers.add(chooser); +// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") +// .add("Command: " + m_choosers.size(), chooser) +// .withSize(4, 1) +// .withPosition(0, m_choosers.size() - 1) +// .withWidget(BuiltInWidgets.kSplitButtonChooser) +// .withWidget(BuiltInWidgets.kComboBoxChooser); + + +// m_widgets.add(widget); +// } + +// // public void nextChooser() { +// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + +// // String[] dirs = m_dir.list(); + +// // if(dirs != null){ // Fix funny error +// // for (String auto : dirs) { +// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); +// // } +// // } + + +// // for (var cmd_name : m_commandPool.keySet()) { +// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); +// // } +// // } + +// public String autoName() { +// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; +// } + +// public Command getCommand() { +// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); +// } +// } diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java index fdbda0b..97233f8 100644 --- a/src/main/java/frc4388/robot/commands/PID.java +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -4,10 +4,10 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.utility.Gains; -public abstract class PID extends CommandBase { +public abstract class PID extends Command { protected Gains gains; private double output = 0; private double tolerance = 0; diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 56e8d43..e92b487 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase { // new Translation2d(out.rightX, out.rightY), // true); - this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // new Translation2d(lerpRX, lerpRY), + // true); + + this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), new Translation2d(lerpRX, lerpRY), true); @@ -138,4 +142,4 @@ public class JoystickPlayback extends CommandBase { public boolean isFinished() { return m_finished; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 84608cc..82ba36e 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -64,11 +64,11 @@ public class JoystickRecorder extends CommandBase { outputs.add(inputs); - swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); - System.out.println("RECORDING"); + //System.out.println("RECORDING"); } // Called once the command ends or is interrupted. @@ -94,4 +94,4 @@ public class JoystickRecorder extends CommandBase { public boolean isFinished() { return false; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java new file mode 100644 index 0000000..8b5afdf --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -0,0 +1,197 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileInputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + + +/** + * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickPlayback extends Command { + private final SwerveDrive swerve; + private final VirtualController[] controllers; + private final ArrayList frames = new ArrayList<>(); + private final Supplier filenameGetter; + private String filename; + private int frame_index = 0; + private long startTime = 0; + private long playbackTime = 0; + private boolean m_finished = false; // ! There is no better way. + private boolean m_shouldfree = false; // should free memory on ending + + private byte m_numAxes = 0; + private byte m_numPOVs = 0; + private byte m_numControllers = 0; + private short m_numFrames = -1; + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree Unloads the auto on compleation or intruption. + * @param instantload Load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this.swerve = swerve; + this.filenameGetter = filenameGetter; + this.controllers = controllers; + this.m_shouldfree = shouldfree; + + if (instantload) loadAuto(); + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree unloads the auto on compleation or intruption. + * @param instantload load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this(swerve, () -> filename, controllers, shouldfree, instantload); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { + this(swerve, filenameGetter, controllers, true, false); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { + this(swerve, () -> filename, controllers, true, false); + } + + /** + * Load the auto file from disk into memory + * @return Returns true if loading was successful, else wise; return false + * @implNote if the auto is already loaded, it will return true. + */ + public boolean loadAuto() { + filename = filenameGetter.get(); + try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { + if (m_numFrames != -1 && m_numFrames == frames.size()) { + System.out.println("AUTOPLAYBACK: Auto Already loaded."); + return true; + } + + m_numAxes = stream.readNBytes(1)[0]; + m_numPOVs = stream.readNBytes(1)[0]; + m_numControllers = stream.readNBytes(1)[0]; + m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); + + if (m_numControllers > controllers.length) { + System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers + + " virtual controllers but only " + controllers.length + " were given"); + return false; + } + + for (int i = 0; i < m_numFrames; i++) { + AutoRecordingFrame frame = new AutoRecordingFrame(); + for (int j = 0; j < m_numControllers; j++) { + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = new double[m_numAxes]; + for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. + axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + } + short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); + short[] POV = new short[m_numPOVs]; + for (int k = 0; k < m_numPOVs; k++) { + POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); + } + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[j] = controllerFrame; + } + frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); + frames.add(frame); + } + + System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); + return true; + + } catch (Exception e) { + e.printStackTrace(); + System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); + return false; + } + } + + /** + * Unloads the auto. + */ + public void unloadAuto() { + System.out.println("AUTOPLAYBACK: Auto unloaded"); + frames.clear(); + } + + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + playbackTime = 0; + frame_index = 0; + + m_finished = !loadAuto(); + } + + @Override + public void execute() { + if (frame_index >= m_numFrames) m_finished = true; + if (m_finished) return; + + // if (frame_index == 0) { + // startTime = System.currentTimeMillis(); + // playbackTime = 0; + // } else { + // playbackTime = System.currentTimeMillis() - startTime; + // } + + AutoRecordingFrame frame = frames.get(frame_index); + for (int i = 0; i < controllers.length; i++) { + AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; + controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); + if (i == 0) { + this.swerve.driveWithInput( + new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), + new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), + true); + } + } + frame_index++; + } + + @Override + public void end(boolean interrupted) { + for (VirtualController controller : controllers) controller.zeroControls(); + swerve.stopModules(); + if (m_shouldfree) unloadAuto(); + } + + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java new file mode 100644 index 0000000..7f48a6c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -0,0 +1,129 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileOutputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.DeadbandedXboxController; + +/** + * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickRecorder extends Command { + private final SwerveDrive swerve; + private final XboxController[] controllers; + private String filename; + private final Supplier filenameGetter; + private long startTime = -1; + private final ArrayList frames = new ArrayList<>(); + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { + this.swerve = swerve; + this.controllers = controllers; + this.filenameGetter = filenameGetter; + this.filename = ""; + + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filename a String containing the name of the auto file you wish to playback. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + this(swerve, controllers, () -> filename); + } + + @Override + public void initialize() { + frames.clear(); + + this.startTime = System.currentTimeMillis(); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; + frames.add(frame); + this.filename = this.filenameGetter.get(); + } + + + @Override + public void execute() { + System.out.println("AUTORECORD: RECORDING"); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.timeStamp = (int) (System.currentTimeMillis() - startTime); + for (int i = 0; i < controllers.length; i++) { + XboxController controller = controllers[i]; + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = {controller.getLeftX(), controller.getLeftY(), + controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), + controller.getRightX(), controller.getRightY()}; + short button = 0; + for (int j = 0; j < 10; j++) + if (controller.getRawButton(j+1)) + button |= 1 << j; + short[] POV = {(short)(controller.getPOV())}; + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[i] = controllerFrame; + } + + frames.add(frame); + + swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), + new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), + true); // Really jank way of doing this. + + } + @Override + public void end(boolean interrupted) { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { + // header: size of 0x5 + // byte Number of axes per controller + // byte Number of POVs per controller + // byte Number of controllers + // short Number of frames + stream.write(new byte[]{6, 1, (byte) controllers.length}); + stream.write(DataUtils.shortToByteArray((short) frames.size())); + + // frame + // controller frame * number of controllers + // int unix time stamp. + for (AutoRecordingFrame frame : frames) { + // controller frame + // double axis * Number of axes per controller + // short button states + // short POV * Number of POVs per controller + for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { + for (double axis: controllerFrame.axes) { + stream.write(DataUtils.doubleToByteArray(axis)); + } + stream.write(DataUtils.shortToByteArray(controllerFrame.button)); + for (short POV: controllerFrame.POV) { + stream.write(DataUtils.shortToByteArray(POV)); + } + } + stream.write(DataUtils.intToByteArray(frame.timeStamp)); + } + System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); + } catch (Exception e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 055230b..965bbcb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,17 +4,18 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class SwerveDrive extends SubsystemBase { +public class SwerveDrive extends SubsystemBase { private SwerveModule leftFront; private SwerveModule rightFront; @@ -24,17 +25,20 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -51,14 +55,51 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + rightFront.go(leftStick); + // if (fieldRelative) { + + // double rot = 0; + + // // ! drift correction + // if (rightStick.getNorm() > 0.05) { + // rotTarget = gyro.getAngle(); + // rot = rightStick.getX(); + // // SmartDashboard.putBoolean("drift correction", false); + // stopped = false; + // } else if(leftStick.getNorm() > 0.05) { + // if (!stopped) { + // stopModules(); + // stopped = true; + // } + + // // SmartDashboard.putBoolean("drift correction", true); + + // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + // } + + // // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + + // // Convert field-relative speeds to robot-relative speeds. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); + // } else { // Create robot-relative speeds. + // chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + // } + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + + public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { double rot = 0; + // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -66,36 +107,63 @@ public class SwerveDrive extends SubsystemBase { stopped = true; } - SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - } else { - // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } + public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + + Translation2d rightStick = new Translation2d(-rightX, rightY); + + if(fieldRelative) { + double rot = 0; + if(rightStick.getNorm() > 0.5) { + orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); + + Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); + double min = tmp.getDegrees(); + min = Math.max(Math.abs(min), 2); + if(tmp.getDegrees() < 0) + min*=-1; + tmp = new Rotation2d(min * Math.PI / 180); + rot = tmp.getRadians(); // x x - y/x + } + + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + /** * Set each module of the swerve drive to the corresponding desired state. * @param desiredStates Array of module states to set. */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } + // public void setModuleStates(SwerveModuleState[] desiredStates) { + // SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + // for (int i = 0; i < desiredStates.length; i++) { + // SwerveModule module = modules[i]; + // SwerveModuleState state = desiredStates[i]; + // module.setDesiredState(state); + // } + // } public boolean rotateToTarget(double angle) { double currentAngle = getGyroAngle(); @@ -114,9 +182,30 @@ public class SwerveDrive extends SubsystemBase { return gyro.getAngle(); } + public void add180() { + gyro.reset(gyro.getAngle()+180); + rotTarget = gyro.getAngle(); + + } + public void resetGyro() { gyro.reset(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); + } + + public void resetGyroFlip() { + gyro.resetFlip(); + rotTarget = gyro.getAngle(); + } + + public void resetGyroRightBlue() { + gyro.resetRightSideBlue(); + rotTarget = gyro.getAngle(); + } + + public void resetGyroRightAmp() { + gyro.resetAmpSide(); + rotTarget = gyro.getAngle(); } public void stopModules() { @@ -129,10 +218,15 @@ public class SwerveDrive extends SubsystemBase { return this.kinematics; } + public boolean getSpeedState() { + + return false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + // SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { @@ -142,6 +236,7 @@ public class SwerveDrive extends SubsystemBase { this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; } else { this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } } @@ -192,4 +287,14 @@ public class SwerveDrive extends SubsystemBase { } } + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + + } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..ef0d062 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,56 +4,92 @@ package frc4388.robot.subsystems; +// import javax.swing.text.StyleContext.SmallAttributeSet; + +// import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +// import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import com.ctre.phoenix.sensors.CANCoder; +// import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +// import frc4388.utility.configurable.ConfigurableDouble; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; - private CANCoder encoder; - + private TalonFX driveMotor; + private TalonFX angleMotor; + private CANcoder encoder; + // private int selfid; + // private ConfigurableDouble offsetGetter; + private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; + /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { + public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; + // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + // this.selfid = swerveId; + // swerveId++; + // TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + // angleConfig.slot0.kP = swerveGains.kP; + // angleConfig.slot0.kI = swerveGains.kI; + // angleConfig.slot0.kD = swerveGains.kD; - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - angleConfig.slot0.kP = swerveGains.kP; - angleConfig.slot0.kI = swerveGains.kI; - angleConfig.slot0.kD = swerveGains.kD; - - // use the CANcoder as the remote sensor for the primary TalonFX PID - angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - angleMotor.configAllSettings(angleConfig); - - encoder.configMagnetOffset(offset); + // // use the CANcoder as the remote sensor for the primary TalonFX PID + // angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + // angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + // angleMotor.configAllSettings(angleConfig); - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); + // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + // reset(0); + // encoder.configMagnetOffset(offset); + + // driveMotor.setSelectedSensorPosition(0); + // driveMotor.config_kP(0, 0.2); } + public void go(Translation2d leftStick){ + System.out.println(leftStick.getY()); + driveMotor.set(leftStick.getY()); + } + + @Override + public void periodic() { + //encoder.configMagnetOffset(offsetGetter.get()); + //SmartDashboard.putString("Error Code: " + selfid, getstuff()); + // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); + // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); + // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); + // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); + } /** * Get the drive motor of the SwerveModule * @return the drive motor of the SwerveModule */ - public WPI_TalonFX getDriveMotor() { + public TalonFX getDriveMotor() { return this.driveMotor; } @@ -61,7 +97,7 @@ public class SwerveModule extends SubsystemBase { * Get the angle motor of the SwerveModule * @return the angle motor of the SwerveModule */ - public WPI_TalonFX getAngleMotor() { + public TalonFX getAngleMotor() { return this.angleMotor; } @@ -69,7 +105,7 @@ public class SwerveModule extends SubsystemBase { * Get the CANcoder of the SwerveModule * @return the CANcoder of the SwerveModule */ - public CANCoder getEncoder() { + public CANcoder getEncoder() { return this.encoder; } @@ -79,19 +115,23 @@ public class SwerveModule extends SubsystemBase { */ public Rotation2d getAngle() { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + return new Rotation2d(); } public double getAngularVel() { - return this.angleMotor.getSelectedSensorVelocity(); + // return this.angleMotor.getSelectedSensorVelocity(); + return 0; } public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + return 0; } public double getDriveVel() { - return this.driveMotor.getSelectedSensorVelocity(0); + // return this.driveMotor.getSelectedSensorVelocity(0); + return 0.0; } public void stop() { @@ -100,62 +140,67 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - angleMotor.set(TalonFXControlMode.Position, angle); + // angleMotor.set(TalonFXControlMode.Position, angle); } /** * Get state of swerve module * @return speed in m/s and angle in degrees */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, - getAngle() - ); - } + // public SwerveModuleState getState() { + // return new SwerveModuleState( + // Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + // getAngle() + // ); + // } /** * Returns the current position of the SwerveModule * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - } + // */ + // public SwerveModulePosition getPosition() { + // return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + // } /** * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module - */ - public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d currentRotation = this.getAngle(); + // */ + // public void setDesiredState(SwerveModuleState desiredState) { + // Rotation2d currentRotation = this.getAngle(); - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); + // // calculate the difference between our current rotational position and our new rotational position + // Rotation2d rotationDelta = state.angle.minus(currentRotation); - // calculate the new absolute position of the SwerveModule based on the difference in rotation - double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + // // calculate the new absolute position of the SwerveModule based on the difference in rotation + // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + // // convert the CANCoder from its position reading to ticks + // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - } + // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + // } - public void reset(double position) { - encoder.setPositionToAbsolute(); - } + // public void reset(double position) { + // encoder.setPositionToAbsolute(); + // } - public double getCurrent() { - return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - } + // public double getCurrent() { + // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + // } - public double getVoltage() { - return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - } + // public double getVoltage() { + // return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + // } + + // public String getstuff() { + // encoder.getPosition(); + // return "" + encoder.getLastError().value; + // } } diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/DataUtils.java new file mode 100644 index 0000000..3d998b6 --- /dev/null +++ b/src/main/java/frc4388/utility/DataUtils.java @@ -0,0 +1,35 @@ +package frc4388.utility; + +import java.nio.ByteBuffer; + +public class DataUtils { + public static byte[] doubleToByteArray(double value) { + byte[] bytes = new byte[8]; + ByteBuffer.wrap(bytes).putDouble(value); + return bytes; + } + + public static double byteArrayToDouble(byte[] bytes) { + return ByteBuffer.wrap(bytes).getDouble(); + } + + public static byte[] intToByteArray(int value) { + byte[] bytes = new byte[4]; + ByteBuffer.wrap(bytes).putInt(value); + return bytes; + } + + public static int byteArrayToInt(byte[] bytes) { + return ByteBuffer.wrap(bytes).getInt(); + } + + public static byte[] shortToByteArray(short value) { + byte[] bytes = new byte[2]; + ByteBuffer.wrap(bytes).putShort(value); + return bytes; + } + + public static short byteArrayToShort(byte[] bytes) { + return ByteBuffer.wrap(bytes).getShort(); + } +} diff --git a/src/main/java/frc4388/utility/DualJoystickButton.java b/src/main/java/frc4388/utility/DualJoystickButton.java new file mode 100644 index 0000000..e4ef5ed --- /dev/null +++ b/src/main/java/frc4388/utility/DualJoystickButton.java @@ -0,0 +1,22 @@ +package frc4388.utility; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + + +/** + * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller} + * @author Zachary Wilke + */ +public class DualJoystickButton extends Trigger { + + /** + * Creates an Button binding on two controllers + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index a037914..294dd6c 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -86,11 +86,12 @@ public class RobotGyro implements Gyro { */ @Override public void calibrate() { - if (m_isGyroAPigeon) { - m_pigeon.calibrate(); - } else { - m_navX.calibrate(); - } + return; + // if (m_isGyroAPigeon) { + // m_pigeon.calibrate(); + // } else { + // m_navX.calibrate(); + // } } @Override @@ -102,6 +103,73 @@ public class RobotGyro implements Gyro { } else { m_navX.reset(); } + + } + + public void reset(double val) { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(val); + } else { + m_navX.reset(); + } + + } + + public void resetFlip() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(180); + } else { + m_navX.reset(); + } + + } + + public void resetNinety() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(90); + } else { + m_navX.reset(); + } + + } + + public void resetTwoSeventy() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(270); + } else { + m_navX.reset(); + } + + } + + public void resetRightSideBlue() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(60); + } else { + m_navX.reset(); + } + + } + + public void resetAmpSide() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(-60); + } else { + m_navX.reset(); + } + } /** diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..935dbbe 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -6,7 +6,21 @@ public class UtilityStructs { public double leftY = 0.0; public double rightX = 0.0; public double rightY = 0.0; + + public boolean OPLB; + public boolean OPRB; + public long timedOffset = 0; } + public static class AutoRecordingControllerFrame { + public double[] axes = new double[6]; + public short button = 0; + public short[] POV = new short[1]; + + } + public static class AutoRecordingFrame { + public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2]; + public int timeStamp; + } } diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c0384db --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableDouble { + private double defualtValue; + private String name; + + /** + * Creates an new ConfigurableDouble through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableDouble(String name, double defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putNumber(name, defualtValue); + } + + public double get() { + return SmartDashboard.getNumber(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java new file mode 100644 index 0000000..34c0290 --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableString { + private String defualtValue; + private String name; + + /** + * Creates an new ConfigurableString through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableString(String name, String defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putString(name, defualtValue); + } + + public String get() { + return SmartDashboard.getString(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java new file mode 100644 index 0000000..85adb64 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -0,0 +1,145 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +/** + * A virtual controller that can be bound like an standard controller. + * @author Zachary Wilke + */ +public class VirtualController extends GenericHID { + private short m_buttonStates = 0; + private short m_buttonStatesLastFrame = 0; + private double[] m_axes = new double[6]; + private short[] m_pov = new short[1]; + + /** + * Create an virtual controller + * @param port virtual port (merely a formality). + */ + public VirtualController(int port) { + super(port); + } + + /** + * Set the curent inputs to the new frames. + * @param axes joystick axes, (i.e. joysticks and triggers). + * @param buttonFlags the bit packed button states. + * @param pov the array of dpads. + */ + public void setFrame(double[] axes, short buttonFlags, short[] pov) { + m_axes = axes; + setOutputs(buttonFlags); + m_pov = pov; + } + + /** + * Zero outs the controls. + */ + public void zeroControls() { + m_axes = new double[6]; + m_buttonStates = 0; + m_buttonStatesLastFrame = 0; + m_pov = new short[] {-1}; + } + + /** + * Gets the value of a bitflag from an int + * @param value int to search + * @param index index of bit + * @return if the bit is set + */ + public static boolean getFlag(int value, int index) { + return ((value & 1 << index) != 0); + } + + @Override + public boolean getRawButton(int button) { // man why are buttons indexed at 1. + return getFlag(m_buttonStates, button - 1); + } + + @Override + public boolean getRawButtonPressed(int button) { + return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button)); + } + + @Override + public boolean getRawButtonReleased(int button) { + return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button)); + } + + @Override + public double getRawAxis(int axis) { + return m_axes[axis]; + } + + @Override + public int getPOV(int pov) { + return m_pov[pov]; + } + + @Override + public int getAxisCount() { + return m_axes.length; + } + + @Override + public int getPOVCount() { + return m_pov.length; + } + + @Override + public int getButtonCount() { + return 10; + } + + @Override + public boolean isConnected() { + return true; + } + + @Override + public HIDType getType() { + return HIDType.kXInputGamepad; + } + + @Override + public String getName() { + return "Virtual Controller"; + } + + @Override + public int getAxisType(int axis) { + return 1; /* ! Warning, does not return accurate data. + Hopefully this isn't a problem */ + } + + /** + * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}. + * this is an no-op overide. + */ + @Override + public void setOutput(int outputNumber, boolean value) { + // do not use + //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1]; + //m_buttonStates[outputNumber - 1] = value; + } + + /** + * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} + */ + @Override + public void setOutputs(int value) { + m_buttonStatesLastFrame = m_buttonStates; + m_buttonStates = (short) value; + } + + /** + * Why are you Setting rumble on an virtual controller? + * @param type the rumble type (even though it won't do anything) + * @param value the rumble strength (always multiplyed by 0.0) + */ + @Override + public void setRumble(RumbleType type, double value) { + System.out.println("Why are you Setting rumble on an virtual controller?"); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 8c2fe88..0a56841 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,13 +1,13 @@ package frc4388.utility.controller; -import edu.wpi.first.wpilibj2.command.button.Button; +//import edu.wpi.first.wpilibj2.command.button.Trigger; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger * exceeds a tolerance defined in {@link XboxController}. */ -public class XboxTriggerButton extends Button { +public class XboxTriggerButton {//extends Trigger { public static final int RIGHT_TRIGGER = 0; public static final int LEFT_TRIGGER = 1; public static final int RIGHT_AXIS_UP_TRIGGER = 2; diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 33d81f6..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,17 +1,18 @@ { "fileName": "NavX.json", - "name": "KauaiLabs_navX_FRC", - "version": "2023.0.4", + "name": "NavX", + "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", "mavenUrls": [ - "https://dev.studica.com/maven/release/2023/" + "https://dev.studica.com/maven/release/2024/" ], - "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", "javaDependencies": [ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2023.0.4" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.4", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..ff7359e --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..0322385 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.3.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.3.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.3.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.3.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.3.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.3.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.3.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.3.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.3.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.3.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.3.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index bd535bf..67bf389 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,6 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ From 5b0132a259466458c76c54be8b051faefb64fd25 Mon Sep 17 00:00:00 2001 From: C4llSqin <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 18 Jun 2024 09:36:52 -0600 Subject: [PATCH 02/18] yeet phoenix --- vendordeps/Phoenix.json | 423 ---------------------------------------- 1 file changed, 423 deletions(-) delete mode 100644 vendordeps/Phoenix.json diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json deleted file mode 100644 index 614dc3a..0000000 --- a/vendordeps/Phoenix.json +++ /dev/null @@ -1,423 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.31.0+23.2.2", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.31.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.31.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "23.2.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "23.2.2", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "23.2.2", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "23.2.2", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "23.2.2", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "23.2.2", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "23.2.2", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "23.2.2", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "23.2.2", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "23.2.2", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "23.2.2", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file From d19ff47543c96e8f623fa270479dd13c9a5ba93c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 18 Jun 2024 11:52:58 -0600 Subject: [PATCH 03/18] PID no work --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 7 +- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 261 ++---------------- src/main/java/frc4388/robot/RobotMap.java | 9 +- .../frc4388/robot/subsystems/Limelight.java | 238 ++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +- .../robot/subsystems/SwerveModule.java | 88 ++++-- vendordeps/photonlib.json | 42 --- 9 files changed, 220 insertions(+), 437 deletions(-) delete mode 100644 vendordeps/photonlib.json diff --git a/build.gradle b/build.gradle index dc84aef..9638f90 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 827a6b5..5aebb9f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,8 +47,8 @@ public final class Constants { } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 0; - public static final int RIGHT_FRONT_STEER_ID = 1; + public static final int RIGHT_FRONT_WHEEL_ID = 3; + public static final int RIGHT_FRONT_STEER_ID = 2; public static final int RIGHT_FRONT_ENCODER_ID = 12; // public static final int LEFT_FRONT_WHEEL_ID = 4; @@ -68,6 +68,9 @@ public final class Constants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + + public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 36c4330..cd9c24c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -124,7 +124,7 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3888775..a0e9a78 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -68,18 +68,12 @@ public class RobotContainer { // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - // m_robotMap.rightFront, - // m_robotMap.leftBack, - // m_robotMap.rightBack, + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.rightFront, + m_robotMap.leftBack, + m_robotMap.rightBack, - // m_robotMap.gyro); - /* Limelight */ - // private final Limelight limelight = new Limelight(); - - // private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); - - // private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); + m_robotMap.gyro); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -119,53 +113,6 @@ public class RobotContainer { // ); - // private SequentialCommandGroup i = new SequentialCommandGroup( - // intakeToShootStuff, intakeToShoot, - // new InstantCommand(() -> m_robotShooter.idle()) - // ); - - // private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - // new WaitCommand(0.75), - // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) - // ); - - // private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) - // // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - // ); - - // private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( - // interrupt, - // new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), - // new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) - // ); - - // private SequentialCommandGroup ampShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.ampPosition()), - // new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed - // ); - - // ! /* Autos */ - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - // private String lastAutoName = "final_red_center_4note_taxi.auto"; - // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); - // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), // lastAutoName - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false); - - - // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - // .addOption("Taxi Auto", taxi.asProxy()) - // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) - // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) - // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) - // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) - // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) - // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) - // .buildDisplay(); - /** @@ -173,7 +120,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - configureVirtualButtonBindings(); + // configureVirtualButtonBindings(); // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -202,7 +149,17 @@ public class RobotContainer { // getDeadbandedDriverController().getRightX(), // getDeadbandedDriverController().getRightY(), // true); - // }, m_robotSwerveDrive) + // }, m_robotSwerveDrive)); + + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.oneModuleTest( + m_robotMap.rightFront, + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight()); + }, m_robotSwerveDrive)); + + + // .withName("SwerveDrive OrientationCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); @@ -223,190 +180,6 @@ public class RobotContainer { */ private void configureButtonBindings() { - // ? /* Driver Buttons */ - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // * /* D-Pad Stuff */ - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // ! /* Auto Recording */ - // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - // () -> autoplaybackName.get())) - // .onFalse(new InstantCommand()); - - // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false)) - // .onFalse(new InstantCommand()); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Taxi.txt")) - // .onFalse(new InstantCommand()); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - // .onFalse(new InstantCommand()); - // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); - - // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); - - - //? /* Operator Buttons */ - - // new Joystick(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotMap.rightFront.go(new Translation2d()))); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) - // .onTrue(emergencyRetract); - - - // Override Intake Position encoder: out - // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); - - // // Override Intake Position encoder: in - // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) - // .onFalse(turnOffShoot); - - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(i) - // .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); - - // //spins up shooter, no wind down - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - - // // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - // // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) - // // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - // .onTrue(emergencyRetract); - - // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); - - } - - /** - * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

- * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. - */ - private void configureVirtualButtonBindings() { - - // ? /* Driver Buttons */ - - /* Notice: the following buttons have not been replicated - * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback - * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. - * Auto Recording controls : We don't want an Null Ouroboros for an auto. - */ - - // ? /* Operator Buttons */ - - /* Notice: the following buttons have not been replicated - * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. - * We don't need it in an auto. - * Climbing controls : We don't need to climb in auto. - */ - - // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getVirtualOperatorController().getPOV() == 0) - // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); - } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index aa9fba8..7769faf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -31,6 +31,7 @@ import frc4388.utility.RobotGyro; public class RobotMap { // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); // public RobotGyro gyro = new RobotGyro(m_pigeon2); + public RobotGyro gyro = null; public SwerveModule leftFront; public SwerveModule rightFront; @@ -152,8 +153,14 @@ public class RobotMap { // rightBackSteer.setNeutralMode(NeutralMode.Brake); // // initialize SwerveModules - // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + this.leftFront = this.rightFront; + this.leftBack = this.rightFront; + this.rightBack = this.rightFront; + + // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..8e7da41 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -1,165 +1,165 @@ -// 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. +// // 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 frc4388.robot.subsystems; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; +// import java.io.IOException; +// import java.util.ArrayList; +// import java.util.List; +// import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; +// // import org.photonvision.EstimatedRobotPose; +// // import org.photonvision.PhotonCamera; +// // import org.photonvision.PhotonPoseEstimator; +// // import org.photonvision.PhotonPoseEstimator.PoseStrategy; +// // import org.photonvision.common.hardware.VisionLEDMode; +// // import org.photonvision.targeting.PhotonPipelineResult; +// // import org.photonvision.targeting.PhotonTrackedTarget; +// // import org.photonvision.targeting.TargetCorner; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; +// import edu.wpi.first.apriltag.AprilTag; +// import edu.wpi.first.apriltag.AprilTagFieldLayout; +// import edu.wpi.first.apriltag.AprilTagFields; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc4388.robot.Constants.VisionConstants; -public class Limelight extends SubsystemBase { +// public class Limelight extends SubsystemBase { - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; +// // private PhotonCamera cam; +// // private PhotonPoseEstimator photonPoseEstimator; - private boolean lightOn; +// private boolean lightOn; - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } +// /** Creates a new Limelight. */ +// public Limelight() { +// // cam = new PhotonCamera(VisionConstants.NAME); +// // cam.setDriverMode(false); +// } - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } +// public void setLEDs(boolean on) { +// lightOn = on; +// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); +// } - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } +// public void toggleLEDs() { +// lightOn = !lightOn; +// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); +// } - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } +// public void setDriverMode(boolean driverMode) { +// // cam.setDriverMode(driverMode); +// } - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } +// public void setToLimePipeline() { +// // cam.setPipelineIndex(1); +// setLEDs(true); +// } - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } +// public void setToAprilPipeline() { +// // cam.setPipelineIndex(0); +// setLEDs(false); +// } - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; +// public PhotonTrackedTarget getAprilPoint() { +// // if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// // PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// // if (!result.hasTargets()) return null; - return result.getBestTarget(); - } +// return result.getBestTarget(); +// } - private List getAprilCorners() { - if (!cam.isConnected()) return null; +// private List getAprilCorners() { +// if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// if (!result.hasTargets()) return null; - return result.getBestTarget().getDetectedCorners(); - } +// return result.getBestTarget().getDetectedCorners(); +// } - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); +// public double getAprilSkew() { +// List corners = getAprilCorners(); +// ArrayList bottomSide = getAprilBottomSide(corners); - if (bottomSide == null) return 0; +// if (bottomSide == null) return 0; - TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); - TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); +// TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); +// TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - return bottomLeft.y - bottomRight.y; - } +// return bottomLeft.y - bottomRight.y; +// } - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; +// private ArrayList getAprilBottomSide(List box) { +// if (box == null) return null; - ArrayList bottomSide = new ArrayList<>(); +// ArrayList bottomSide = new ArrayList<>(); - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); +// TargetCorner l1 = new TargetCorner(-1, -1); +// TargetCorner l2 = new TargetCorner(-1, -1); - for (TargetCorner c : box) { - if (c.y > l1.y) l1 = c; - } +// for (TargetCorner c : box) { +// if (c.y > l1.y) l1 = c; +// } - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } +// for (TargetCorner c : box) { +// if (c.y == l1.y) continue; +// if (c.y > l2.y) l2 = c; +// } - bottomSide.add(l1); - bottomSide.add(l2); +// bottomSide.add(l1); +// bottomSide.add(l2); - return bottomSide; - } +// return bottomSide; +// } - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; +// public double getDistanceToApril() { +// PhotonTrackedTarget aprilPoint = getAprilPoint(); +// if (aprilPoint == null) return -1; - double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + aprilPoint.getPitch(); +// double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; +// double theta = 35.0 + aprilPoint.getPitch(); - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } +// double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); +// return distanceToApril; +// } - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; +// public PhotonTrackedTarget getLowestTape() { +// if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// if (!result.hasTargets()) return null; - ArrayList points = (ArrayList) result.getTargets(); +// ArrayList points = (ArrayList) result.getTargets(); - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } +// PhotonTrackedTarget lowest = points.get(0); +// for (PhotonTrackedTarget point : points) { +// if (point.getPitch() < lowest.getPitch()) { +// lowest = point; +// } +// } - return lowest; - } +// return lowest; +// } - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; +// public double getDistanceToTape() { +// PhotonTrackedTarget tapePoint = getLowestTape(); +// if (tapePoint == null) return -1; - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + tapePoint.getPitch(); +// double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; +// double theta = 35.0 + tapePoint.getPitch(); - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; - } +// double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); +// return distanceToTape; +// } - @Override - public void periodic() {} -} +// @Override +// public void periodic() {} +// } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 965bbcb..adf7d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -53,9 +53,15 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } + public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); + System.out.println(ang); + module.go(ang); + } + boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - rightFront.go(leftStick); + // if (fieldRelative) { // double rot = 0; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ef0d062..8d5322f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -14,12 +14,16 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; // import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; // import com.ctre.phoenix.sensors.CANCoder; // import com.ctre.phoenix.sensors.SensorInitializationStrategy; - +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.hardware.CANcoder; @@ -38,6 +42,8 @@ public class SwerveModule extends SubsystemBase { private TalonFX driveMotor; private TalonFX angleMotor; private CANcoder encoder; + // private final StatusSignal cc_pos; + // private final StatusSignal cc_vel; // private int selfid; // private ConfigurableDouble offsetGetter; private static int swerveId = 0; @@ -49,19 +55,42 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; + + // TalonFXConfiguration pidConfigs = new TalonFXConfiguration(); + // pidConfigs.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output + // pidConfigs.Slot0.kI = 0; // no output for integrated error + // pidConfigs.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output + + // angleMotor.getConfigurator().apply(slot0Configs); + + // var fx_cfg = new FeedbackConfigs(); + // fx_cfg.FeedbackRemoteSensorID = encoder.getDeviceID(); + // fx_cfg.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + // angleMotor.getConfigurator().apply(fx_cfg); // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); // this.selfid = swerveId; // swerveId++; - // TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - // angleConfig.slot0.kP = swerveGains.kP; - // angleConfig.slot0.kI = swerveGains.kI; - // angleConfig.slot0.kD = swerveGains.kD; + TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + // angleConfig.Slot0.kP = swerveGains.kP; + // angleConfig.Slot0.kI = swerveGains.kI; + // angleConfig.Slot0.kD = swerveGains.kD; + angleConfig.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output + angleConfig.Slot0.kI = 0; // no output for integrated error + angleConfig.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output - // // use the CANcoder as the remote sensor for the primary TalonFX PID - // angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - // angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - // angleMotor.configAllSettings(angleConfig); + // var fx_cfg = new FeedbackConfigs(); + angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); + angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + // angleConfig.Feedback. = FeedbackDevice.RemoteSensor0; + // angleConfig.Feedback = fx_cfg; + + angleMotor.getConfigurator().apply(angleConfig); + + // use the CANcoder as the remote sensor for the primary TalonFX PID + // angleConfig.Fee = encoder.getDeviceID(); + // angleConfig.FeedbackSensorSource = RemoteSensorSource.CANCoder; + // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + // angleMotor.getConfigurator().apply(angleConfig); // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); // reset(0); @@ -71,9 +100,16 @@ public class SwerveModule extends SubsystemBase { // driveMotor.config_kP(0, 0.2); } - public void go(Translation2d leftStick){ - System.out.println(leftStick.getY()); - driveMotor.set(leftStick.getY()); + public void go(double ang){ + double curang = this.encoder.getAbsolutePosition().getValue(); + System.out.println(ang-curang); + + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + + // set position to 10 rotations + angleMotor.setControl(m_request.withPosition(ang)); + + // System.out.println(this.cc_pos.getValue()); } @Override @@ -166,26 +202,26 @@ public class SwerveModule extends SubsystemBase { * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module // */ - // public void setDesiredState(SwerveModuleState desiredState) { - // Rotation2d currentRotation = this.getAngle(); + public void setDesiredState(SwerveModuleState desiredState) { + // Rotation2d currentRotation = this.getAngle(); - // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - // // calculate the difference between our current rotational position and our new rotational position - // Rotation2d rotationDelta = state.angle.minus(currentRotation); + // // calculate the difference between our current rotational position and our new rotational position + // Rotation2d rotationDelta = state.angle.minus(currentRotation); - // // calculate the new absolute position of the SwerveModule based on the difference in rotation - // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + // // calculate the new absolute position of the SwerveModule based on the difference in rotation + // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - // // convert the CANCoder from its position reading to ticks - // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + // // convert the CANCoder from its position reading to ticks + // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - // } + // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + } // public void reset(double position) { // encoder.setPositionToAbsolute(); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index c940b75..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,42 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.1.1-beta-3.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "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": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2024.1.1-beta-3.2", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2024.1.1-beta-3.2" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2024.1.1-beta-3.2" - } - ] -} \ No newline at end of file From 44dd9f3a4e290c4d2b9b7d3411db361a7fd0ba7b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 20 Jun 2024 11:29:41 -0600 Subject: [PATCH 04/18] Pointing to angle --- src/main/java/frc4388/robot/Constants.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../robot/subsystems/SwerveModule.java | 67 ++++++------------- 3 files changed, 21 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5aebb9f..296c8d4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,7 +67,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(32, 0.0, 0.0, 0.0, 0, 0.0); public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index adf7d5e..a48a906 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -55,7 +55,7 @@ public class SwerveDrive extends SubsystemBase { public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); - System.out.println(ang); + // System.out.println(ang); module.go(ang); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 8d5322f..32fe785 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -16,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; // import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -25,6 +26,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.geometry.Translation2d; @@ -56,60 +58,28 @@ public class SwerveModule extends SubsystemBase { this.angleMotor = angleMotor; this.encoder = encoder; - // TalonFXConfiguration pidConfigs = new TalonFXConfiguration(); - // pidConfigs.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output - // pidConfigs.Slot0.kI = 0; // no output for integrated error - // pidConfigs.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output - - // angleMotor.getConfigurator().apply(slot0Configs); - - // var fx_cfg = new FeedbackConfigs(); - // fx_cfg.FeedbackRemoteSensorID = encoder.getDeviceID(); - // fx_cfg.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - // angleMotor.getConfigurator().apply(fx_cfg); - // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); - // this.selfid = swerveId; - // swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - // angleConfig.Slot0.kP = swerveGains.kP; - // angleConfig.Slot0.kI = swerveGains.kI; - // angleConfig.Slot0.kD = swerveGains.kD; - angleConfig.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output - angleConfig.Slot0.kI = 0; // no output for integrated error - angleConfig.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output + angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + angleConfig.Slot0.kP = swerveGains.kP; + angleConfig.Slot0.kI = swerveGains.kI; + angleConfig.Slot0.kD = swerveGains.kD; - // var fx_cfg = new FeedbackConfigs(); angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - // angleConfig.Feedback. = FeedbackDevice.RemoteSensor0; - // angleConfig.Feedback = fx_cfg; - angleMotor.getConfigurator().apply(angleConfig); - // use the CANcoder as the remote sensor for the primary TalonFX PID - // angleConfig.Fee = encoder.getDeviceID(); - // angleConfig.FeedbackSensorSource = RemoteSensorSource.CANCoder; - // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - // angleMotor.getConfigurator().apply(angleConfig); - - // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); - // reset(0); - // encoder.configMagnetOffset(offset); - - // driveMotor.setSelectedSensorPosition(0); - // driveMotor.config_kP(0, 0.2); + CANcoderConfiguration canconfig = new CANcoderConfiguration(); + canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + encoder.getConfigurator().apply(canconfig); + + rotateToAngle(0); } public void go(double ang){ - double curang = this.encoder.getAbsolutePosition().getValue(); - System.out.println(ang-curang); - - final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); - - // set position to 10 rotations - angleMotor.setControl(m_request.withPosition(ang)); - - // System.out.println(this.cc_pos.getValue()); + // double curang = this.encoder.getAbsolutePosition().getValue(); + System.out.println(getAngle().getDegrees()); + rotateToAngle(ang); } @Override @@ -152,7 +122,7 @@ public class SwerveModule extends SubsystemBase { public Rotation2d getAngle() { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - return new Rotation2d(); + return Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue()); } public double getAngularVel() { @@ -176,7 +146,8 @@ public class SwerveModule extends SubsystemBase { } public void rotateToAngle(double angle) { - // angleMotor.set(TalonFXControlMode.Position, angle); + final PositionVoltage m_request = new PositionVoltage(angle); + angleMotor.setControl(m_request); } /** @@ -203,7 +174,7 @@ public class SwerveModule extends SubsystemBase { * @param desiredState a SwerveModuleState representing the desired new state of the module // */ public void setDesiredState(SwerveModuleState desiredState) { - // Rotation2d currentRotation = this.getAngle(); + Rotation2d currentRotation = this.getAngle(); // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); From 3df6fbf7d9f34904868400e91a68e57a51c8b784 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Sun, 23 Jun 2024 11:16:55 -0600 Subject: [PATCH 05/18] Use SwerveModuleState --- .../robot/subsystems/SwerveModule.java | 41 ++++++++----------- 1 file changed, 16 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 32fe785..a8f7a6f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -154,12 +154,14 @@ public class SwerveModule extends SubsystemBase { * Get state of swerve module * @return speed in m/s and angle in degrees */ - // public SwerveModuleState getState() { - // return new SwerveModuleState( - // Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, - // getAngle() - // ); - // } + public SwerveModuleState getState() { + return new SwerveModuleState( + Units.inchesToMeters(driveMotor.getVelocity().getValue() * + SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * + SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), + getAngle() + ); + } /** * Returns the current position of the SwerveModule @@ -174,29 +176,18 @@ public class SwerveModule extends SubsystemBase { * @param desiredState a SwerveModuleState representing the desired new state of the module // */ public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d currentRotation = this.getAngle(); + SwerveModuleState state = SwerveModuleState.optimize(desiredState, this.getAngle()); - // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + Rotation2d rot = state.angle; + double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - // // calculate the difference between our current rotational position and our new rotational position - // Rotation2d rotationDelta = state.angle.minus(currentRotation); - - // // calculate the new absolute position of the SwerveModule based on the difference in rotation - // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - - // // convert the CANCoder from its position reading to ticks - // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - - // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - - // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - - // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + rotateToAngle(rot.getRotations()); + driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); } - // public void reset(double position) { - // encoder.setPositionToAbsolute(); - // } + public void reset() { + // encoder.setPosition(0); + } // public double getCurrent() { // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); From da1c29f913a280aef70cbcb40198ed787e817b45 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 27 Jun 2024 11:35:13 -0600 Subject: [PATCH 06/18] readd rotation delta calculations in swerve module --- .../frc4388/robot/subsystems/SwerveDrive.java | 10 +++++-- .../robot/subsystems/SwerveModule.java | 28 +++++++++++++------ 2 files changed, 27 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a48a906..06a9cf1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -54,9 +54,15 @@ public class SwerveDrive extends SubsystemBase { } public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); + // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // rightStick.getAngle() + double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); // System.out.println(ang); - module.go(ang); + // module.go(ang); + // Rotation2d rot = Rotation2d.fromRadians(ang); + Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + SwerveModuleState state = new SwerveModuleState(speed, rot); + module.setDesiredState(state); } boolean stopped = false; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index a8f7a6f..0bd2649 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -76,11 +76,11 @@ public class SwerveModule extends SubsystemBase { rotateToAngle(0); } - public void go(double ang){ - // double curang = this.encoder.getAbsolutePosition().getValue(); - System.out.println(getAngle().getDegrees()); - rotateToAngle(ang); - } + // public void go(double ang){ + // // double curang = this.encoder.getAbsolutePosition().getValue(); + // System.out.println(getAngle().getDegrees()); + // rotateToAngle(ang); + // } @Override public void periodic() { @@ -122,7 +122,7 @@ public class SwerveModule extends SubsystemBase { public Rotation2d getAngle() { // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - return Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue()); + return Rotation2d.fromRotations(encoder.getPosition().getValue()); } public double getAngularVel() { @@ -163,6 +163,11 @@ public class SwerveModule extends SubsystemBase { ); } + // private SwerveModuleState optimizeState(SwerveModuleState desiredState) { + // Rotation2d curRot = this.getAngle(); + + // } + /** * Returns the current position of the SwerveModule * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. @@ -176,12 +181,17 @@ public class SwerveModule extends SubsystemBase { * @param desiredState a SwerveModuleState representing the desired new state of the module // */ public void setDesiredState(SwerveModuleState desiredState) { - SwerveModuleState state = SwerveModuleState.optimize(desiredState, this.getAngle()); + Rotation2d currentRotation = this.getAngle(); + + SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + + // calculate the difference between our current rotational position and our new rotational position + Rotation2d rotationDelta = state.angle.minus(currentRotation); - Rotation2d rot = state.angle; double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - rotateToAngle(rot.getRotations()); + rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations()); + driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); } From eb232fbe815d2a1419d6b0d673a28fbc28c57cdb Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 23 Jul 2024 09:16:55 -0600 Subject: [PATCH 07/18] Kracken stuff --- .../java/frc4388/robot/RobotContainer.java | 6 +- src/main/java/frc4388/robot/RobotMap.java | 7 +- .../frc4388/robot/subsystems/SwerveDrive.java | 66 +++++++++---------- src/main/java/frc4388/utility/RobotGyro.java | 23 ++++--- 4 files changed, 55 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a0e9a78..c671420 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -152,10 +152,10 @@ public class RobotContainer { // }, m_robotSwerveDrive)); m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.oneModuleTest( - m_robotMap.rightFront, + m_robotSwerveDrive.driveWithInput( getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight()); + getDeadbandedDriverController().getRight(), + true); }, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 7769faf..c6f0224 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -14,6 +14,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; @@ -29,9 +30,9 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public RobotGyro gyro = null; + private Pigeon2 m_pigeon2 = new Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); + // public RobotGyro gyro = null; public SwerveModule leftFront; public SwerveModule rightFront; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 06a9cf1..40b30e5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -68,38 +68,38 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - // if (fieldRelative) { + if (fieldRelative) { - // double rot = 0; + double rot = 0; - // // ! drift correction - // if (rightStick.getNorm() > 0.05) { - // rotTarget = gyro.getAngle(); - // rot = rightStick.getX(); - // // SmartDashboard.putBoolean("drift correction", false); - // stopped = false; - // } else if(leftStick.getNorm() > 0.05) { - // if (!stopped) { - // stopModules(); - // stopped = true; - // } + // ! drift correction + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX(); + // SmartDashboard.putBoolean("drift correction", false); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } - // // SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); - // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - // } + } - // // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - // // Convert field-relative speeds to robot-relative speeds. - // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); - // } else { // Create robot-relative speeds. - // chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - // } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { @@ -168,14 +168,14 @@ public class SwerveDrive extends SubsystemBase { * Set each module of the swerve drive to the corresponding desired state. * @param desiredStates Array of module states to set. */ - // public void setModuleStates(SwerveModuleState[] desiredStates) { - // SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - // for (int i = 0; i < desiredStates.length; i++) { - // SwerveModule module = modules[i]; - // SwerveModuleState state = desiredStates[i]; - // module.setDesiredState(state); - // } - // } + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } + } public boolean rotateToTarget(double angle) { double currentAngle = getGyroAngle(); diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 294dd6c..2f966d7 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -8,11 +8,13 @@ package frc4388.utility; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.hardware.Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; /** * Gyro class that allows for interchangeable use between a pigeon and a navX @@ -20,7 +22,7 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private WPI_Pigeon2 m_pigeon = null; + private Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX @@ -34,7 +36,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(WPI_Pigeon2 gyro) { + public RobotGyro(Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -54,8 +56,8 @@ public class RobotGyro implements Gyro { public void resetZeroValues() { if (!m_isGyroAPigeon) return; - pitchZero = m_pigeon.getPitch(); - rollZero = m_pigeon.getRoll(); + // pitchZero = m_pigeon.getPitch(); + // rollZero = m_pigeon.getRoll(); } /** @@ -181,10 +183,15 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] ypr = new double[3]; - m_pigeon.getYawPitchRoll(ypr); + m_pigeon.getAngle(); + var rotation = m_pigeon.getRotation3d(); - return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)}; + } + + @Override + public Rotation2d getRotation2d() { + return m_pigeon.getRotation2d(); } @Override @@ -253,7 +260,7 @@ public class RobotGyro implements Gyro { } } - public WPI_Pigeon2 getPigeon(){ + public Pigeon2 getPigeon(){ return m_pigeon; } From c08506dd5e424be48621089fe5cd716ffbc27d36 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 25 Jul 2024 11:41:41 -0600 Subject: [PATCH 08/18] Make swerve drive work. Some stuff on drift correction --- src/main/java/frc4388/robot/Constants.java | 32 ++++++------- .../java/frc4388/robot/RobotContainer.java | 3 +- src/main/java/frc4388/robot/RobotMap.java | 48 +++++++++---------- .../frc4388/robot/subsystems/SwerveDrive.java | 32 +++++++++---- .../robot/subsystems/SwerveModule.java | 40 ++++++++++++++-- src/main/java/frc4388/utility/RobotGyro.java | 4 +- 6 files changed, 103 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 296c8d4..aee6cae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,28 +40,28 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 220; - public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -279.08349884; - public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656; + public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25; + public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 3; - public static final int RIGHT_FRONT_STEER_ID = 2; - public static final int RIGHT_FRONT_ENCODER_ID = 12; + public static final int RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; - // public static final int LEFT_FRONT_WHEEL_ID = 4; - // public static final int LEFT_FRONT_STEER_ID = 5; - // public static final int LEFT_FRONT_ENCODER_ID = 11; + public static final int LEFT_FRONT_WHEEL_ID = 4; + public static final int LEFT_FRONT_STEER_ID = 5; + public static final int LEFT_FRONT_ENCODER_ID = 11; - // public static final int LEFT_BACK_WHEEL_ID = 6; - // public static final int LEFT_BACK_STEER_ID = 7; - // public static final int LEFT_BACK_ENCODER_ID = 12; + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; - // public static final int RIGHT_BACK_WHEEL_ID = 8; - // public static final int RIGHT_BACK_STEER_ID = 9; - // public static final int RIGHT_BACK_ENCODER_ID = 13; + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c671420..8ccef78 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,7 +179,8 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c6f0224..9595f48 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -48,27 +48,27 @@ public class RobotMap { // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + /* Swreve Drive Subsystem */ + public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + + public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - /* Swreve Drive Subsystem */ - // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - - - // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - - // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); /* Shooter Subsystem */ // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); @@ -104,7 +104,7 @@ public class RobotMap { // rightBackSteer.configFactoryDefault(); // // config open loop ramp - // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); @@ -117,7 +117,7 @@ public class RobotMap { // rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // // config closed loop ramp - // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); // rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); @@ -139,7 +139,7 @@ public class RobotMap { // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // // set neutral mode @@ -156,9 +156,9 @@ public class RobotMap { // // initialize SwerveModules this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - this.leftFront = this.rightFront; - this.leftBack = this.rightFront; - this.rightBack = this.rightFront; + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 40b30e5..45f68f7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -10,10 +10,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; +import frc4388.utility.RobotUnits; public class SwerveDrive extends SubsystemBase { @@ -68,6 +70,9 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0; + SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); + if (fieldRelative) { double rot = 0; @@ -75,7 +80,8 @@ public class SwerveDrive extends SubsystemBase { // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); - rot = rightStick.getX(); + rot_correction = 0; + // rot = rightStick.getX(); // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { @@ -86,7 +92,7 @@ public class SwerveDrive extends SubsystemBase { // SmartDashboard.putBoolean("drift correction", true); - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } @@ -95,7 +101,8 @@ public class SwerveDrive extends SubsystemBase { // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1)); + // chassisSpeeds = chassisSpeeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -120,8 +127,8 @@ public class SwerveDrive extends SubsystemBase { } // SmartDashboard.putBoolean("drift correction", true); + // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } @@ -137,9 +144,15 @@ public class SwerveDrive extends SubsystemBase { // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - Translation2d rightStick = new Translation2d(-rightX, rightY); + // Translation2d rightStick = new Translation2d(-rightX, rightY); + double rightX = rightStick.getX(); + double rightY = rightStick.getY(); + + double rot_correction = 0; + + // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; if(fieldRelative) { double rot = 0; @@ -157,7 +170,7 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -191,7 +204,7 @@ public class SwerveDrive extends SubsystemBase { } public double getGyroAngle() { - return gyro.getAngle(); + return -gyro.getAngle(); } public void add180() { @@ -238,7 +251,8 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run\ - // SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("RotTartget", rotTarget); } public void shiftDown() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 0bd2649..8042080 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -17,7 +17,10 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; @@ -26,6 +29,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.hardware.CANcoder; @@ -58,7 +62,34 @@ public class SwerveModule extends SubsystemBase { this.angleMotor = angleMotor; this.encoder = encoder; - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + var motorCfg = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ); + + driveMotor.getConfigurator().apply(motorCfg); + + TalonFXConfiguration angleConfig = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ); + angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; angleConfig.Slot0.kP = swerveGains.kP; @@ -71,6 +102,7 @@ public class SwerveModule extends SubsystemBase { CANcoderConfiguration canconfig = new CANcoderConfiguration(); canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + canconfig.MagnetSensor.MagnetOffset = offset; encoder.getConfigurator().apply(canconfig); rotateToAngle(0); @@ -127,17 +159,17 @@ public class SwerveModule extends SubsystemBase { public double getAngularVel() { // return this.angleMotor.getSelectedSensorVelocity(); - return 0; + return angleMotor.getVelocity().getValueAsDouble(); } public double getDrivePos() { // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - return 0; + return driveMotor.getPosition().getValueAsDouble(); } public double getDriveVel() { // return this.driveMotor.getSelectedSensorVelocity(0); - return 0.0; + return driveMotor.getVelocity().getValueAsDouble(); } public void stop() { diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 2f966d7..e1fed56 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -186,7 +186,7 @@ public class RobotGyro implements Gyro { m_pigeon.getAngle(); var rotation = m_pigeon.getRotation3d(); - return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)}; + return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; } @Override @@ -197,7 +197,7 @@ public class RobotGyro implements Gyro { @Override public double getAngle() { if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; + return getPigeonAngles()[2]; } else { return m_navX.getAngle(); } From 0304eb13844bdb4848aca6ea9e72f25670144feb Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 8 Aug 2024 09:10:18 -0600 Subject: [PATCH 09/18] re add subsystems --- .../java/frc4388/robot/RobotContainer.java | 270 ++++++++++++++++-- src/main/java/frc4388/robot/RobotMap.java | 10 +- .../robot/commands/Intake/ArmIntakeIn.java | 53 ++++ .../frc4388/robot/subsystems/Climber.java | 62 ++++ .../java/frc4388/robot/subsystems/Intake.java | 110 +++++++ .../java/frc4388/robot/subsystems/LED.java | 69 +++-- .../frc4388/robot/subsystems/Limelight.java | 205 ++++--------- .../frc4388/robot/subsystems/Shooter.java | 114 ++++++++ src/main/java/frc4388/utility/RobotGyro.java | 12 +- 9 files changed, 705 insertions(+), 200 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java create mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java create mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8ccef78..c3287b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; // Autos import frc4388.robot.commands.Autos.PlaybackChooser; +import frc4388.robot.commands.Intake.ArmIntakeIn; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.utility.controller.VirtualController; @@ -39,7 +40,10 @@ import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; // import frc4388.robot.commands.Intake.ArmIntakeIn; //import frc4388.robot.commands.Autos.AutoAlign; - +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.Shooter; // Subsystems // import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.Limelight; @@ -66,7 +70,7 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); - // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, @@ -78,27 +82,89 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - // private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + private final Limelight limelight = new Limelight(); + + private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + + private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); + /* Virtual Controllers */ - // private final VirtualController m_virtualDriver = new VirtualController(0); - // private final VirtualController m_virtualOperator = new VirtualController(1); + private final VirtualController m_virtualDriver = new VirtualController(0); + private final VirtualController m_virtualOperator = new VirtualController(1); - // private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - // private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); - // private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.PIDIn()), - // new InstantCommand(() -> m_robotShooter.idle()) - // // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), - // // new InstantCommand(() -> m_robotShooter.spin()) - // ); + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.PIDIn()), + new InstantCommand(() -> m_robotShooter.idle()) + // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), + // new InstantCommand(() -> m_robotShooter.spin()) + ); // ! Teleop Commands + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( + // MoveToSpeaker, + //autoAlign, + new InstantCommand(() -> m_robotShooter.spin()), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotShooter.idle()) + // new InstantCommand(() -> autoAlign.reverse()), + // autoAlign.asProxy() + ); - //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + + private SequentialCommandGroup i = new SequentialCommandGroup( + intakeToShootStuff, intakeToShoot, + new InstantCommand(() -> m_robotShooter.idle()) + ); + + private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(0.75), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) + ); + + private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); + + private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( + interrupt, + new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + ); + + private SequentialCommandGroup ampShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.ampPosition()), + new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed + ); + + // ! /* Autos */ + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private String lastAutoName = "final_red_center_4note_taxi.auto"; + private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); + private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, + () -> autoplaybackName.get(), // lastAutoName + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false); + + // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + // .addOption("Taxi Auto", taxi.asProxy()) + // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) + // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) + // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) + // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) + // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) + // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) + // .buildDisplay(); // private SequentialCommandGroup autoShoot = new SequentialCommandGroup( // // MoveToSpeaker, @@ -179,8 +245,170 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + + // ? /* Driver Buttons */ + + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); + + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + + // * /* D-Pad Stuff */ + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // ! /* Auto Recording */ + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // () -> autoplaybackName.get())) + // .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) + // .onFalse(new InstantCommand()); + + // ! /* Speed */ + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + + + + //? /* Operator Buttons */ + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + .onTrue(emergencyRetract); + + + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); + + // Override Intake Position encoder: in + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) + .onFalse(turnOffShoot); + + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(i) + .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); + + //spins up shooter, no wind down + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + .onTrue(emergencyRetract); + + new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + + } + + /** + * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

+ * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. + */ + private void configureVirtualButtonBindings() { + + // ? /* Driver Buttons */ + + /* Notice: the following buttons have not been replicated + * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback + * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. + * Auto Recording controls : We don't want an Null Ouroboros for an auto. + */ + + // ? /* Operator Buttons */ + + /* Notice: the following buttons have not been replicated + * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. + * We don't need it in an auto. + * Climbing controls : We don't need to climb in auto. + */ + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getVirtualOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); } /** @@ -216,11 +444,11 @@ public class RobotContainer { return this.m_operatorXbox; } - // public VirtualController getVirtualDriverController() { - // return m_virtualDriver; - // } + public VirtualController getVirtualDriverController() { + return m_virtualDriver; + } - // public VirtualController getVirtualOperatorController() { - // return m_virtualOperator; - // } + public VirtualController getVirtualOperatorController() { + return m_virtualOperator; + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9595f48..d1a2b91 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -71,15 +71,15 @@ public class RobotMap { public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); /* Shooter Subsystem */ - // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); - // public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); + public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); + public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); /* Intake Subsystem */ - // public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); - // public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); /* Climber Subsystem */ - // public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); + public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); void configureLEDMotorControllers() { } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java new file mode 100644 index 0000000..42f9dfc --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -0,0 +1,53 @@ +// 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 frc4388.robot.commands.Intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.SwerveDrive; + +public class ArmIntakeIn extends Command { + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; + + public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; + + addRequirements(this.robotIntake, this.robotShooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + robotIntake.PIDOut(); + robotIntake.spinIntakeMotor(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return robotIntake.getIntakeLimitSwitchState(); + // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) + // { + // return !true==true; + // } + // else + // { + // return !false==!(!(true)); + // } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..ec78e53 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,62 @@ +// 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 frc4388.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import frc4388.robot.Constants.ClimbConstants; +import frc4388.robot.Constants.IntakeConstants; + +//! 6.5C Scoring Criteria for Stage + +public class Climber extends SubsystemBase { + /** Creates a new Climber. */ + TalonFX climbMotor; + + public Climber(TalonFX climbMotor) { + this.climbMotor = climbMotor; + this.climbMotor.setInverted(true); + + climbMotor.setNeutralMode(NeutralModeValue.Brake); + + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + + climbMotor.getConfigurator().apply(slot0Configs); + } + + public void climbOut() { + //PositionVoltage request = new PositionVoltage(0); + //climbMotor.setControl(request.withPosition(-520)); + + climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED); + } + + public void climbIn() { + //PositionVoltage request = new PositionVoltage(-520); + //climbMotor.setControl(request.withPosition(0)); + climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + ; + } + + public void stopClimb() { + climbMotor.set(0.d); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java new file mode 100644 index 0000000..7d7294c --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -0,0 +1,110 @@ +// 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 frc4388.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.utility.Gains; + +public class Intake extends SubsystemBase { + private TalonFX intakeMotor; + private TalonFX pivotMotor; + public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; + + /** Creates a new Intake. */ + public Intake(TalonFX intakeMotor, TalonFX pivotMotor) { + this.intakeMotor = intakeMotor; + this.pivotMotor = pivotMotor; + + intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); + pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); + + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); + + // in init function, set slot 0 gains + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output + + pivotMotor.getConfigurator().apply(slot0Configs); + } + + // ! Talon Methods + public void PIDIn() { + PIDPosition(0); + } + + public void PIDOut() { + PIDPosition(-53); + } + + public void PIDPosition(double pos) { + PositionVoltage request = new PositionVoltage(pos); + pivotMotor.setControl(request); + } + + public void handoff() { + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void spinIntakeMotor() { + intakeMotor.set(IntakeConstants.INTAKE_SPEED); + } + + public void spinIntakeMotor(double speed) { + intakeMotor.set(speed); + } + + public boolean getIntakeLimitSwitchState() { + return intakeMotor.getForwardLimit().getValue().value == 0; + } + + public void setPivotEncoderPosition(double val) { + pivotMotor.setPosition(val); + } + + public void stopIntakeMotors() { + intakeMotor.set(0); + } + + public void stopArmMotor() { + pivotMotor.set(0); + } + + public void stop() { + intakeMotor.set(0); + pivotMotor.set(0); + } + + public double getArmPos() { + return pivotMotor.getPosition().getValue(); + } + + public void resetArmPosition() { + if (getIntakeLimitSwitchState()) { + // talonPivot.setPosition(0); + } + } + + public void ampPosition() { + PIDPosition(-59); //TODO: Find actual value + } + + public void ampOuttake(double speed) { + spinIntakeMotor(speed); + } + + @Override + public void periodic() { + resetArmPosition(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 0d4af80..e9e070c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - private LEDPatterns m_currentPattern; - private Spark m_LEDController; - + static AddressableLED m_led; + static AddressableLEDBuffer m_ledBuffer; + static LED m_self; /** * Add your docs here. */ - public LED(Spark LEDController){ - m_LEDController = LEDController; - setPattern(LEDConstants.DEFAULT_PATTERN); - updateLED(); + + public LED(){ + if(m_self != null) + return; + m_led = new AddressableLED(9); + m_ledBuffer = new AddressableLEDBuffer(10); + m_led.setLength(m_ledBuffer.getLength()); + m_led.setData(m_ledBuffer); + m_led.start(); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } - + public static LED getInstance() { + if(m_self == null) + m_self = new LED(); + return m_self; + } @Override public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + //gamermode(); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + return; + } + static int firstcolor = 0; + static void gamermode() { + for(int i = 0; i < m_ledBuffer.getLength(); i++) { + final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; + setLEDHSV(i, hue, 255, 128); + } + firstcolor +=3; + firstcolor %= 180; + } + /** + * Add your docs here. + */ + public static void updateLED (){ + gamermode(); + // m_LEDController.set(m_currentPattern.getValue()); } /** * Add your docs here. */ - public void updateLED(){ - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDRGB(int lednum, int r, int g, int b){ + m_ledBuffer.setRGB(lednum, r, g, b); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - - /** - * Add your docs here. - */ - public void setPattern(LEDPatterns pattern){ - m_currentPattern = pattern; - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDHSV(int lednum, int hue, int sat, int val){ + m_ledBuffer.setRGB(lednum, hue, sat, val); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - /** * Add your docs here. * @return */ - public LEDPatterns getPattern() { - return m_currentPattern; + public AddressableLEDBuffer getBuffer() { + return m_ledBuffer; } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 8e7da41..2200b07 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -1,165 +1,82 @@ -// // 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. +// 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 frc4388.robot.subsystems; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +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; -// import java.io.IOException; -// import java.util.ArrayList; -// import java.util.List; -// import java.util.Optional; +import frc4388.robot.Constants.VisionConstants; -// // import org.photonvision.EstimatedRobotPose; -// // import org.photonvision.PhotonCamera; -// // import org.photonvision.PhotonPoseEstimator; -// // import org.photonvision.PhotonPoseEstimator.PoseStrategy; -// // import org.photonvision.common.hardware.VisionLEDMode; -// // import org.photonvision.targeting.PhotonPipelineResult; -// // import org.photonvision.targeting.PhotonTrackedTarget; -// // import org.photonvision.targeting.TargetCorner; +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data -// import edu.wpi.first.apriltag.AprilTag; -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// import edu.wpi.first.apriltag.AprilTagFields; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.wpilibj.DriverStation; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import frc4388.robot.Constants.VisionConstants; +public class Limelight extends SubsystemBase { -// public class Limelight extends SubsystemBase { - -// // private PhotonCamera cam; -// // private PhotonPoseEstimator photonPoseEstimator; + // // [X, Y, Z, Roll, Pitch, Yaw] + // private double[] cameraPose; + // private boolean isTag; -// private boolean lightOn; + // private Pose2d pose; + // private boolean isNearSpeaker; -// /** Creates a new Limelight. */ -// public Limelight() { -// // cam = new PhotonCamera(VisionConstants.NAME); -// // cam.setDriverMode(false); -// } + // public boolean getIsTag() { + // return isTag; + // } -// public void setLEDs(boolean on) { -// lightOn = on; -// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); -// } + // private void update() { + // SmartDashboard.putBoolean("Apriltag", isTag); + // if(!isTag){ + // return; + // } -// public void toggleLEDs() { -// lightOn = !lightOn; -// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); -// } + // double x = cameraPose[0]; + // double y = cameraPose[1]; + // double yaw = cameraPose[5]; -// public void setDriverMode(boolean driverMode) { -// // cam.setDriverMode(driverMode); -// } + // Rotation2d rot = Rotation2d.fromDegrees(yaw); -// public void setToLimePipeline() { -// // cam.setPipelineIndex(1); -// setLEDs(true); -// } + // pose = new Pose2d(x, y, rot); -// public void setToAprilPipeline() { -// // cam.setPipelineIndex(0); -// setLEDs(false); -// } - -// public PhotonTrackedTarget getAprilPoint() { -// // if (!cam.isConnected()) return null; - -// // PhotonPipelineResult result = cam.getLatestResult(); - -// // if (!result.hasTargets()) return null; - -// return result.getBestTarget(); -// } - -// private List getAprilCorners() { -// if (!cam.isConnected()) return null; - -// PhotonPipelineResult result = cam.getLatestResult(); - -// if (!result.hasTargets()) return null; - -// return result.getBestTarget().getDetectedCorners(); -// } - -// public double getAprilSkew() { -// List corners = getAprilCorners(); -// ArrayList bottomSide = getAprilBottomSide(corners); - -// if (bottomSide == null) return 0; - -// TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); -// TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - -// return bottomLeft.y - bottomRight.y; -// } - -// private ArrayList getAprilBottomSide(List box) { -// if (box == null) return null; - -// ArrayList bottomSide = new ArrayList<>(); - -// TargetCorner l1 = new TargetCorner(-1, -1); -// TargetCorner l2 = new TargetCorner(-1, -1); + // boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; -// for (TargetCorner c : box) { -// if (c.y > l1.y) l1 = c; -// } + // double distance; -// for (TargetCorner c : box) { -// if (c.y == l1.y) continue; -// if (c.y > l2.y) l2 = c; -// } + // if(isRed){ + // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + // }else{ + // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + // } + + // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; -// bottomSide.add(l1); -// bottomSide.add(l2); + // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + // //SmartDashboard.putNumber("speakerDistance", distance); + // } -// return bottomSide; -// } + // public Pose2d getPose() { + // return pose; + // } -// public double getDistanceToApril() { -// PhotonTrackedTarget aprilPoint = getAprilPoint(); -// if (aprilPoint == null) return -1; + // public boolean isNearSpeaker() { + // return isNearSpeaker; + // } -// double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; -// double theta = 35.0 + aprilPoint.getPitch(); + // @Override + // public void periodic() { + // // This method will be called once per scheduler run -// double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); -// return distanceToApril; -// } + // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; + // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); -// public PhotonTrackedTarget getLowestTape() { -// if (!cam.isConnected()) return null; - -// PhotonPipelineResult result = cam.getLatestResult(); - -// if (!result.hasTargets()) return null; - -// ArrayList points = (ArrayList) result.getTargets(); - -// PhotonTrackedTarget lowest = points.get(0); -// for (PhotonTrackedTarget point : points) { -// if (point.getPitch() < lowest.getPitch()) { -// lowest = point; -// } -// } - -// return lowest; -// } - -// public double getDistanceToTape() { -// PhotonTrackedTarget tapePoint = getLowestTape(); -// if (tapePoint == null) return -1; - -// double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; -// double theta = 35.0 + tapePoint.getPitch(); - -// double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); -// return distanceToTape; -// } - -// @Override -// public void periodic() {} -// } + // //if(newPose != cameraPose){ + // // cameraPose = newPose; + // //update(); + // //} + // } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..fb80e19 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -0,0 +1,114 @@ +// 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 frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.Constants.ShooterConstants; + +import frc4388.robot.subsystems.Limelight; + +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +// import com.ctre.phoenix.motorcontrol.NeutralMode; + + +public class Shooter extends SubsystemBase { + + private TalonFX leftShooter; + private TalonFX rightShooter; + + private Limelight limelight; + + private int spinMode = 0; + // 0 = Stop / Coast + // 1 = Idle + // 2 = Idle Near Speaker + // 3 = Spin + // 4 = SingleSpin + + private double smartDashboardShooterSpeed; + + /** Creates a new Shooter. */ + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { + leftShooter = leftTalonFX; + rightShooter = rightTalonFX; + + limelight = tmplimelight; + + leftShooter.setNeutralMode(NeutralModeValue.Coast); + rightShooter.setNeutralMode(NeutralModeValue.Coast); + SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + + } + + public Shooter(TalonFX leftShooter) { + this.leftShooter = leftShooter; + leftShooter.setNeutralMode(NeutralModeValue.Coast); + } + + public void singleSpin() { + leftShooter.set(1.0); + spinMode = 4; + } + + public void singleSpin(double speed) { + leftShooter.set(speed); + spinMode = 4; + } + + public void spin() { + spin(smartDashboardShooterSpeed); + spinMode = 3; + } + + public void spin(double speed) { + leftShooter.set(-speed); + rightShooter.set(-speed); + spinMode = 3; + } + + public void spin(double leftSpeed, double rightSpeed) { + leftShooter.set(leftSpeed); + rightShooter.set(-rightSpeed); + spinMode = 3; + } + + public void stop() { + spin(0.d); + spinMode = 0; + } + + public void idle() { + spin(ShooterConstants.SHOOTER_IDLE); + spinMode = 1; + } + + public int getMode(){ + return spinMode; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + + // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. + // Else if the robot is not near the speaker, then set the speed back to idle. + // if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ + // leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + // spinMode = 2; + // }else if(!limelight.isNearSpeaker() && spinMode == 2){ + // idle(); + // } + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index e1fed56..966d2e0 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -7,19 +7,19 @@ package frc4388.utility; -import com.ctre.phoenix.sensors.WPI_Pigeon2; +// import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; -import edu.wpi.first.wpilibj.interfaces.Gyro; +// import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; /** * Gyro class that allows for interchangeable use between a pigeon and a navX */ -public class RobotGyro implements Gyro { +public class RobotGyro { private RobotTime m_robotTime = RobotTime.getInstance(); private Pigeon2 m_pigeon = null; @@ -86,7 +86,6 @@ public class RobotGyro implements Gyro { * is typically done when the robot is first turned on while it's sitting at rest before the * competition starts. */ - @Override public void calibrate() { return; // if (m_isGyroAPigeon) { @@ -96,7 +95,6 @@ public class RobotGyro implements Gyro { // } } - @Override public void reset() { resetZeroValues(); @@ -189,12 +187,10 @@ public class RobotGyro implements Gyro { return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; } - @Override public Rotation2d getRotation2d() { return m_pigeon.getRotation2d(); } - @Override public double getAngle() { if (m_isGyroAPigeon) { return getPigeonAngles()[2]; @@ -251,7 +247,6 @@ public class RobotGyro implements Gyro { } } - @Override public double getRate() { if (m_isGyroAPigeon) { return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; @@ -268,7 +263,6 @@ public class RobotGyro implements Gyro { return m_navX; } - @Override public void close() throws Exception { } From f77712aa945c16d4ae6e070a71d9c6e912f15c1a Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 27 Aug 2024 17:15:22 -0600 Subject: [PATCH 10/18] merge conflict --- src/main/java/frc4388/robot/subsystems/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 7d7294c..5ffa72a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -107,4 +107,4 @@ public class Intake extends SubsystemBase { public void periodic() { resetArmPosition(); } -} +} \ No newline at end of file From 721f160f3779d3506aba585a87cddaa14e05b7d1 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 27 Aug 2024 17:45:51 -0600 Subject: [PATCH 11/18] update gearshifting logic --- src/main/java/frc4388/robot/Constants.java | 29 ++++++++++--------- .../java/frc4388/robot/RobotContainer.java | 16 +++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 11 +++---- 3 files changed, 29 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aee6cae..3376b08 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -35,19 +35,20 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.5; - public static final double FAST_SPEED = 1.0; - public static final double TURBO_SPEED = 4.0; + public static final double SLOW_SPEED = 0.25; + public static final double FAST_SPEED = 0.5; + public static final double TURBO_SPEED = 1.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25; - public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25; + public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5; + public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ; + public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; + + public static final int RIGHT_FRONT_WHEEL_ID = 2; public static final int RIGHT_FRONT_STEER_ID = 3; public static final int RIGHT_FRONT_ENCODER_ID = 10; @@ -59,7 +60,7 @@ public final class Constants { public static final int LEFT_BACK_STEER_ID = 7; public static final int LEFT_BACK_ENCODER_ID = 12; - public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_ENCODER_ID = 13; } @@ -84,15 +85,15 @@ public final class Constants { } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; + public static final int CANCODER_TICKS_PER_ROTATION = 1; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.3; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; public static final double MOTOR_REV_PER_STEER_REV = 12.8; - public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double TICKS_PER_MOTOR_REV = 0.5; public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; @@ -111,7 +112,7 @@ public final class Constants { public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value } - public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; // TODO: find the actual value public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c3287b7..3a2cab0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -251,13 +251,13 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); // * /* D-Pad Stuff */ // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) @@ -308,11 +308,11 @@ public class RobotContainer { // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 45f68f7..243093e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.SwerveDriveConstants.Conversions; import frc4388.utility.RobotGyro; import frc4388.utility.RobotUnits; @@ -70,7 +71,7 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0; + double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); if (fieldRelative) { @@ -102,7 +103,7 @@ public class SwerveDrive extends SubsystemBase { // Convert field-relative speeds to robot-relative speeds. // chassisSpeeds = chassisSpeeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -267,7 +268,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; System.out.println("SLOW"); System.out.println("SLOW"); System.out.println("SLOW"); @@ -276,7 +277,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; System.out.println("FAST"); System.out.println("FAST"); System.out.println("FAST"); @@ -285,7 +286,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; System.out.println("TURBO"); System.out.println("TURBO"); System.out.println("TURBO"); From a10ccf2cb2a1ebf57bc489ed152e1874c3296df2 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 29 Aug 2024 17:03:25 -0600 Subject: [PATCH 12/18] update speeds --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3376b08..915335b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -68,7 +68,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(32, 0.0, 0.0, 0.0, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0); public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); @@ -88,7 +88,7 @@ public final class Constants { public static final int CANCODER_TICKS_PER_ROTATION = 1; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.3; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; public static final double MOTOR_REV_PER_STEER_REV = 12.8; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3a2cab0..c6164f0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -308,8 +308,8 @@ public class RobotContainer { // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 243093e..a2e0c30 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -36,7 +36,7 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; From 96f614d16be84595bcc601129e4c1a8b4ed5f697 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 5 Sep 2024 17:50:21 -0600 Subject: [PATCH 13/18] 4 letter diff We need to make our gear shifting code better. This simple change broke the robot's turning. --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a2e0c30..243093e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -36,7 +36,7 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; From 157770b1b8ea7d55e0e05f82fffabbd943c93b0f Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 24 Sep 2024 15:51:41 -0600 Subject: [PATCH 14/18] Cleanup and re-enable auto recording --- .../java/frc4388/robot/RobotContainer.java | 224 +++++------------- .../robot/commands/Intake/ArmIntakeIn.java | 1 - .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- 3 files changed, 56 insertions(+), 171 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6164f0..8c2a9a7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,46 +11,33 @@ package frc4388.robot; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; // Autos -import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.robot.commands.Swerve.JoystickPlayback; -import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; -// import frc4388.robot.commands.Intake.ArmIntakeIn; -//import frc4388.robot.commands.Autos.AutoAlign; + +// Subsystems import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.Shooter; -// Subsystems // import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.robot.subsystems.Shooter; -// import frc4388.robot.subsystems.Climber; -// import frc4388.robot.subsystems.Intake; // Utilites import frc4388.utility.DeferredBlock; @@ -98,28 +85,13 @@ public class RobotContainer { private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + // ! Teleop Commands + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.PIDIn()), new InstantCommand(() -> m_robotShooter.idle()) - // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), - // new InstantCommand(() -> m_robotShooter.spin()) ); - - - // ! Teleop Commands - private SequentialCommandGroup autoShoot = new SequentialCommandGroup( - // MoveToSpeaker, - //autoAlign, - new InstantCommand(() -> m_robotShooter.spin()), - new WaitCommand(3.0), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), - new WaitCommand(3.0), - new InstantCommand(() -> m_robotShooter.idle()) - // new InstantCommand(() -> autoAlign.reverse()), - // autoAlign.asProxy() - ); - - + private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot, new InstantCommand(() -> m_robotShooter.idle()) @@ -148,66 +120,38 @@ public class RobotContainer { ); // ! /* Autos */ - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private String lastAutoName = "final_red_center_4note_taxi.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); - - // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - // .addOption("Taxi Auto", taxi.asProxy()) - // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) - // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) - // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) - // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) - // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) - // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) - // .buildDisplay(); - - // private SequentialCommandGroup autoShoot = new SequentialCommandGroup( - // // MoveToSpeaker, - // //autoAlign, - // new InstantCommand(() -> m_robotShooter.spin()), - // new WaitCommand(3.0), - // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), - // new WaitCommand(3.0), - // new InstantCommand(() -> m_robotShooter.idle()) - // // new InstantCommand(() -> autoAlign.reverse()), - // // autoAlign.asProxy() - // ); - - - + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); - // configureVirtualButtonBindings(); - // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); - // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + configureVirtualButtonBindings(); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + DriverStation.silenceJoystickConnectionWarning(true); + // CameraServer.startAutomaticCapture(); + /* Default Commands */ + // ! Swerve Drive Default Command (Regular Rotation) + // drives the robot with a two-axis input from the driver controller + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); - // DriverStation.silenceJoystickConnectionWarning(true); - // // CameraServer.startAutomaticCapture(); - - // /* Default Commands */ - // // drives the robot with a two-axis input from the driver controller - // ! Swerve Drive Default Command (Regular Rotation) - + // ! Swerve Drive One Module Test // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); // } - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRight(), - // true); - // }, m_robotSwerveDrive) - // .withName("SwerveDrive DefaultCommand")); - // m_robotSwerveDrive.setToSlow(); // ! Swerve Drive Default Command (Orientation Rotation) // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -215,7 +159,10 @@ public class RobotContainer { // getDeadbandedDriverController().getRightX(), // getDeadbandedDriverController().getRightY(), // true); - // }, m_robotSwerveDrive)); + // }, m_robotSwerveDrive)) + // .withName("SwerveDrive OrientationCommand")); + // continually sends updates to the Blinkin LED controller to keep the lights on + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.driveWithInput( @@ -226,18 +173,9 @@ public class RobotContainer { - // .withName("SwerveDrive OrientationCommand")); - // continually sends updates to the Blinkin LED controller to keep the lights on - // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } - // private void changeAuto() { - // autoPlayback.unloadAuto(); - // autoPlayback.loadAuto(); - // lastAutoName = autoplaybackName.get(); - // System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`"); - // } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -250,66 +188,11 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // * /* D-Pad Stuff */ - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // ! /* Auto Recording */ - // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - // () -> autoplaybackName.get())) - // .onFalse(new InstantCommand()); - - // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false)) - // .onFalse(new InstantCommand()); - + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); @@ -319,15 +202,14 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - - - - //? /* Operator Buttons */ - + + + // ? /* Operator Buttons */ + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); @@ -343,7 +225,7 @@ public class RobotContainer { // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); - + // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); @@ -351,13 +233,13 @@ public class RobotContainer { DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) .onFalse(turnOffShoot); - + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(i) .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); - - //spins up shooter, no wind down + + // Spins up shooter, no wind down DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); @@ -374,7 +256,22 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + + // ? /* Programer Buttons (Controller 3)*/ + // * /* Auto Recording */ + new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + () -> autoplaybackName.get())) + .onFalse(new InstantCommand()); + + new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + () -> autoplaybackName.get(), + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false)) + .onFalse(new InstantCommand()); } /** @@ -398,17 +295,9 @@ public class RobotContainer { * We don't need it in an auto. * Climbing controls : We don't need to climb in auto. */ + + // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto. - // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - new Trigger(() -> getVirtualOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); } /** @@ -418,9 +307,9 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //no auto - // return autoPlayback; + return autoPlayback; //return playbackChooser.getCommand(); - return new Command() {}; + // return new Command() {}; } /** @@ -433,9 +322,6 @@ public class RobotContainer { return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); } - /** - * Add your docs here. - */ public DeadbandedXboxController getDeadbandedDriverController() { return this.m_driverXbox; } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 42f9dfc..5fb161a 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -7,7 +7,6 @@ package frc4388.robot.commands.Intake; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.SwerveDrive; public class ArmIntakeIn extends Command { /** Creates a new ArmIntakeIn. */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 243093e..70f05b6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -37,7 +37,7 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default - public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double rotTarget = 0.0; From e9dcabce6241a9af209d48b764a0e812dc252ee6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 24 Oct 2024 17:04:03 -0600 Subject: [PATCH 15/18] add changes --- .../java/frc4388/robot/RobotContainer.java | 36 +++++++++++-------- .../robot/subsystems/SwerveModule.java | 7 ++++ 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8c2a9a7..262a593 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -120,13 +120,16 @@ public class RobotContainer { ); // ! /* Autos */ - private String lastAutoName = "final_red_center_4note_taxi.auto"; + private String lastAutoName = "four_note_taxi_kracken.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - () -> autoplaybackName.get(), // lastAutoName + lastAutoName, // () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false); - + true, true); + + private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + false, true); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -134,7 +137,7 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -147,6 +150,7 @@ public class RobotContainer { true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); // ! Swerve Drive One Module Test // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -164,12 +168,12 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive)); + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInput( + // getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRight(), + // true); + // }, m_robotSwerveDrive)); @@ -187,7 +191,7 @@ public class RobotContainer { // ? /* Driver Buttons */ DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final @@ -254,8 +258,12 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1) + .onTrue(amp_shoot) + .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive)); // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 8042080..007918e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; @@ -73,6 +74,12 @@ public class SwerveModule extends SubsystemBase { new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(110) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(110) + .withSupplyCurrentLimitEnable(true) ); driveMotor.getConfigurator().apply(motorCfg); From eaf60aa4758ea2646e7a882df782e5c68dae1d20 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Sat, 26 Oct 2024 11:10:05 -0600 Subject: [PATCH 16/18] add gear shifts --- src/main/java/frc4388/robot/Constants.java | 77 +++++++------------ .../java/frc4388/robot/RobotContainer.java | 13 ++-- .../frc4388/robot/subsystems/SwerveDrive.java | 50 ++++++------ 3 files changed, 61 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 915335b..d01a5be 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -34,35 +34,37 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - + + public static final double[] GEARS = {0.25, 0.5, 1.0}; + public static final double SLOW_SPEED = 0.25; public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5; - public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ; - public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ; + public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 + 0.5; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5; + public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5; + public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; - public static final int RIGHT_FRONT_STEER_ID = 3; - public static final int RIGHT_FRONT_ENCODER_ID = 10; - - public static final int LEFT_FRONT_WHEEL_ID = 4; - public static final int LEFT_FRONT_STEER_ID = 5; - public static final int LEFT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; - - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; + public static final int RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; + + public static final int LEFT_FRONT_WHEEL_ID = 4; + public static final int LEFT_FRONT_STEER_ID = 5; + public static final int LEFT_FRONT_ENCODER_ID = 11; + + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; + + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { @@ -85,8 +87,6 @@ public final class Constants { } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 1; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; @@ -107,13 +107,13 @@ public final class Constants { } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + public static final double OPEN_LOOP_RAMP_RATE = 0.2; + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; + public static final double NEUTRAL_DEADBAND = 0.04; } - public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; // TODO: find the actual value - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // dimensions public static final double WIDTH = 18.5; @@ -127,23 +127,6 @@ public final class Constants { } public static final class VisionConstants { - // public static final String NAME = "photonCamera"; - - // public static final int LIME_HIXELS = 640; - // public static final int LIME_VIXELS = 480; - - // public static final double H_FOV = 59.6; - // public static final double V_FOV = 45.7; - - // public static final double LIME_HEIGHT = 6.0; - // public static final double LIME_ANGLE = 55.0; - - // // public static final double HIGH_TARGET_HEIGHT = 46.0; - // public static final double HIGH_TAPE_HEIGHT = 44.0; - - // // public static final double MID_TARGET_HEIGHT = 34.0; - // public static final double MID_TAPE_HEIGHT = 24.0; - // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); @@ -153,11 +136,6 @@ public final class Constants { public static final double targetPosDistance = 1.5; } - - public static final class AutoAlignConstants { - public static final double MoveSpeed = 0.0; - public static final double RotSpeed = 0.0; - } public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 14; @@ -202,6 +180,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + public static final int XBOX_PROGRAMMER_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 262a593..5a178f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,7 +69,7 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); private final Limelight limelight = new Limelight(); @@ -77,7 +77,6 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); - /* Virtual Controllers */ private final VirtualController m_virtualDriver = new VirtualController(0); private final VirtualController m_virtualOperator = new VirtualController(1); @@ -123,9 +122,9 @@ public class RobotContainer { private String lastAutoName = "four_note_taxi_kracken.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - lastAutoName, // () -> autoplaybackName.get(), // lastAutoName + () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, true); + true, false); private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, @@ -195,11 +194,10 @@ public class RobotContainer { // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); @@ -207,7 +205,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - // ? /* Operator Buttons */ DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 70f05b6..bd35742 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,10 +33,13 @@ public class SwerveDrive extends SubsystemBase { private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - + private RobotGyro gyro; + + private int gear_index; + private boolean stopped = false; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; @@ -52,7 +55,7 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - + reset_index(); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } @@ -68,7 +71,6 @@ public class SwerveDrive extends SubsystemBase { module.setDesiredState(state); } - boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; @@ -256,15 +258,29 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("RotTartget", rotTarget); } - public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + private void reset_index() { + gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?) + } - } + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void setPercentOutput(double speed) { + speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; + gear_index = -1; } public void setToSlow() { @@ -294,16 +310,6 @@ public class SwerveDrive extends SubsystemBase { System.out.println("TURBO"); } - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { - - } - } - public void toggleGear(double angle) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; From c27f2a7c6a22ed236339c580a5c03f4fb3651bdd Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Sat, 26 Oct 2024 11:39:50 -0600 Subject: [PATCH 17/18] Code clean up, compleatly remove phoenix5 --- src/main/java/frc4388/robot/Constants.java | 5 +- src/main/java/frc4388/robot/Robot.java | 2 - .../java/frc4388/robot/RobotContainer.java | 21 +-- src/main/java/frc4388/robot/RobotMap.java | 92 +---------- .../commands/Swerve/JoystickPlayback.java | 4 +- .../commands/Swerve/JoystickRecorder.java | 4 +- .../frc4388/robot/subsystems/Climber.java | 9 +- .../frc4388/robot/subsystems/DiffDrive.java | 17 +- .../robot/subsystems/SwerveModule.java | 15 +- ...kPigeonIMU.java => MockPigeonIMU.java.old} | 0 vendordeps/Phoenix5.json | 151 ------------------ 11 files changed, 30 insertions(+), 290 deletions(-) rename src/test/java/frc4388/mocks/{MockPigeonIMU.java => MockPigeonIMU.java.old} (100%) delete mode 100644 vendordeps/Phoenix5.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d01a5be..7027b5e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -49,7 +49,6 @@ public final class Constants { } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; public static final int RIGHT_FRONT_STEER_ID = 3; public static final int RIGHT_FRONT_ENCODER_ID = 10; @@ -65,6 +64,8 @@ public final class Constants { public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_ENCODER_ID = 13; + + public static final int DRIVE_PIGEON_ID = 14; } public static final class PIDConstants { @@ -138,7 +139,7 @@ public final class Constants { } public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 14; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index cd9c24c..58adaea 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,8 +7,6 @@ package frc4388.robot; -import edu.wpi.first.cameraserver.CameraServer; - import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5a178f5..10caf89 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; // Autos import frc4388.robot.commands.Intake.ArmIntakeIn; @@ -36,7 +35,6 @@ import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.Shooter; // import frc4388.robot.subsystems.LED; -// import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -91,17 +89,11 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.idle()) ); - private SequentialCommandGroup i = new SequentialCommandGroup( + private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot, new InstantCommand(() -> m_robotShooter.idle()) ); - private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(0.75), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) - ); - private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) @@ -113,11 +105,6 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); - private SequentialCommandGroup ampShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.ampPosition()), - new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed - ); - // ! /* Autos */ private String lastAutoName = "four_note_taxi_kracken.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); @@ -126,7 +113,7 @@ public class RobotContainer { new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); - private neoJoystickPlayback amp_shoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", + private neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, false, true); @@ -237,7 +224,7 @@ public class RobotContainer { DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(i) + .onTrue(intakeNotePullInIdle) .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); // Spins up shooter, no wind down @@ -259,7 +246,7 @@ public class RobotContainer { // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1) - .onTrue(amp_shoot) + .onTrue(ampShoot) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive)); // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d1a2b91..4d1869f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,17 +7,12 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.NeutralMode; -// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// import com.ctre.phoenix.sensors.CANCoder; -// import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import frc4388.robot.Constants.LEDConstants; +// import edu.wpi.first.wpilibj.motorcontrol.Spark; +// import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.ClimbConstants; @@ -30,10 +25,9 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private Pigeon2 m_pigeon2 = new Pigeon2(14); + private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID); public RobotGyro gyro = new RobotGyro(m_pigeon2); - // public RobotGyro gyro = null; - + public SwerveModule leftFront; public SwerveModule rightFront; public SwerveModule leftBack; @@ -47,11 +41,6 @@ public class RobotMap { /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - - // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - /* Swreve Drive Subsystem */ public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); @@ -85,84 +74,13 @@ public class RobotMap { } void configureIntakeMotorControllers() { - // intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); - // pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); } void configureDriveMotorControllers() { - // config factory default - // leftFrontWheel.configFactoryDefault(); - // leftFrontSteer.configFactoryDefault(); - - // rightFrontWheel.configFactoryDefault(); - // rightFrontSteer.configFactoryDefault(); - - // leftBackWheel.configFactoryDefault(); - // leftBackSteer.configFactoryDefault(); - - // rightBackWheel.configFactoryDefault(); - // rightBackSteer.configFactoryDefault(); - - // // config open loop ramp - // leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // // config closed loop ramp - // leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - // rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // // config neutral deadband - // leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - // rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - // leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - // rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - // rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - // // set neutral mode - // leftFrontWheel.setNeutralMode(NeutralMode.Brake); - // rightFrontWheel.setNeutralMode(NeutralMode.Brake); - // leftBackWheel.setNeutralMode(NeutralMode.Brake); - // rightBackWheel.setNeutralMode(NeutralMode.Brake); - - // leftFrontSteer.setNeutralMode(NeutralMode.Brake); - // rightFrontSteer.setNeutralMode(NeutralMode.Brake); - // leftBackSteer.setNeutralMode(NeutralMode.Brake); - // rightBackSteer.setNeutralMode(NeutralMode.Brake); - - // // initialize SwerveModules - + // initialize SwerveModules this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); - - // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); - // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); - // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } } diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index e92b487..ae054ed 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -9,11 +9,11 @@ import java.io.FileNotFoundException; import java.util.ArrayList; import java.util.Scanner; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickPlayback extends CommandBase { +public class JoystickPlayback extends Command { private final SwerveDrive swerve; private String filename; private int mult = 1; diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 82ba36e..0224fcf 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -11,11 +11,11 @@ import java.util.ArrayList; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; -public class JoystickRecorder extends CommandBase { +public class JoystickRecorder extends Command { public final SwerveDrive swerve; public final Supplier leftX; diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index ec78e53..5ed1472 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -12,11 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; import frc4388.robot.Constants.ClimbConstants; -import frc4388.robot.Constants.IntakeConstants; - -//! 6.5C Scoring Criteria for Stage public class Climber extends SubsystemBase { /** Creates a new Climber. */ @@ -40,14 +36,13 @@ public class Climber extends SubsystemBase { //PositionVoltage request = new PositionVoltage(0); //climbMotor.setControl(request.withPosition(-520)); - climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED); + climbMotor.set(ClimbConstants.CLIMB_OUT_SPEED); } public void climbIn() { //PositionVoltage request = new PositionVoltage(-520); //climbMotor.setControl(request.withPosition(0)); - climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); - ; + climbMotor.set(ClimbConstants.CLIMB_IN_SPEED); } public void stopClimb() { diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 9ec39df..91de2e9 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -7,7 +7,8 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -26,23 +27,25 @@ public class DiffDrive extends SubsystemBase { private RobotTime m_robotTime = RobotTime.getInstance(); - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; + private TalonFX m_leftFrontMotor; + private TalonFX m_rightFrontMotor; + private TalonFX m_leftBackMotor; + private TalonFX m_rightBackMotor; private DifferentialDrive m_driveTrain; private RobotGyro m_gyro; /** * Add your docs here. */ - public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { + public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, + TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { m_leftFrontMotor = leftFrontMotor; m_rightFrontMotor = rightFrontMotor; m_leftBackMotor = leftBackMotor; m_rightBackMotor = rightBackMotor; + m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); + m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); m_driveTrain = driveTrain; m_gyro = gyro; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 007918e..b9895fb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,16 +4,6 @@ package frc4388.robot.subsystems; -// import javax.swing.text.StyleContext.SmallAttributeSet; - -// import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -// import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -// import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// import com.ctre.phoenix.sensors.CANCoder; -// import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -43,7 +33,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -// import frc4388.utility.configurable.ConfigurableDouble; public class SwerveModule extends SubsystemBase { private TalonFX driveMotor; @@ -76,9 +65,9 @@ public class SwerveModule extends SubsystemBase { .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) ).withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(110) + .withStatorCurrentLimit(100) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(110) + .withSupplyCurrentLimit(100) .withSupplyCurrentLimitEnable(true) ); diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java.old similarity index 100% rename from src/test/java/frc4388/mocks/MockPigeonIMU.java rename to src/test/java/frc4388/mocks/MockPigeonIMU.java.old diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json deleted file mode 100644 index ff7359e..0000000 --- a/vendordeps/Phoenix5.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file From 1bf11ace87a1f9c8b0445fad27c562b08b1913ba Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Sat, 26 Oct 2024 12:39:14 -0600 Subject: [PATCH 18/18] update readme --- README.md | 36 ++++++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9020b7f..981c2e9 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,34 @@ -# Robot-Essentials - Basic code for any Ridgebotics robot project +# FIRST FRC 2024 Season (Crescendo) "Kickdrum" Robot Code. +Code for our robot named "Kickdrum" + +## Driving instructions + +### Driver (Port 0) +**A**: Reset the gryo. + +**Left Bumper**: Gear shift down. + +**Right Bumper**: Gear shift up. + +### Operator (Port 1) + +**Right Bumper**: Moves out the intake and starts intaking a note, when the note enters the intake move the intake back into the robot, then idles the shooter. + +**Left Bumper**: Spins up the shooter to speaker speed. + +**A**: *Outtakes* the *intake*. If the intake is in the robot then it will pass it off to the shooter (and its recomended that the shooter is at speaker speed when it gets handed off), if its outside the robot it will just eject it onto the floor. + +**Right Trigger**: While held it moves the climbers up. + +**Left Trigger**: While held it moves the climbers down. + +### Programer (Port 2) + +**Left Bumper**: While pressed it records using the new autonomus recording and playback system, and when released it saves the autonomus to the robot under the name that you put into shuffleboard in the "Auto Playback Name" section. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository) + +**Right Bumper**: When pressed it loads the auto from the "Auto Playback Name" in shuffleboard with the new autonomus recording and playback system, and plays it back. If the file you put in doesn't exist in autos/ in the home directory of the robot it will print the error to console but the robot will remain functional. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository) + +### Shuffleboard + +**Auto Playback Name**: a string value of the auto name you wish to load or save to, this gets read at the start of the atonomus phase and loads the corisponding auto from ~/autos/ on the robot +