From 1026b3ebd29decfdd371de0e7bc3584c9edc8e74 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sun, 11 Feb 2024 17:29:17 -0500 Subject: [PATCH] Changes from WPI day 2 --- build.gradle | 2 - src/main/deploy/trajectories/.gitkeep | 0 .../trajectories/driveStraight.pathblob | Bin 0 -> 9347 bytes .../littletonrobotics/frc2024/Constants.java | 84 +-- .../frc2024/FieldConstants.java | 144 ++-- .../org/littletonrobotics/frc2024/Main.java | 18 +- .../org/littletonrobotics/frc2024/Robot.java | 334 ++++----- .../frc2024/RobotContainer.java | 542 +++++++------- .../littletonrobotics/frc2024/RobotState.java | 414 +++++------ .../commands/FeedForwardCharacterization.java | 132 ++-- .../frc2024/commands/auto/AutoCommands.java | 62 +- .../apriltagvision/AprilTagVision.java | 352 ++++----- .../AprilTagVisionConstants.java | 74 +- .../apriltagvision/AprilTagVisionIO.java | 54 +- .../AprilTagVisionIONorthstar.java | 130 ++-- .../frc2024/subsystems/drive/Drive.java | 674 +++++++++--------- .../subsystems/drive/DriveConstants.java | 446 ++++++------ .../frc2024/subsystems/drive/GyroIO.java | 16 +- .../subsystems/drive/GyroIOPigeon2.java | 54 +- .../frc2024/subsystems/drive/Module.java | 192 ++--- .../frc2024/subsystems/drive/ModuleIO.java | 76 +- .../subsystems/drive/ModuleIOKrakenFOC.java | 422 +++++------ .../frc2024/subsystems/drive/ModuleIOSim.java | 154 ++-- .../subsystems/drive/ModuleIOSparkMax.java | 318 ++++----- .../drive/PhoenixOdometryThread.java | 120 ++-- .../drive/SparkMaxOdometryThread.java | 68 +- .../drive/controllers/AutoAimController.java | 68 +- .../controllers/AutoAlignController.java | 280 ++++---- .../controllers/TeleopDriveController.java | 92 +-- .../controllers/TrajectoryController.java | 154 ++-- .../drive/trajectory/DriveTrajectories.java | 22 +- .../trajectory/GenerateTrajectories.java | 230 +++--- .../TrajectoryGenerationHelpers.java | 226 +++--- .../rollers/GenericRollerSubsystem.java | 27 + .../rollers/GenericRollerSystemIO.java | 21 + .../GenericRollerSystemIOKrakenFOC.java | 69 ++ .../GenericRollerSystemIOSparkFlex.java | 44 ++ .../frc2024/subsystems/rollers/Rollers.java | 67 ++ .../subsystems/rollers/RollersSensorsIO.java | 14 + .../rollers/RollersSensorsIOReal.java | 20 + .../subsystems/rollers/feeder/Feeder.java | 27 + .../subsystems/rollers/feeder/FeederIO.java | 5 + .../rollers/feeder/FeederIOKrakenFOC.java | 16 + .../subsystems/rollers/indexer/Indexer.java | 30 + .../subsystems/rollers/indexer/IndexerIO.java | 5 + .../rollers/indexer/IndexerIOSparkFlex.java | 14 + .../subsystems/rollers/intake/Intake.java | 26 + .../subsystems/rollers/intake/IntakeIO.java | 5 + .../rollers/intake/IntakeIOKrakenFOC.java | 16 + .../superstructure/Superstructure.java | 200 +++--- .../SuperstructureConstants.java | 153 ++-- .../subsystems/superstructure/arm/Arm.java | 229 +++--- .../subsystems/superstructure/arm/ArmIO.java | 68 +- .../superstructure/arm/ArmIOKrakenFOC.java | 380 +++++----- .../superstructure/arm/ArmIOSim.java | 204 +++--- .../superstructure/feeder/Feeder.java | 66 -- .../superstructure/feeder/FeederIO.java | 21 - .../feeder/FeederIOKrakenFOC.java | 64 -- .../superstructure/feeder/FeederIOSim.java | 33 - .../feeder/FeederIOSparkFlex.java | 42 -- .../superstructure/flywheels/Flywheels.java | 214 +++--- .../superstructure/flywheels/FlywheelsIO.java | 58 +- .../flywheels/FlywheelsIOSim.java | 170 ++--- .../flywheels/FlywheelsIOSparkFlex.java | 236 +++--- .../superstructure/intake/Intake.java | 40 -- .../superstructure/intake/IntakeIO.java | 22 - .../intake/IntakeIOKrakenFOC.java | 69 -- .../superstructure/intake/IntakeIOSim.java | 31 - .../intake/IntakeIOSparkMax.java | 41 -- .../littletonrobotics/frc2024/util/Alert.java | 216 +++--- .../frc2024/util/AllianceFlipUtil.java | 110 +-- .../frc2024/util/EqualsUtil.java | 26 +- .../frc2024/util/GeomUtil.java | 238 +++---- .../frc2024/util/LoggedTunableNumber.java | 174 ++--- .../frc2024/util/PolynomialRegression.java | 334 ++++----- .../frc2024/util/PoseEstimator.java | 274 +++---- .../frc2024/util/VirtualSubsystem.java | 18 +- .../frc2024/util/shooting/ShotCalculator.java | 70 +- .../frc2024/util/swerve/ModuleLimits.java | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 672 ++++++++--------- .../trajectory/HolonomicDriveController.java | 106 +-- .../util/trajectory/HolonomicTrajectory.java | 172 ++--- .../frc2024/RobotContainerTest.java | 10 +- 83 files changed, 5568 insertions(+), 5555 deletions(-) create mode 100644 src/main/deploy/trajectories/.gitkeep create mode 100644 src/main/deploy/trajectories/driveStraight.pathblob create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSparkFlex.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOReal.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOKrakenFOC.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOKrakenFOC.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java diff --git a/build.gradle b/build.gradle index 8f1d9c68..8701bbea 100644 --- a/build.gradle +++ b/build.gradle @@ -233,8 +233,6 @@ spotless { toggleOffOn() googleJavaFormat() removeUnusedImports() - indentWithTabs(2) - indentWithSpaces(4) trimTrailingWhitespace() endWithNewline() } diff --git a/src/main/deploy/trajectories/.gitkeep b/src/main/deploy/trajectories/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/src/main/deploy/trajectories/driveStraight.pathblob b/src/main/deploy/trajectories/driveStraight.pathblob new file mode 100644 index 0000000000000000000000000000000000000000..88acafbb5b533343ffa3c452dc24c5cfc289c528 GIT binary patch literal 9347 zcmZwNcRW{L{5WtsN$L{1RJthM4P0a-owt;hRir^AG-+8Wln5Egh-fHMGO|b3H8ZnU zcJ|EPqu;sr^L>23|NQ*#>2=P%=QYm7!NkqLp|bv> zR-2z)m_RKqr!mrg(FkmHLN(~=o)3WbL0Ro*rHTuI57~LFU0(4b{+D=15_;OS=X-YR z0>pereFZUbkU%4Cz{LbLRRa5LSl4kz>N}vZ8EKv55D6ggYiQhs<2CO{!kI6gk>zp( zNt|`#4k;2h1<2!^(3G|MQv_NXH>ag4qeEa{7G4a7N2UT={YR$Q*Q_51+|1*h7LR?z z|3wi)5^jowKz-#sfSj|c_>g{7k3i+$49TDBG$JrAqsM^)hjIWdxl>1}sVAJk)sdK= zl@DV`V(X1;J?CEuByn8pd9a6~9YBul$yZPRbcH}aw$^aDR$eEt4xR;t(?j`ycEO9& zOCdRuz~7HbbqmHN{NF-SNW%8VZ~2fYCxC2Q_*dX8d7VImRPK4%zPU$Wr>r;M+no9n z(C`U0--nj51pei?gL^%13Q7De42y9YO(%&)o14RQUpIhA3x0iESA2&+v+ZIICbHWQ zn0NAv*TybofHve4dEn*d1Oji;uNP6em;Qfykwp@Qb2Tp9XI=s%DXXVXE6I{T&m7vd zA3I=AU<_~V4yo!^!iVbrV((eQBm$T7UoM%=&LoNM%|{svSaV5YL#?B~?5rO^(yS`8 z{y0A*&?zRo`i{2~f$i{nGttIf188l_jcG+;DFnXtscrn1wH%W8Z#(ly;>GWcvsDs7 z0Lkw-cEq91fj|q~_Pbf@KPRwn3PY2oE46@T@#joxczYUw@9keoDKzr(VWxxG_ECLtd zT-&BDUq})e7PRw)qQxX(A#~o({XjTCZ1eN$cIvnj=&#-N_V>(v32Y|5ucbY^8PL+) z7C*J0$szE&wcYIgnMEWqK|RzGk_dj``hLg$F92~oGMJV)=1HJS9D-KOi~$6;sq4mA zOjHY?eX9t2lZodN_|(g70;d&XmeMM*_LGU2s|;{=7>UU2}u}GhrNO%q7VCi-jf#tkT$pZi8Ngx#>+yDpqeDKsTaJCBs`KBPx&-t0A%vUtW&(;M*@A^Yj>Yr;46XU za7w!U=I;Zvi*W^R?>`n3xES-iRpUHGjNF=T6tSx(3G`*4jMAxWfV@!jR4lOlM4-M( zxj87;Hv+qR;OMKZ-2H&oQ7+i+e6NJSl~a+ECtuc(QMgiPq=6*DF0k8_w-5knce}2+ z+7U*e;WZO4b-ANq6fF(6iZ}-VEn|QBVFtxg0`!>(T9R|J7(UFUCYKM@CCaDK)GGv*;cOZrlKE-|r;z_Sn4yBFYfWEAJo zey7ju%_O1D`6yOQpb#Ko8G~|kov#G?qw&4LY*svh8Li1gpZ^DmuRIHgfk`=myBQo{ zHb|t1De9zBM6pS9s#Qo4K%Q0owvXwKgs;}DNt$L`0?b+;3x$%FhXKv{hnBPRh6)0I zSg3i6Wq&;xh4($9Ce!NQWE9059g#KS#Q*_5Y65eQq6oB8_>Q9pQxbt4HI}fqSr`Gd zhpl}lTB0Dq64r;ff1(I+>ae$v#Qc+}y+?~m0OGQ~U@Al~hCtJuZ;7H3$pkhxpK2dE zI|^u0&b$}7RDKb-$)z@)_)QID6a~f2`*yu)C5hTiRYc9*GJv4ps)V-`#uDiLhtZ=o zMk#P17n!c_ogM?U*q)nprN1B{pJgDWYD*D@RK92)1HV|0EFe-^PdftaRl0A zmr*<#m`Y&XV(Ke=lYarNHE_8)QM;1BGn`AKSeq$gamU$beMaph(dkeosWbiyAX_F2 z48>l@6DZPGw4b#u4OVu5-7*{>2Q&XYN&4nT)ZOWS>#5zf0 z;Yxo`PF*!X)RIOIIqN18=(~@sn4eIDI=7LEzy2R`A(qx1)_eZc17vuH$y zxFmKe-iAcDla4ogDMj>W3z}PkZZe9WHDf=5xtjnYCtohVD?bgsTH*_hk!T))9kG5| z(>V(XmYjTfep^VGe82perHdkxsPsz_&&n$&ndF)Qvc^VJ7>r3LP;Nb)I*vj}D2QlB zud|r}v=fGf4}Ly|gwKj)0d0&Tp5AN@)Dh_+qc~u+&d&SFZ-59mRd_oG{UA^Si2`4z zJNX3GT;;qKpM%7_k@xL+Ei2m#*Lc{1yI$tXCy-dM4ew*uteN|~z5 ztxN*ltNmB*aXutC;(9+^n}bB!%{wBM`j99Ajdnf_6fr;*0g7neqxbSJM>{|^i2s~g zJCjAAj})WSF5fPMJ$Qu7MkAX!K)a{OyI4er1QxG20MaNTIrq#c&j>{vK3KeS_Dlyr z)Qf|Gy?i!-Mj37s;pQnKFt6$T&+s`&EUdSkX50=5&M9=Ux zCqQkGL$UGR)}ZGJQ?p+^y#=3M+Ul@#HV zGF}w%s0$#!c(>YEEkWYXkdLYN<(~v*`bTNEnnw3SK-%!TRk4FCN_Ks(Z3p6HnZ({IDF zR_XQ`DtA6Ba5GO1q=;<~Yj>YM(E}Ial~DIkJdZ#xj$5ejtB1t2SInlIzmOIubQUViS4R0rIF9Di& z6(Yf~6A}&!EjPapQ>AFz-J?MPk0?S=f^UeU5E2JkHY-?;LBjMb>%}x!$1z`v%5%(v zkVxdLWqK+NYbs0mkwfb=DPoAKsT5J)rNg`2@1@ znwu;G8gpugZQVXt+1;&oA1?SvmE}|c#(XQoA#x#axjDF=YxDzTV{P+}ws1(e%^j(l z-%w6qv>E2(t?iKD-uFZK!T<3 z#+xWFNU(vW0Lk+!fOf;?i@qhu8G`O!aP6-VGyj+G(!;LLre?I@Bn&3yw6hy=&`c z`bx=yc!ugPC?YU&JhY8(6d>ZwW)Y#!Az`;mg9BfNMB2~}$?Kc`0h*kuY_paZoXvs; z%nEgHQd7~6(rNSAF^Wh%sA#)o9umu6eIAg|b(z+&@eU^N)>+-Etv9Iq3n8je8YiRRy%L)0ngS=S;1aQ+u8CbMqR{=mea;w2UA3_&L!^w#YvbuAE+LE z&It)QW8)XH+acj_C9~RX01^#axnj}EuK+HfwOW01y(hVlcBiT%?SaU-qpiG(l7x-pqTBHlCjddsO4fLo`0$XLFSOcH*+XRzpa ziui%Gmdxy)1_&;e8Daba61HD<{^5#*gjQ$uocaq$WR*+rxpXrV;CGLnv`)pcNy3gA zbtz(oZ}?%=XGn;!tjd~dkP9&y@fbIO1Q*XIdy^PQRBmc~EcZJX;Kieu%_vqt5?2P| zAH+z%ANjBqZ?C$?A<{L6nHOZ78G(>qyz+VXjFn zNLV8XW5{wjLZT&sji1Up5cWA_brd+ zGfA+&n5~#(iFbj-g>Sv_k2;$GZn)7nG*j>oNj#w%=xK60QyuQ>S*AhaL`A53cNT2d zwrRR?wSI$*($koeN~irGu}S-fjmG!i0B?9p>kAL>CW&hSZl6vYP=xYANhsT)ML3U) zN_Z(BfPH8Fu}@k09cKx5e9z?}sDJSggDnPbMU!zwDUxz5NF!zLXLf zUz=f)7@{TtiWttX9pP$(gxj@=S@Csn$ZYU6Z){P5I58hx3gW|luSV4>ZOf5 zmPdO4Zc?uN>+}0Dl1QV535wXz%lne|4`bfga50VIZ zpD-(e_X7Ot?k;{ksd19{Fv2juL3EbfPKK4&KVDv2g|}-SbM06B;VlYd*5=DUYvC2h zU0n&eQ{+N~@7iB#*xm(pYuu939|& zXHn-^=Ys@zS^0LyR*&=qNbF)cbh5E~0N~p%EPYBznWnog)dmj|WXpCC`&#t0Ic*!mvOg z_;HTOxBV#qosbkd;D0XzD3yrNuuZ0htPNksTyPggA#Na6%8 zuut^~6CHWNEtPD2Fb|-WZbiXAoXY{mD^}n$C;VO|EtE6Lv;Yt`QFfAya;R8F}_yc(t;^~XM1u) zKbKo0iB*g7G?PCR;eO*quOW)K1bnYO>-!rzI09#yW5zxC1 ziL;d_kE#XF0(=YOtBynqMv^$Eo;_ZwyG#;R-39)8V_D&LdTsn{lHUYSV_%MADSSNu z<2TB4nq!BA?D^@c^)qvj2=i5AcVZ$5C#BCD9Jp3UqIQG+%~}gKI>NAMDVQtP22i8z zPXszb`vI1iArX2$7&d3i(RIZyl@|c6S({Rz@r0QqV)uB8NtIAU_alZEuHx(Hh*|b| z58Bl(fLe;4&%n+O15DylE5j~6NQ_#aH)pxC2=EW#k|y)UEF|$8eW2Rov`R*C%$KJD(MDj^3cE~cCo(z3x5!&a(o$%4x$TDErE}g;1l`G=RUWSl_Xf#Uo;9- zS|bS%?Q&4jogHrHE4IDbs(k=0?+>z7+b{{Rte3Z}-iN{&Xu{>wTjJa@zysqG{IilM z0$%`+PtH;V(UQ%2d+&NWa`x+%uXE)C0OcLF-f;4nAkJT3Bl5)xID z;C2KL8yUsrf!#qX-~W+Oq^KPkyi^1U@ndiGRSkv#+EYJsH)QiHz*cGm_SU$<**K@; z*b@%56@W*q&vP<3N)g9KTz{XJWuQZ1Q@}7%caDROBpnt}Fo#18I=5> z9L4+KRYPw$x09J6#`XFe=*X1|%)LUZ699es z!gCvqa~WWs=)S1*40r{VZ9KHh_5tq29Zyen>^rrc>&_ZG_nu-T7oy3m7yqgn5)r!> zB&Fo002=m6bt-0l1z?J0qlF=`!qGzXNMFdO8pt8gkMsxmW;}Y&>$8j zG75tYT{kPjAwg&IVg2qo1JG9A#rgzo20E6oiGjJp1KzQ!`+?coQ}FG)mXLSuNfY&g zm!>Q2zi&(tocG5~&6qaQ5sn5fi>8QKfL@QAN~+k;NXMFjOFK{vcv-v@a$G1@3!af? z{!cCwoD^XsC7DT9B^bq6@9psYmmrbv`|4t2(i}j2`|CrO7?|i7Bh5#pmkuw^@hcBL z>#EKJoV}^w5tB4^QfI#kpA;2jCW)_X#=?%#kSNbCTI5KX2k50#yX#@4kVu{Fs_cK3mqG}{jQax3w{~miemY5pkW%|ag#E2-!iD&nZ~s`6_2x!3;E5tL-W;RNLZ%3 zrzl@v0%(l19)7tC5?)p_>cxNIC$8AOjXudAVEXlC_EihZTp`o1hU>&r5BPk^?QBY) zDoKHaG}Btyywoy4{XErj4IZ=7v6=26*`J%?hqkA;LTa6jVG$4)FlhRevP2R+t36z% zBNQ?BvhQ5UFu9P&$4fVkLgM7ZLTxt=HaZrTz98PO3cnbRP0%v@cTB*yGjeicO!NXt z*jYEdQ$E2;Mq#V+Gd)9K6CJS)4_-O;W(AWdz_Nr!>kJ{Hd}WVVaXZ!N^8N5q&BDJhwR{PR4KDs9ykyVmhu_b`KJ^xZve( zQ}`vhB!3`n&3ATA3k5yj_YZ$?%562I~^lew8(VzV^-8<5&%e z33b`Sk}^$z*1rk2z6!5o@ry&llg$oPXH}yUQ6HX35sIf@<+n#cLZZ$t_isET&bnt4 z2BPeAEa22r8v|bWVg9Ga$7WGs*dkg)ChmwGpt{WKkXRPjcEW6X1w11nkIu%}z|rB&NK~&q$a$y+|6wDNN6TcK8IDmtprZTd*DmtCph;VD za!9h13;7o23Pfih5tKV~XP^oanVs?}I;)W2>K58A{y&W#x^@3i-DM0CsjnP!6rv*FeX~KbwX#quzzf8b z^|^*}$rf?sjYewVvGw%-Z4v)}1dSng?`7`S%0Ia22XJXAX){G*6I@0Hmy(h*k&-sW vjTIFXaYboGGt;YzxRipFEG{c+CXFj7UNcjaku@=uH#1d~GnF%wF_r&+Uc5GM literal 0 HcmV?d00001 diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 0949b15a..50dea543 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -26,59 +26,59 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; - public static final boolean tuningMode = true; + public static final int loopPeriodMs = 20; + private static RobotType robotType = RobotType.DEVBOT; + public static final boolean tuningMode = true; - private static boolean invalidRobotAlertSent = false; + private static boolean invalidRobotAlertSent = false; - public static RobotType getRobot() { - if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { - new Alert("Invalid Robot Selected, using COMPBOT as default", Alert.AlertType.ERROR) - .set(true); - invalidRobotAlertSent = true; - robotType = RobotType.COMPBOT; - } - return robotType; + public static RobotType getRobot() { + if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert("Invalid Robot Selected, using COMPBOT as default", Alert.AlertType.ERROR) + .set(true); + invalidRobotAlertSent = true; + robotType = RobotType.COMPBOT; } + return robotType; + } - public static Mode getMode() { - return switch (robotType) { - case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; - case SIMBOT -> Mode.SIM; - }; - } + public static Mode getMode() { + return switch (robotType) { + case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; + } - public static final Map logFolders = Map.of(RobotType.DEVBOT, "/media/sda1/"); + public static final Map logFolders = Map.of(RobotType.DEVBOT, "/media/sda1/"); - public enum Mode { - /** Running on a real robot. */ - REAL, + public enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY - } + /** Replaying from a log file. */ + REPLAY + } - public enum RobotType { - SIMBOT, - DEVBOT, - COMPBOT - } + public enum RobotType { + SIMBOT, + DEVBOT, + COMPBOT + } - public static boolean disableHAL = false; + public static boolean disableHAL = false; - public static void disableHAL() { - disableHAL = true; - } + public static void disableHAL() { + disableHAL = true; + } - /** Checks whether the robot the correct robot is selected when deploying. */ - public static void main(String... args) { - if (robotType == RobotType.SIMBOT) { - System.err.println("Cannot deploy, invalid robot selected: " + robotType.toString()); - System.exit(1); - } + /** Checks whether the robot the correct robot is selected when deploying. */ + public static void main(String... args) { + if (robotType == RobotType.SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robotType.toString()); + System.exit(1); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java index 6d5c1005..ea186dab 100644 --- a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java @@ -19,81 +19,81 @@ * Width refers to the y direction (as described by wpilib) */ public class FieldConstants { - public static double fieldLength = Units.inchesToMeters(651.223); - public static double fieldWidth = Units.inchesToMeters(323.277); - public static double wingX = Units.inchesToMeters(229.201); - public static double podiumX = Units.inchesToMeters(126.75); - public static double startingLineX = Units.inchesToMeters(74.111); - - public static Translation2d ampCenter = - new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); - - /** Staging locations for each note */ - public static final class StagingLocations { - public static double centerlineX = fieldLength / 2.0; - - // need to update - public static double centerlineFirstY = Units.inchesToMeters(29.638); - public static double centerlineSeparationY = Units.inchesToMeters(66); - public static double spikeX = Units.inchesToMeters(114); - // need - public static double spikeFirstY = Units.inchesToMeters(161.638); - public static double spikeSeparationY = Units.inchesToMeters(57); - - public static Translation2d[] centerlineTranslations = new Translation2d[5]; - public static Translation2d[] spikeTranslations = new Translation2d[3]; - - static { - for (int i = 0; i < centerlineTranslations.length; i++) { - centerlineTranslations[i] = - new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); - } - } - - static { - for (int i = 0; i < spikeTranslations.length; i++) { - spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); - } - } - } + public static double fieldLength = Units.inchesToMeters(651.223); + public static double fieldWidth = Units.inchesToMeters(323.277); + public static double wingX = Units.inchesToMeters(229.201); + public static double podiumX = Units.inchesToMeters(126.75); + public static double startingLineX = Units.inchesToMeters(74.111); + + public static Translation2d ampCenter = + new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + + /** Staging locations for each note */ + public static final class StagingLocations { + public static double centerlineX = fieldLength / 2.0; + + // need to update + public static double centerlineFirstY = Units.inchesToMeters(29.638); + public static double centerlineSeparationY = Units.inchesToMeters(66); + public static double spikeX = Units.inchesToMeters(114); + // need + public static double spikeFirstY = Units.inchesToMeters(161.638); + public static double spikeSeparationY = Units.inchesToMeters(57); + + public static Translation2d[] centerlineTranslations = new Translation2d[5]; + public static Translation2d[] spikeTranslations = new Translation2d[3]; - /** Each corner of the speaker * */ - public static final class Speaker { - - // corners (blue alliance origin) - public static Translation3d topRightSpeaker = - new Translation3d( - Units.inchesToMeters(18.055), - Units.inchesToMeters(238.815), - Units.inchesToMeters(13.091)); - - public static Translation3d topLeftSpeaker = - new Translation3d( - Units.inchesToMeters(18.055), - Units.inchesToMeters(197.765), - Units.inchesToMeters(83.091)); - - public static Translation3d bottomRightSpeaker = - new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); - public static Translation3d bottomLeftSpeaker = - new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); - - /** Center of the speaker opening (blue alliance) */ - public static Translation3d centerSpeakerOpening = - new Translation3d( - topLeftSpeaker.getX() / 2.0, - fieldWidth - Units.inchesToMeters(104.0), - (bottomLeftSpeaker.getZ() + bottomRightSpeaker.getZ()) / 2.0); + static { + for (int i = 0; i < centerlineTranslations.length; i++) { + centerlineTranslations[i] = + new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); + } } - public static double aprilTagWidth = Units.inchesToMeters(6.50); - public static AprilTagFieldLayout aprilTags; - static { - try { - aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); - } catch (IOException e) { - throw new RuntimeException(e); - } + for (int i = 0; i < spikeTranslations.length; i++) { + spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); + } + } + } + + /** Each corner of the speaker * */ + public static final class Speaker { + + // corners (blue alliance origin) + public static Translation3d topRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(13.091)); + + public static Translation3d topLeftSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static Translation3d bottomRightSpeaker = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static Translation3d bottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + + /** Center of the speaker opening (blue alliance) */ + public static Translation3d centerSpeakerOpening = + new Translation3d( + topLeftSpeaker.getX() / 2.0, + fieldWidth - Units.inchesToMeters(104.0), + (bottomLeftSpeaker.getZ() + bottomRightSpeaker.getZ()) / 2.0); + } + + public static double aprilTagWidth = Units.inchesToMeters(6.50); + public static AprilTagFieldLayout aprilTags; + + static { + try { + aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/Main.java b/src/main/java/org/littletonrobotics/frc2024/Main.java index 45218afe..ca8e2378 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Main.java +++ b/src/main/java/org/littletonrobotics/frc2024/Main.java @@ -21,14 +21,14 @@ * call. */ public final class Main { - private Main() {} + private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 3b26c2da..b6eaa561 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -38,181 +38,181 @@ * project. */ public class Robot extends LoggedRobot { - private Command autoCommand; - private RobotContainer robotContainer; - - private double autoStart; - private boolean autoMessagePrinted; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } - - // Set up data receivers & replay source - switch (Constants.getMode()) { - case REAL: - // Running on a real robot, log to a USB stick ("/U/logs") - String folder = Constants.logFolders.get(Constants.getRobot()); - Logger.addDataReceiver(new WPILOGWriter(folder)); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case SIM: - // Running a physics simulator, log to NT - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"), 0.01)); - break; - } - - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - - // Start AdvantageKit logger - Logger.start(); - - // Log active commands - Map commandCounts = new HashMap<>(); - BiConsumer logCommandFunction = - (Command command, Boolean active) -> { - String name = command.getName(); - int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); - commandCounts.put(name, count); - Logger.recordOutput( - "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); - Logger.recordOutput("CommandsAll/" + name, count > 0); - }; - CommandScheduler.getInstance() - .onCommandInitialize( - (Command command) -> { - logCommandFunction.accept(command, true); - }); - CommandScheduler.getInstance() - .onCommandFinish( - (Command command) -> { - logCommandFunction.accept(command, false); - }); - CommandScheduler.getInstance() - .onCommandInterrupt( - (Command command) -> { - logCommandFunction.accept(command, false); - }); - - // Default to blue alliance in sim - if (Constants.getMode() == Constants.Mode.SIM) { - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - } - - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. - robotContainer = new RobotContainer(); + private Command autoCommand; + private RobotContainer robotContainer; + + private double autoStart; + private boolean autoMessagePrinted; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; } - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { - Threads.setCurrentThreadPriority(true, 99); - VirtualSubsystem.periodicAll(); - CommandScheduler.getInstance().run(); - - // Print auto duration - if (autoCommand != null) { - if (!autoCommand.isScheduled() && !autoMessagePrinted) { - if (DriverStation.isAutonomousEnabled()) { - System.out.printf( - "*** Auto finished in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); - } else { - System.out.printf( - "*** Auto cancelled in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); - } - autoMessagePrinted = true; - } - } - - Threads.setCurrentThreadPriority(true, 10); + // Set up data receivers & replay source + switch (Constants.getMode()) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + String folder = Constants.logFolders.get(Constants.getRobot()); + Logger.addDataReceiver(new WPILOGWriter(folder)); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"), 0.01)); + break; } - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() {} - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - autoStart = Timer.getFPGATimestamp(); - autoMessagePrinted = false; - autoCommand = robotContainer.getAutonomousCommand(); - if (autoCommand != null) { - autoCommand.schedule(); - } + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger + Logger.start(); + + // Log active commands + Map commandCounts = new HashMap<>(); + BiConsumer logCommandFunction = + (Command command, Boolean active) -> { + String name = command.getName(); + int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.recordOutput("CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize( + (Command command) -> { + logCommandFunction.accept(command, true); + }); + CommandScheduler.getInstance() + .onCommandFinish( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + CommandScheduler.getInstance() + .onCommandInterrupt( + (Command command) -> { + logCommandFunction.accept(command, false); + }); + + // Default to blue alliance in sim + if (Constants.getMode() == Constants.Mode.SIM) { + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); } - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (autoCommand != null) { - autoCommand.cancel(); + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + Threads.setCurrentThreadPriority(true, 99); + VirtualSubsystem.periodicAll(); + CommandScheduler.getInstance().run(); + + // Print auto duration + if (autoCommand != null) { + if (!autoCommand.isScheduled() && !autoMessagePrinted) { + if (DriverStation.isAutonomousEnabled()) { + System.out.printf( + "*** Auto finished in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); + } else { + System.out.printf( + "*** Auto cancelled in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); } + autoMessagePrinted = true; + } } - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); + Threads.setCurrentThreadPriority(true, 10); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() {} + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + autoStart = Timer.getFPGATimestamp(); + autoMessagePrinted = false; + autoCommand = robotContainer.getAutonomousCommand(); + if (autoCommand != null) { + autoCommand.schedule(); } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autoCommand != null) { + autoCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3cc8a521..797d4181 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -17,10 +17,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; import org.littletonrobotics.frc2024.commands.auto.AutoCommands; @@ -29,23 +31,27 @@ import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIONorthstar; import org.littletonrobotics.frc2024.subsystems.drive.*; +import org.littletonrobotics.frc2024.subsystems.rollers.Rollers; +import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO; +import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal; +import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; +import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO; +import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex; +import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; +import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO; +import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder; -import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIO; -import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSparkFlex; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIO; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOKrakenFOC; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOSim; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -56,254 +62,304 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state - private final RobotState robotState = RobotState.getInstance(); - - // Subsystems - private Drive drive; - private AprilTagVision aprilTagVision; - private Flywheels flywheels; - private Intake intake; - private Arm arm; - - private Feeder feeder; - - private Superstructure superstructure; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - if (Constants.getMode() != Constants.Mode.REPLAY) { - switch (Constants.getRobot()) { - case DEVBOT -> { - drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - arm = new Arm(new ArmIOKrakenFOC()); - flywheels = new Flywheels(new FlywheelsIOSparkFlex()); - feeder = new Feeder(new FeederIOSparkFlex()); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar( - AprilTagVisionConstants.instanceNames[0], - AprilTagVisionConstants.cameraIds[0]), - new AprilTagVisionIONorthstar( - AprilTagVisionConstants.instanceNames[1], - AprilTagVisionConstants.cameraIds[1])); - intake = new Intake(new IntakeIOKrakenFOC()); - superstructure = new Superstructure(arm, flywheels, feeder, intake); - } - case SIMBOT -> { - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(DriveConstants.moduleConfigs[0]), - new ModuleIOSim(DriveConstants.moduleConfigs[1]), - new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3])); - arm = new Arm(new ArmIOSim()); - flywheels = new Flywheels(new FlywheelsIOSim()); - intake = new Intake(new IntakeIOSim()); - feeder = new Feeder(new FeederIOSim()); - superstructure = new Superstructure(arm, flywheels, feeder, intake); - } - case COMPBOT -> { - // No impl yet - } - } - } + // Load robot state + private final RobotState robotState = RobotState.getInstance(); - if (drive == null) { - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - } + // Subsystems + private Drive drive; + private AprilTagVision aprilTagVision; - if (aprilTagVision == null) { - switch (Constants.getRobot()) { - case DEVBOT -> - aprilTagVision = - new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}); - default -> aprilTagVision = new AprilTagVision(); - } - } + private Feeder feeder; + private Indexer indexer; + private Intake intake; + private Rollers rollers; - if (flywheels == null) { - flywheels = new Flywheels(new FlywheelsIO() {}); - } + private Flywheels flywheels; + private Arm arm; + private Superstructure superstructure; - if (intake == null) { - intake = new Intake(new IntakeIO() {}); - } + // Controller + private final CommandXboxController controller = new CommandXboxController(0); - if (arm == null) { - arm = new Arm(new ArmIO() {}); - } + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; - if (feeder == null) { - feeder = new Feeder(new FeederIO() {}); - } + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + if (Constants.getMode() != Constants.Mode.REPLAY) { + switch (Constants.getRobot()) { + case DEVBOT -> { + drive = + new Drive( + new GyroIOPigeon2(false), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); + + feeder = new Feeder(new FeederIOKrakenFOC()); + indexer = new Indexer(new IndexerIOSparkFlex()); + intake = new Intake(new IntakeIOKrakenFOC()); + rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal()); - if (superstructure == null) { - superstructure = new Superstructure(arm, flywheels, feeder, intake); + arm = new Arm(new ArmIOKrakenFOC()); + flywheels = new Flywheels(new FlywheelsIOSparkFlex()); + superstructure = new Superstructure(arm, flywheels); + + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar( + AprilTagVisionConstants.instanceNames[0], + AprilTagVisionConstants.cameraIds[0]), + new AprilTagVisionIONorthstar( + AprilTagVisionConstants.instanceNames[1], + AprilTagVisionConstants.cameraIds[1])); + } + case SIMBOT -> { + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3])); + arm = new Arm(new ArmIOSim()); + flywheels = new Flywheels(new FlywheelsIOSim()); + // intake = new Intake(new IntakeIOSim()); + // feeder = new Feeder(new FeederIOSim()); + superstructure = new Superstructure(arm, flywheels); } + case COMPBOT -> { + // No impl yet + } + } + } - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); - autoChooser.addDefaultOption("Do Nothing", Commands.none()); - // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) - .finallyDo(drive::endCharacterization)); - autoChooser.addOption( - "Left Flywheels FF Characterization", - new FeedForwardCharacterization( - flywheels, - flywheels::runLeftCharacterizationVolts, - flywheels::getLeftCharacterizationVelocity)); - autoChooser.addOption( - "Right Flywheels FF Characterization", - new FeedForwardCharacterization( - flywheels, - flywheels::runRightCharacterizationVolts, - flywheels::getRightCharacterizationVelocity)); - autoChooser.addOption("Arm get static current", arm.getStaticCurrent()); - - AutoCommands autoCommands = new AutoCommands(drive, superstructure); - - autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); - - // Testing autos paths - // Function> trajectoryCommandFactory = - // trajectoryFile -> { - // Optional trajectory = - // ChoreoTrajectoryDeserializer.deserialize(trajectoryFile); - // return trajectory.map( - // traj -> - // Commands.runOnce( - // () -> - // robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - // .andThen(Commands.runOnce(() -> - // drive.setTrajectoryGoal(traj))) - // .until(drive::isTrajectoryGoalCompleted)); - // }; - // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - // trajectoryCommandFactory - // .apply(trajectoryFile) - // .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); - // } - - // Configure the button bindings - configureButtonBindings(); + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); } - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - drive.setDefaultCommand( - drive.run( - () -> - drive.setTeleopDriveGoal( - -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))); - - controller - .leftTrigger() - .onTrue(Commands.runOnce(drive::setAutoAimGoal)) - .onFalse(Commands.runOnce(drive::clearAutoAimGoal)); - - if (superstructure != null) { - controller - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT))) - .onFalse( - Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); - - Trigger readyToShootTrigger = - new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); - readyToShootTrigger - .whileTrue( - Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) - .whileFalse( - Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); - controller - .rightTrigger() - .and(readyToShootTrigger) - .onTrue( - Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.SHOOT)) - .andThen(Commands.waitSeconds(0.5)) - .andThen( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.IDLE)))); - - controller - .leftBumper() - .onTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.STATION_INTAKE))) - .onFalse( - Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); - - controller - .a() - .onTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.REVERSE_INTAKE))) - .onFalse( - Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); - } + if (aprilTagVision == null) { + switch (Constants.getRobot()) { + case DEVBOT -> + aprilTagVision = + new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}); + default -> aprilTagVision = new AprilTagVision(); + } + } + + if (flywheels == null) { + flywheels = new Flywheels(new FlywheelsIO() {}); + } + + if (intake == null) { + intake = new Intake(new IntakeIO() {}); + } + + if (arm == null) { + arm = new Arm(new ArmIO() {}); + } - controller - .y() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - AllianceFlipUtil.apply( - new Pose2d( - Units.inchesToMeters(36.0), - FieldConstants.Speaker.centerSpeakerOpening.getY(), - new Rotation2d()))))); - - controller - .b() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - new Pose2d( - robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) - .ignoringDisable(true)); + if (feeder == null) { + feeder = new Feeder(new FeederIO() {}); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); + if (indexer == null) { + indexer = new Indexer(new IndexerIO() {}); } + + if (rollers == null) { + rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {}); + } + + if (superstructure == null) { + superstructure = new Superstructure(arm, flywheels); + } + + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + autoChooser.addDefaultOption("Do Nothing", Commands.none()); + // Set up feedforward characterization + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); + autoChooser.addOption( + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + flywheels, + flywheels::runLeftCharacterizationVolts, + flywheels::getLeftCharacterizationVelocity)); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + flywheels, + flywheels::runRightCharacterizationVolts, + flywheels::getRightCharacterizationVelocity)); + autoChooser.addOption("Arm get static current", arm.getStaticCurrent()); + + AutoCommands autoCommands = new AutoCommands(drive, superstructure); + + autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); + + // Testing autos paths + // Function> trajectoryCommandFactory = + // trajectoryFile -> { + // Optional trajectory = + // ChoreoTrajectoryDeserializer.deserialize(trajectoryFile); + // return trajectory.map( + // traj -> + // Commands.runOnce( + // () -> + // robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + // .andThen(Commands.runOnce(() -> + // drive.setTrajectoryGoal(traj))) + // .until(drive::isTrajectoryGoalCompleted)); + // }; + // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + // trajectoryCommandFactory + // .apply(trajectoryFile) + // .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + // } + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link Joystick} or {@link + * XboxController}), and then passing it to a {@link JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + drive.run( + () -> + drive.setTeleopDriveGoal( + -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))); + + controller + .a() + .onTrue(Commands.runOnce(drive::setAutoAimGoal)) + .onFalse(Commands.runOnce(drive::clearAutoAimGoal)); + + if (superstructure != null) { + controller + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT))) + .onFalse( + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); + + Trigger readyToShootTrigger = + new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); + readyToShootTrigger + .whileTrue( + Commands.run( + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) + .whileFalse( + Commands.run( + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); + controller + .rightTrigger() + .and(readyToShootTrigger) + .onTrue( + Commands.runOnce( + () -> { + superstructure.setGoalState(Superstructure.SystemState.SHOOT); + rollers.setGoal(Rollers.Goal.FEED_SHOOTER); + }) + .andThen(Commands.waitSeconds(0.5)) + .andThen( + Commands.runOnce( + () -> { + superstructure.setGoalState(Superstructure.SystemState.IDLE); + rollers.setGoal(Rollers.Goal.IDLE); + }))); + + controller + .leftTrigger() + .whileTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), + superstructure) + .andThen( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers)) + .finallyDo( + () -> { + rollers.setGoal(Rollers.Goal.IDLE); + superstructure.setGoalState(Superstructure.SystemState.IDLE); + })); + + controller + .leftBumper() + .whileTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), + superstructure) + .andThen( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers)) + .finallyDo( + () -> { + rollers.setGoal(Rollers.Goal.IDLE); + superstructure.setGoalState(Superstructure.SystemState.IDLE); + })); + + // controller + // .a() + // .onTrue( + // Commands.runOnce( + // () -> { + // superstructure.setGoalState(Superstructure.SystemState.REVERSE_INTAKE); + // rollers.setGoal(Rollers.Goal.STATION_INTAKE); + // })) + // .onFalse( + // Commands.runOnce( + // () -> { + // superstructure.setGoalState(Superstructure.SystemState.IDLE); + // rollers.setGoal(Rollers.Goal.IDLE); + // })); + } + + controller + .y() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + AllianceFlipUtil.apply( + new Pose2d( + Units.inchesToMeters(36.0), + FieldConstants.Speaker.centerSpeakerOpening.getY(), + new Rotation2d()))))); + + controller + .b() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + new Pose2d( + robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) + .ignoringDisable(true)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 36595e3a..909a376a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -13,225 +13,231 @@ import java.util.NoSuchElementException; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants; +import org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; @ExtensionMethod({GeomUtil.class}) public class RobotState { - // Pose Estimation - public record OdometryObservation( - SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {} - - public record VisionObservation(Pose2d visionPose, double timestamp, Matrix stdDevs) {} - - public record AimingParameters(Rotation2d driveHeading, Rotation2d armAngle, double radialFF) {} - - private static final LoggedTunableNumber lookahead = - new LoggedTunableNumber("RobotState/lookaheadS", 0.0); - - private static LoggedTunableNumber shotHeightCompensation = - new LoggedTunableNumber("Superstructure/CompensationInches", 6.0); - - private static final double poseBufferSizeSeconds = 2.0; - - private static RobotState instance; - - public static RobotState getInstance() { - if (instance == null) instance = new RobotState(); - return instance; - } - - // Pose Estimation Members - private Pose2d odometryPose = new Pose2d(); - private Pose2d estimatedPose = new Pose2d(); - private final TimeInterpolatableBuffer poseBuffer = - TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds); - private final Matrix qStdDevs = new Matrix<>(Nat.N3(), Nat.N1()); - // odometry - private final SwerveDriveKinematics kinematics; - private SwerveDriveWheelPositions lastWheelPositions = - new SwerveDriveWheelPositions( - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }); - private Rotation2d lastGyroAngle = new Rotation2d(); - private Twist2d robotVelocity = new Twist2d(); - - private AimingParameters latestParameters = null; - - private RobotState() { - for (int i = 0; i < 3; ++i) { - qStdDevs.set(i, 0, Math.pow(DriveConstants.odometryStateStdDevs.get(i, 0), 2)); - } - kinematics = DriveConstants.kinematics; + // Pose Estimation + public record OdometryObservation( + SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {} + + public record VisionObservation(Pose2d visionPose, double timestamp, Matrix stdDevs) {} + + public record AimingParameters( + Rotation2d driveHeading, Rotation2d armAngle, double driveFeedVelocity) {} + + private static final LoggedTunableNumber lookahead = + new LoggedTunableNumber("RobotState/lookaheadS", 0.0); + + private static LoggedTunableNumber shotHeightCompensation = + new LoggedTunableNumber("Superstructure/CompensationInches", 6.0); + + private static final double poseBufferSizeSeconds = 2.0; + + private static RobotState instance; + + public static RobotState getInstance() { + if (instance == null) instance = new RobotState(); + return instance; + } + + // Pose Estimation Members + private Pose2d odometryPose = new Pose2d(); + private Pose2d estimatedPose = new Pose2d(); + private final TimeInterpolatableBuffer poseBuffer = + TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds); + private final Matrix qStdDevs = new Matrix<>(Nat.N3(), Nat.N1()); + // odometry + private final SwerveDriveKinematics kinematics; + private SwerveDriveWheelPositions lastWheelPositions = + new SwerveDriveWheelPositions( + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); + private Rotation2d lastGyroAngle = new Rotation2d(); + private Twist2d robotVelocity = new Twist2d(); + + private AimingParameters latestParameters = null; + + private RobotState() { + for (int i = 0; i < 3; ++i) { + qStdDevs.set(i, 0, Math.pow(DriveConstants.odometryStateStdDevs.get(i, 0), 2)); } - - /** Add odometry observation */ - public void addOdometryObservation(OdometryObservation observation) { - latestParameters = null; - Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); - lastWheelPositions = observation.wheelPositions(); - // Check gyro connected - if (observation.gyroAngle != null) { - // Update dtheta for twist if gyro connected - twist = - new Twist2d( - twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians()); - lastGyroAngle = observation.gyroAngle(); - } - // Add twist to odometry pose - odometryPose = odometryPose.exp(twist); - // Add pose to buffer at timestamp - poseBuffer.addSample(observation.timestamp(), odometryPose); - // Calculate diff from last odometry pose and add onto pose estimate - estimatedPose = estimatedPose.exp(twist); + kinematics = DriveConstants.kinematics; + } + + /** Add odometry observation */ + public void addOdometryObservation(OdometryObservation observation) { + latestParameters = null; + Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); + lastWheelPositions = observation.wheelPositions(); + // Check gyro connected + if (observation.gyroAngle != null) { + // Update dtheta for twist if gyro connected + twist = + new Twist2d( + twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians()); + lastGyroAngle = observation.gyroAngle(); } - - public void addVisionObservation(VisionObservation observation) { - latestParameters = null; - // If measurement is old enough to be outside the pose buffer's timespan, skip. - try { - if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds - > observation.timestamp()) { - return; - } - } catch (NoSuchElementException ex) { - return; - } - // Get odometry based pose at timestamp - var sample = poseBuffer.getSample(observation.timestamp()); - if (sample.isEmpty()) { - // exit if not there - return; - } - - // sample --> odometryPose transform and backwards of that - var sampleToOdometryTransform = new Transform2d(sample.get(), odometryPose); - var odometryToSampleTransform = new Transform2d(odometryPose, sample.get()); - // get old estimate by applying odometryToSample Transform - Pose2d estimateAtTime = estimatedPose.plus(odometryToSampleTransform); - - // Calculate 3 x 3 vision matrix - var r = new double[3]; - for (int i = 0; i < 3; ++i) { - r[i] = observation.stdDevs().get(i, 0) * observation.stdDevs().get(i, 0); - } - // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 - // and C = I. See wpimath/algorithms.md. - Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); - for (int row = 0; row < 3; ++row) { - double stdDev = qStdDevs.get(row, 0); - if (stdDev == 0.0) { - visionK.set(row, row, 0.0); - } else { - visionK.set(row, row, stdDev / (stdDev + Math.sqrt(stdDev * r[row]))); - } - } - // difference between estimate and vision pose - Twist2d twist = estimateAtTime.log(observation.visionPose()); - // scale twist by visionK - var kTimesTwist = visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); - Twist2d scaledTwist = - new Twist2d(kTimesTwist.get(0, 0), kTimesTwist.get(1, 0), kTimesTwist.get(2, 0)); - - // Recalculate current estimate by applying scaled twist to old estimate - // then replaying odometry data - estimatedPose = estimateAtTime.exp(scaledTwist).plus(sampleToOdometryTransform); + // Add twist to odometry pose + odometryPose = odometryPose.exp(twist); + // Add pose to buffer at timestamp + poseBuffer.addSample(observation.timestamp(), odometryPose); + // Calculate diff from last odometry pose and add onto pose estimate + estimatedPose = estimatedPose.exp(twist); + } + + public void addVisionObservation(VisionObservation observation) { + latestParameters = null; + // If measurement is old enough to be outside the pose buffer's timespan, skip. + try { + if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds + > observation.timestamp()) { + return; + } + } catch (NoSuchElementException ex) { + return; } - - public void addVelocityData(Twist2d robotVelocity) { - this.latestParameters = null; - this.robotVelocity = robotVelocity; + // Get odometry based pose at timestamp + var sample = poseBuffer.getSample(observation.timestamp()); + if (sample.isEmpty()) { + // exit if not there + return; } - public AimingParameters getAimingParameters() { - if (latestParameters != null) { - // Cache previously calculated aiming parameters. Cache is invalidated whenever new - // observations are added. - return latestParameters; - } - - Transform2d fieldToTarget = - AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening) - .toTranslation2d() - .toTransform2d(); - Pose2d fieldToPredictedVehicle = getPredictedPose(lookahead.get(), lookahead.get()); - Pose2d fieldToPredictedVehicleFixed = - new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d()); - - Translation2d predictedVehicleToTargetTranslation = - fieldToPredictedVehicle.inverse().transformBy(fieldToTarget).getTranslation(); - Translation2d predictedVehicleFixedToTargetTranslation = - fieldToPredictedVehicleFixed.inverse().transformBy(fieldToTarget).getTranslation(); - - Rotation2d vehicleToGoalDirection = predictedVehicleToTargetTranslation.getAngle(); - - Rotation2d targetVehicleDirection = predictedVehicleFixedToTargetTranslation.getAngle(); - double targetDistance = predictedVehicleToTargetTranslation.getNorm(); - - double feedVelocity = - robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance - - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance; - - latestParameters = - new AimingParameters( - targetVehicleDirection, - new Rotation2d( - targetDistance - SuperstructureConstants.ArmConstants.armOrigin.getX(), - FieldConstants.Speaker.centerSpeakerOpening.getZ() - - SuperstructureConstants.ArmConstants.armOrigin.getY() - + shotHeightCompensation.get()), - feedVelocity); - return latestParameters; - } + // sample --> odometryPose transform and backwards of that + var sampleToOdometryTransform = new Transform2d(sample.get(), odometryPose); + var odometryToSampleTransform = new Transform2d(odometryPose, sample.get()); + // get old estimate by applying odometryToSample Transform + Pose2d estimateAtTime = estimatedPose.plus(odometryToSampleTransform); - /** - * Reset estimated pose and odometry pose to pose
- * Clear pose buffer - */ - public void resetPose(Pose2d initialPose) { - estimatedPose = initialPose; - odometryPose = initialPose; - poseBuffer.clear(); + // Calculate 3 x 3 vision matrix + var r = new double[3]; + for (int i = 0; i < 3; ++i) { + r[i] = observation.stdDevs().get(i, 0) * observation.stdDevs().get(i, 0); } - - @AutoLogOutput(key = "Odometry/FieldVelocity") - public Twist2d fieldVelocity() { - Translation2d linearFieldVelocity = - new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation()); - return new Twist2d( - linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta); + // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 + // and C = I. See wpimath/algorithms.md. + Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); + for (int row = 0; row < 3; ++row) { + double stdDev = qStdDevs.get(row, 0); + if (stdDev == 0.0) { + visionK.set(row, row, 0.0); + } else { + visionK.set(row, row, stdDev / (stdDev + Math.sqrt(stdDev * r[row]))); + } } - - @AutoLogOutput(key = "Odometry/EstimatedPose") - public Pose2d getEstimatedPose() { - return estimatedPose; - } - - /** - * Predicts what our pose will be in the future. Allows separate translation and rotation - * lookaheads to account for varying latencies in the different measurements. - * - * @param translationLookaheadS The lookahead time for the translation of the robot - * @param rotationLookaheadS The lookahead time for the rotation of the robot - * @return The predicted pose. - */ - public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) { - return getEstimatedPose() - .exp( - new Twist2d( - robotVelocity.dx * translationLookaheadS, - robotVelocity.dy * translationLookaheadS, - robotVelocity.dtheta * rotationLookaheadS)); + // difference between estimate and vision pose + Twist2d twist = estimateAtTime.log(observation.visionPose()); + // scale twist by visionK + var kTimesTwist = visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); + Twist2d scaledTwist = + new Twist2d(kTimesTwist.get(0, 0), kTimesTwist.get(1, 0), kTimesTwist.get(2, 0)); + + // Recalculate current estimate by applying scaled twist to old estimate + // then replaying odometry data + estimatedPose = estimateAtTime.exp(scaledTwist).plus(sampleToOdometryTransform); + } + + public void addVelocityData(Twist2d robotVelocity) { + this.latestParameters = null; + this.robotVelocity = robotVelocity; + } + + public AimingParameters getAimingParameters() { + if (latestParameters != null) { + // Cache previously calculated aiming parameters. Cache is invalidated whenever new + // observations are added. + return latestParameters; } - @AutoLogOutput(key = "Odometry/OdometryPose") - public Pose2d getOdometryPose() { - return odometryPose; - } + Transform2d fieldToTarget = + AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening) + .toTranslation2d() + .toTransform2d(); + Pose2d fieldToPredictedVehicle = getPredictedPose(lookahead.get(), lookahead.get()); + Pose2d fieldToPredictedVehicleFixed = + new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d()); + + Translation2d predictedVehicleToTargetTranslation = + fieldToPredictedVehicle.inverse().transformBy(fieldToTarget).getTranslation(); + Translation2d predictedVehicleFixedToTargetTranslation = + fieldToPredictedVehicleFixed.inverse().transformBy(fieldToTarget).getTranslation(); + + Rotation2d vehicleToGoalDirection = predictedVehicleToTargetTranslation.getAngle(); + + Rotation2d targetVehicleDirection = predictedVehicleFixedToTargetTranslation.getAngle(); + double targetDistance = predictedVehicleToTargetTranslation.getNorm(); + + double feedVelocity = + robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance + - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance; + + latestParameters = + new AimingParameters( + targetVehicleDirection, + new Rotation2d( + targetDistance - SuperstructureConstants.ArmConstants.armOrigin.getX(), + FieldConstants.Speaker.centerSpeakerOpening.getZ() + - SuperstructureConstants.ArmConstants.armOrigin.getY() + + shotHeightCompensation.get()), + feedVelocity); + Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading); + Logger.recordOutput("RobotState/AimingParameters/ArmAngle", latestParameters.armAngle); + Logger.recordOutput("RobotState/AimingParameters/DriveFeedVelocityRadPerS", feedVelocity); + return latestParameters; + } + + /** + * Reset estimated pose and odometry pose to pose
+ * Clear pose buffer + */ + public void resetPose(Pose2d initialPose) { + estimatedPose = initialPose; + odometryPose = initialPose; + poseBuffer.clear(); + } + + @AutoLogOutput(key = "Odometry/FieldVelocity") + public Twist2d fieldVelocity() { + Translation2d linearFieldVelocity = + new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation()); + return new Twist2d( + linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta); + } + + @AutoLogOutput(key = "Odometry/EstimatedPose") + public Pose2d getEstimatedPose() { + return estimatedPose; + } + + /** + * Predicts what our pose will be in the future. Allows separate translation and rotation + * lookaheads to account for varying latencies in the different measurements. + * + * @param translationLookaheadS The lookahead time for the translation of the robot + * @param rotationLookaheadS The lookahead time for the rotation of the robot + * @return The predicted pose. + */ + public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) { + return getEstimatedPose() + .exp( + new Twist2d( + robotVelocity.dx * translationLookaheadS, + robotVelocity.dy * translationLookaheadS, + robotVelocity.dtheta * rotationLookaheadS)); + } + + @AutoLogOutput(key = "Odometry/OdometryPose") + public Pose2d getOdometryPose() { + return odometryPose; + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/FeedForwardCharacterization.java b/src/main/java/org/littletonrobotics/frc2024/commands/FeedForwardCharacterization.java index f2ac25da..01d93eaa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/FeedForwardCharacterization.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/FeedForwardCharacterization.java @@ -23,84 +23,84 @@ import org.littletonrobotics.frc2024.util.PolynomialRegression; public class FeedForwardCharacterization extends Command { - private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_VOLTS_PER_SEC = 0.1; + private static final double START_DELAY_SECS = 2.0; + private static final double RAMP_VOLTS_PER_SEC = 0.1; - private FeedForwardCharacterizationData data; - private final Consumer voltageConsumer; - private final Supplier velocitySupplier; + private FeedForwardCharacterizationData data; + private final Consumer voltageConsumer; + private final Supplier velocitySupplier; - private final Timer timer = new Timer(); + private final Timer timer = new Timer(); - /** Creates a new FeedForwardCharacterization command. */ - public FeedForwardCharacterization( - Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { - addRequirements(subsystem); - this.voltageConsumer = voltageConsumer; - this.velocitySupplier = velocitySupplier; - } + /** Creates a new FeedForwardCharacterization command. */ + public FeedForwardCharacterization( + Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { + addRequirements(subsystem); + this.voltageConsumer = voltageConsumer; + this.velocitySupplier = velocitySupplier; + } - // Called when the command is initially scheduled. - @Override - public void initialize() { - data = new FeedForwardCharacterizationData(); - timer.reset(); - timer.start(); - } + // Called when the command is initially scheduled. + @Override + public void initialize() { + data = new FeedForwardCharacterizationData(); + timer.reset(); + timer.start(); + } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (timer.get() < START_DELAY_SECS) { - voltageConsumer.accept(0.0); - } else { - double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; - voltageConsumer.accept(voltage); - data.add(velocitySupplier.get(), voltage); - } + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (timer.get() < START_DELAY_SECS) { + voltageConsumer.accept(0.0); + } else { + double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; + voltageConsumer.accept(voltage); + data.add(velocitySupplier.get(), voltage); } + } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - voltageConsumer.accept(0.0); - timer.stop(); - data.print(); - } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + voltageConsumer.accept(0.0); + timer.stop(); + data.print(); + } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } - public static class FeedForwardCharacterizationData { - private final List velocityData = new LinkedList<>(); - private final List voltageData = new LinkedList<>(); + public static class FeedForwardCharacterizationData { + private final List velocityData = new LinkedList<>(); + private final List voltageData = new LinkedList<>(); - public void add(double velocity, double voltage) { - if (Math.abs(velocity) > 1E-4) { - velocityData.add(Math.abs(velocity)); - voltageData.add(Math.abs(voltage)); - } - } + public void add(double velocity, double voltage) { + if (Math.abs(velocity) > 1E-4) { + velocityData.add(Math.abs(velocity)); + voltageData.add(Math.abs(voltage)); + } + } - public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { - return; - } + public void print() { + if (velocityData.size() == 0 || voltageData.size() == 0) { + return; + } - PolynomialRegression regression = - new PolynomialRegression( - velocityData.stream().mapToDouble(Double::doubleValue).toArray(), - voltageData.stream().mapToDouble(Double::doubleValue).toArray(), - 1); + PolynomialRegression regression = + new PolynomialRegression( + velocityData.stream().mapToDouble(Double::doubleValue).toArray(), + voltageData.stream().mapToDouble(Double::doubleValue).toArray(), + 1); - System.out.println("FF Characterization Results:"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); - } + System.out.println("FF Characterization Results:"); + System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); + System.out.println(String.format("\tR2=%.5f", regression.R2())); + System.out.println(String.format("\tkS=%.5f", regression.beta(0))); + System.out.println(String.format("\tkV=%.5f", regression.beta(1))); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java index 674cc08e..fcbccc60 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java @@ -10,35 +10,35 @@ import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory; public class AutoCommands { - private Drive drive; - private Superstructure superstructure; - - public AutoCommands(Drive drive, Superstructure superstructure) { - this.drive = drive; - this.superstructure = superstructure; - } - - private Command path(String pathName) { - HolonomicTrajectory trajectory = new HolonomicTrajectory(pathName); - - return startEnd( - () -> { - drive.setTrajectoryGoal(trajectory); - }, - () -> { - drive.clearTrajectoryGoal(); - }) - .until(() -> drive.isTrajectoryGoalCompleted()); - } - - private Command reset(String path) { - HolonomicTrajectory trajectory = new HolonomicTrajectory(path); - return runOnce( - () -> - RobotState.getInstance().resetPose(AllianceFlipUtil.apply(trajectory.getStartPose()))); - } - - public Command driveStraight() { - return reset("driveStraight").andThen(path("driveStraight")); - } + private Drive drive; + private Superstructure superstructure; + + public AutoCommands(Drive drive, Superstructure superstructure) { + this.drive = drive; + this.superstructure = superstructure; + } + + private Command path(String pathName) { + HolonomicTrajectory trajectory = new HolonomicTrajectory(pathName); + + return startEnd( + () -> { + drive.setTrajectoryGoal(trajectory); + }, + () -> { + drive.clearTrajectoryGoal(); + }) + .until(() -> drive.isTrajectoryGoalCompleted()); + } + + private Command reset(String path) { + HolonomicTrajectory trajectory = new HolonomicTrajectory(path); + return runOnce( + () -> + RobotState.getInstance().resetPose(AllianceFlipUtil.apply(trajectory.getStartPose()))); + } + + public Command driveStraight() { + return null; // reset("driveStraight").andThen(path("driveStraight")); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java index 87d75cc2..771187c2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java @@ -24,197 +24,197 @@ @ExtensionMethod({GeomUtil.class}) public class AprilTagVision extends VirtualSubsystem { - private final AprilTagVisionIO[] io; - private final AprilTagVisionIOInputs[] inputs; - - private final RobotState robotState = RobotState.getInstance(); - private boolean enableVisionUpdates = true; - private final Alert enableVisionUpdatesAlert = - new Alert("Vision updates are temporarily disabled.", Alert.AlertType.WARNING); - private final Map lastFrameTimes = new HashMap<>(); - private final Map lastTagDetectionTimes = new HashMap<>(); - - public AprilTagVision(AprilTagVisionIO... io) { - System.out.println("[Init] Creating AprilTagVision"); - this.io = io; - inputs = new AprilTagVisionIOInputs[io.length]; - for (int i = 0; i < io.length; i++) { - inputs[i] = new AprilTagVisionIOInputs(); - } - - // Create map of last frame times for instances - for (int i = 0; i < io.length; i++) { - lastFrameTimes.put(i, 0.0); - } + private final AprilTagVisionIO[] io; + private final AprilTagVisionIOInputs[] inputs; + + private final RobotState robotState = RobotState.getInstance(); + private boolean enableVisionUpdates = true; + private final Alert enableVisionUpdatesAlert = + new Alert("Vision updates are temporarily disabled.", Alert.AlertType.WARNING); + private final Map lastFrameTimes = new HashMap<>(); + private final Map lastTagDetectionTimes = new HashMap<>(); + + public AprilTagVision(AprilTagVisionIO... io) { + System.out.println("[Init] Creating AprilTagVision"); + this.io = io; + inputs = new AprilTagVisionIOInputs[io.length]; + for (int i = 0; i < io.length; i++) { + inputs[i] = new AprilTagVisionIOInputs(); + } - // Create map of last detection times for tags - FieldConstants.aprilTags - .getTags() - .forEach( - (AprilTag tag) -> { - lastTagDetectionTimes.put(tag.ID, 0.0); - }); + // Create map of last frame times for instances + for (int i = 0; i < io.length; i++) { + lastFrameTimes.put(i, 0.0); } - /** Sets whether vision updates for odometry are enabled. */ - public void setVisionUpdatesEnabled(boolean enabled) { - enableVisionUpdates = enabled; - enableVisionUpdatesAlert.set(!enabled); + // Create map of last detection times for tags + FieldConstants.aprilTags + .getTags() + .forEach( + (AprilTag tag) -> { + lastTagDetectionTimes.put(tag.ID, 0.0); + }); + } + + /** Sets whether vision updates for odometry are enabled. */ + public void setVisionUpdatesEnabled(boolean enabled) { + enableVisionUpdates = enabled; + enableVisionUpdatesAlert.set(!enabled); + } + + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]); } - public void periodic() { - for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[i]); - Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]); + // Loop over instances + List allRobotPoses = new ArrayList<>(); + List allRobotPoses3d = new ArrayList<>(); + List allVisionObservations = new ArrayList<>(); + for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { + // Loop over frames + for (int frameIndex = 0; frameIndex < inputs[instanceIndex].timestamps.length; frameIndex++) { + lastFrameTimes.put(instanceIndex, Timer.getFPGATimestamp()); + var timestamp = inputs[instanceIndex].timestamps[frameIndex]; + var values = inputs[instanceIndex].frames[frameIndex]; + + // Exit if blank frame + if (values.length == 0 || values[0] == 0) { + continue; } - // Loop over instances - List allRobotPoses = new ArrayList<>(); - List allRobotPoses3d = new ArrayList<>(); - List allVisionObservations = new ArrayList<>(); - for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { - // Loop over frames - for (int frameIndex = 0; frameIndex < inputs[instanceIndex].timestamps.length; frameIndex++) { - lastFrameTimes.put(instanceIndex, Timer.getFPGATimestamp()); - var timestamp = inputs[instanceIndex].timestamps[frameIndex]; - var values = inputs[instanceIndex].frames[frameIndex]; - - // Exit if blank frame - if (values.length == 0 || values[0] == 0) { - continue; - } - - // Switch based on number of poses - Pose3d cameraPose = null; - Pose3d robotPose3d = null; - switch ((int) values[0]) { - case 1: - // One pose (multi-tag), use directly - cameraPose = - new Pose3d( - values[2], - values[3], - values[4], - new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); - robotPose3d = - cameraPose.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); - break; - case 2: - // Two poses (one tag), disambiguate - double error0 = values[1]; - double error1 = values[9]; - Pose3d cameraPose0 = - new Pose3d( - values[2], - values[3], - values[4], - new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); - Pose3d cameraPose1 = - new Pose3d( - values[10], - values[11], - values[12], - new Rotation3d(new Quaternion(values[13], values[14], values[15], values[16]))); - Pose3d robotPose3d0 = - cameraPose0.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); - Pose3d robotPose3d1 = - cameraPose1.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); - - // Select pose using projection errors - if (error0 < error1 * ambiguityThreshold) { - cameraPose = cameraPose0; - robotPose3d = robotPose3d0; - } else if (error1 < error0 * ambiguityThreshold) { - cameraPose = cameraPose1; - robotPose3d = robotPose3d1; - } - break; - } - - // Exit if no data - if (cameraPose == null || robotPose3d == null) { - continue; - } - - // Exit if robot pose is off the field - if (robotPose3d.getX() < -fieldBorderMargin - || robotPose3d.getX() > FieldConstants.fieldLength + fieldBorderMargin - || robotPose3d.getY() < -fieldBorderMargin - || robotPose3d.getY() > FieldConstants.fieldWidth + fieldBorderMargin - || robotPose3d.getZ() < -zMargin - || robotPose3d.getZ() > zMargin) { - continue; - } - - // Get 2D robot pose - Pose2d robotPose = robotPose3d.toPose2d(); - - // Get tag poses and update last detection times - List tagPoses = new ArrayList<>(); - for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i++) { - int tagId = (int) values[i]; - lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); - Optional tagPose = FieldConstants.aprilTags.getTagPose((int) values[i]); - tagPose.ifPresent(tagPoses::add); - } - - // Calculate average distance to tag - double totalDistance = 0.0; - for (Pose3d tagPose : tagPoses) { - totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); - } - double avgDistance = totalDistance / tagPoses.size(); - - // Add observation to list - double xyStdDev = xyStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); - double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); - allVisionObservations.add( - new VisionObservation( - robotPose, timestamp, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); - allRobotPoses.add(robotPose); - allRobotPoses3d.add(robotPose3d); - - // Log data from instance - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/LatencySecs", - Timer.getFPGATimestamp() - timestamp); - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", robotPose); - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", robotPose3d); - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/TagPoses", tagPoses.toArray(Pose3d[]::new)); + // Switch based on number of poses + Pose3d cameraPose = null; + Pose3d robotPose3d = null; + switch ((int) values[0]) { + case 1: + // One pose (multi-tag), use directly + cameraPose = + new Pose3d( + values[2], + values[3], + values[4], + new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); + robotPose3d = + cameraPose.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); + break; + case 2: + // Two poses (one tag), disambiguate + double error0 = values[1]; + double error1 = values[9]; + Pose3d cameraPose0 = + new Pose3d( + values[2], + values[3], + values[4], + new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); + Pose3d cameraPose1 = + new Pose3d( + values[10], + values[11], + values[12], + new Rotation3d(new Quaternion(values[13], values[14], values[15], values[16]))); + Pose3d robotPose3d0 = + cameraPose0.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); + Pose3d robotPose3d1 = + cameraPose1.transformBy(cameraPoses[instanceIndex].toTransform3d().inverse()); + + // Select pose using projection errors + if (error0 < error1 * ambiguityThreshold) { + cameraPose = cameraPose0; + robotPose3d = robotPose3d0; + } else if (error1 < error0 * ambiguityThreshold) { + cameraPose = cameraPose1; + robotPose3d = robotPose3d1; } + break; + } - // If no frames from instances, clear robot pose - if (inputs[instanceIndex].timestamps.length == 0) { - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d()); - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d()); - } + // Exit if no data + if (cameraPose == null || robotPose3d == null) { + continue; + } - // If no recent frames from instance, clear tag poses - if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { - //noinspection RedundantArrayCreation - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {}); - } + // Exit if robot pose is off the field + if (robotPose3d.getX() < -fieldBorderMargin + || robotPose3d.getX() > FieldConstants.fieldLength + fieldBorderMargin + || robotPose3d.getY() < -fieldBorderMargin + || robotPose3d.getY() > FieldConstants.fieldWidth + fieldBorderMargin + || robotPose3d.getZ() < -zMargin + || robotPose3d.getZ() > zMargin) { + continue; } - // Log robot poses - Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); - Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new)); + // Get 2D robot pose + Pose2d robotPose = robotPose3d.toPose2d(); - // Log tag poses - List allTagPoses = new ArrayList<>(); - for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { - if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { - allTagPoses.add(FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()).get()); - } + // Get tag poses and update last detection times + List tagPoses = new ArrayList<>(); + for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i++) { + int tagId = (int) values[i]; + lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); + Optional tagPose = FieldConstants.aprilTags.getTagPose((int) values[i]); + tagPose.ifPresent(tagPoses::add); } - Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); - // Send results to robot state - if (enableVisionUpdates) { - allVisionObservations.stream() - .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) - .forEach(robotState::addVisionObservation); + // Calculate average distance to tag + double totalDistance = 0.0; + for (Pose3d tagPose : tagPoses) { + totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); } + double avgDistance = totalDistance / tagPoses.size(); + + // Add observation to list + double xyStdDev = xyStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); + double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); + allVisionObservations.add( + new VisionObservation( + robotPose, timestamp, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); + allRobotPoses.add(robotPose); + allRobotPoses3d.add(robotPose3d); + + // Log data from instance + Logger.recordOutput( + "AprilTagVision/Inst" + instanceIndex + "/LatencySecs", + Timer.getFPGATimestamp() - timestamp); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", robotPose); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", robotPose3d); + Logger.recordOutput( + "AprilTagVision/Inst" + instanceIndex + "/TagPoses", tagPoses.toArray(Pose3d[]::new)); + } + + // If no frames from instances, clear robot pose + if (inputs[instanceIndex].timestamps.length == 0) { + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d()); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d()); + } + + // If no recent frames from instance, clear tag poses + if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { + //noinspection RedundantArrayCreation + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {}); + } + } + + // Log robot poses + Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); + Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new)); + + // Log tag poses + List allTagPoses = new ArrayList<>(); + for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { + if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { + allTagPoses.add(FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()).get()); + } + } + Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); + + // Send results to robot state + if (enableVisionUpdates) { + allVisionObservations.stream() + .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) + .forEach(robotState::addVisionObservation); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionConstants.java index 541c74fa..c0f2fc37 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionConstants.java @@ -6,46 +6,46 @@ import org.littletonrobotics.frc2024.Constants; public class AprilTagVisionConstants { - public static final double ambiguityThreshold = 0.15; - public static final double targetLogTimeSecs = 0.1; - public static final double fieldBorderMargin = 0.5; - public static final double zMargin = 0.75; - public static final double xyStdDevCoefficient = 0.01; - public static final double thetaStdDevCoefficient = 0.01; + public static final double ambiguityThreshold = 0.15; + public static final double targetLogTimeSecs = 0.1; + public static final double fieldBorderMargin = 0.5; + public static final double zMargin = 0.75; + public static final double xyStdDevCoefficient = 0.01; + public static final double thetaStdDevCoefficient = 0.01; - public static final Pose3d[] cameraPoses = - switch (Constants.getRobot()) { - case DEVBOT -> - new Pose3d[] { - new Pose3d( - Units.inchesToMeters(9.735), - Units.inchesToMeters(9.974), - Units.inchesToMeters(8.837), - new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0) - .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)))), - new Pose3d( - Units.inchesToMeters(9.735), - Units.inchesToMeters(-9.974), - Units.inchesToMeters(8.837), - new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0) - .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(-30.0)))) - }; - default -> new Pose3d[] {}; + public static final Pose3d[] cameraPoses = + switch (Constants.getRobot()) { + case DEVBOT -> + new Pose3d[] { + new Pose3d( + Units.inchesToMeters(9.735), + Units.inchesToMeters(9.974), + Units.inchesToMeters(8.837), + new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0) + .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)))), + new Pose3d( + Units.inchesToMeters(9.735), + Units.inchesToMeters(-9.974), + Units.inchesToMeters(8.837), + new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0) + .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(-30.0)))) }; + default -> new Pose3d[] {}; + }; - public static final String[] instanceNames = - switch (Constants.getRobot()) { - case DEVBOT -> new String[] {"northstar_0", "northstar_1"}; - default -> new String[] {}; - }; + public static final String[] instanceNames = + switch (Constants.getRobot()) { + case DEVBOT -> new String[] {"northstar_0", "northstar_1"}; + default -> new String[] {}; + }; - public static final String[] cameraIds = - switch (Constants.getRobot()) { - case DEVBOT -> - new String[] { - "/dev/v4l/by-path/platform-fc800000.usb-usb-0:1:1.0-video-index0", - "/dev/v4l/by-path/platform-fc880000.usb-usb-0:1:1.0-video-index0" - }; - default -> new String[] {}; + public static final String[] cameraIds = + switch (Constants.getRobot()) { + case DEVBOT -> + new String[] { + "/dev/v4l/by-path/platform-fc800000.usb-usb-0:1:1.0-video-index0", + "/dev/v4l/by-path/platform-fc880000.usb-usb-0:1:1.0-video-index0" }; + default -> new String[] {}; + }; } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIO.java index c8c4b0ff..af994f4d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIO.java @@ -11,35 +11,35 @@ import org.littletonrobotics.junction.inputs.LoggableInputs; public interface AprilTagVisionIO { - class AprilTagVisionIOInputs implements LoggableInputs { - public double[] timestamps = new double[] {}; - public double[][] frames = new double[][] {}; - public double[] demoFrame = new double[] {}; - public long fps = 0; + class AprilTagVisionIOInputs implements LoggableInputs { + public double[] timestamps = new double[] {}; + public double[][] frames = new double[][] {}; + public double[] demoFrame = new double[] {}; + public long fps = 0; - @Override - public void toLog(LogTable table) { - table.put("Timestamps", timestamps); - table.put("FrameCount", frames.length); - for (int i = 0; i < frames.length; i++) { - table.put("Frame/" + i, frames[i]); - } - table.put("DemoFrame", demoFrame); - table.put("Fps", fps); - } + @Override + public void toLog(LogTable table) { + table.put("Timestamps", timestamps); + table.put("FrameCount", frames.length); + for (int i = 0; i < frames.length; i++) { + table.put("Frame/" + i, frames[i]); + } + table.put("DemoFrame", demoFrame); + table.put("Fps", fps); + } - @Override - public void fromLog(LogTable table) { - timestamps = table.get("Timestamps", new double[] {0.0}); - int frameCount = table.get("FrameCount", 0); - frames = new double[frameCount][]; - for (int i = 0; i < frameCount; i++) { - frames[i] = table.get("Frame/" + i, new double[] {}); - } - demoFrame = table.get("DemoFrame", new double[] {}); - fps = table.get("Fps", 0); - } + @Override + public void fromLog(LogTable table) { + timestamps = table.get("Timestamps", new double[] {0.0}); + int frameCount = table.get("FrameCount", 0); + frames = new double[frameCount][]; + for (int i = 0; i < frameCount; i++) { + frames[i] = table.get("Frame/" + i, new double[] {}); + } + demoFrame = table.get("DemoFrame", new double[] {}); + fps = table.get("Fps", 0); } + } - default void updateInputs(AprilTagVisionIOInputs inputs) {} + default void updateInputs(AprilTagVisionIOInputs inputs) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java index d18f18ec..1f8f4c1e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java @@ -18,78 +18,78 @@ import org.littletonrobotics.frc2024.util.Alert; public class AprilTagVisionIONorthstar implements AprilTagVisionIO { - private static final int cameraResolutionWidth = 1600; - private static final int cameraResolutionHeight = 1200; - private static final int cameraAutoExposure = 1; - private static final int cameraExposure = 10; - private static final int cameraGain = 25; + private static final int cameraResolutionWidth = 1600; + private static final int cameraResolutionHeight = 1200; + private static final int cameraAutoExposure = 1; + private static final int cameraExposure = 10; + private static final int cameraGain = 25; - private final DoubleArraySubscriber observationSubscriber; - private final DoubleArraySubscriber demoObservationSubscriber; - private final IntegerSubscriber fpsSubscriber; + private final DoubleArraySubscriber observationSubscriber; + private final DoubleArraySubscriber demoObservationSubscriber; + private final IntegerSubscriber fpsSubscriber; - private static final double disconnectedTimeout = 0.5; - private final Alert disconnectedAlert; - private final Timer disconnectedTimer = new Timer(); - private final String identifier; + private static final double disconnectedTimeout = 0.5; + private final Alert disconnectedAlert; + private final Timer disconnectedTimer = new Timer(); + private final String identifier; - public AprilTagVisionIONorthstar(String instanceId, String cameraId) { - this.identifier = instanceId; - System.out.println("[Init] Creating AprilTagVisionIONorthstar (" + instanceId + ")"); - var northstarTable = NetworkTableInstance.getDefault().getTable(instanceId); + public AprilTagVisionIONorthstar(String instanceId, String cameraId) { + this.identifier = instanceId; + System.out.println("[Init] Creating AprilTagVisionIONorthstar (" + instanceId + ")"); + var northstarTable = NetworkTableInstance.getDefault().getTable(instanceId); - var configTable = northstarTable.getSubTable("config"); - configTable.getStringTopic("camera_id").publish().set(cameraId); - configTable.getIntegerTopic("camera_resolution_width").publish().set(cameraResolutionWidth); - configTable.getIntegerTopic("camera_resolution_height").publish().set(cameraResolutionHeight); - configTable.getIntegerTopic("camera_auto_exposure").publish().set(cameraAutoExposure); - configTable.getIntegerTopic("camera_exposure").publish().set(cameraExposure); - configTable.getIntegerTopic("camera_gain").publish().set(cameraGain); - configTable.getDoubleTopic("fiducial_size_m").publish().set(FieldConstants.aprilTagWidth); - try { - configTable - .getStringTopic("tag_layout") - .publish() - .set(new ObjectMapper().writeValueAsString(FieldConstants.aprilTags)); - } catch (JsonProcessingException e) { - throw new RuntimeException("Failed to serialize AprilTag layout JSON for Northstar"); - } + var configTable = northstarTable.getSubTable("config"); + configTable.getStringTopic("camera_id").publish().set(cameraId); + configTable.getIntegerTopic("camera_resolution_width").publish().set(cameraResolutionWidth); + configTable.getIntegerTopic("camera_resolution_height").publish().set(cameraResolutionHeight); + configTable.getIntegerTopic("camera_auto_exposure").publish().set(cameraAutoExposure); + configTable.getIntegerTopic("camera_exposure").publish().set(cameraExposure); + configTable.getIntegerTopic("camera_gain").publish().set(cameraGain); + configTable.getDoubleTopic("fiducial_size_m").publish().set(FieldConstants.aprilTagWidth); + try { + configTable + .getStringTopic("tag_layout") + .publish() + .set(new ObjectMapper().writeValueAsString(FieldConstants.aprilTags)); + } catch (JsonProcessingException e) { + throw new RuntimeException("Failed to serialize AprilTag layout JSON for Northstar"); + } - var outputTable = northstarTable.getSubTable("output"); - observationSubscriber = - outputTable - .getDoubleArrayTopic("observations") - .subscribe( - new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); - demoObservationSubscriber = - outputTable - .getDoubleArrayTopic("demo_observations") - .subscribe( - new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); - fpsSubscriber = outputTable.getIntegerTopic("fps").subscribe(0); + var outputTable = northstarTable.getSubTable("output"); + observationSubscriber = + outputTable + .getDoubleArrayTopic("observations") + .subscribe( + new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); + demoObservationSubscriber = + outputTable + .getDoubleArrayTopic("demo_observations") + .subscribe( + new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); + fpsSubscriber = outputTable.getIntegerTopic("fps").subscribe(0); - disconnectedAlert = new Alert("No data from \"" + instanceId + "\"", Alert.AlertType.ERROR); - disconnectedTimer.start(); - } + disconnectedAlert = new Alert("No data from \"" + instanceId + "\"", Alert.AlertType.ERROR); + disconnectedTimer.start(); + } - public void updateInputs(AprilTagVisionIOInputs inputs) { - var queue = observationSubscriber.readQueue(); - inputs.timestamps = new double[queue.length]; - inputs.frames = new double[queue.length][]; - for (int i = 0; i < queue.length; i++) { - inputs.timestamps[i] = queue[i].timestamp / 1000000.0; - inputs.frames[i] = queue[i].value; - } - inputs.demoFrame = new double[] {}; - for (double[] demoFrame : demoObservationSubscriber.readQueueValues()) { - inputs.demoFrame = demoFrame; - } - inputs.fps = fpsSubscriber.get(); + public void updateInputs(AprilTagVisionIOInputs inputs) { + var queue = observationSubscriber.readQueue(); + inputs.timestamps = new double[queue.length]; + inputs.frames = new double[queue.length][]; + for (int i = 0; i < queue.length; i++) { + inputs.timestamps[i] = queue[i].timestamp / 1000000.0; + inputs.frames[i] = queue[i].value; + } + inputs.demoFrame = new double[] {}; + for (double[] demoFrame : demoObservationSubscriber.readQueueValues()) { + inputs.demoFrame = demoFrame; + } + inputs.fps = fpsSubscriber.get(); - // Update disconnected alert - if (queue.length > 0) { - disconnectedTimer.reset(); - } - disconnectedAlert.set(disconnectedTimer.hasElapsed(disconnectedTimeout)); + // Update disconnected alert + if (queue.length > 0) { + disconnectedTimer.reset(); } + disconnectedAlert.set(disconnectedTimer.hasElapsed(disconnectedTimeout)); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index bf66b29f..0819613a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -44,358 +44,358 @@ @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - private static final LoggedTunableNumber coastSpeedLimit = - new LoggedTunableNumber( - "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); - private static final LoggedTunableNumber coastDisableTime = - new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); - - public enum DriveMode { - /** Driving with input from driver joysticks. (Default) */ - TELEOP, - - /** Driving based on a trajectory. */ - TRAJECTORY, - - /** Driving to a location on the field automatically. */ - AUTO_ALIGN, - - /** Characterizing (modules oriented forwards, motor outputs supplied externally). */ - CHARACTERIZATION + private static final LoggedTunableNumber coastSpeedLimit = + new LoggedTunableNumber( + "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); + private static final LoggedTunableNumber coastDisableTime = + new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); + + public enum DriveMode { + /** Driving with input from driver joysticks. (Default) */ + TELEOP, + + /** Driving based on a trajectory. */ + TRAJECTORY, + + /** Driving to a location on the field automatically. */ + AUTO_ALIGN, + + /** Characterizing (modules oriented forwards, motor outputs supplied externally). */ + CHARACTERIZATION + } + + @AutoLog + public static class OdometryTimestampInputs { + public double[] timestamps = new double[] {}; + } + + public static final Lock odometryLock = new ReentrantLock(); + // TODO: DO THIS BETTER! + public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); + + private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimestampInputsAutoLogged(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; + + /** Active drive mode. */ + private DriveMode currentDriveMode = DriveMode.TELEOP; + + private double characterizationVolts = 0.0; + private boolean modulesOrienting = false; + private final Timer coastTimer = new Timer(); + private boolean brakeModeEnabled = true; + + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); + private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; + private SwerveSetpoint currentSetpoint = + new SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); + private SwerveSetpointGenerator setpointGenerator; + + private final TeleopDriveController teleopDriveController; + private TrajectoryController trajectoryController = null; + private AutoAlignController autoAlignController = null; + private AutoAimController autoAimController = null; + + public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + System.out.println("[Init] Creating Drive"); + this.gyroIO = gyroIO; + modules[0] = new Module(fl, 0); + modules[1] = new Module(fr, 1); + modules[2] = new Module(bl, 2); + modules[3] = new Module(br, 3); + + setpointGenerator = + SwerveSetpointGenerator.builder() + .kinematics(DriveConstants.kinematics) + .moduleLocations(DriveConstants.moduleTranslations) + .build(); + teleopDriveController = new TeleopDriveController(); + } + + public void periodic() { + // Update & process inputs + odometryLock.lock(); + // Read timestamps from odometry thread and fake sim timestamps + odometryTimestampInputs.timestamps = + timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + if (odometryTimestampInputs.timestamps.length == 0) { + odometryTimestampInputs.timestamps = new double[] {Timer.getFPGATimestamp()}; } - - @AutoLog - public static class OdometryTimestampInputs { - public double[] timestamps = new double[] {}; - } - - public static final Lock odometryLock = new ReentrantLock(); - // TODO: DO THIS BETTER! - public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); - - private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = - new OdometryTimestampInputsAutoLogged(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; - - /** Active drive mode. */ - private DriveMode currentDriveMode = DriveMode.TELEOP; - - private double characterizationVolts = 0.0; - private boolean modulesOrienting = false; - private final Timer coastTimer = new Timer(); - private boolean brakeModeEnabled = true; - - private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); - private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; - private SwerveSetpoint currentSetpoint = - new SwerveSetpoint( - new ChassisSpeeds(), - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }); - private SwerveSetpointGenerator setpointGenerator; - - private final TeleopDriveController teleopDriveController; - private TrajectoryController trajectoryController = null; - private AutoAlignController autoAlignController = null; - private AutoAimController autoAimController = null; - - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { - System.out.println("[Init] Creating Drive"); - this.gyroIO = gyroIO; - modules[0] = new Module(fl, 0); - modules[1] = new Module(fr, 1); - modules[2] = new Module(bl, 2); - modules[3] = new Module(br, 3); - - setpointGenerator = - SwerveSetpointGenerator.builder() - .kinematics(DriveConstants.kinematics) - .moduleLocations(DriveConstants.moduleTranslations) - .build(); - teleopDriveController = new TeleopDriveController(); + timestampQueue.clear(); + Logger.processInputs("Drive/OdometryTimestamps", odometryTimestampInputs); + // Read inputs from gyro + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + // Read inputs from modules + Arrays.stream(modules).forEach(Module::updateInputs); + odometryLock.unlock(); + + // Calculate the min odometry position updates across all modules + int minOdometryUpdates = + IntStream.of( + odometryTimestampInputs.timestamps.length, + Arrays.stream(modules) + .mapToInt(module -> module.getModulePositions().length) + .min() + .orElse(0)) + .min() + .orElse(0); + if (gyroInputs.connected) { + minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates); } - - public void periodic() { - // Update & process inputs - odometryLock.lock(); - // Read timestamps from odometry thread and fake sim timestamps - odometryTimestampInputs.timestamps = - timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); - if (odometryTimestampInputs.timestamps.length == 0) { - odometryTimestampInputs.timestamps = new double[] {Timer.getFPGATimestamp()}; - } - timestampQueue.clear(); - Logger.processInputs("Drive/OdometryTimestamps", odometryTimestampInputs); - // Read inputs from gyro - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); - // Read inputs from modules - Arrays.stream(modules).forEach(Module::updateInputs); - odometryLock.unlock(); - - // Calculate the min odometry position updates across all modules - int minOdometryUpdates = - IntStream.of( - odometryTimestampInputs.timestamps.length, - Arrays.stream(modules) - .mapToInt(module -> module.getModulePositions().length) - .min() - .orElse(0)) - .min() - .orElse(0); - if (gyroInputs.connected) { - minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates); - } - // Pass odometry data to robot state - for (int i = 0; i < minOdometryUpdates; i++) { - int odometryIndex = i; - Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; - // Get all four swerve module positions at that odometry update - // and store in SwerveDriveWheelPositions object - SwerveDriveWheelPositions wheelPositions = - new SwerveDriveWheelPositions( - Arrays.stream(modules) - .map(module -> module.getModulePositions()[odometryIndex]) - .toArray(SwerveModulePosition[]::new)); - // Add observation to robot state - RobotState.getInstance() - .addOdometryObservation( - new RobotState.OdometryObservation( - wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); - } - - // update current velocities use gyro when possible - ChassisSpeeds robotRelativeVelocity = getSpeeds(); - robotRelativeVelocity.omegaRadiansPerSecond = - gyroInputs.connected - ? gyroInputs.yawVelocityRadPerSec - : robotRelativeVelocity.omegaRadiansPerSecond; - RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); - - // Disabled, stop modules and coast - if (DriverStation.isDisabled()) { - Arrays.stream(modules).forEach(Module::stop); - if (Math.hypot( - robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) - <= coastSpeedLimit.get() - && brakeModeEnabled) { - setBrakeMode(false); - coastTimer.stop(); - coastTimer.reset(); - } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { - setBrakeMode(false); - coastTimer.stop(); - coastTimer.reset(); - } else { - coastTimer.start(); - } - return; - } else { - // Brake mode - setBrakeMode(true); - } - - // Run drive based on current mode - ChassisSpeeds teleopSpeeds = teleopDriveController.update(); - switch (currentDriveMode) { - case TELEOP -> { - // Plain teleop drive - desiredSpeeds = teleopSpeeds; - // Add auto aim if present - if (autoAimController != null) { - desiredSpeeds.omegaRadiansPerSecond = autoAimController.update(); - } - } - case TRAJECTORY -> { - // Run trajectory - desiredSpeeds = trajectoryController.update(); - } - case AUTO_ALIGN -> { - // Run auto align with drive input - desiredSpeeds = autoAlignController.update(); - desiredSpeeds.vxMetersPerSecond += teleopSpeeds.vxMetersPerSecond * 0.1; - desiredSpeeds.vyMetersPerSecond += teleopSpeeds.vyMetersPerSecond * 0.1; - desiredSpeeds.omegaRadiansPerSecond += teleopSpeeds.omegaRadiansPerSecond * 0.1; - } - case CHARACTERIZATION -> { - // run characterization - for (Module module : modules) { - module.runCharacterization(characterizationVolts); - } - } - default -> {} - } - - // Run robot at desiredSpeeds - // Generate feasible next setpoint - currentSetpoint = - setpointGenerator.generateSetpoint( - currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); - // run modules - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - for (int i = 0; i < modules.length; i++) { - // Optimize setpoints - optimizedSetpointStates[i] = - SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle()); - modules[i].runSetpoint(optimizedSetpointStates[i]); - } - // Log chassis speeds and swerve states - Logger.recordOutput( - "Drive/SwerveStates/Desired(b4 Poofs)", - DriveConstants.kinematics.toSwerveModuleStates(desiredSpeeds)); - Logger.recordOutput("Drive/SwerveStates/Setpoints", optimizedSetpointStates); - Logger.recordOutput("Drive/DesiredSpeeds", desiredSpeeds); - Logger.recordOutput("Drive/SetpointSpeeds", currentSetpoint.chassisSpeeds()); - Logger.recordOutput("Drive/DriveMode", currentDriveMode); + // Pass odometry data to robot state + for (int i = 0; i < minOdometryUpdates; i++) { + int odometryIndex = i; + Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; + // Get all four swerve module positions at that odometry update + // and store in SwerveDriveWheelPositions object + SwerveDriveWheelPositions wheelPositions = + new SwerveDriveWheelPositions( + Arrays.stream(modules) + .map(module -> module.getModulePositions()[odometryIndex]) + .toArray(SwerveModulePosition[]::new)); + // Add observation to robot state + RobotState.getInstance() + .addOdometryObservation( + new RobotState.OdometryObservation( + wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); } - /** Pass controller input into teleopDriveController in field relative input */ - public void setTeleopDriveGoal(double controllerX, double controllerY, double controllerOmega) { - if (DriverStation.isTeleopEnabled()) { - if (currentDriveMode != DriveMode.AUTO_ALIGN) { - currentDriveMode = DriveMode.TELEOP; - } - teleopDriveController.acceptDriveInput(controllerX, controllerY, controllerOmega); - } + // update current velocities use gyro when possible + ChassisSpeeds robotRelativeVelocity = getSpeeds(); + robotRelativeVelocity.omegaRadiansPerSecond = + gyroInputs.connected + ? gyroInputs.yawVelocityRadPerSec + : robotRelativeVelocity.omegaRadiansPerSecond; + RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + + // Disabled, stop modules and coast + if (DriverStation.isDisabled()) { + Arrays.stream(modules).forEach(Module::stop); + if (Math.hypot( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) + <= coastSpeedLimit.get() + && brakeModeEnabled) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else { + coastTimer.start(); + } + return; + } else { + // Brake mode + setBrakeMode(true); } - /** Sets the trajectory for the robot to follow. */ - public void setTrajectoryGoal(HolonomicTrajectory trajectory) { - if (DriverStation.isAutonomousEnabled()) { - currentDriveMode = DriveMode.TRAJECTORY; - trajectoryController = new TrajectoryController(trajectory); + // Run drive based on current mode + ChassisSpeeds teleopSpeeds = teleopDriveController.update(); + switch (currentDriveMode) { + case TELEOP -> { + // Plain teleop drive + desiredSpeeds = teleopSpeeds; + // Add auto aim if present + if (autoAimController != null) { + desiredSpeeds.omegaRadiansPerSecond = autoAimController.update(); } - } - - /** Clears the current trajectory goal. */ - public void clearTrajectoryGoal() { - trajectoryController = null; - currentDriveMode = DriveMode.TELEOP; - } - - /** Returns true if the robot is done with trajectory. */ - @AutoLogOutput(key = "Drive/TrajectoryCompleted") - public boolean isTrajectoryGoalCompleted() { - return trajectoryController != null && trajectoryController.isFinished(); - } - - /** Sets the goal pose for the robot to drive to */ - public void setAutoAlignGoal(Pose2d goalPose) { - if (DriverStation.isTeleopEnabled()) { - currentDriveMode = DriveMode.AUTO_ALIGN; - autoAlignController = new AutoAlignController(goalPose); + } + case TRAJECTORY -> { + // Run trajectory + desiredSpeeds = trajectoryController.update(); + } + case AUTO_ALIGN -> { + // Run auto align with drive input + desiredSpeeds = autoAlignController.update(); + desiredSpeeds.vxMetersPerSecond += teleopSpeeds.vxMetersPerSecond * 0.1; + desiredSpeeds.vyMetersPerSecond += teleopSpeeds.vyMetersPerSecond * 0.1; + desiredSpeeds.omegaRadiansPerSecond += teleopSpeeds.omegaRadiansPerSecond * 0.1; + } + case CHARACTERIZATION -> { + // run characterization + for (Module module : modules) { + module.runCharacterization(characterizationVolts); } + } + default -> {} } - /** Clears the current auto align goal. */ - public void clearAutoAlignGoal() { - autoAlignController = null; - currentDriveMode = DriveMode.TELEOP; - } - - /** Returns true if the robot is at current goal pose. */ - @AutoLogOutput(key = "Drive/AutoAlignCompleted") - public boolean isAutoAlignGoalCompleted() { - return autoAlignController != null && autoAlignController.atGoal(); - } - - /** Enable auto aiming on drive */ - public void setAutoAimGoal() { - autoAimController = new AutoAimController(); + // Run robot at desiredSpeeds + // Generate feasible next setpoint + currentSetpoint = + setpointGenerator.generateSetpoint( + currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); + // run modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < modules.length; i++) { + // Optimize setpoints + optimizedSetpointStates[i] = + SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle()); + modules[i].runSetpoint(optimizedSetpointStates[i]); } - - /** Disable auto aiming on drive */ - public void clearAutoAimGoal() { - autoAimController = null; - } - - /** Returns true if robot is aimed at speaker */ - @AutoLogOutput(key = "Drive/AutoAimCompleted") - public boolean isAutoAimGoalCompleted() { - return autoAimController != null && autoAimController.atSetpoint(); - } - - /** Runs forwards at the commanded voltage. */ - public void runCharacterizationVolts(double volts) { - currentDriveMode = DriveMode.CHARACTERIZATION; - characterizationVolts = volts; - } - - /** Disables the characterization mode. */ - public void endCharacterization() { + // Log chassis speeds and swerve states + Logger.recordOutput( + "Drive/SwerveStates/Desired(b4 Poofs)", + DriveConstants.kinematics.toSwerveModuleStates(desiredSpeeds)); + Logger.recordOutput("Drive/SwerveStates/Setpoints", optimizedSetpointStates); + Logger.recordOutput("Drive/DesiredSpeeds", desiredSpeeds); + Logger.recordOutput("Drive/SetpointSpeeds", currentSetpoint.chassisSpeeds()); + Logger.recordOutput("Drive/DriveMode", currentDriveMode); + } + + /** Pass controller input into teleopDriveController in field relative input */ + public void setTeleopDriveGoal(double controllerX, double controllerY, double controllerOmega) { + if (DriverStation.isTeleopEnabled()) { + if (currentDriveMode != DriveMode.AUTO_ALIGN) { currentDriveMode = DriveMode.TELEOP; + } + teleopDriveController.acceptDriveInput(controllerX, controllerY, controllerOmega); } + } - /** Returns the average drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - double driveVelocityAverage = 0.0; - for (var module : modules) { - driveVelocityAverage += module.getCharacterizationVelocity(); - } - return driveVelocityAverage / 4.0; - } - - /** Set brake mode enabled */ - public void setBrakeMode(boolean enabled) { - brakeModeEnabled = enabled; - Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); - } - - public Command orientModules(Rotation2d orientation) { - return orientModules(new Rotation2d[] {orientation, orientation, orientation, orientation}); + /** Sets the trajectory for the robot to follow. */ + public void setTrajectoryGoal(HolonomicTrajectory trajectory) { + if (DriverStation.isAutonomousEnabled()) { + currentDriveMode = DriveMode.TRAJECTORY; + trajectoryController = new TrajectoryController(trajectory); } - - public Command orientModules(Rotation2d[] orientations) { - return run(() -> { - for (int i = 0; i < orientations.length; i++) { - modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); - } - }) - .until( - () -> - Arrays.stream(modules) - .allMatch( - module -> - Math.abs( - module.getAngle().getDegrees() - - module.getSetpointState().angle.getDegrees()) - <= 2.0)) - .beforeStarting(() -> modulesOrienting = true) - .finallyDo(() -> modulesOrienting = false); + } + + /** Clears the current trajectory goal. */ + public void clearTrajectoryGoal() { + trajectoryController = null; + currentDriveMode = DriveMode.TELEOP; + } + + /** Returns true if the robot is done with trajectory. */ + @AutoLogOutput(key = "Drive/TrajectoryCompleted") + public boolean isTrajectoryGoalCompleted() { + return trajectoryController != null && trajectoryController.isFinished(); + } + + /** Sets the goal pose for the robot to drive to */ + public void setAutoAlignGoal(Pose2d goalPose) { + if (DriverStation.isTeleopEnabled()) { + currentDriveMode = DriveMode.AUTO_ALIGN; + autoAlignController = new AutoAlignController(goalPose); } - - @AutoLogOutput(key = "Drive/GyroYaw") - public Rotation2d getGyroYaw() { - return gyroInputs.connected - ? gyroInputs.yawPosition - : RobotState.getInstance().getEstimatedPose().getRotation(); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "Drive/SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - } - - /** Returns the measured speeds of the robot in the robot's frame of reference. */ - @AutoLogOutput(key = "Drive/MeasuredSpeeds") - private ChassisSpeeds getSpeeds() { - return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); - } - - public static Rotation2d[] getStraightOrientations() { - return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); - } - - public static Rotation2d[] getXOrientations() { - return Arrays.stream(DriveConstants.moduleTranslations) - .map(Translation2d::getAngle) - .toArray(Rotation2d[]::new); + } + + /** Clears the current auto align goal. */ + public void clearAutoAlignGoal() { + autoAlignController = null; + currentDriveMode = DriveMode.TELEOP; + } + + /** Returns true if the robot is at current goal pose. */ + @AutoLogOutput(key = "Drive/AutoAlignCompleted") + public boolean isAutoAlignGoalCompleted() { + return autoAlignController != null && autoAlignController.atGoal(); + } + + /** Enable auto aiming on drive */ + public void setAutoAimGoal() { + autoAimController = new AutoAimController(); + } + + /** Disable auto aiming on drive */ + public void clearAutoAimGoal() { + autoAimController = null; + } + + /** Returns true if robot is aimed at speaker */ + @AutoLogOutput(key = "Drive/AutoAimCompleted") + public boolean isAutoAimGoalCompleted() { + return autoAimController != null && autoAimController.atSetpoint(); + } + + /** Runs forwards at the commanded voltage. */ + public void runCharacterizationVolts(double volts) { + currentDriveMode = DriveMode.CHARACTERIZATION; + characterizationVolts = volts; + } + + /** Disables the characterization mode. */ + public void endCharacterization() { + currentDriveMode = DriveMode.TELEOP; + } + + /** Returns the average drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + double driveVelocityAverage = 0.0; + for (var module : modules) { + driveVelocityAverage += module.getCharacterizationVelocity(); } + return driveVelocityAverage / 4.0; + } + + /** Set brake mode enabled */ + public void setBrakeMode(boolean enabled) { + brakeModeEnabled = enabled; + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); + } + + public Command orientModules(Rotation2d orientation) { + return orientModules(new Rotation2d[] {orientation, orientation, orientation, orientation}); + } + + public Command orientModules(Rotation2d[] orientations) { + return run(() -> { + for (int i = 0; i < orientations.length; i++) { + modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); + } + }) + .until( + () -> + Arrays.stream(modules) + .allMatch( + module -> + Math.abs( + module.getAngle().getDegrees() + - module.getSetpointState().angle.getDegrees()) + <= 2.0)) + .beforeStarting(() -> modulesOrienting = true) + .finallyDo(() -> modulesOrienting = false); + } + + @AutoLogOutput(key = "Drive/GyroYaw") + public Rotation2d getGyroYaw() { + return gyroInputs.connected + ? gyroInputs.yawPosition + : RobotState.getInstance().getEstimatedPose().getRotation(); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "Drive/SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + } + + /** Returns the measured speeds of the robot in the robot's frame of reference. */ + @AutoLogOutput(key = "Drive/MeasuredSpeeds") + private ChassisSpeeds getSpeeds() { + return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + } + + public static Rotation2d[] getStraightOrientations() { + return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); + } + + public static Rotation2d[] getXOrientations() { + return Arrays.stream(DriveConstants.moduleTranslations) + .map(Translation2d::getAngle) + .toArray(Rotation2d[]::new); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java index 23478222..6ed6c0d8 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java @@ -13,231 +13,231 @@ /** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - // For Kraken - public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = false; - public static final boolean useMotionMagic = false; - public static final double motionMagicCruiseVelocity = 0.0; - public static final double motionMagicAcceleration = 0.0; - } - - // Drive Constants - public static DriveConfig driveConfig = - switch (Constants.getRobot()) { - case SIMBOT, COMPBOT -> - new DriveConfig( - Units.inchesToMeters(2.0), - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(13.05), - Units.feetToMeters(30.02), - 8.86, - 43.97); - case DEVBOT -> - new DriveConfig( - Units.inchesToMeters(2.0), - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(12.16), - Units.feetToMeters(21.32), - 7.93, - 29.89); - }; - public static final Translation2d[] moduleTranslations = - new Translation2d[] { - new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), - new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0), - new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), - new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) + // For Kraken + public static class KrakenDriveConstants { + public static final boolean useTorqueCurrentFOC = false; + public static final boolean useMotionMagic = false; + public static final double motionMagicCruiseVelocity = 0.0; + public static final double motionMagicAcceleration = 0.0; + } + + // Drive Constants + public static DriveConfig driveConfig = + switch (Constants.getRobot()) { + case SIMBOT, COMPBOT -> + new DriveConfig( + Units.inchesToMeters(2.0), + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(13.05), + Units.feetToMeters(30.02), + 8.86, + 43.97); + case DEVBOT -> + new DriveConfig( + Units.inchesToMeters(2.0), + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) + }; + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(moduleTranslations); + + // Odometry Constants + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case DEVBOT -> 100.0; + case COMPBOT -> 250.0; + }; + + public static final Matrix odometryStateStdDevs = + switch (Constants.getRobot()) { + default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); + }; + + // Module Constants + public static ModuleConfig[] moduleConfigs = + switch (Constants.getRobot()) { + case COMPBOT, DEVBOT -> + new ModuleConfig[] { + new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), + new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), + new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), + new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) }; - public static final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics(moduleTranslations); - - // Odometry Constants - public static final double odometryFrequency = - switch (Constants.getRobot()) { - case SIMBOT -> 50.0; - case DEVBOT -> 100.0; - case COMPBOT -> 250.0; - }; - - public static final Matrix odometryStateStdDevs = - switch (Constants.getRobot()) { - default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); - }; - - // Module Constants - public static ModuleConfig[] moduleConfigs = - switch (Constants.getRobot()) { - case COMPBOT, DEVBOT -> - new ModuleConfig[] { - new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), - new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), - new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), - new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) - }; - case SIMBOT -> { - ModuleConfig[] configs = new ModuleConfig[4]; - for (int i = 0; i < configs.length; i++) - configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); - yield configs; - } - }; - - public static ModuleConstants moduleConstants = - switch (Constants.getRobot()) { - case COMPBOT -> - new ModuleConstants( - 2.0, - 0.0, - 200.0, - 0.0, - 200.0, - 20.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - case DEVBOT -> - new ModuleConstants( - 0.1, - 0.13, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L2.reduction, - Mk4iReductions.TURN.reduction); - case SIMBOT -> - new ModuleConstants( - 0.014, - 0.134, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - }; - - public static ModuleLimits moduleLimits = - new ModuleLimits( - driveConfig.maxLinearVelocity(), - driveConfig.maxLinearVelocity() * 5, - Units.degreesToRadians(1080.0)); - - // Trajectory Following - public static TrajectoryConstants trajectoryConstants = - switch (Constants.getRobot()) { - case COMPBOT, DEVBOT -> - new TrajectoryConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - case SIMBOT -> - new TrajectoryConstants( - 4.0, - 0.0, - 2.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - }; - - // Auto Align - public static AutoAlignConstants autoAlignConstants = - new AutoAlignConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(2.0), - Units.degreesToRadians(2.0), - driveConfig.maxLinearVelocity(), - driveConfig.maxLinearAcceleration() * 0.5, - driveConfig.maxAngularVelocity() * 0.3, - driveConfig.maxAngularAcceleration() * 0.5); - - // Swerve Heading Control - public static HeadingControllerConstants headingControllerConstants = - switch (Constants.getRobot()) { - default -> new HeadingControllerConstants(5.0, 0.0); - }; - - public record DriveConfig( - double wheelRadius, - double trackwidthX, - double trackwidthY, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - public double driveBaseRadius() { - return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); + yield configs; } + }; + + public static ModuleConstants moduleConstants = + switch (Constants.getRobot()) { + case COMPBOT -> + new ModuleConstants( + 2.0, + 0.0, + 200.0, + 0.0, + 200.0, + 20.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + case DEVBOT -> + new ModuleConstants( + 0.1, + 0.13, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + case SIMBOT -> + new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + }; + + public static ModuleLimits moduleLimits = + new ModuleLimits( + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearVelocity() * 5, + Units.degreesToRadians(1080.0)); + + // Trajectory Following + public static TrajectoryConstants trajectoryConstants = + switch (Constants.getRobot()) { + case COMPBOT, DEVBOT -> + new TrajectoryConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + case SIMBOT -> + new TrajectoryConstants( + 4.0, + 0.0, + 2.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + }; + + // Auto Align + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + + // Swerve Heading Control + public static HeadingControllerConstants headingControllerConstants = + switch (Constants.getRobot()) { + default -> new HeadingControllerConstants(5.0, 0.0); + }; + + public record DriveConfig( + double wheelRadius, + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); } - - public record ModuleConfig( - int driveID, - int turnID, - int absoluteEncoderChannel, - Rotation2d absoluteEncoderOffset, - boolean turnMotorInverted) {} - - public record ModuleConstants( - double ffkS, - double ffkV, - double drivekP, - double drivekD, - double turnkP, - double turnkD, - double driveReduction, - double turnReduction) {} - - public record TrajectoryConstants( - double linearkP, - double linearkD, - double thetakP, - double thetakD, - double linearTolerance, - double thetaTolerance, - double goalLinearTolerance, - double goalThetaTolerance, - double linearVelocityTolerance, - double angularVelocityTolerance) {} - - public record AutoAlignConstants( - double linearkP, - double linearkD, - double thetakP, - double thetakD, - double linearTolerance, - double thetaTolerance, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) {} - - public record HeadingControllerConstants(double kP, double kD) {} - - private enum Mk4iReductions { - L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), - L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), - TURN((150.0 / 7.0)); - - final double reduction; - - Mk4iReductions(double reduction) { - this.reduction = reduction; - } + } + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) {} + + public record ModuleConstants( + double ffkS, + double ffkV, + double drivekP, + double drivekD, + double turnkP, + double turnkD, + double driveReduction, + double turnReduction) {} + + public record TrajectoryConstants( + double linearkP, + double linearkD, + double thetakP, + double thetakD, + double linearTolerance, + double thetaTolerance, + double goalLinearTolerance, + double goalThetaTolerance, + double linearVelocityTolerance, + double angularVelocityTolerance) {} + + public record AutoAlignConstants( + double linearkP, + double linearkD, + double thetakP, + double thetakD, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + + public record HeadingControllerConstants(double kP, double kD) {} + + private enum Mk4iReductions { + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + TURN((150.0 / 7.0)); + + final double reduction; + + Mk4iReductions(double reduction) { + this.reduction = reduction; } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIO.java index 01f670a9..c387f673 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIO.java @@ -17,13 +17,13 @@ import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog - class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; - } + @AutoLog + class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } - default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIOPigeon2.java index 40bb3690..1c3bca7d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/GyroIOPigeon2.java @@ -24,35 +24,35 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(0); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final Pigeon2 pigeon = new Pigeon2(0); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(DriveConstants.odometryFrequency); - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); - } else { - yawPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(() -> pigeon.getYaw().getValueAsDouble()); - } + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(DriveConstants.odometryFrequency); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); + } else { + yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> pigeon.getYaw().getValueAsDouble()); } + } - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - inputs.odometryYawPositions = - yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); - yawPositionQueue.clear(); - } + inputs.odometryYawPositions = + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); + yawPositionQueue.clear(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java index eba0b484..f8a45224 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -12,107 +12,107 @@ import org.littletonrobotics.junction.Logger; public class Module { - private static final LoggedTunableNumber drivekP = - new LoggedTunableNumber("Drive/Module/DrivekP", moduleConstants.drivekP()); - private static final LoggedTunableNumber drivekD = - new LoggedTunableNumber("Drive/Module/DrivekD", moduleConstants.drivekD()); - private static final LoggedTunableNumber drivekS = - new LoggedTunableNumber("Drive/Module/DrivekS", moduleConstants.ffkS()); - private static final LoggedTunableNumber drivekV = - new LoggedTunableNumber("Drive/Module/DrivekV", moduleConstants.ffkV()); - private static final LoggedTunableNumber turnkP = - new LoggedTunableNumber("Drive/Module/TurnkP", moduleConstants.turnkP()); - private static final LoggedTunableNumber turnkD = - new LoggedTunableNumber("Drive/Module/TurnkD", moduleConstants.turnkD()); - - private final int index; - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private SimpleMotorFeedforward driveFF = - new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0); - - @Getter private SwerveModuleState setpointState = new SwerveModuleState(); - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; + private static final LoggedTunableNumber drivekP = + new LoggedTunableNumber("Drive/Module/DrivekP", moduleConstants.drivekP()); + private static final LoggedTunableNumber drivekD = + new LoggedTunableNumber("Drive/Module/DrivekD", moduleConstants.drivekD()); + private static final LoggedTunableNumber drivekS = + new LoggedTunableNumber("Drive/Module/DrivekS", moduleConstants.ffkS()); + private static final LoggedTunableNumber drivekV = + new LoggedTunableNumber("Drive/Module/DrivekV", moduleConstants.ffkV()); + private static final LoggedTunableNumber turnkP = + new LoggedTunableNumber("Drive/Module/TurnkP", moduleConstants.turnkP()); + private static final LoggedTunableNumber turnkD = + new LoggedTunableNumber("Drive/Module/TurnkD", moduleConstants.turnkD()); + + private final int index; + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private SimpleMotorFeedforward driveFF = + new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0); + + @Getter private SwerveModuleState setpointState = new SwerveModuleState(); + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + } + + /** Called while blocking odometry thread */ + public void updateInputs() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + index, inputs); + + // Update FF and controllers + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + driveFF = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0); + io.setDriveFF(drivekS.get(), drivekV.get(), 0); + }, + drivekS, + drivekV); + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setDrivePID(drivekP.get(), 0, drivekD.get()), drivekP, drivekD); + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD); + } + + public void runSetpoint(SwerveModuleState setpoint) { + setpointState = setpoint; + io.setDriveVelocitySetpoint( + setpoint.speedMetersPerSecond / driveConfig.wheelRadius(), + driveFF.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius())); + io.setTurnPositionSetpoint(setpoint.angle.getRadians()); + } + + public void runCharacterization(double volts) { + io.setTurnPositionSetpoint(0.0); + io.setDriveVoltage(volts); + } + + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + public void stop() { + io.stop(); + } + + public SwerveModulePosition[] getModulePositions() { + int minOdometryPositions = + Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length); + SwerveModulePosition[] positions = new SwerveModulePosition[minOdometryPositions]; + for (int i = 0; i < minOdometryPositions; i++) { + positions[i] = + new SwerveModulePosition( + inputs.odometryDrivePositionsMeters[i], inputs.odometryTurnPositions[i]); } + return positions; + } - /** Called while blocking odometry thread */ - public void updateInputs() { - io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + index, inputs); - - // Update FF and controllers - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - driveFF = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0); - io.setDriveFF(drivekS.get(), drivekV.get(), 0); - }, - drivekS, - drivekV); - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setDrivePID(drivekP.get(), 0, drivekD.get()), drivekP, drivekD); - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD); - } - - public void runSetpoint(SwerveModuleState setpoint) { - setpointState = setpoint; - io.setDriveVelocitySetpoint( - setpoint.speedMetersPerSecond / driveConfig.wheelRadius(), - driveFF.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius())); - io.setTurnPositionSetpoint(setpoint.angle.getRadians()); - } - - public void runCharacterization(double volts) { - io.setTurnPositionSetpoint(0.0); - io.setDriveVoltage(volts); - } - - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - public void stop() { - io.stop(); - } + public Rotation2d getAngle() { + return inputs.turnAbsolutePosition; + } - public SwerveModulePosition[] getModulePositions() { - int minOdometryPositions = - Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length); - SwerveModulePosition[] positions = new SwerveModulePosition[minOdometryPositions]; - for (int i = 0; i < minOdometryPositions; i++) { - positions[i] = - new SwerveModulePosition( - inputs.odometryDrivePositionsMeters[i], inputs.odometryTurnPositions[i]); - } - return positions; - } + public double getPositionMeters() { + return inputs.drivePositionRad * driveConfig.wheelRadius(); + } - public Rotation2d getAngle() { - return inputs.turnAbsolutePosition; - } + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * driveConfig.wheelRadius(); + } - public double getPositionMeters() { - return inputs.drivePositionRad * driveConfig.wheelRadius(); - } + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * driveConfig.wheelRadius(); - } + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java index bb6b3b95..f0625734 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java @@ -17,55 +17,55 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double driveSupplyCurrentAmps = 0.0; - public double driveTorqueCurrentAmps = 0.0; + @AutoLog + class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTorqueCurrentAmps = 0.0; - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double turnSupplyCurrentAmps = 0.0; - public double turnTorqueCurrentAmps = 0.0; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTorqueCurrentAmps = 0.0; - public double[] odometryDrivePositionsMeters = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } + public double[] odometryDrivePositionsMeters = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } - /** Updates the set of loggable inputs. */ - default void updateInputs(ModuleIOInputs inputs) {} + /** Updates the set of loggable inputs. */ + default void updateInputs(ModuleIOInputs inputs) {} - /** Run drive motor at volts */ - default void setDriveVoltage(double volts) {} + /** Run drive motor at volts */ + default void setDriveVoltage(double volts) {} - /** Run turn motor at volts */ - default void setTurnVoltage(double volts) {} + /** Run turn motor at volts */ + default void setTurnVoltage(double volts) {} - /** Set drive velocity setpoint */ - default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} + /** Set drive velocity setpoint */ + default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} - /** Set turn position setpoint */ - default void setTurnPositionSetpoint(double angleRads) {} + /** Set turn position setpoint */ + default void setTurnPositionSetpoint(double angleRads) {} - /** Configure drive PID */ - default void setDrivePID(double kP, double kI, double kD) {} + /** Configure drive PID */ + default void setDrivePID(double kP, double kI, double kD) {} - /** Configure turn PID */ - default void setTurnPID(double kP, double kI, double kD) {} + /** Configure turn PID */ + default void setTurnPID(double kP, double kI, double kD) {} - /** Configure drive feedforward for TorqueCurrentFOC */ - default void setDriveFF(double kS, double kV, double kA) {} + /** Configure drive feedforward for TorqueCurrentFOC */ + default void setDriveFF(double kS, double kV, double kA) {} - /** Enable or disable brake mode on the drive motor. */ - default void setDriveBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the drive motor. */ + default void setDriveBrakeMode(boolean enable) {} - /** Enable or disable brake mode on the turn motor. */ - default void setTurnBrakeMode(boolean enable) {} + /** Enable or disable brake mode on the turn motor. */ + default void setTurnBrakeMode(boolean enable) {} - /** Disable output to all motors */ - default void stop() {} + /** Disable output to all motors */ + default void stop() {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java index 543575c3..eb214506 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -19,245 +19,245 @@ import java.util.function.Supplier; public class ModuleIOKrakenFOC implements ModuleIO { - // Hardware - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final AnalogInput turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; + // Hardware + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; - // Status Signals - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveSupplyCurrent; - private final StatusSignal driveTorqueCurrent; + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; - private final StatusSignal turnPosition; - private final Supplier turnAbsolutePosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnSupplyCurrent; - private final StatusSignal turnTorqueCurrent; + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; - private static final int shouldResetCounts = 100; - private int resetCounter = shouldResetCounts; + private static final int shouldResetCounts = 100; + private int resetCounter = shouldResetCounts; - private Slot0Configs driveFeedbackConfig = new Slot0Configs(); - private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + private Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private Slot0Configs turnFeedbackConfig = new Slot0Configs(); - public ModuleIOKrakenFOC(ModuleConfig config) { - // Init controllers and encoders from config constants - driveTalon = new TalonFX(config.driveID(), "canivore"); - turnTalon = new TalonFX(config.turnID(), "canivore"); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); + public ModuleIOKrakenFOC(ModuleConfig config) { + // Init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID(), "canivore"); + turnTalon = new TalonFX(config.turnID(), "canivore"); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); - // Config Motors - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveConfig.Voltage.PeakForwardVoltage = 12.0; - driveConfig.Voltage.PeakReverseVoltage = -12.0; + // Config Motors + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnConfig.Voltage.PeakForwardVoltage = 12.0; - turnConfig.Voltage.PeakReverseVoltage = -12.0; - turnConfig.MotorOutput.Inverted = - config.turnMotorInverted() - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + turnConfig.MotorOutput.Inverted = + config.turnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; - // If in motoControl mode, set reference points in rotations convert from radians - // Affects getPosition() and getVelocity() - driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); - turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + // If in motoControl mode, set reference points in rotations convert from radians + // Affects getPosition() and getVelocity() + driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - // Config Motion Magic - if (KrakenDriveConstants.useMotionMagic) { - turnConfig.MotionMagic.MotionMagicCruiseVelocity = - KrakenDriveConstants.motionMagicCruiseVelocity; - turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; - } + // Config Motion Magic + if (KrakenDriveConstants.useMotionMagic) { + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + KrakenDriveConstants.motionMagicCruiseVelocity; + turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; + } + + // Apply configs + for (int i = 0; i < 4; i++) { + boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; + setDriveBrakeMode(true); + error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); + setTurnBrakeMode(true); + if (!error) break; + } - // Apply configs - for (int i = 0; i < 4; i++) { - boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; - setDriveBrakeMode(true); - error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); - setTurnBrakeMode(true); - if (!error) break; - } + // Get signals and set update rate + // 100hz signals + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTorqueCurrent = driveTalon.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTorqueCurrent = turnTalon.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); - // Get signals and set update rate - // 100hz signals - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveSupplyCurrent = driveTalon.getSupplyCurrent(); - driveTorqueCurrent = driveTalon.getTorqueCurrent(); - turnAbsolutePosition = - () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) - .minus(absoluteEncoderOffset); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnSupplyCurrent = turnTalon.getSupplyCurrent(); - turnTorqueCurrent = turnTalon.getTorqueCurrent(); - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - driveVelocity, - driveAppliedVolts, - driveSupplyCurrent, - driveTorqueCurrent, - turnVelocity, - turnAppliedVolts, - turnSupplyCurrent, - turnTorqueCurrent); + // 250hz signals + drivePosition = driveTalon.getPosition(); + turnPosition = turnTalon.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + } - // 250hz signals - drivePosition = driveTalon.getPosition(); - turnPosition = turnTalon.getPosition(); - BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCounts + && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); + resetCounter = 0; } - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCounts - && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { - turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); - resetCounter = 0; - } + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveSupplyCurrent, - driveTorqueCurrent, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnSupplyCurrent, - turnTorqueCurrent); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTorqueCurrentAmps = driveTorqueCurrent.getValueAsDouble(); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); - inputs.driveTorqueCurrentAmps = driveTorqueCurrent.getValueAsDouble(); + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTorqueCurrentAmps = turnTorqueCurrent.getValueAsDouble(); - inputs.turnAbsolutePosition = turnAbsolutePosition.get(); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); - inputs.turnTorqueCurrentAmps = turnTorqueCurrent.getValueAsDouble(); + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius()) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius()) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); - } + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); + } else { + driveTalon.setControl( + new VelocityVoltage(velocityRotationsPerSec) + .withFeedForward(ffVolts) + .withEnableFOC(true)); } + } - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); - } else { - driveTalon.setControl( - new VelocityVoltage(velocityRotationsPerSec) - .withFeedForward(ffVolts) - .withEnableFOC(true)); - } + @Override + public void setTurnPositionSetpoint(double angleRads) { + double angleRotations = Units.radiansToRotations(angleRads); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); + } else { + turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); + } + } else { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); + } else { + turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); + } } + } - @Override - public void setTurnPositionSetpoint(double angleRads) { - double angleRotations = Units.radiansToRotations(angleRads); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); - } else { - turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); - } - } else { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); - } else { - turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); - } - } - } + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedbackConfig.kP = kP; + driveFeedbackConfig.kI = kI; + driveFeedbackConfig.kD = kD; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveFeedbackConfig.kP = kP; - driveFeedbackConfig.kI = kI; - driveFeedbackConfig.kD = kD; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); - } + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedbackConfig.kP = kP; + turnFeedbackConfig.kI = kI; + turnFeedbackConfig.kD = kD; + turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + } - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnFeedbackConfig.kP = kP; - turnFeedbackConfig.kI = kI; - turnFeedbackConfig.kD = kD; - turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); - } - - @Override - public void setDriveFF(double kS, double kV, double kA) { - driveFeedbackConfig.kS = kS; - driveFeedbackConfig.kV = kV; - driveFeedbackConfig.kA = kA; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); - } + @Override + public void setDriveFF(double kS, double kV, double kA) { + driveFeedbackConfig.kS = kS; + driveFeedbackConfig.kV = kV; + driveFeedbackConfig.kA = kA; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } - @Override - public void setDriveBrakeMode(boolean enable) { - driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } + @Override + public void setDriveBrakeMode(boolean enable) { + driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } - @Override - public void setTurnBrakeMode(boolean enable) { - turnTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } + @Override + public void setTurnBrakeMode(boolean enable) { + turnTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } - @Override - public void stop() { - driveTalon.setControl(new NeutralOut()); - turnTalon.setControl(new NeutralOut()); - } + @Override + public void stop() { + driveTalon.setControl(new NeutralOut()); + turnTalon.setControl(new NeutralOut()); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index 1268002e..2c339eae 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -23,81 +23,81 @@ public class ModuleIOSim implements ModuleIO { - private final DCMotorSim driveSim = - new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); - private final DCMotorSim turnSim = - new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.turnReduction(), 0.004); - - private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); - private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); - - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; - private final Rotation2d turnAbsoluteInitPosition; - - public ModuleIOSim(ModuleConfig config) { - turnAbsoluteInitPosition = config.absoluteEncoderOffset(); - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(0.02); - turnSim.update(0.02); - - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); - - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); - - inputs.odometryDrivePositionsMeters = - new double[] {driveSim.getAngularPositionRad() * driveConfig.wheelRadius()}; - inputs.odometryTurnPositions = - new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())}; - } - - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveSim.setInputVoltage(driveAppliedVolts); - } - - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - turnSim.setInputVoltage(turnAppliedVolts); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - setDriveVoltage( - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) - + ffVolts); - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveFeedback.setPID(kP, kI, kD); - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnFeedback.setPID(kP, kI, kD); - } - - @Override - public void stop() { - driveSim.setInputVoltage(0.0); - turnSim.setInputVoltage(0.0); - } + private final DCMotorSim driveSim = + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); + private final DCMotorSim turnSim = + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.turnReduction(), 0.004); + + private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); + private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); + + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + private final Rotation2d turnAbsoluteInitPosition; + + public ModuleIOSim(ModuleConfig config) { + turnAbsoluteInitPosition = config.absoluteEncoderOffset(); + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(0.02); + turnSim.update(0.02); + + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + inputs.odometryDrivePositionsMeters = + new double[] {driveSim.getAngularPositionRad() * driveConfig.wheelRadius()}; + inputs.odometryTurnPositions = + new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())}; + } + + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + setDriveVoltage( + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) + + ffVolts); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedback.setPID(kP, kI, kD); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedback.setPID(kP, kI, kD); + } + + @Override + public void stop() { + driveSim.setInputVoltage(0.0); + turnSim.setInputVoltage(0.0); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java index 00103456..0173272c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java @@ -27,165 +27,165 @@ import java.util.function.Supplier; public class ModuleIOSparkMax implements ModuleIO { - // Hardware - private final CANSparkMax driveMotor; - private final CANSparkMax turnMotor; - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - - // Controllers - private final PIDController driveController; - private final PIDController turnController; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final Rotation2d absoluteEncoderOffset; - private final Supplier absoluteEncoderValue; - private static final int shouldResetCount = 100; - private int resetCounter = shouldResetCount; - - public ModuleIOSparkMax(ModuleConfig config) { - // Init motor & encoder objects - driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); - turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - driveEncoder = driveMotor.getEncoder(); - turnRelativeEncoder = turnMotor.getEncoder(); - - driveMotor.restoreFactoryDefaults(); - turnMotor.restoreFactoryDefaults(); - driveMotor.setCANTimeout(250); - turnMotor.setCANTimeout(250); - - for (int i = 0; i < 30; i++) { - turnMotor.setInverted(config.turnMotorInverted()); - driveMotor.setSmartCurrentLimit(40); - turnMotor.setSmartCurrentLimit(30); - driveMotor.enableVoltageCompensation(12.0); - turnMotor.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - } - - driveMotor.burnFlash(); - turnMotor.burnFlash(); - - driveMotor.setCANTimeout(0); - turnMotor.setCANTimeout(0); - - absoluteEncoderValue = - () -> - Rotation2d.fromRotations( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) - .minus(absoluteEncoderOffset); - - drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(() -> absoluteEncoderValue.get().getRadians()); - - // Init Controllers - driveController = new PIDController(moduleConstants.drivekP(), 0.0, moduleConstants.drivekD()); - turnController = new PIDController(moduleConstants.turnkP(), 0.0, moduleConstants.turnkD()); - turnController.enableContinuousInput(-Math.PI, Math.PI); + // Hardware + private final CANSparkMax driveMotor; + private final CANSparkMax turnMotor; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + + // Controllers + private final PIDController driveController; + private final PIDController turnController; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final Rotation2d absoluteEncoderOffset; + private final Supplier absoluteEncoderValue; + private static final int shouldResetCount = 100; + private int resetCounter = shouldResetCount; + + public ModuleIOSparkMax(ModuleConfig config) { + // Init motor & encoder objects + driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); + turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + driveEncoder = driveMotor.getEncoder(); + turnRelativeEncoder = turnMotor.getEncoder(); + + driveMotor.restoreFactoryDefaults(); + turnMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(250); + turnMotor.setCANTimeout(250); + + for (int i = 0; i < 30; i++) { + turnMotor.setInverted(config.turnMotorInverted()); + driveMotor.setSmartCurrentLimit(40); + turnMotor.setSmartCurrentLimit(30); + driveMotor.enableVoltageCompensation(12.0); + turnMotor.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); } - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - driveEncoder.getVelocity() / moduleConstants.driveReduction()); - inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); - - inputs.turnAbsolutePosition = absoluteEncoderValue.get(); - inputs.turnPosition = - Rotation2d.fromRadians( - Units.rotationsToRadians( - turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); - inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); - inputs.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - motorPositionRevs -> - Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) - * driveConfig.wheelRadius()) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnMotor.setVoltage(volts); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - double feedback = - driveController.calculate( - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) - / moduleConstants.driveReduction(), - velocityRadsPerSec); - setDriveVoltage(feedback + ffVolts); - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage(turnController.calculate(absoluteEncoderValue.get().getRadians(), angleRads)); - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveController.setPID(kP, kI, kD); - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnController.setPID(kP, kI, kD); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void stop() { - driveMotor.stopMotor(); - turnMotor.stopMotor(); - } + driveMotor.burnFlash(); + turnMotor.burnFlash(); + + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); + + absoluteEncoderValue = + () -> + Rotation2d.fromRotations( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) + .minus(absoluteEncoderOffset); + + drivePositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + + // Init Controllers + driveController = new PIDController(moduleConstants.drivekP(), 0.0, moduleConstants.drivekD()); + turnController = new PIDController(moduleConstants.turnkP(), 0.0, moduleConstants.turnkD()); + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() / moduleConstants.driveReduction()); + inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); + inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); + + inputs.turnAbsolutePosition = absoluteEncoderValue.get(); + inputs.turnPosition = + Rotation2d.fromRadians( + Units.rotationsToRadians( + turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); + inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); + inputs.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + motorPositionRevs -> + Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) + * driveConfig.wheelRadius()) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnMotor.setVoltage(volts); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double feedback = + driveController.calculate( + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) + / moduleConstants.driveReduction(), + velocityRadsPerSec); + setDriveVoltage(feedback + ffVolts); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + setTurnVoltage(turnController.calculate(absoluteEncoderValue.get().getRadians(), angleRads)); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveController.setPID(kP, kI, kD); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnController.setPID(kP, kI, kD); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void stop() { + driveMotor.stopMotor(); + turnMotor.stopMotor(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/PhoenixOdometryThread.java index bfbe2044..40eb75ab 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/PhoenixOdometryThread.java @@ -31,74 +31,74 @@ * time synchronization. */ public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = - new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private boolean isCANFD = false; + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private boolean isCANFD = false; - private static PhoenixOdometryThread instance = null; + private static PhoenixOdometryThread instance = null; - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); } + return instance; + } - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - start(); - } + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + start(); + } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayDeque<>(100); - signalsLock.lock(); - Drive.odometryLock.lock(); - try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - } finally { - signalsLock.unlock(); - Drive.odometryLock.unlock(); - } - return queue; + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + Queue queue = new ArrayDeque<>(100); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); } + return queue; + } - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / DriveConstants.odometryFrequency, signals); - } else { - Thread.sleep((long) (1000.0 / DriveConstants.odometryFrequency)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } - double fpgaTimestamp = Logger.getRealTimestamp() / 1.0e6; + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll(2.0 / DriveConstants.odometryFrequency, signals); + } else { + Thread.sleep((long) (1000.0 / DriveConstants.odometryFrequency)); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + double fpgaTimestamp = Logger.getRealTimestamp() / 1.0e6; - // Save new data to queues - Drive.odometryLock.lock(); - try { - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - Drive.timestampQueue.offer(fpgaTimestamp); - } finally { - Drive.odometryLock.unlock(); - } + // Save new data to queues + Drive.odometryLock.lock(); + try { + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); } + Drive.timestampQueue.offer(fpgaTimestamp); + } finally { + Drive.odometryLock.unlock(); + } } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/SparkMaxOdometryThread.java index e4a05c77..07e3586d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/SparkMaxOdometryThread.java @@ -28,45 +28,45 @@ * blocking thread. A Notifier thread is used to gather samples with consistent timing. */ public class SparkMaxOdometryThread { - private List signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; + private List signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); } + return instance; + } - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); - } + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); + } - public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(100); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(100); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); } + return queue; + } - private void periodic() { - Drive.odometryLock.lock(); - Drive.timestampQueue.offer(Logger.getRealTimestamp() / 1.0e6); - try { - for (int i = 0; i < signals.size(); i++) { - queues.get(i).offer(signals.get(i).getAsDouble()); - } - } finally { - Drive.odometryLock.unlock(); - } + private void periodic() { + Drive.odometryLock.lock(); + Drive.timestampQueue.offer(Logger.getRealTimestamp() / 1.0e6); + try { + for (int i = 0; i < signals.size(); i++) { + queues.get(i).offer(signals.get(i).getAsDouble()); + } + } finally { + Drive.odometryLock.unlock(); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java index 7977d021..09c68c4c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java @@ -9,38 +9,38 @@ import org.littletonrobotics.junction.Logger; public class AutoAimController { - private static final LoggedTunableNumber headingkP = - new LoggedTunableNumber("AutoAim/kP", DriveConstants.headingControllerConstants.kP()); - private static final LoggedTunableNumber headingkD = - new LoggedTunableNumber("AutoAim/kD", DriveConstants.headingControllerConstants.kD()); - private static final LoggedTunableNumber tolerance = - new LoggedTunableNumber("AutoAim/ToleranceDegrees", 4.0); - - private PIDController headingController; - - public AutoAimController() { - headingController = new PIDController(0, 0, 0, 0.02); - headingController.enableContinuousInput(-Math.PI, Math.PI); - } - - /** Returns the rotation rate to turn to aim at speaker */ - public double update() { - headingController.setPID(headingkP.get(), 0, headingkD.get()); - headingController.setTolerance(Units.degreesToRadians(tolerance.get())); - - var aimingParams = RobotState.getInstance().getAimingParameters(); - double output = - headingController.calculate( - RobotState.getInstance().getEstimatedPose().getRotation().getRadians(), - aimingParams.driveHeading().getRadians()); - - Logger.recordOutput("AutoAim/HeadingError", headingController.getPositionError()); - return output; - } - - /** Returns true if within tolerance of aiming at speaker */ - @AutoLogOutput(key = "AutoAim/AtSetpoint") - public boolean atSetpoint() { - return headingController.atSetpoint(); - } + private static final LoggedTunableNumber headingkP = + new LoggedTunableNumber("AutoAim/kP", DriveConstants.headingControllerConstants.kP()); + private static final LoggedTunableNumber headingkD = + new LoggedTunableNumber("AutoAim/kD", DriveConstants.headingControllerConstants.kD()); + private static final LoggedTunableNumber tolerance = + new LoggedTunableNumber("AutoAim/ToleranceDegrees", 4.0); + + private PIDController headingController; + + public AutoAimController() { + headingController = new PIDController(0, 0, 0, 0.02); + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** Returns the rotation rate to turn to aim at speaker */ + public double update() { + headingController.setPID(headingkP.get(), 0, headingkD.get()); + headingController.setTolerance(Units.degreesToRadians(tolerance.get())); + + var aimingParams = RobotState.getInstance().getAimingParameters(); + double output = + headingController.calculate( + RobotState.getInstance().getEstimatedPose().getRotation().getRadians(), + aimingParams.driveHeading().getRadians()); + + Logger.recordOutput("AutoAim/HeadingError", headingController.getPositionError()); + return output; + } + + /** Returns true if within tolerance of aiming at speaker */ + @AutoLogOutput(key = "AutoAim/AtSetpoint") + public boolean atSetpoint() { + return headingController.atSetpoint(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java index c78d321f..1bbd8586 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java @@ -15,144 +15,144 @@ import org.littletonrobotics.junction.Logger; public class AutoAlignController { - private static LoggedTunableNumber linearkP = - new LoggedTunableNumber("AutoAlign/drivekP", DriveConstants.autoAlignConstants.linearkP()); - private static LoggedTunableNumber linearkD = - new LoggedTunableNumber("AutoAlign/drivekD", DriveConstants.autoAlignConstants.linearkD()); - private static LoggedTunableNumber thetakP = - new LoggedTunableNumber("AutoAlign/thetakP", DriveConstants.autoAlignConstants.thetakP()); - private static LoggedTunableNumber thetakD = - new LoggedTunableNumber("AutoAlign/thetakD", DriveConstants.autoAlignConstants.thetakD()); - private static LoggedTunableNumber linearTolerance = - new LoggedTunableNumber( - "AutoAlign/controllerLinearTolerance", - DriveConstants.autoAlignConstants.linearTolerance()); - private static LoggedTunableNumber thetaTolerance = - new LoggedTunableNumber( - "AutoAlign/controllerThetaTolerance", DriveConstants.autoAlignConstants.thetaTolerance()); - private static LoggedTunableNumber maxLinearVelocity = - new LoggedTunableNumber( - "AutoAlign/maxLinearVelocity", DriveConstants.autoAlignConstants.maxLinearVelocity()); - private static LoggedTunableNumber maxLinearAcceleration = - new LoggedTunableNumber( - "AutoAlign/maxLinearAcceleration", - DriveConstants.autoAlignConstants.maxLinearAcceleration()); - private static LoggedTunableNumber maxAngularVelocity = - new LoggedTunableNumber( - "AutoAlign/maxAngularVelocity", DriveConstants.autoAlignConstants.maxAngularVelocity()); - private static LoggedTunableNumber maxAngularAcceleration = - new LoggedTunableNumber( - "AutoAlign/maxAngularAcceleration", - DriveConstants.autoAlignConstants.maxAngularAcceleration()); - - @Getter(onMethod_ = @AutoLogOutput(key = "AutoAlign/goalPose")) - private Pose2d goalPose = null; - - // Controllers for translation and rotation - private final ProfiledPIDController linearController; - private final ProfiledPIDController thetaController; - - // Store previous velocities for acceleration limiting - private Translation2d prevLinearVelocity; - - public AutoAlignController(Pose2d goalPose) { - this.goalPose = goalPose; - // Set up both controllers - linearController = - new ProfiledPIDController( - linearkP.get(), - 0, - linearkD.get(), - new TrapezoidProfile.Constraints(maxLinearVelocity.get(), maxLinearAcceleration.get())); - linearController.setTolerance(linearTolerance.get()); - thetaController = - new ProfiledPIDController( - thetakP.get(), - 0, - thetakD.get(), - new TrapezoidProfile.Constraints( - maxAngularVelocity.get(), maxAngularAcceleration.get())); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaController.setTolerance(thetaTolerance.get()); - - // Reset measurements and velocities - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); - // Linear controller will control to 0 so distance is the measurement - Rotation2d rotationToGoal = - goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); - double velocity = - -new Translation2d(fieldVelocity.dx, fieldVelocity.dy) - .rotateBy(rotationToGoal.unaryMinus()) - .getX(); - linearController.reset( - currentPose.getTranslation().getDistance(goalPose.getTranslation()), velocity); - thetaController.reset(currentPose.getRotation().getRadians(), fieldVelocity.dtheta); - - // Set goal positions - linearController.setGoal(0.0); - thetaController.setGoal(goalPose.getRotation().getRadians()); - - // Store linear velocity for acceleration limiting - prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); - - // Log goal pose - Logger.recordOutput("AutoAlign/GoalPose", goalPose); - } - - public ChassisSpeeds update() { - // Update PID Controllers - LoggedTunableNumber.ifChanged( - hashCode(), - () -> linearController.setPID(linearkP.get(), 0, linearkD.get()), - linearkP, - linearkD); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> thetaController.setPID(thetakP.get(), 0, thetakD.get()), - thetakP, - thetakD); - LoggedTunableNumber.ifChanged( - hashCode(), () -> linearController.setTolerance(linearTolerance.get()), linearTolerance); - LoggedTunableNumber.ifChanged( - hashCode(), () -> thetaController.setTolerance(thetaTolerance.get()), thetaTolerance); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> - linearController.setConstraints( - new TrapezoidProfile.Constraints( - maxLinearVelocity.get(), maxLinearAcceleration.get())), - maxLinearVelocity, - maxLinearAcceleration); - - // Control to setpoint - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - - // Calculate feedback velocities (based on position error). - double linearVelocityScalar = - linearController.calculate( - currentPose.getTranslation().getDistance(goalPose.getTranslation())) - + linearController.getSetpoint().velocity; - Rotation2d rotationToGoal = - goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); - Translation2d desiredLinearVelocity = new Translation2d(-linearVelocityScalar, rotationToGoal); - - double angularVelocity = - thetaController.calculate( - currentPose.getRotation().getRadians(), goalPose.getRotation().getRadians()) - + thetaController.getSetpoint().velocity; - - ChassisSpeeds fieldRelativeSpeeds = - new ChassisSpeeds( - desiredLinearVelocity.getX(), desiredLinearVelocity.getY(), angularVelocity); - Logger.recordOutput("AutoAlign/FieldRelativeSpeeds", fieldRelativeSpeeds); - Logger.recordOutput("AutoAlign/LinearError", linearController.getPositionError()); - Logger.recordOutput("AutoAlign/RotationError", thetaController.getPositionError()); - return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, currentPose.getRotation()); - } - - @AutoLogOutput(key = "AutoAlign/AtGoal") - public boolean atGoal() { - return linearController.atGoal() && thetaController.atGoal(); - } + private static LoggedTunableNumber linearkP = + new LoggedTunableNumber("AutoAlign/drivekP", DriveConstants.autoAlignConstants.linearkP()); + private static LoggedTunableNumber linearkD = + new LoggedTunableNumber("AutoAlign/drivekD", DriveConstants.autoAlignConstants.linearkD()); + private static LoggedTunableNumber thetakP = + new LoggedTunableNumber("AutoAlign/thetakP", DriveConstants.autoAlignConstants.thetakP()); + private static LoggedTunableNumber thetakD = + new LoggedTunableNumber("AutoAlign/thetakD", DriveConstants.autoAlignConstants.thetakD()); + private static LoggedTunableNumber linearTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerLinearTolerance", + DriveConstants.autoAlignConstants.linearTolerance()); + private static LoggedTunableNumber thetaTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerThetaTolerance", DriveConstants.autoAlignConstants.thetaTolerance()); + private static LoggedTunableNumber maxLinearVelocity = + new LoggedTunableNumber( + "AutoAlign/maxLinearVelocity", DriveConstants.autoAlignConstants.maxLinearVelocity()); + private static LoggedTunableNumber maxLinearAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxLinearAcceleration", + DriveConstants.autoAlignConstants.maxLinearAcceleration()); + private static LoggedTunableNumber maxAngularVelocity = + new LoggedTunableNumber( + "AutoAlign/maxAngularVelocity", DriveConstants.autoAlignConstants.maxAngularVelocity()); + private static LoggedTunableNumber maxAngularAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxAngularAcceleration", + DriveConstants.autoAlignConstants.maxAngularAcceleration()); + + @Getter(onMethod_ = @AutoLogOutput(key = "AutoAlign/goalPose")) + private Pose2d goalPose = null; + + // Controllers for translation and rotation + private final ProfiledPIDController linearController; + private final ProfiledPIDController thetaController; + + // Store previous velocities for acceleration limiting + private Translation2d prevLinearVelocity; + + public AutoAlignController(Pose2d goalPose) { + this.goalPose = goalPose; + // Set up both controllers + linearController = + new ProfiledPIDController( + linearkP.get(), + 0, + linearkD.get(), + new TrapezoidProfile.Constraints(maxLinearVelocity.get(), maxLinearAcceleration.get())); + linearController.setTolerance(linearTolerance.get()); + thetaController = + new ProfiledPIDController( + thetakP.get(), + 0, + thetakD.get(), + new TrapezoidProfile.Constraints( + maxAngularVelocity.get(), maxAngularAcceleration.get())); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(thetaTolerance.get()); + + // Reset measurements and velocities + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + // Linear controller will control to 0 so distance is the measurement + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double velocity = + -new Translation2d(fieldVelocity.dx, fieldVelocity.dy) + .rotateBy(rotationToGoal.unaryMinus()) + .getX(); + linearController.reset( + currentPose.getTranslation().getDistance(goalPose.getTranslation()), velocity); + thetaController.reset(currentPose.getRotation().getRadians(), fieldVelocity.dtheta); + + // Set goal positions + linearController.setGoal(0.0); + thetaController.setGoal(goalPose.getRotation().getRadians()); + + // Store linear velocity for acceleration limiting + prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); + + // Log goal pose + Logger.recordOutput("AutoAlign/GoalPose", goalPose); + } + + public ChassisSpeeds update() { + // Update PID Controllers + LoggedTunableNumber.ifChanged( + hashCode(), + () -> linearController.setPID(linearkP.get(), 0, linearkD.get()), + linearkP, + linearkD); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> thetaController.setPID(thetakP.get(), 0, thetakD.get()), + thetakP, + thetakD); + LoggedTunableNumber.ifChanged( + hashCode(), () -> linearController.setTolerance(linearTolerance.get()), linearTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), () -> thetaController.setTolerance(thetaTolerance.get()), thetaTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + linearController.setConstraints( + new TrapezoidProfile.Constraints( + maxLinearVelocity.get(), maxLinearAcceleration.get())), + maxLinearVelocity, + maxLinearAcceleration); + + // Control to setpoint + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + + // Calculate feedback velocities (based on position error). + double linearVelocityScalar = + linearController.calculate( + currentPose.getTranslation().getDistance(goalPose.getTranslation())) + + linearController.getSetpoint().velocity; + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + Translation2d desiredLinearVelocity = new Translation2d(-linearVelocityScalar, rotationToGoal); + + double angularVelocity = + thetaController.calculate( + currentPose.getRotation().getRadians(), goalPose.getRotation().getRadians()) + + thetaController.getSetpoint().velocity; + + ChassisSpeeds fieldRelativeSpeeds = + new ChassisSpeeds( + desiredLinearVelocity.getX(), desiredLinearVelocity.getY(), angularVelocity); + Logger.recordOutput("AutoAlign/FieldRelativeSpeeds", fieldRelativeSpeeds); + Logger.recordOutput("AutoAlign/LinearError", linearController.getPositionError()); + Logger.recordOutput("AutoAlign/RotationError", thetaController.getPositionError()); + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, currentPose.getRotation()); + } + + @AutoLogOutput(key = "AutoAlign/AtGoal") + public boolean atGoal() { + return linearController.atGoal() && thetaController.atGoal(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java index c6811465..faec85f4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java @@ -13,56 +13,56 @@ /** Drive controller for outputting {@link ChassisSpeeds} from driver joysticks. */ public class TeleopDriveController { - private static final LoggedTunableNumber controllerDeadband = - new LoggedTunableNumber("TeleopDrive/Deadband", 0.1); + private static final LoggedTunableNumber controllerDeadband = + new LoggedTunableNumber("TeleopDrive/Deadband", 0.1); - private double controllerX = 0; - private double controllerY = 0; - private double controllerOmega = 0; + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; - /** - * Accepts new drive input from joysticks. - * - * @param x Desired x velocity scalar, -1..1 - * @param y Desired y velocity scalar, -1..1 - * @param omega Desired omega velocity scalar, -1..1 - */ - public void acceptDriveInput(double x, double y, double omega) { - controllerX = x; - controllerY = y; - controllerOmega = omega; - } - - /** - * Updates the controller with the currently stored state. - * - * @return {@link ChassisSpeeds} with driver's requested speeds. - */ - public ChassisSpeeds update() { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband(Math.hypot(controllerX, controllerY), controllerDeadband.get()); - Rotation2d linearDirection = new Rotation2d(controllerX, controllerY); - double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); + /** + * Accepts new drive input from joysticks. + * + * @param x Desired x velocity scalar, -1..1 + * @param y Desired y velocity scalar, -1..1 + * @param omega Desired omega velocity scalar, -1..1 + */ + public void acceptDriveInput(double x, double y, double omega) { + controllerX = x; + controllerY = y; + controllerOmega = omega; + } - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); + /** + * Updates the controller with the currently stored state. + * + * @return {@link ChassisSpeeds} with driver's requested speeds. + */ + public ChassisSpeeds update() { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband(Math.hypot(controllerX, controllerY), controllerDeadband.get()); + Rotation2d linearDirection = new Rotation2d(controllerX, controllerY); + double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - if (DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI)); - } + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); - return ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), - linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), - omega * DriveConstants.driveConfig.maxAngularVelocity(), - RobotState.getInstance().getEstimatedPose().getRotation()); + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI)); } + + return ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), + linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), + omega * DriveConstants.driveConfig.maxAngularVelocity(), + RobotState.getInstance().getEstimatedPose().getRotation()); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java index d9a0b915..4b8e52f0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java @@ -18,90 +18,90 @@ import org.littletonrobotics.junction.Logger; public class TrajectoryController { - private static LoggedTunableNumber trajectoryLinearkP = - new LoggedTunableNumber("Trajectory/linearkP", trajectoryConstants.linearkP()); - private static LoggedTunableNumber trajectoryLinearkD = - new LoggedTunableNumber("Trajectory/linearkD", trajectoryConstants.linearkD()); - private static LoggedTunableNumber trajectoryThetakP = - new LoggedTunableNumber("Trajectory/thetakP", trajectoryConstants.thetakP()); - private static LoggedTunableNumber trajectoryThetakD = - new LoggedTunableNumber("Trajectory/thetakD", trajectoryConstants.thetakD()); + private static LoggedTunableNumber trajectoryLinearkP = + new LoggedTunableNumber("Trajectory/linearkP", trajectoryConstants.linearkP()); + private static LoggedTunableNumber trajectoryLinearkD = + new LoggedTunableNumber("Trajectory/linearkD", trajectoryConstants.linearkD()); + private static LoggedTunableNumber trajectoryThetakP = + new LoggedTunableNumber("Trajectory/thetakP", trajectoryConstants.thetakP()); + private static LoggedTunableNumber trajectoryThetakD = + new LoggedTunableNumber("Trajectory/thetakD", trajectoryConstants.thetakD()); - private HolonomicDriveController controller; - private HolonomicTrajectory trajectory; - private double startTime; + private HolonomicDriveController controller; + private HolonomicTrajectory trajectory; + private double startTime; - public TrajectoryController(HolonomicTrajectory trajectory) { - this.trajectory = trajectory; - controller = - new HolonomicDriveController( - trajectoryLinearkP.get(), - trajectoryLinearkD.get(), - trajectoryThetakP.get(), - trajectoryThetakD.get()); + public TrajectoryController(HolonomicTrajectory trajectory) { + this.trajectory = trajectory; + controller = + new HolonomicDriveController( + trajectoryLinearkP.get(), + trajectoryLinearkD.get(), + trajectoryThetakP.get(), + trajectoryThetakD.get()); - startTime = Timer.getFPGATimestamp(); - // Log poses - Logger.recordOutput( - "Trajectory/trajectoryPoses", - Arrays.stream(trajectory.getTrajectoryPoses()) - .map(AllianceFlipUtil::apply) - .toArray(Pose2d[]::new)); - } + startTime = Timer.getFPGATimestamp(); + // Log poses + Logger.recordOutput( + "Trajectory/trajectoryPoses", + Arrays.stream(trajectory.getTrajectoryPoses()) + .map(AllianceFlipUtil::apply) + .toArray(Pose2d[]::new)); + } - public ChassisSpeeds update() { - // Update PID Controllers - LoggedTunableNumber.ifChanged( - hashCode(), - pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]), - trajectoryLinearkP, - trajectoryLinearkD, - trajectoryThetakP, - trajectoryThetakP); + public ChassisSpeeds update() { + // Update PID Controllers + LoggedTunableNumber.ifChanged( + hashCode(), + pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]), + trajectoryLinearkP, + trajectoryLinearkD, + trajectoryThetakP, + trajectoryThetakP); - // Run trajectory - Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); - Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); - double sampletime = Timer.getFPGATimestamp() - startTime; - VehicleState currentState = - VehicleState.newBuilder() - .setX(currentPose.getTranslation().getX()) - .setY(currentPose.getTranslation().getY()) - .setTheta(currentPose.getRotation().getRadians()) - .setVx(fieldVelocity.dx) - .setVy(fieldVelocity.dy) - .setOmega(fieldVelocity.dtheta) - .build(); - // Sample and flip state - VehicleState setpointState = trajectory.sample(sampletime); - setpointState = AllianceFlipUtil.apply(setpointState); + // Run trajectory + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + double sampletime = Timer.getFPGATimestamp() - startTime; + VehicleState currentState = + VehicleState.newBuilder() + .setX(currentPose.getTranslation().getX()) + .setY(currentPose.getTranslation().getY()) + .setTheta(currentPose.getRotation().getRadians()) + .setVx(fieldVelocity.dx) + .setVy(fieldVelocity.dy) + .setOmega(fieldVelocity.dtheta) + .build(); + // Sample and flip state + VehicleState setpointState = trajectory.sample(sampletime); + setpointState = AllianceFlipUtil.apply(setpointState); - // calculate trajectory speeds - ChassisSpeeds speeds = controller.calculate(currentState, setpointState); - // Log trajectory data - Logger.recordOutput( - "Trajectory/SetpointPose", - new Pose2d( - setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta()))); - Logger.recordOutput("Trajectory/SetpointSpeeds/vx", setpointState.getVx()); - Logger.recordOutput("Trajectory/SetpointSpeeds/vy", setpointState.getVy()); - Logger.recordOutput("Trajectory/SetpointSpeeds/omega", setpointState.getOmega()); - Logger.recordOutput("Trajectory/FieldRelativeSpeeds", speeds); - return speeds; - } + // calculate trajectory speeds + ChassisSpeeds speeds = controller.calculate(currentState, setpointState); + // Log trajectory data + Logger.recordOutput( + "Trajectory/SetpointPose", + new Pose2d( + setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta()))); + Logger.recordOutput("Trajectory/SetpointSpeeds/vx", setpointState.getVx()); + Logger.recordOutput("Trajectory/SetpointSpeeds/vy", setpointState.getVy()); + Logger.recordOutput("Trajectory/SetpointSpeeds/omega", setpointState.getOmega()); + Logger.recordOutput("Trajectory/FieldRelativeSpeeds", speeds); + return speeds; + } - @AutoLogOutput(key = "Trajectory/TranslationError") - public double getTranslationError() { - return controller.getPoseError().getTranslation().getNorm(); - } + @AutoLogOutput(key = "Trajectory/TranslationError") + public double getTranslationError() { + return controller.getPoseError().getTranslation().getNorm(); + } - @AutoLogOutput(key = "Trajectory/RotationError") - public Rotation2d getRotationError() { - return controller.getPoseError().getRotation(); - } + @AutoLogOutput(key = "Trajectory/RotationError") + public Rotation2d getRotationError() { + return controller.getPoseError().getRotation(); + } - @AutoLogOutput(key = "Trajectory/Finished") - public boolean isFinished() { - return Timer.getFPGATimestamp() - startTime > trajectory.getDuration(); - } + @AutoLogOutput(key = "Trajectory/Finished") + public boolean isFinished() { + return Timer.getFPGATimestamp() - startTime > trajectory.getDuration(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java index a45318c3..89f73eba 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java @@ -11,16 +11,16 @@ @ExtensionMethod({TrajectoryGenerationHelpers.class}) public class DriveTrajectories { - public static final Map> paths = new HashMap<>(); + public static final Map> paths = new HashMap<>(); - static { - paths.put( - "driveStraight", - List.of( - PathSegment.newBuilder() - .addPoseWaypoint(new Pose2d()) - .addPoseWaypoint(new Pose2d(2.0, 0.0, new Rotation2d()), 100) - .addPoseWaypoint(new Pose2d(4.0, 2.0, new Rotation2d(Math.PI))) - .build())); - } + static { + paths.put( + "driveStraight", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(new Pose2d()) + .addPoseWaypoint(new Pose2d(2.0, 0.0, new Rotation2d()), 100) + .addPoseWaypoint(new Pose2d(4.0, 2.0, new Rotation2d(Math.PI))) + .build())); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java index 41d80708..609163cf 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java @@ -18,133 +18,133 @@ import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceGrpc; public class GenerateTrajectories { - public static void main(String[] args) { - Constants.disableHAL(); - - // Create vehicle model - VehicleModel model = - VehicleModel.newBuilder() - .setMass(70) - .setMoi(6) - .setVehicleLength(DriveConstants.driveConfig.trackwidthX()) - .setVehicleWidth(DriveConstants.driveConfig.trackwidthY()) - .setWheelRadius(DriveConstants.driveConfig.wheelRadius()) - .setMaxWheelTorque(2) - .setMaxWheelOmega( - DriveConstants.moduleLimits.maxDriveVelocity() - / DriveConstants.driveConfig.wheelRadius()) - .build(); - - // Check hashcodes - Map> pathQueue = new HashMap<>(); - for (Map.Entry> entry : DriveTrajectories.paths.entrySet()) { - String hashCode = getHashCode(model, entry.getValue()); - File pathFile = - Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); - if (pathFile.exists()) { - try { - InputStream fileStream = new FileInputStream(pathFile); - Trajectory trajectory = Trajectory.parseFrom(fileStream); - // trajectory.getStatesC - if (!trajectory.getHashCode().equals(hashCode)) { - pathQueue.put(entry.getKey(), entry.getValue()); - } - } catch (IOException e) { - e.printStackTrace(); - } - } else { - pathQueue.put(entry.getKey(), entry.getValue()); - } + public static void main(String[] args) { + Constants.disableHAL(); + + // Create vehicle model + VehicleModel model = + VehicleModel.newBuilder() + .setMass(70) + .setMoi(6) + .setVehicleLength(DriveConstants.driveConfig.trackwidthX()) + .setVehicleWidth(DriveConstants.driveConfig.trackwidthY()) + .setWheelRadius(DriveConstants.driveConfig.wheelRadius()) + .setMaxWheelTorque(2) + .setMaxWheelOmega( + DriveConstants.moduleLimits.maxDriveVelocity() + / DriveConstants.driveConfig.wheelRadius()) + .build(); + + // Check hashcodes + Map> pathQueue = new HashMap<>(); + for (Map.Entry> entry : DriveTrajectories.paths.entrySet()) { + String hashCode = getHashCode(model, entry.getValue()); + File pathFile = + Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); + if (pathFile.exists()) { + try { + InputStream fileStream = new FileInputStream(pathFile); + Trajectory trajectory = Trajectory.parseFrom(fileStream); + // trajectory.getStatesC + if (!trajectory.getHashCode().equals(hashCode)) { + pathQueue.put(entry.getKey(), entry.getValue()); + } + } catch (IOException e) { + e.printStackTrace(); } + } else { + pathQueue.put(entry.getKey(), entry.getValue()); + } + } - // Exit if trajectories up-to-date - if (pathQueue.isEmpty()) { - return; - } + // Exit if trajectories up-to-date + if (pathQueue.isEmpty()) { + return; + } - // Connect to service - var channel = - Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build(); - var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel); + // Connect to service + var channel = + Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build(); + var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel); - // Generate trajectories - for (Map.Entry> entry : pathQueue.entrySet()) { - PathRequest request = - PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build(); + // Generate trajectories + for (Map.Entry> entry : pathQueue.entrySet()) { + PathRequest request = + PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build(); - TrajectoryResponse response = service.generateTrajectory(request); + TrajectoryResponse response = service.generateTrajectory(request); - String responseHashCode = getHashCode(model, entry.getValue()); + String responseHashCode = getHashCode(model, entry.getValue()); - response = - response.toBuilder() - .setTrajectory( - response.getTrajectory().toBuilder().setHashCode(responseHashCode).build()) - .build(); + response = + response.toBuilder() + .setTrajectory( + response.getTrajectory().toBuilder().setHashCode(responseHashCode).build()) + .build(); - File pathFile = - Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); + File pathFile = + Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); - try { - OutputStream fileStream = new FileOutputStream(pathFile); - System.out.println("Writing to " + pathFile.getAbsolutePath()); - response.getTrajectory().writeTo(fileStream); - } catch (IOException e) { - e.printStackTrace(); - } - } + try { + OutputStream fileStream = new FileOutputStream(pathFile); + System.out.println("Writing to " + pathFile.getAbsolutePath()); + response.getTrajectory().writeTo(fileStream); + } catch (IOException e) { + e.printStackTrace(); + } } + } + + // create a hashcode for the vehicle model and path segments + + private static String getHashCode(VehicleModel model, List segements) { + StringBuilder hashString = new StringBuilder(); + + DecimalFormat format = new DecimalFormat("#.000000"); + format.setRoundingMode(RoundingMode.HALF_DOWN); + + hashString.append(format.format(model.getMass())); + hashString.append(format.format(model.getMoi())); + hashString.append(format.format(model.getVehicleLength())); + hashString.append(format.format(model.getVehicleWidth())); + hashString.append(format.format(model.getWheelRadius())); + hashString.append(format.format(model.getMaxWheelTorque())); + hashString.append(format.format(model.getMaxWheelOmega())); + + for (PathSegment segment : segements) { + for (Waypoint waypoint : segment.getWaypointsList()) { + hashString.append(format.format(waypoint.getX())); + hashString.append(format.format(waypoint.getY())); + if (waypoint.hasHeadingConstraint()) { + hashString.append(format.format(waypoint.getHeadingConstraint())); + } - // create a hashcode for the vehicle model and path segments - - private static String getHashCode(VehicleModel model, List segements) { - StringBuilder hashString = new StringBuilder(); - - DecimalFormat format = new DecimalFormat("#.000000"); - format.setRoundingMode(RoundingMode.HALF_DOWN); - - hashString.append(format.format(model.getMass())); - hashString.append(format.format(model.getMoi())); - hashString.append(format.format(model.getVehicleLength())); - hashString.append(format.format(model.getVehicleWidth())); - hashString.append(format.format(model.getWheelRadius())); - hashString.append(format.format(model.getMaxWheelTorque())); - hashString.append(format.format(model.getMaxWheelOmega())); - - for (PathSegment segment : segements) { - for (Waypoint waypoint : segment.getWaypointsList()) { - hashString.append(format.format(waypoint.getX())); - hashString.append(format.format(waypoint.getY())); - if (waypoint.hasHeadingConstraint()) { - hashString.append(format.format(waypoint.getHeadingConstraint())); - } - - if (waypoint.hasSamples()) { - hashString.append(waypoint.getSamples()); - } - - switch (waypoint.getVelocityConstraintCase()) { - case ZERO_VELOCITY -> { - hashString.append(format.format(0)); - } - case VEHICLE_VELOCITY -> { - hashString.append(format.format(waypoint.getVehicleVelocity().getVx())); - hashString.append(format.format(waypoint.getVehicleVelocity().getVy())); - hashString.append(format.format(waypoint.getVehicleVelocity().getOmega())); - } - } - } - - if (segment.hasMaxVelocity()) { - hashString.append(format.format(segment.getMaxVelocity())); - } - if (segment.hasMaxOmega()) { - hashString.append(format.format(segment.getMaxOmega())); - } - - hashString.append(segment.getStraightLine()); + if (waypoint.hasSamples()) { + hashString.append(waypoint.getSamples()); } - return Hashing.sha256().hashString(hashString, StandardCharsets.UTF_8).toString(); + switch (waypoint.getVelocityConstraintCase()) { + case ZERO_VELOCITY -> { + hashString.append(format.format(0)); + } + case VEHICLE_VELOCITY -> { + hashString.append(format.format(waypoint.getVehicleVelocity().getVx())); + hashString.append(format.format(waypoint.getVehicleVelocity().getVy())); + hashString.append(format.format(waypoint.getVehicleVelocity().getOmega())); + } + } + } + + if (segment.hasMaxVelocity()) { + hashString.append(format.format(segment.getMaxVelocity())); + } + if (segment.hasMaxOmega()) { + hashString.append(format.format(segment.getMaxOmega())); + } + + hashString.append(segment.getStraightLine()); } + + return Hashing.sha256().hashString(hashString, StandardCharsets.UTF_8).toString(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/TrajectoryGenerationHelpers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/TrajectoryGenerationHelpers.java index 16cafcde..911365fe 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/TrajectoryGenerationHelpers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/TrajectoryGenerationHelpers.java @@ -7,126 +7,126 @@ import edu.wpi.first.math.geometry.Translation2d; public class TrajectoryGenerationHelpers { - /** - * Gets the pose of the state - * - * @param state The {@link VehicleState} - * @return {@link Pose2d} of the state - */ - public static Pose2d getPose(VehicleState state) { - return new Pose2d(state.getX(), state.getY(), Rotation2d.fromRadians(state.getTheta())); - } + /** + * Gets the pose of the state + * + * @param state The {@link VehicleState} + * @return {@link Pose2d} of the state + */ + public static Pose2d getPose(VehicleState state) { + return new Pose2d(state.getX(), state.getY(), Rotation2d.fromRadians(state.getTheta())); + } - /** - * Creates a {@link VehicleVelocityConstraint} from a {@link VehicleState}, useful for chaining a - * generated trajectory into a new trajectory. - * - * @param state The {@link VehicleState} - * @return The {@link VehicleVelocityConstraint} representing the velocity of the state. - */ - public static VehicleVelocityConstraint createVelocityConstraint(VehicleState state) { - return VehicleVelocityConstraint.newBuilder() - .setVx(state.getVx()) - .setVy(state.getVy()) - .setOmega(state.getOmega()) - .build(); - } + /** + * Creates a {@link VehicleVelocityConstraint} from a {@link VehicleState}, useful for chaining a + * generated trajectory into a new trajectory. + * + * @param state The {@link VehicleState} + * @return The {@link VehicleVelocityConstraint} representing the velocity of the state. + */ + public static VehicleVelocityConstraint createVelocityConstraint(VehicleState state) { + return VehicleVelocityConstraint.newBuilder() + .setVx(state.getVx()) + .setVy(state.getVy()) + .setOmega(state.getOmega()) + .build(); + } - public static VehicleVelocityConstraint endVelocityConstraint(Trajectory trajectory) { - return createVelocityConstraint( - trajectory.getStates(trajectory.getStatesCount() - 1).getState()); - } + public static VehicleVelocityConstraint endVelocityConstraint(Trajectory trajectory) { + return createVelocityConstraint( + trajectory.getStates(trajectory.getStatesCount() - 1).getState()); + } - /** - * Adds a waypoint to an existing {@link PathSegment.Builder} which is a continuation of a - * previously generated trajectory. The pose and velocity of the waypoint are set to the ending - * pose and velocity of the trajectory. - * - * @param builder The {@link PathSegment.Builder}. - * @param trajectory The generated trajectory. - * @return The {@link PathSegment.Builder} with the new waypoint added. - */ - public static PathSegment.Builder addContinuationWaypoint( - PathSegment.Builder builder, Trajectory trajectory) { - Pose2d endPose = getPose(trajectory.getStates(trajectory.getStatesCount() - 1).getState()); - return builder.addWaypoints( - fromPose(Waypoint.newBuilder(), endPose) - .setVehicleVelocity(endVelocityConstraint(trajectory))); - } + /** + * Adds a waypoint to an existing {@link PathSegment.Builder} which is a continuation of a + * previously generated trajectory. The pose and velocity of the waypoint are set to the ending + * pose and velocity of the trajectory. + * + * @param builder The {@link PathSegment.Builder}. + * @param trajectory The generated trajectory. + * @return The {@link PathSegment.Builder} with the new waypoint added. + */ + public static PathSegment.Builder addContinuationWaypoint( + PathSegment.Builder builder, Trajectory trajectory) { + Pose2d endPose = getPose(trajectory.getStates(trajectory.getStatesCount() - 1).getState()); + return builder.addWaypoints( + fromPose(Waypoint.newBuilder(), endPose) + .setVehicleVelocity(endVelocityConstraint(trajectory))); + } - /** - * Adds a translation waypoint to an existing {@link PathSegment.Builder} without setting any - * other constraints. - * - * @param builder The {@link PathSegment.Builder}. - * @param translation The translation. - * @return The {@link PathSegment.Builder} with the new waypoint added. - */ - public static PathSegment.Builder addTranslationWaypoint( - PathSegment.Builder builder, Translation2d translation) { - return builder.addWaypoints(fromTranslation(Waypoint.newBuilder(), translation)); - } + /** + * Adds a translation waypoint to an existing {@link PathSegment.Builder} without setting any + * other constraints. + * + * @param builder The {@link PathSegment.Builder}. + * @param translation The translation. + * @return The {@link PathSegment.Builder} with the new waypoint added. + */ + public static PathSegment.Builder addTranslationWaypoint( + PathSegment.Builder builder, Translation2d translation) { + return builder.addWaypoints(fromTranslation(Waypoint.newBuilder(), translation)); + } - /** - * Adds a waypoint to an existing {@link PathSegment.Builder} without setting any other - * costraints. - * - * @param builder The {@link PathSegment.Builder} - * @param pose The pose. - * @return The {@link PathSegment.Builder} with the new waypoint added. - */ - public static PathSegment.Builder addPoseWaypoint(PathSegment.Builder builder, Pose2d pose) { - return builder.addWaypoints(fromPose(Waypoint.newBuilder(), pose)); - } + /** + * Adds a waypoint to an existing {@link PathSegment.Builder} without setting any other + * costraints. + * + * @param builder The {@link PathSegment.Builder} + * @param pose The pose. + * @return The {@link PathSegment.Builder} with the new waypoint added. + */ + public static PathSegment.Builder addPoseWaypoint(PathSegment.Builder builder, Pose2d pose) { + return builder.addWaypoints(fromPose(Waypoint.newBuilder(), pose)); + } - /** - * Adds a translation waypoint to an existing {@link PathSegment.Builder} without setting any - * other constraints. - * - * @param builder The {@link PathSegment.Builder}. - * @param translation The translation. - * @return The {@link PathSegment.Builder} with the new waypoint added. - */ - public static PathSegment.Builder addTranslationWaypoint( - PathSegment.Builder builder, Translation2d translation, int samples) { - return builder.addWaypoints( - fromTranslation(Waypoint.newBuilder(), translation).setSamples(samples)); - } + /** + * Adds a translation waypoint to an existing {@link PathSegment.Builder} without setting any + * other constraints. + * + * @param builder The {@link PathSegment.Builder}. + * @param translation The translation. + * @return The {@link PathSegment.Builder} with the new waypoint added. + */ + public static PathSegment.Builder addTranslationWaypoint( + PathSegment.Builder builder, Translation2d translation, int samples) { + return builder.addWaypoints( + fromTranslation(Waypoint.newBuilder(), translation).setSamples(samples)); + } - /** - * Adds a waypoint to an existing {@link PathSegment.Builder} without setting any other - * costraints. - * - * @param builder The {@link PathSegment.Builder} - * @param pose The pose. - * @return The {@link PathSegment.Builder} with the new waypoint added. - */ - public static PathSegment.Builder addPoseWaypoint( - PathSegment.Builder builder, Pose2d pose, int samples) { - return builder.addWaypoints(fromPose(Waypoint.newBuilder(), pose).setSamples(samples)); - } + /** + * Adds a waypoint to an existing {@link PathSegment.Builder} without setting any other + * costraints. + * + * @param builder The {@link PathSegment.Builder} + * @param pose The pose. + * @return The {@link PathSegment.Builder} with the new waypoint added. + */ + public static PathSegment.Builder addPoseWaypoint( + PathSegment.Builder builder, Pose2d pose, int samples) { + return builder.addWaypoints(fromPose(Waypoint.newBuilder(), pose).setSamples(samples)); + } - /** - * Adds {@link Translation2d} to an existing {@link Waypoint.Builder} - * - * @param builder The {@link Waypoint.Builder} - * @param translation The translation. - * @return The {@link Waypoint.Builder} with the translation added. - */ - public static Waypoint.Builder fromTranslation( - Waypoint.Builder builder, Translation2d translation) { - return builder.setX(translation.getX()).setY(translation.getY()); - } + /** + * Adds {@link Translation2d} to an existing {@link Waypoint.Builder} + * + * @param builder The {@link Waypoint.Builder} + * @param translation The translation. + * @return The {@link Waypoint.Builder} with the translation added. + */ + public static Waypoint.Builder fromTranslation( + Waypoint.Builder builder, Translation2d translation) { + return builder.setX(translation.getX()).setY(translation.getY()); + } - /** - * Adds {@link Pose2d} to an existing {@link Waypoint.Builder} - * - * @param builder The {@link Waypoint.Builder} - * @param pose The pose. - * @return The {@link Waypoint.Builder} with the pose added. - */ - public static Waypoint.Builder fromPose(Waypoint.Builder builder, Pose2d pose) { - return fromTranslation(builder, pose.getTranslation()) - .setHeadingConstraint(pose.getRotation().getRadians()); - } + /** + * Adds {@link Pose2d} to an existing {@link Waypoint.Builder} + * + * @param builder The {@link Waypoint.Builder} + * @param pose The pose. + * @return The {@link Waypoint.Builder} with the pose added. + */ + public static Waypoint.Builder fromPose(Waypoint.Builder builder, Pose2d pose) { + return fromTranslation(builder, pose.getTranslation()) + .setHeadingConstraint(pose.getRotation().getRadians()); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java new file mode 100644 index 00000000..8640d18e --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java @@ -0,0 +1,27 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import java.util.function.DoubleSupplier; +import lombok.RequiredArgsConstructor; +import org.littletonrobotics.junction.Logger; + +@RequiredArgsConstructor +public abstract class GenericRollerSubsystem { + protected interface VoltageGoal { + DoubleSupplier getVoltageSupplier(); + } + + public abstract G getGoal(); + + private final String name; + private final GenericRollerSystemIO io; + private final GenericRollerSystemIOInputsAutoLogged inputs = + new GenericRollerSystemIOInputsAutoLogged(); + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(name, inputs); + + io.runVolts(getGoal().getVoltageSupplier().getAsDouble()); + Logger.recordOutput(name + "/Goal", getGoal().toString()); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java new file mode 100644 index 00000000..cf3dead8 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java @@ -0,0 +1,21 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import org.littletonrobotics.junction.AutoLog; + +public interface GenericRollerSystemIO { + @AutoLog + abstract class GenericRollerSystemIOInputs { + public double positionRads = 0.0; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double outputCurrent = 0.0; + } + + default void updateInputs(GenericRollerSystemIOInputs inputs) {} + + /** Run feeder at volts */ + default void runVolts(double volts) {} + + /** Stop feeder */ + default void stop() {} +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java new file mode 100644 index 00000000..16aa46c4 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java @@ -0,0 +1,69 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +/** Generic roller IO implementation for a roller or series of rollers using a Kraken. */ +public abstract class GenericRollerSystemIOKrakenFOC implements GenericRollerSystemIO { + private final TalonFX motor; + + private final StatusSignal position; + private final StatusSignal velocity; + private final StatusSignal appliedVoltage; + private final StatusSignal outputCurrent; + + // Single shot for voltage mode, robot loop will call continuously + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); + private final NeutralOut neutralOut = new NeutralOut(); + + private final double reduction; + + public GenericRollerSystemIOKrakenFOC( + int id, String bus, int currentLimitAmps, boolean invert, boolean brake, double reduction) { + this.reduction = reduction; + motor = new TalonFX(id, bus); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = + invert ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.NeutralMode = brake ? NeutralModeValue.Brake : NeutralModeValue.Coast; + config.CurrentLimits.SupplyCurrentLimit = currentLimitAmps; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + motor.getConfigurator().apply(config); + + position = motor.getPosition(); + velocity = motor.getVelocity(); + appliedVoltage = motor.getMotorVoltage(); + outputCurrent = motor.getTorqueCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, position, velocity, appliedVoltage, outputCurrent); + } + + @Override + public void updateInputs(GenericRollerSystemIOInputs inputs) { + BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, outputCurrent); + + inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction; + inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.outputCurrent = outputCurrent.getValueAsDouble(); + } + + @Override + public void runVolts(double volts) { + motor.setControl(voltageOut.withOutput(volts)); + } + + @Override + public void stop() { + motor.setControl(neutralOut); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSparkFlex.java new file mode 100644 index 00000000..48f3814d --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSparkFlex.java @@ -0,0 +1,44 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.util.Units; + +/** Generic roller IO implementation for a roller or series of rollers using a SPARK Flex. */ +public abstract class GenericRollerSystemIOSparkFlex implements GenericRollerSystemIO { + private final CANSparkFlex motor; + private final RelativeEncoder encoder; + + private final double reduction; + + public GenericRollerSystemIOSparkFlex( + int id, int currentLimitAmps, boolean invert, boolean brake, double reduction) { + this.reduction = reduction; + motor = new CANSparkFlex(id, CANSparkBase.MotorType.kBrushless); + + motor.setSmartCurrentLimit(currentLimitAmps); + motor.setInverted(invert); + motor.setIdleMode(brake ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); + + encoder = motor.getEncoder(); + } + + public void updateInputs(GenericRollerSystemIOInputs inputs) { + inputs.positionRads = Units.rotationsToRadians(encoder.getPosition()) / reduction; + inputs.velocityRadsPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()) / reduction; + inputs.appliedVoltage = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.outputCurrent = motor.getOutputCurrent(); + } + + @Override + public void runVolts(double volts) { + motor.setVoltage(volts); + } + + @Override + public void stop() { + motor.stopMotor(); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java new file mode 100644 index 00000000..f76e6cec --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -0,0 +1,67 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; +import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; +import org.littletonrobotics.junction.Logger; + +@RequiredArgsConstructor() +public class Rollers extends SubsystemBase { + private final Feeder feeder; + private final Indexer indexer; + private final Intake intake; + private final RollersSensorsIO sensorsIO; + + private final RollersSensorsIOInputsAutoLogged inputs = new RollersSensorsIOInputsAutoLogged(); + + public enum Goal { + IDLE, + FLOOR_INTAKE, + STATION_INTAKE, + EJECT_TO_FLOOR, + FEED_SHOOTER + } + + @Getter @Setter private Goal goal = Goal.IDLE; + + @Override + public void periodic() { + sensorsIO.updateInputs(inputs); + Logger.processInputs("RollersSensors", inputs); + + switch (goal) { + case IDLE -> { + feeder.setGoal(Feeder.Goal.IDLE); + indexer.setGoal(Indexer.Goal.IDLE); + intake.setGoal(Intake.Goal.IDLE); + } + case FLOOR_INTAKE -> { + feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); + indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); + intake.setGoal(Intake.Goal.FLOOR_INTAKING); + if (inputs.shooterStaged) { + goal = Goal.IDLE; + } + } + case STATION_INTAKE -> { + indexer.setGoal(Indexer.Goal.STATION_INTAKING); + } + case FEED_SHOOTER -> indexer.setGoal(Indexer.Goal.SHOOTING); + case EJECT_TO_FLOOR -> { + feeder.setGoal(Feeder.Goal.EJECTING); + indexer.setGoal(Indexer.Goal.EJECTING); + intake.setGoal(Intake.Goal.EJECTING); + } + } + + Logger.recordOutput("Rollers/Goal", goal); + + feeder.periodic(); + indexer.periodic(); + intake.periodic(); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIO.java new file mode 100644 index 00000000..67208c65 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIO.java @@ -0,0 +1,14 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import org.littletonrobotics.junction.AutoLog; + +public interface RollersSensorsIO { + @AutoLog + class RollersSensorsIOInputs { + boolean shooterStaged; + boolean backbackStaged; + boolean indexerCleared; + } + + default void updateInputs(RollersSensorsIOInputs inputs) {} +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOReal.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOReal.java new file mode 100644 index 00000000..95d875bf --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOReal.java @@ -0,0 +1,20 @@ +package org.littletonrobotics.frc2024.subsystems.rollers; + +import edu.wpi.first.wpilibj.DigitalGlitchFilter; +import edu.wpi.first.wpilibj.DigitalInput; +import java.time.Duration; + +public class RollersSensorsIOReal implements RollersSensorsIO { + private final DigitalInput shooterStagedSensor = new DigitalInput(0); + + public RollersSensorsIOReal() { + DigitalGlitchFilter glitchFilter = new DigitalGlitchFilter(); + glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(5).toNanos()); + glitchFilter.add(shooterStagedSensor); + } + + @Override + public void updateInputs(RollersSensorsIOInputs inputs) { + inputs.shooterStaged = shooterStagedSensor.get(); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java new file mode 100644 index 00000000..4104bda4 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java @@ -0,0 +1,27 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.feeder; + +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; + +public class Feeder extends GenericRollerSubsystem { + @RequiredArgsConstructor + @Getter + public enum Goal implements GenericRollerSubsystem.VoltageGoal { + IDLE(() -> 0.0), + FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 8.0)), + BACKSTOPPING(new LoggedTunableNumber("Feeder/BackstoppingVoltage", -4.0)), + EJECTING(new LoggedTunableNumber("Feeder/EjectingVoltage", -6.0)); + + private final DoubleSupplier voltageSupplier; + } + + @Getter @Setter private Feeder.Goal goal = Feeder.Goal.IDLE; + + public Feeder(FeederIO io) { + super("Feeder", io); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIO.java new file mode 100644 index 00000000..c5fa91b7 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIO.java @@ -0,0 +1,5 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.feeder; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIO; + +public interface FeederIO extends GenericRollerSystemIO {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOKrakenFOC.java new file mode 100644 index 00000000..9cdea2a0 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOKrakenFOC.java @@ -0,0 +1,16 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.feeder; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOKrakenFOC; + +public class FeederIOKrakenFOC extends GenericRollerSystemIOKrakenFOC implements FeederIO { + private static final int id = 3; + private static final String bus = "rio"; + private static final int currentLimitAmps = 40; + private static final boolean invert = false; + private static final boolean brake = false; + private static final double reduction = 18.0 / 12.0; + + public FeederIOKrakenFOC() { + super(id, bus, currentLimitAmps, invert, brake, reduction); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java new file mode 100644 index 00000000..784d8049 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java @@ -0,0 +1,30 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.indexer; + +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; + +@Setter +@Getter +public class Indexer extends GenericRollerSubsystem { + @RequiredArgsConstructor + @Getter + public enum Goal implements VoltageGoal { + IDLE(() -> 0.0), + FLOOR_INTAKING(new LoggedTunableNumber("Indexer/FloorIntakingVoltage", 2.0)), + STATION_INTAKING(new LoggedTunableNumber("Indexer/StationIntakingVoltage", -2.0)), + SHOOTING(new LoggedTunableNumber("Indexer/ShootingVoltage", 12.0)), + EJECTING(new LoggedTunableNumber("Indexer/EjectingVoltage", -8.0)); + + private final DoubleSupplier voltageSupplier; + } + + @Getter @Setter private Indexer.Goal goal = Indexer.Goal.IDLE; + + public Indexer(IndexerIO io) { + super("Indexer", io); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIO.java new file mode 100644 index 00000000..699603d1 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIO.java @@ -0,0 +1,5 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.indexer; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIO; + +public interface IndexerIO extends GenericRollerSystemIO {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java new file mode 100644 index 00000000..1e7fa204 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSparkFlex.java @@ -0,0 +1,14 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.indexer; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOSparkFlex; + +public class IndexerIOSparkFlex extends GenericRollerSystemIOSparkFlex implements IndexerIO { + private static final double reduction = (18.0 / 12.0); + private static final int id = 6; + private static final int currentLimitAmps = 40; + private static final boolean inverted = false; + + public IndexerIOSparkFlex() { + super(id, currentLimitAmps, inverted, true, reduction); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java new file mode 100644 index 00000000..29e734f4 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java @@ -0,0 +1,26 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.intake; + +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; + +public class Intake extends GenericRollerSubsystem { + @RequiredArgsConstructor + @Getter + public enum Goal implements VoltageGoal { + IDLE(() -> 0.0), + FLOOR_INTAKING(new LoggedTunableNumber("Intake/FloorIntakingVoltage", 8.0)), + EJECTING(new LoggedTunableNumber("Intake/EjectingVoltage", -8.0)); + + private final DoubleSupplier voltageSupplier; + } + + @Getter @Setter private Goal goal = Goal.IDLE; + + public Intake(IntakeIO io) { + super("Intake", io); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIO.java new file mode 100644 index 00000000..fc32b817 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIO.java @@ -0,0 +1,5 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.intake; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIO; + +public interface IntakeIO extends GenericRollerSystemIO {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOKrakenFOC.java new file mode 100644 index 00000000..d5e9e8ab --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOKrakenFOC.java @@ -0,0 +1,16 @@ +package org.littletonrobotics.frc2024.subsystems.rollers.intake; + +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOKrakenFOC; + +public class IntakeIOKrakenFOC extends GenericRollerSystemIOKrakenFOC implements IntakeIO { + private static final int id = 2; + private static final String bus = "canivore"; + private static final int currentLimitAmps = 40; + private static final boolean invert = false; + private static final boolean brake = false; + private static final double reduction = 18.0 / 12.0; + + public IntakeIOKrakenFOC() { + super(id, bus, currentLimitAmps, invert, brake, reduction); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index a4992142..d1ba1133 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -1,143 +1,121 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.FieldConstants; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; -import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @RequiredArgsConstructor public class Superstructure extends SubsystemBase { - private static LoggedTunableNumber armIdleSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0); - private static LoggedTunableNumber armStationIntakeSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0); - private static LoggedTunableNumber armIntakeSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); - private static LoggedTunableNumber yCompensation = - new LoggedTunableNumber("Superstructure/CompensationInches", 6.0); - private static LoggedTunableNumber followThroughTime = - new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5); + private static LoggedTunableNumber armIdleSetpointDegrees = + new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0); + private static LoggedTunableNumber armStationIntakeSetpointDegrees = + new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0); + private static LoggedTunableNumber armIntakeSetpointDegrees = + new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); + private static LoggedTunableNumber yCompensation = + new LoggedTunableNumber("Superstructure/CompensationMeters", 0.55); + private static LoggedTunableNumber followThroughTime = + new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5); - public enum SystemState { - PREPARE_SHOOT, - SHOOT, + public enum SystemState { + PREPARE_SHOOT, + SHOOT, + PREPARE_INTAKE, + INTAKE, + STATION_INTAKE, + REVERSE_INTAKE, + IDLE + } - PREPARE_INTAKE, - INTAKE, - STATION_INTAKE, - REVERSE_INTAKE, - IDLE - } - - public enum GamepieceState { - NO_GAMEPIECE, + public enum GamepieceState { + NO_GAMEPIECE, - HOLDING_SHOOTER, + HOLDING_SHOOTER, - HOLDING_BACKPACK - } + HOLDING_BACKPACK + } - @Getter private SystemState currentState = SystemState.IDLE; - @Getter @Setter private SystemState goalState = SystemState.IDLE; + @Getter private SystemState currentState = SystemState.IDLE; + @Getter @Setter private SystemState goalState = SystemState.IDLE; - @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NO_GAMEPIECE; + @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NO_GAMEPIECE; - private final Arm arm; - private final Flywheels flywheels; - private final Feeder feeder; - private final Intake intake; + private final Arm arm; + private final Flywheels flywheels; - private final Timer followThroughTimer = new Timer(); + private final Timer followThroughTimer = new Timer(); - @Override - public void periodic() { - switch (goalState) { - case IDLE -> { - if (currentState == SystemState.SHOOT) { - if (followThroughTimer.hasElapsed(followThroughTime.get())) { - currentState = SystemState.IDLE; - followThroughTimer.stop(); - followThroughTimer.reset(); - } else { - currentState = SystemState.SHOOT; - } - } else { - currentState = SystemState.IDLE; - } - } - case STATION_INTAKE -> currentState = SystemState.STATION_INTAKE; - case INTAKE -> currentState = SystemState.INTAKE; - case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT; - case SHOOT -> { - if (currentState != SystemState.PREPARE_SHOOT) { - currentState = SystemState.PREPARE_SHOOT; - } else if (atShootingSetpoint()) { - currentState = SystemState.SHOOT; - followThroughTimer.restart(); - goalState = SystemState.IDLE; - } - } + @Override + public void periodic() { + switch (goalState) { + case IDLE -> { + if (currentState == SystemState.SHOOT) { + if (followThroughTimer.hasElapsed(followThroughTime.get())) { + currentState = SystemState.IDLE; + followThroughTimer.stop(); + followThroughTimer.reset(); + } else { + currentState = SystemState.SHOOT; + } + } else { + currentState = SystemState.IDLE; } - - switch (currentState) { - case IDLE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.IDLE); - feeder.setGoal(Feeder.Goal.IDLE); - intake.setGoal(Intake.Goal.IDLE); - } - case INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.IDLE); - feeder.setGoal(Feeder.Goal.INTAKING); - intake.setGoal(Intake.Goal.INTAKE); - } - case STATION_INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.INTAKING); - feeder.setGoal(Feeder.Goal.REVERSE_INTAKING); - intake.setGoal(Intake.Goal.IDLE); - } - case REVERSE_INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); - feeder.setGoal(Feeder.Goal.REVERSE_INTAKING); - intake.setGoal(Intake.Goal.REVERSE_INTAKE); - } - case PREPARE_SHOOT -> { - var aimingParams = RobotState.getInstance().getAimingParameters(); - arm.setSetpoint( - new Rotation2d( - aimingParams.effectiveDistance(), - FieldConstants.Speaker.centerSpeakerOpening.getZ() - + Units.inchesToMeters(yCompensation.get()))); - flywheels.setGoal(Flywheels.Goal.SHOOTING); - feeder.setGoal(Feeder.Goal.IDLE); - intake.setGoal(Intake.Goal.IDLE); - } - case SHOOT -> { - feeder.setGoal(Feeder.Goal.FEEDING); - gamepieceState = GamepieceState.NO_GAMEPIECE; - } + } + case STATION_INTAKE -> currentState = SystemState.STATION_INTAKE; + case INTAKE -> currentState = SystemState.INTAKE; + case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT; + case SHOOT -> { + if (currentState != SystemState.PREPARE_SHOOT) { + currentState = SystemState.PREPARE_SHOOT; + } else if (atShootingSetpoint()) { + currentState = SystemState.SHOOT; + followThroughTimer.restart(); + goalState = SystemState.IDLE; } - - Logger.recordOutput("Superstructure/GoalState", goalState); - Logger.recordOutput("Superstructure/CurrentState", currentState); + } } - @AutoLogOutput(key = "Superstructure/ReadyToShoot") - public boolean atShootingSetpoint() { - return flywheels.atSetpoint(); + switch (currentState) { + case IDLE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); + flywheels.setGoal(Flywheels.Goal.IDLE); + } + case INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); + flywheels.setGoal(Flywheels.Goal.IDLE); + } + case STATION_INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get())); + flywheels.setGoal(Flywheels.Goal.INTAKING); + } + case REVERSE_INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); + } + case PREPARE_SHOOT -> { + var aimingParams = RobotState.getInstance().getAimingParameters(); + arm.setSetpoint(aimingParams.armAngle()); + flywheels.setGoal(Flywheels.Goal.SHOOTING); + } + case SHOOT -> { + gamepieceState = GamepieceState.NO_GAMEPIECE; + } } + + Logger.recordOutput("Superstructure/GoalState", goalState); + Logger.recordOutput("Superstructure/CurrentState", currentState); + } + + @AutoLogOutput(key = "Superstructure/ReadyToShoot") + public boolean atShootingSetpoint() { + return flywheels.atSetpoint(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java index 9a81948b..c0de5346 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java @@ -7,87 +7,74 @@ public class SuperstructureConstants { - public static class FeederConstants { - public static double reduction = (1.0 / 1.0); - - public static int id = - switch (Constants.getRobot()) { - default -> 6; - }; - public static boolean inverted = - switch (Constants.getRobot()) { - default -> false; - }; - } - - public static class FlywheelConstants { - // encoder / flywheelReduction = flywheel - public static double reduction = (1.0 / 2.0); - public static double shooterToleranceRPM = 50.0; - public static int leftID = 5; - public static int rightID = 4; - - public static Gains gains = - switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); - case DEVBOT -> new Gains(0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0); - case COMPBOT -> null; - }; - - public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} - } - - public static class IntakeConstants { - public static double reduction = (18.0 / 12.0); - public static int id = - switch (Constants.getRobot()) { - default -> 45; - }; - public static boolean inverted = - switch (Constants.getRobot()) { - default -> false; - }; - } - - public static class ArmConstants { - // reduction is 12:62 18:60 12:65 - public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); - public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); - public static Translation2d armOrigin = - new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); - public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); - public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); - - public static int leaderID = 25; - public static int followerID = 26; - public static int armEncoderID = 42; - - public static boolean leaderInverted = false; - public static boolean followerInverted = false; - - /** The offset of the arm encoder in rotations. */ - public static double armEncoderOffsetRotations = - Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); - - public static double armLength = - switch (Constants.getRobot()) { - case DEVBOT -> Units.inchesToMeters(24.8); - default -> Units.inchesToMeters(25.866); - }; - - public static Gains gains = - switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); - case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12); - case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - }; - - public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); - - public record ProfileConstraints( - double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {} - - public record Gains( - double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {} - } + public static class FlywheelConstants { + // encoder / flywheelReduction = flywheel + public static double reduction = (1.0 / 2.0); + public static double shooterToleranceRPM = 50.0; + public static int leftID = 5; + public static int rightID = 4; + + public static Gains gains = + switch (Constants.getRobot()) { + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); + case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); + case COMPBOT -> null; + }; + + public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} + } + + public static class IntakeConstants { + public static double reduction = (18.0 / 12.0); + public static int id = + switch (Constants.getRobot()) { + default -> 45; + }; + public static boolean inverted = + switch (Constants.getRobot()) { + default -> false; + }; + } + + public static class ArmConstants { + // reduction is 12:62 18:60 12:65 + public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); + public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); + public static Translation2d armOrigin = + new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); + public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); + public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); + + public static int leaderID = 25; + public static int followerID = 26; + public static int armEncoderID = 42; + + public static boolean leaderInverted = false; + public static boolean followerInverted = false; + + /** The offset of the arm encoder in rotations. */ + public static double armEncoderOffsetRotations = + Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); + + public static double armLength = + switch (Constants.getRobot()) { + case DEVBOT -> Units.inchesToMeters(24.8); + default -> Units.inchesToMeters(25.866); + }; + + public static Gains gains = + switch (Constants.getRobot()) { + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); + case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12); + case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + }; + + public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); + + public record ProfileConstraints( + double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {} + + public record Gains( + double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {} + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 75960f07..14446fc6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -2,6 +2,7 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -19,120 +20,124 @@ import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { - private static final LoggedTunableNumber kP = new LoggedTunableNumber("Arm/kP", gains.kP()); - private static final LoggedTunableNumber kI = new LoggedTunableNumber("Arm/kI", gains.kI()); - private static final LoggedTunableNumber kD = new LoggedTunableNumber("Arm/kD", gains.kD()); - private static final LoggedTunableNumber kS = new LoggedTunableNumber("Arm/kS", gains.ffkS()); - private static final LoggedTunableNumber kV = new LoggedTunableNumber("Arm/kV", gains.ffkV()); - private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA()); - private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG()); - private static final LoggedTunableNumber armVelocity = - new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec()); - private static final LoggedTunableNumber armAcceleration = - new LoggedTunableNumber("Arm/Acceleration", profileConstraints.accelerationRadPerSec2()); - private static final LoggedTunableNumber armToleranceDegreees = - new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); - private static final LoggedTunableNumber armLowerLimit = - new LoggedTunableNumber("Arm/LowerLimitDegrees", 7.0); - private static final LoggedTunableNumber armUpperLimit = - new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); - - private final ArmIO io; - private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - - private final Mechanism2d armMechanism; - private final MechanismRoot2d armRoot; - private final MechanismLigament2d armMeasured; - - private boolean homed = false; - @Setter private Rotation2d setpoint = null; - - // private final MechanismLigament2d armSetpoint; - - public Arm(ArmIO io) { - System.out.println("[Init] Creating Arm"); - this.io = io; - io.setBrakeMode(true); - - // Create a mechanism - armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite)); - armRoot = armMechanism.getRoot("Arm Joint", armOrigin.getX(), armOrigin.getY()); - armMeasured = - new MechanismLigament2d( - "Arm Measured", - armLength, - Units.radiansToDegrees(inputs.armPositionRads), - 2.0, - new Color8Bit(Color.kBlack)); - armRoot.append(armMeasured); + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Arm/kP", gains.kP()); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Arm/kI", gains.kI()); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Arm/kD", gains.kD()); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Arm/kS", gains.ffkS()); + private static final LoggedTunableNumber kV = new LoggedTunableNumber("Arm/kV", gains.ffkV()); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA()); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG()); + private static final LoggedTunableNumber armVelocity = + new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec()); + private static final LoggedTunableNumber armAcceleration = + new LoggedTunableNumber("Arm/Acceleration", profileConstraints.accelerationRadPerSec2()); + private static final LoggedTunableNumber armToleranceDegreees = + new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); + private static final LoggedTunableNumber armLowerLimit = + new LoggedTunableNumber("Arm/LowerLimitDegrees", 15.0); + private static final LoggedTunableNumber armUpperLimit = + new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); + + private final ArmIO io; + private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + + private final Mechanism2d armMechanism; + private final MechanismRoot2d armRoot; + private final MechanismLigament2d armMeasured; + + private boolean homed = false; + @Setter private Rotation2d setpoint = null; + + // private final MechanismLigament2d armSetpoint; + + public Arm(ArmIO io) { + System.out.println("[Init] Creating Arm"); + this.io = io; + io.setBrakeMode(true); + + // Create a mechanism + armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite)); + armRoot = armMechanism.getRoot("Arm Joint", armOrigin.getX(), armOrigin.getY()); + armMeasured = + new MechanismLigament2d( + "Arm Measured", + armLength, + Units.radiansToDegrees(inputs.armPositionRads), + 2.0, + new Color8Bit(Color.kBlack)); + armRoot.append(armMeasured); + } + + @Override + public void periodic() { + // Process inputs + io.updateInputs(inputs); + Logger.processInputs("Arm", inputs); + + // Update controllers + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); + LoggedTunableNumber.ifChanged( + hashCode(), () -> io.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); + LoggedTunableNumber.ifChanged( + hashCode(), + constraints -> io.setProfileConstraints(constraints[0], constraints[1]), + armVelocity, + armAcceleration); + + if (setpoint != null) { + io.setSetpoint( + MathUtil.clamp( + setpoint.getRadians(), + Units.degreesToRadians(armLowerLimit.get()), + Units.degreesToRadians(armUpperLimit.get()))); } - @Override - public void periodic() { - // Process inputs - io.updateInputs(inputs); - Logger.processInputs("Arm", inputs); - - // Update controllers - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); - LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); - LoggedTunableNumber.ifChanged( - hashCode(), - constraints -> io.setProfileConstraints(constraints[0], constraints[1]), - armVelocity, - armAcceleration); - - if (setpoint != null) { - io.setSetpoint(setpoint.getRadians()); - } - - if (DriverStation.isDisabled()) { - io.stop(); - } - - // Logs - armMeasured.setAngle(Units.radiansToDegrees(inputs.armPositionRads)); - Logger.recordOutput("Arm/Mechanism", armMechanism); + if (DriverStation.isDisabled()) { + io.stop(); } - public void runVolts(double volts) { - setpoint = null; - io.runVolts(volts); - } - - public void stop() { - io.stop(); - } - - public Rotation2d getAngle() { - return Rotation2d.fromRadians(inputs.armPositionRads); - } - - @AutoLogOutput(key = "Arm/SetpointAngle") - public Rotation2d getSetpoint() { - return setpoint != null ? setpoint : new Rotation2d(); - } - - @AutoLogOutput(key = "Arm/Homed") - public boolean homed() { - return homed; - } - - @AutoLogOutput(key = "Arm/AtSetpoint") - public boolean atSetpoint() { - return setpoint != null - && Math.abs(Rotation2d.fromRadians(inputs.armPositionRads).minus(setpoint).getDegrees()) - <= armToleranceDegreees.get(); - } - - public Command getStaticCurrent() { - Timer timer = new Timer(); - return run(() -> io.runCurrent(0.5 * timer.get())) - .beforeStarting(timer::restart) - .until(() -> Math.abs(inputs.armVelocityRadsPerSec) >= Units.degreesToRadians(10)) - .andThen(() -> Logger.recordOutput("Arm/staticCurrent", inputs.armTorqueCurrentAmps[0])) - .andThen(io::stop); - } + // Logs + armMeasured.setAngle(Units.radiansToDegrees(inputs.armPositionRads)); + Logger.recordOutput("Arm/Mechanism", armMechanism); + } + + public void runVolts(double volts) { + setpoint = null; + io.runVolts(volts); + } + + public void stop() { + io.stop(); + } + + public Rotation2d getAngle() { + return Rotation2d.fromRadians(inputs.armPositionRads); + } + + @AutoLogOutput(key = "Arm/SetpointAngle") + public Rotation2d getSetpoint() { + return setpoint != null ? setpoint : new Rotation2d(); + } + + @AutoLogOutput(key = "Arm/Homed") + public boolean homed() { + return homed; + } + + @AutoLogOutput(key = "Arm/AtSetpoint") + public boolean atSetpoint() { + return setpoint != null + && Math.abs(Rotation2d.fromRadians(inputs.armPositionRads).minus(setpoint).getDegrees()) + <= armToleranceDegreees.get(); + } + + public Command getStaticCurrent() { + Timer timer = new Timer(); + return run(() -> io.runCurrent(0.5 * timer.get())) + .beforeStarting(timer::restart) + .until(() -> Math.abs(inputs.armVelocityRadsPerSec) >= Units.degreesToRadians(10)) + .andThen(() -> Logger.recordOutput("Arm/staticCurrent", inputs.armTorqueCurrentAmps[0])) + .andThen(io::stop); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java index 6df3240c..3a88b23d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java @@ -3,50 +3,50 @@ import org.littletonrobotics.junction.AutoLog; public interface ArmIO { - @AutoLog - class ArmIOInputs { - public boolean hasFoc = false; - public boolean hasAbsoluteSensor = false; - public double armPositionRads = 0.0; + @AutoLog + class ArmIOInputs { + public boolean hasFoc = false; + public boolean hasAbsoluteSensor = false; + public double armPositionRads = 0.0; - public double armEncoderPositionRads = 0.0; + public double armEncoderPositionRads = 0.0; - public double armEncoderAbsolutePositionRads = 0.0; - public double armTrajectorySetpointRads = 0.0; - public double armVelocityRadsPerSec = 0.0; - public double[] armAppliedVolts = new double[] {}; - public double[] armCurrentAmps = new double[] {}; - public double[] armTorqueCurrentAmps = new double[] {}; - public double[] armTempCelcius = new double[] {}; - } + public double armEncoderAbsolutePositionRads = 0.0; + public double armTrajectorySetpointRads = 0.0; + public double armVelocityRadsPerSec = 0.0; + public double[] armAppliedVolts = new double[] {}; + public double[] armCurrentAmps = new double[] {}; + public double[] armTorqueCurrentAmps = new double[] {}; + public double[] armTempCelcius = new double[] {}; + } - default void updateInputs(ArmIOInputs inputs) {} + default void updateInputs(ArmIOInputs inputs) {} - /** Run to setpoint angle in radians */ - default void setSetpoint(double setpointRads) {} + /** Run to setpoint angle in radians */ + default void setSetpoint(double setpointRads) {} - /** Run motors at volts */ - default void runVolts(double volts) {} + /** Run motors at volts */ + default void runVolts(double volts) {} - /** Run motors at current */ - default void runCurrent(double amps) {} + /** Run motors at current */ + default void runCurrent(double amps) {} - /** Set brake mode enabled */ - default void setBrakeMode(boolean enabled) {} + /** Set brake mode enabled */ + default void setBrakeMode(boolean enabled) {} - /** Set FF values */ - default void setFF(double s, double v, double a, double g) {} + /** Set FF values */ + default void setFF(double s, double v, double a, double g) {} - /** Set PID values */ - default void setPID(double p, double i, double d) {} + /** Set PID values */ + default void setPID(double p, double i, double d) {} - /** Set MotionMagic constraints */ - default void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {} + /** Set MotionMagic constraints */ + default void setProfileConstraints( + double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {} - /** Sets position of internal encoder */ - default void setPosition(double positionRads) {} + /** Sets position of internal encoder */ + default void setPosition(double positionRads) {} - /** Stops motors */ - default void stop() {} + /** Stops motors */ + default void stop() {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index 591e8c74..1ed6ed22 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -16,194 +16,194 @@ import java.util.List; public class ArmIOKrakenFOC implements ArmIO { - private final TalonFX leaderMotor; - private final TalonFX followerMotor; - private final CANcoder armEncoder; - - private final StatusSignal armPositionRotations; - - private final StatusSignal armEncoderPositionRotations; - private final StatusSignal armEncoderAbsolutePositionRotations; - private final StatusSignal armTrajectorySetpointPositionRotations; - private final StatusSignal armVelocityRps; - private final List> armAppliedVoltage; - private final List> armOutputCurrent; - private final List> armTorqueCurrent; - private final List> armTempCelsius; - - Slot0Configs controllerConfig; - - public ArmIOKrakenFOC() { - leaderMotor = new TalonFX(leaderID, "canivore"); - followerMotor = new TalonFX(followerID, "canivore"); - followerMotor.setControl(new Follower(leaderID, true)); - armEncoder = new CANcoder(armEncoderID, "canivore"); - - // Arm Encoder Configs - CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration(); - armEncoderConfig.MagnetSensor.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - armEncoderConfig.MagnetSensor.MagnetOffset = armEncoderOffsetRotations; - armEncoder.getConfigurator().apply(armEncoderConfig, 1); - - // Leader motor configs - TalonFXConfiguration leaderConfig = new TalonFXConfiguration(); - leaderConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - leaderConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - leaderConfig.MotorOutput.Inverted = - leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - leaderConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - leaderConfig.Feedback.FeedbackRemoteSensorID = armEncoderID; - leaderConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder; - leaderConfig.Feedback.SensorToMechanismRatio = 1.0; - leaderConfig.Feedback.RotorToSensorRatio = reduction; - - controllerConfig = - new Slot0Configs() - .withKP(gains.kP()) - .withKI(gains.kI()) - .withKD(gains.kD()) - .withKS(gains.ffkS()) - .withKV(gains.ffkV()) - .withKA(gains.ffkA()) - .withKG(gains.ffkG()) - .withGravityType(GravityTypeValue.Arm_Cosine); - leaderConfig.Slot0 = controllerConfig; - - leaderConfig.MotionMagic = - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity( - Units.radiansToRotations(profileConstraints.cruiseVelocityRadPerSec())) - .withMotionMagicAcceleration( - Units.radiansToRotations(profileConstraints.accelerationRadPerSec2())); - leaderMotor.getConfigurator().apply(leaderConfig, 1); - - // Follower configs - TalonFXConfiguration followerConfig = new TalonFXConfiguration(); - followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - // Status signals - armPositionRotations = leaderMotor.getPosition(); - armEncoderPositionRotations = armEncoder.getPosition(); - armEncoderAbsolutePositionRotations = armEncoder.getAbsolutePosition(); - armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); - armVelocityRps = leaderMotor.getVelocity(); - armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage()); - armOutputCurrent = List.of(leaderMotor.getSupplyCurrent(), followerMotor.getSupplyCurrent()); - armTorqueCurrent = List.of(leaderMotor.getTorqueCurrent(), followerMotor.getTorqueCurrent()); - armTempCelsius = List.of(leaderMotor.getDeviceTemp(), followerMotor.getDeviceTemp()); - - BaseStatusSignal.setUpdateFrequencyForAll( - 100, - armPositionRotations, - armEncoderPositionRotations, - armEncoderAbsolutePositionRotations, - armTrajectorySetpointPositionRotations, - armVelocityRps, - armAppliedVoltage.get(0), - armAppliedVoltage.get(1), - armOutputCurrent.get(0), - armOutputCurrent.get(1), - armTorqueCurrent.get(0), - armTorqueCurrent.get(1), - armTempCelsius.get(0), - armTempCelsius.get(1)); - } - - public void updateInputs(ArmIOInputs inputs) { - inputs.hasFoc = true; - inputs.hasAbsoluteSensor = true; - - BaseStatusSignal.refreshAll( - armPositionRotations, - armEncoderPositionRotations, - armEncoderAbsolutePositionRotations, - armTrajectorySetpointPositionRotations, - armVelocityRps, - armAppliedVoltage.get(0), - armAppliedVoltage.get(1), - armOutputCurrent.get(0), - armOutputCurrent.get(1), - armTorqueCurrent.get(0), - armTorqueCurrent.get(1), - armTempCelsius.get(0), - armTempCelsius.get(1)); - - inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); - inputs.armEncoderPositionRads = - Units.rotationsToRadians(armEncoderPositionRotations.getValue()); - inputs.armEncoderAbsolutePositionRads = - Units.rotationsToRadians(armEncoderAbsolutePositionRotations.getValue()); - inputs.armTrajectorySetpointRads = - Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); - inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); - inputs.armAppliedVolts = - armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); - inputs.armCurrentAmps = - armOutputCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); - inputs.armTorqueCurrentAmps = - armTorqueCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); - inputs.armTempCelcius = - armTempCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); - } - - @Override - public void setSetpoint(double setpointRads) { - leaderMotor.setControl(new MotionMagicTorqueCurrentFOC(Units.radiansToRotations(setpointRads))); - } - - @Override - public void runVolts(double volts) { - leaderMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); - } - - @Override - public void runCurrent(double amps) { - leaderMotor.setControl(new TorqueCurrentFOC(amps)); - } - - @Override - public void setBrakeMode(boolean enabled) { - leaderMotor.setNeutralMode(enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast); - followerMotor.setNeutralMode(enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast); - } - - @Override - public void setPID(double p, double i, double d) { - controllerConfig.kP = p; - controllerConfig.kI = i; - controllerConfig.kD = d; - leaderMotor.getConfigurator().apply(controllerConfig); - } - - @Override - public void setFF(double s, double v, double a, double g) { - controllerConfig.kS = s; - controllerConfig.kV = v; - controllerConfig.kA = a; - controllerConfig.kG = g; - leaderMotor.getConfigurator().apply(controllerConfig); - } - - @Override - public void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { - leaderMotor - .getConfigurator() - .apply( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(Units.radiansToRotations(cruiseVelocityRadsPerSec)) - .withMotionMagicAcceleration(Units.radiansToRotations(accelerationRadsPerSec2))); - } - - @Override - public void setPosition(double positionRads) { - leaderMotor.setPosition(Units.radiansToRotations(positionRads)); - } - - @Override - public void stop() { - leaderMotor.setControl(new NeutralOut()); - } + private final TalonFX leaderMotor; + private final TalonFX followerMotor; + private final CANcoder armEncoder; + + private final StatusSignal armPositionRotations; + + private final StatusSignal armEncoderPositionRotations; + private final StatusSignal armEncoderAbsolutePositionRotations; + private final StatusSignal armTrajectorySetpointPositionRotations; + private final StatusSignal armVelocityRps; + private final List> armAppliedVoltage; + private final List> armOutputCurrent; + private final List> armTorqueCurrent; + private final List> armTempCelsius; + + Slot0Configs controllerConfig; + + public ArmIOKrakenFOC() { + leaderMotor = new TalonFX(leaderID, "canivore"); + followerMotor = new TalonFX(followerID, "canivore"); + followerMotor.setControl(new Follower(leaderID, true)); + armEncoder = new CANcoder(armEncoderID, "canivore"); + + // Arm Encoder Configs + CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration(); + armEncoderConfig.MagnetSensor.AbsoluteSensorRange = + AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + armEncoderConfig.MagnetSensor.MagnetOffset = armEncoderOffsetRotations; + armEncoder.getConfigurator().apply(armEncoderConfig, 1); + + // Leader motor configs + TalonFXConfiguration leaderConfig = new TalonFXConfiguration(); + leaderConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + leaderConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + leaderConfig.MotorOutput.Inverted = + leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + leaderConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + leaderConfig.Feedback.FeedbackRemoteSensorID = armEncoderID; + leaderConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder; + leaderConfig.Feedback.SensorToMechanismRatio = 1.0; + leaderConfig.Feedback.RotorToSensorRatio = reduction; + + controllerConfig = + new Slot0Configs() + .withKP(gains.kP()) + .withKI(gains.kI()) + .withKD(gains.kD()) + .withKS(gains.ffkS()) + .withKV(gains.ffkV()) + .withKA(gains.ffkA()) + .withKG(gains.ffkG()) + .withGravityType(GravityTypeValue.Arm_Cosine); + leaderConfig.Slot0 = controllerConfig; + + leaderConfig.MotionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity( + Units.radiansToRotations(profileConstraints.cruiseVelocityRadPerSec())) + .withMotionMagicAcceleration( + Units.radiansToRotations(profileConstraints.accelerationRadPerSec2())); + leaderMotor.getConfigurator().apply(leaderConfig, 1); + + // Follower configs + TalonFXConfiguration followerConfig = new TalonFXConfiguration(); + followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + // Status signals + armPositionRotations = leaderMotor.getPosition(); + armEncoderPositionRotations = armEncoder.getPosition(); + armEncoderAbsolutePositionRotations = armEncoder.getAbsolutePosition(); + armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); + armVelocityRps = leaderMotor.getVelocity(); + armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage()); + armOutputCurrent = List.of(leaderMotor.getSupplyCurrent(), followerMotor.getSupplyCurrent()); + armTorqueCurrent = List.of(leaderMotor.getTorqueCurrent(), followerMotor.getTorqueCurrent()); + armTempCelsius = List.of(leaderMotor.getDeviceTemp(), followerMotor.getDeviceTemp()); + + BaseStatusSignal.setUpdateFrequencyForAll( + 100, + armPositionRotations, + armEncoderPositionRotations, + armEncoderAbsolutePositionRotations, + armTrajectorySetpointPositionRotations, + armVelocityRps, + armAppliedVoltage.get(0), + armAppliedVoltage.get(1), + armOutputCurrent.get(0), + armOutputCurrent.get(1), + armTorqueCurrent.get(0), + armTorqueCurrent.get(1), + armTempCelsius.get(0), + armTempCelsius.get(1)); + } + + public void updateInputs(ArmIOInputs inputs) { + inputs.hasFoc = true; + inputs.hasAbsoluteSensor = true; + + BaseStatusSignal.refreshAll( + armPositionRotations, + armEncoderPositionRotations, + armEncoderAbsolutePositionRotations, + armTrajectorySetpointPositionRotations, + armVelocityRps, + armAppliedVoltage.get(0), + armAppliedVoltage.get(1), + armOutputCurrent.get(0), + armOutputCurrent.get(1), + armTorqueCurrent.get(0), + armTorqueCurrent.get(1), + armTempCelsius.get(0), + armTempCelsius.get(1)); + + inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); + inputs.armEncoderPositionRads = + Units.rotationsToRadians(armEncoderPositionRotations.getValue()); + inputs.armEncoderAbsolutePositionRads = + Units.rotationsToRadians(armEncoderAbsolutePositionRotations.getValue()); + inputs.armTrajectorySetpointRads = + Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); + inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); + inputs.armAppliedVolts = + armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); + inputs.armCurrentAmps = + armOutputCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); + inputs.armTorqueCurrentAmps = + armTorqueCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); + inputs.armTempCelcius = + armTempCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); + } + + @Override + public void setSetpoint(double setpointRads) { + leaderMotor.setControl(new MotionMagicTorqueCurrentFOC(Units.radiansToRotations(setpointRads))); + } + + @Override + public void runVolts(double volts) { + leaderMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void runCurrent(double amps) { + leaderMotor.setControl(new TorqueCurrentFOC(amps)); + } + + @Override + public void setBrakeMode(boolean enabled) { + leaderMotor.setNeutralMode(enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast); + followerMotor.setNeutralMode(enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void setPID(double p, double i, double d) { + controllerConfig.kP = p; + controllerConfig.kI = i; + controllerConfig.kD = d; + leaderMotor.getConfigurator().apply(controllerConfig); + } + + @Override + public void setFF(double s, double v, double a, double g) { + controllerConfig.kS = s; + controllerConfig.kV = v; + controllerConfig.kA = a; + controllerConfig.kG = g; + leaderMotor.getConfigurator().apply(controllerConfig); + } + + @Override + public void setProfileConstraints( + double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { + leaderMotor + .getConfigurator() + .apply( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(Units.radiansToRotations(cruiseVelocityRadsPerSec)) + .withMotionMagicAcceleration(Units.radiansToRotations(accelerationRadsPerSec2))); + } + + @Override + public void setPosition(double positionRads) { + leaderMotor.setPosition(Units.radiansToRotations(positionRads)); + } + + @Override + public void stop() { + leaderMotor.setControl(new NeutralOut()); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index 9f8d2c4f..f3499b2f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -12,113 +12,113 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; public class ArmIOSim implements ArmIO { - private final SingleJointedArmSim sim = - new SingleJointedArmSim( - DCMotor.getKrakenX60Foc(2), - reduction, - 1.06328, - armLength, - minAngle.getRadians(), - maxAngle.getRadians(), - true, - Units.degreesToRadians(0.0)); - - private final ProfiledPIDController profiledController; - private ArmFeedforward ff; - private double appliedVoltage = 0.0; - private double positionOffset = 0.0; - - private boolean controllerNeedsReset = false; - private boolean closedLoop = false; - - public ArmIOSim() { - ff = new ArmFeedforward(gains.ffkS(), gains.ffkG(), gains.ffkV(), gains.ffkA()); - profiledController = - new ProfiledPIDController( - gains.kP(), - gains.kI(), - gains.kD(), - new TrapezoidProfile.Constraints( - profileConstraints.cruiseVelocityRadPerSec(), - profileConstraints.accelerationRadPerSec2()), - 0.001); - sim.setState(Units.degreesToRadians(45.0), 0.0); + private final SingleJointedArmSim sim = + new SingleJointedArmSim( + DCMotor.getKrakenX60Foc(2), + reduction, + 1.06328, + armLength, + minAngle.getRadians(), + maxAngle.getRadians(), + true, + Units.degreesToRadians(0.0)); + + private final ProfiledPIDController profiledController; + private ArmFeedforward ff; + private double appliedVoltage = 0.0; + private double positionOffset = 0.0; + + private boolean controllerNeedsReset = false; + private boolean closedLoop = false; + + public ArmIOSim() { + ff = new ArmFeedforward(gains.ffkS(), gains.ffkG(), gains.ffkV(), gains.ffkA()); + profiledController = + new ProfiledPIDController( + gains.kP(), + gains.kI(), + gains.kD(), + new TrapezoidProfile.Constraints( + profileConstraints.cruiseVelocityRadPerSec(), + profileConstraints.accelerationRadPerSec2()), + 0.001); + sim.setState(Units.degreesToRadians(45.0), 0.0); + } + + @Override + public void updateInputs(ArmIOInputs inputs) { + if (DriverStation.isDisabled()) { + controllerNeedsReset = true; } - @Override - public void updateInputs(ArmIOInputs inputs) { - if (DriverStation.isDisabled()) { - controllerNeedsReset = true; - } - - if (controllerNeedsReset) { - profiledController.reset(sim.getAngleRads() + positionOffset, sim.getVelocityRadPerSec()); - controllerNeedsReset = false; - } - - for (int i = 0; i < 20; i++) { - // control - if (closedLoop) { - appliedVoltage = - profiledController.calculate(sim.getAngleRads() + positionOffset) - + ff.calculate( - profiledController.getSetpoint().position, - profiledController.getSetpoint().velocity); - sim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); - } - sim.update(0.001); - } - - inputs.armPositionRads = sim.getAngleRads() + positionOffset; - inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; - inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); - inputs.armAppliedVolts = new double[] {appliedVoltage}; - inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; - inputs.armTorqueCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; - inputs.armTempCelcius = new double[] {0.0}; + if (controllerNeedsReset) { + profiledController.reset(sim.getAngleRads() + positionOffset, sim.getVelocityRadPerSec()); + controllerNeedsReset = false; } - @Override - public void setSetpoint(double setpointRads) { - if (!closedLoop) { - controllerNeedsReset = true; - } - closedLoop = true; - profiledController.setGoal(setpointRads); + for (int i = 0; i < 20; i++) { + // control + if (closedLoop) { + appliedVoltage = + profiledController.calculate(sim.getAngleRads() + positionOffset) + + ff.calculate( + profiledController.getSetpoint().position, + profiledController.getSetpoint().velocity); + sim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + } + sim.update(0.001); } - @Override - public void runVolts(double volts) { - closedLoop = false; - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void setPID(double p, double i, double d) { - profiledController.setPID(p, i, d); - } - - @Override - public void setFF(double s, double v, double a, double g) { - ff = new ArmFeedforward(s, g, v, a); - } - - @Override - public void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { - profiledController.setConstraints( - new TrapezoidProfile.Constraints(cruiseVelocityRadsPerSec, accelerationRadsPerSec2)); - } - - @Override - public void stop() { - appliedVoltage = 0.0; - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void setPosition(double position) { - positionOffset = position - sim.getAngleRads(); + inputs.armPositionRads = sim.getAngleRads() + positionOffset; + inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; + inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); + inputs.armAppliedVolts = new double[] {appliedVoltage}; + inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; + inputs.armTorqueCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; + inputs.armTempCelcius = new double[] {0.0}; + } + + @Override + public void setSetpoint(double setpointRads) { + if (!closedLoop) { + controllerNeedsReset = true; } + closedLoop = true; + profiledController.setGoal(setpointRads); + } + + @Override + public void runVolts(double volts) { + closedLoop = false; + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void setPID(double p, double i, double d) { + profiledController.setPID(p, i, d); + } + + @Override + public void setFF(double s, double v, double a, double g) { + ff = new ArmFeedforward(s, g, v, a); + } + + @Override + public void setProfileConstraints( + double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { + profiledController.setConstraints( + new TrapezoidProfile.Constraints(cruiseVelocityRadsPerSec, accelerationRadsPerSec2)); + } + + @Override + public void stop() { + appliedVoltage = 0.0; + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void setPosition(double position) { + positionOffset = position - sim.getAngleRads(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java deleted file mode 100644 index d8e1a1c4..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.frc2024.util.LoggedTunableNumber; -import org.littletonrobotics.junction.Logger; - -public class Feeder extends SubsystemBase { - private static final LoggedTunableNumber intakeVoltage = - new LoggedTunableNumber("Feeder/IntakeVoltage", 4.0); - private static final LoggedTunableNumber feedVoltage = - new LoggedTunableNumber("Feeder/FeedVoltage", 12.0); - - private final FeederIO io; - private FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); - - private double leftSetpointRpm; - private double rightSetpointRPM; - - public Feeder(FeederIO io) { - this.io = io; - } - - public enum Goal { - IDLE, - FEEDING, - INTAKING, - REVERSE_INTAKING - } - - @Getter @Setter private Goal goal = Goal.IDLE; - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Feeder", inputs); - - if (DriverStation.isDisabled()) { - stop(); - setGoal(Goal.IDLE); - } else { - switch (goal) { - case IDLE -> runVolts(0.0); - case FEEDING -> runVolts(feedVoltage.get()); - case INTAKING -> runVolts(intakeVoltage.get()); - case REVERSE_INTAKING -> runVolts(-intakeVoltage.get()); - } - } - - Logger.recordOutput("Feeder/Goal", goal); - } - - public boolean hasGamepiece() { - return inputs.outputCurrent >= 40.0; - } - - public void runVolts(double volts) { - io.runVolts(volts); - } - - public void stop() { - io.stop(); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java deleted file mode 100644 index d5095e1c..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; - -import org.littletonrobotics.junction.AutoLog; - -public interface FeederIO { - @AutoLog - class FeederIOInputs { - public double positionRads = 0.0; - public double velocityRadsPerSec = 0.0; - public double appliedVoltage = 0.0; - public double outputCurrent = 0.0; - } - - default void updateInputs(FeederIOInputs inputs) {} - - /** Run feeder at volts */ - default void runVolts(double volts) {} - - /** Stop feeder */ - default void stop() {} -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java deleted file mode 100644 index 583251d8..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; - -public class FeederIOKrakenFOC implements FeederIO { - private final TalonFX motor; - - private StatusSignal position; - private StatusSignal velocity; - private StatusSignal appliedVoltage; - private StatusSignal outputCurrent; - - public FeederIOKrakenFOC() { - motor = new TalonFX(id, "canivore"); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = - inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.CurrentLimits.SupplyCurrentLimit = 30.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.Voltage.PeakForwardVoltage = 12.0; - config.Voltage.PeakReverseVoltage = -12.0; - motor.getConfigurator().apply(config); - - position = motor.getPosition(); - velocity = motor.getVelocity(); - appliedVoltage = motor.getMotorVoltage(); - outputCurrent = motor.getTorqueCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, position, velocity, appliedVoltage, outputCurrent); - } - - @Override - public void updateInputs(FeederIOInputs inputs) { - BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, outputCurrent); - - inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction; - inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - inputs.outputCurrent = outputCurrent.getValueAsDouble(); - } - - @Override - public void runVolts(double volts) { - motor.setControl(new VoltageOut(volts).withEnableFOC(true)); - } - - @Override - public void stop() { - motor.setControl(new NeutralOut()); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java deleted file mode 100644 index 659ade9f..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -public class FeederIOSim implements FeederIO { - private final FlywheelSim sim = new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.01); - - private double appliedVoltage = 0.0; - - @Override - public void updateInputs(FeederIOInputs inputs) { - sim.update(0.02); - inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02; - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.outputCurrent = sim.getCurrentDrawAmps(); - } - - @Override - public void runVolts(double volts) { - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void stop() { - runVolts(0.0); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java deleted file mode 100644 index bd4d8a27..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; - -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.util.Units; - -public class FeederIOSparkFlex implements FeederIO { - private final CANSparkFlex motor; - private final RelativeEncoder encoder; - - public FeederIOSparkFlex() { - motor = new CANSparkFlex(id, CANSparkBase.MotorType.kBrushless); - - motor.setSmartCurrentLimit(80); - motor.setInverted(inverted); - motor.setIdleMode(CANSparkBase.IdleMode.kBrake); - - encoder = motor.getEncoder(); - } - - @Override - public void updateInputs(FeederIOInputs inputs) { - inputs.positionRads = Units.rotationsToRadians(encoder.getPosition()) / reduction; - inputs.velocityRadsPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()) / reduction; - inputs.appliedVoltage = motor.getAppliedOutput() * motor.getBusVoltage(); - inputs.outputCurrent = motor.getOutputCurrent(); - } - - @Override - public void runVolts(double volts) { - motor.setVoltage(volts); - } - - @Override - public void stop() { - motor.stopMotor(); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java index b559dffe..826beb1d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java @@ -11,113 +11,113 @@ import org.littletonrobotics.junction.Logger; public class Flywheels extends SubsystemBase { - private static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheels/kP", gains.kP()); - private static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheels/kI", gains.kI()); - private static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheels/kD", gains.kD()); - private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); - private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); - private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); - - private static LoggedTunableNumber shootingLeftRPM = - new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0); - private static LoggedTunableNumber shootingRightRPM = - new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0); - private static LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0); - private static LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); - - private static LoggedTunableNumber intakingLeftRPM = - new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); - private static LoggedTunableNumber intakingRightRPM = - new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); - private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM); - - private final FlywheelsIO io; - private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); - - private Double leftSetpointRpm = null; - private Double rightSetpointRPM = null; - - public enum Goal { - IDLE, - SHOOTING, - INTAKING, - - CHARACTERIZATION + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheels/kP", gains.kP()); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheels/kI", gains.kI()); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheels/kD", gains.kD()); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); + private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); + + private static LoggedTunableNumber shootingLeftRPM = + new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0); + private static LoggedTunableNumber shootingRightRPM = + new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0); + private static LoggedTunableNumber idleLeftRPM = + new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0); + private static LoggedTunableNumber idleRightRPM = + new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); + + private static LoggedTunableNumber intakingLeftRPM = + new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); + private static LoggedTunableNumber intakingRightRPM = + new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); + private static final LoggedTunableNumber shooterTolerance = + new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM); + + private final FlywheelsIO io; + private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); + + private Double leftSetpointRpm = null; + private Double rightSetpointRPM = null; + + public enum Goal { + IDLE, + SHOOTING, + INTAKING, + + CHARACTERIZATION + } + + @Getter @Setter private Goal goal = Goal.IDLE; + + private void setSetpoint(double left, double right) { + leftSetpointRpm = left; + rightSetpointRPM = right; + io.runVelocity(left, right); + } + + private void stop() { + leftSetpointRpm = null; + rightSetpointRPM = null; + io.stop(); + } + + public Flywheels(FlywheelsIO io) { + System.out.println("[Init] Creating Shooter"); + this.io = io; + } + + @Override + public void periodic() { + // check controllers + LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD); + LoggedTunableNumber.ifChanged( + hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); + + io.updateInputs(inputs); + Logger.processInputs("Flywheels", inputs); + + if (DriverStation.isDisabled()) { + stop(); + setGoal(Goal.IDLE); + } else { + switch (goal) { + case IDLE -> setSetpoint(idleLeftRPM.get(), idleRightRPM.get()); + case INTAKING -> setSetpoint(intakingLeftRPM.get(), intakingRightRPM.get()); + case SHOOTING -> setSetpoint(shootingLeftRPM.get(), shootingRightRPM.get()); + } } - @Getter @Setter private Goal goal = Goal.IDLE; - - private void setSetpoint(double left, double right) { - leftSetpointRpm = left; - rightSetpointRPM = right; - io.runVelocity(left, right); - } - - private void stop() { - leftSetpointRpm = null; - rightSetpointRPM = null; - io.stop(); - } - - public Flywheels(FlywheelsIO io) { - System.out.println("[Init] Creating Shooter"); - this.io = io; - } - - @Override - public void periodic() { - // check controllers - LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD); - LoggedTunableNumber.ifChanged( - hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); - - io.updateInputs(inputs); - Logger.processInputs("Flywheels", inputs); - - if (DriverStation.isDisabled()) { - stop(); - setGoal(Goal.IDLE); - } else { - switch (goal) { - case IDLE -> setSetpoint(idleLeftRPM.get(), idleRightRPM.get()); - case INTAKING -> setSetpoint(intakingLeftRPM.get(), intakingRightRPM.get()); - case SHOOTING -> setSetpoint(shootingLeftRPM.get(), shootingRightRPM.get()); - } - } - - Logger.recordOutput("Flywheels/Goal", goal); - Logger.recordOutput( - "Flywheels/LeftSetpointRPM", leftSetpointRpm != null ? leftSetpointRpm : 0.0); - Logger.recordOutput( - "Flywheels/RightSetpointRPM", rightSetpointRPM != null ? rightSetpointRPM : 0.0); - Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); - Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); - } - - public void runLeftCharacterizationVolts(double volts) { - io.runCharacterizationLeftVolts(volts); - } - - public void runRightCharacterizationVolts(double volts) { - io.runCharacterizationRightVolts(volts); - } - - public double getLeftCharacterizationVelocity() { - return inputs.leftVelocityRpm; - } - - public double getRightCharacterizationVelocity() { - return inputs.rightVelocityRpm; - } - - @AutoLogOutput(key = "Shooter/AtSetpoint") - public boolean atSetpoint() { - return leftSetpointRpm != null - && rightSetpointRPM != null - && Math.abs(inputs.leftVelocityRpm - leftSetpointRpm) <= shooterTolerance.get() - && Math.abs(inputs.rightVelocityRpm - rightSetpointRPM) <= shooterTolerance.get(); - } + Logger.recordOutput("Flywheels/Goal", goal); + Logger.recordOutput( + "Flywheels/LeftSetpointRPM", leftSetpointRpm != null ? leftSetpointRpm : 0.0); + Logger.recordOutput( + "Flywheels/RightSetpointRPM", rightSetpointRPM != null ? rightSetpointRPM : 0.0); + Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); + Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); + } + + public void runLeftCharacterizationVolts(double volts) { + io.runCharacterizationLeftVolts(volts); + } + + public void runRightCharacterizationVolts(double volts) { + io.runCharacterizationRightVolts(volts); + } + + public double getLeftCharacterizationVelocity() { + return inputs.leftVelocityRpm; + } + + public double getRightCharacterizationVelocity() { + return inputs.rightVelocityRpm; + } + + @AutoLogOutput(key = "Shooter/AtSetpoint") + public boolean atSetpoint() { + return leftSetpointRpm != null + && rightSetpointRPM != null + && Math.abs(inputs.leftVelocityRpm - leftSetpointRpm) <= shooterTolerance.get() + && Math.abs(inputs.rightVelocityRpm - rightSetpointRPM) <= shooterTolerance.get(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java index a9ee2783..ce155b32 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java @@ -3,42 +3,42 @@ import org.littletonrobotics.junction.AutoLog; public interface FlywheelsIO { - @AutoLog - class FlywheelsIOInputs { - public double leftPositionRads = 0.0; - public double leftVelocityRpm = 0.0; - public double leftAppliedVolts = 0.0; - public double leftOutputCurrent = 0.0; - public double leftTempCelsius = 0.0; + @AutoLog + class FlywheelsIOInputs { + public double leftPositionRads = 0.0; + public double leftVelocityRpm = 0.0; + public double leftAppliedVolts = 0.0; + public double leftOutputCurrent = 0.0; + public double leftTempCelsius = 0.0; - public double rightPositionRads = 0.0; - public double rightVelocityRpm = 0.0; - public double rightAppliedVolts = 0.0; - public double rightOutputCurrent = 0.0; - public double rightTempCelsius = 0.0; - } + public double rightPositionRads = 0.0; + public double rightVelocityRpm = 0.0; + public double rightAppliedVolts = 0.0; + public double rightOutputCurrent = 0.0; + public double rightTempCelsius = 0.0; + } - /** Update inputs */ - default void updateInputs(FlywheelsIOInputs inputs) {} + /** Update inputs */ + default void updateInputs(FlywheelsIOInputs inputs) {} - /** Run both motors at voltage */ - default void runVolts(double leftVolts, double rightVolts) {} + /** Run both motors at voltage */ + default void runVolts(double leftVolts, double rightVolts) {} - /** Stop both flywheels */ - default void stop() {} + /** Stop both flywheels */ + default void stop() {} - /** Run left and right flywheels at velocity in rpm */ - default void runVelocity(double leftRpm, double rightRpm) {} + /** Run left and right flywheels at velocity in rpm */ + default void runVelocity(double leftRpm, double rightRpm) {} - /** Config PID values for both motors */ - default void setPID(double kP, double kI, double kD) {} + /** Config PID values for both motors */ + default void setPID(double kP, double kI, double kD) {} - /** Config FF values for both motors */ - default void setFF(double kS, double kV, double kA) {} + /** Config FF values for both motors */ + default void setFF(double kS, double kV, double kA) {} - /** Run left flywheels at voltage */ - default void runCharacterizationLeftVolts(double volts) {} + /** Run left flywheels at voltage */ + default void runCharacterizationLeftVolts(double volts) {} - /** Run right flywheels at voltage */ - default void runCharacterizationRightVolts(double volts) {} + /** Run right flywheels at voltage */ + default void runCharacterizationRightVolts(double volts) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java index 741487e3..4e686c4d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java @@ -10,91 +10,91 @@ import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FlywheelsIOSim implements FlywheelsIO { - private final FlywheelSim leftSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); - private final FlywheelSim rightSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); - - private final PIDController leftController = - new PIDController(gains.kP(), gains.kI(), gains.kD()); - private final PIDController rightController = - new PIDController(gains.kP(), gains.kI(), gains.kD()); - private SimpleMotorFeedforward ff = - new SimpleMotorFeedforward(gains.kS(), gains.kV(), gains.kA()); - - private double leftAppliedVolts = 0.0; - private double rightAppliedVolts = 0.0; - - private Double leftSetpointRPM = null; - private Double rightSetpointRPM = null; - - @Override - public void updateInputs(FlywheelsIOInputs inputs) { - leftSim.update(0.02); - rightSim.update(0.02); - // control to setpoint - if (leftSetpointRPM != null && rightSetpointRPM != null) { - runVolts( - leftController.calculate(leftSim.getAngularVelocityRPM(), leftSetpointRPM) - + ff.calculate(leftSetpointRPM), - rightController.calculate(rightSim.getAngularVelocityRPM(), rightSetpointRPM) - + ff.calculate(rightSetpointRPM)); - } - - inputs.leftPositionRads += - Units.radiansToRotations(leftSim.getAngularVelocityRadPerSec() * 0.02); - inputs.leftVelocityRpm = leftSim.getAngularVelocityRPM(); - inputs.leftAppliedVolts = leftAppliedVolts; - inputs.leftOutputCurrent = leftSim.getCurrentDrawAmps(); - - inputs.rightPositionRads += - Units.radiansToRotations(rightSim.getAngularVelocityRadPerSec() * 0.02); - inputs.rightVelocityRpm = rightSim.getAngularVelocityRPM(); - inputs.rightAppliedVolts = rightAppliedVolts; - inputs.rightOutputCurrent = rightSim.getCurrentDrawAmps(); + private final FlywheelSim leftSim = + new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + private final FlywheelSim rightSim = + new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + + private final PIDController leftController = + new PIDController(gains.kP(), gains.kI(), gains.kD()); + private final PIDController rightController = + new PIDController(gains.kP(), gains.kI(), gains.kD()); + private SimpleMotorFeedforward ff = + new SimpleMotorFeedforward(gains.kS(), gains.kV(), gains.kA()); + + private double leftAppliedVolts = 0.0; + private double rightAppliedVolts = 0.0; + + private Double leftSetpointRPM = null; + private Double rightSetpointRPM = null; + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + leftSim.update(0.02); + rightSim.update(0.02); + // control to setpoint + if (leftSetpointRPM != null && rightSetpointRPM != null) { + runVolts( + leftController.calculate(leftSim.getAngularVelocityRPM(), leftSetpointRPM) + + ff.calculate(leftSetpointRPM), + rightController.calculate(rightSim.getAngularVelocityRPM(), rightSetpointRPM) + + ff.calculate(rightSetpointRPM)); } - @Override - public void runVolts(double leftVolts, double rightVolts) { - leftAppliedVolts = MathUtil.clamp(leftVolts, -12.0, 12.0); - rightAppliedVolts = MathUtil.clamp(rightVolts, -12.0, 12.0); - leftSim.setInputVoltage(leftAppliedVolts); - rightSim.setInputVoltage(rightAppliedVolts); - } - - @Override - public void runVelocity(double leftRpm, double rightRpm) { - leftSetpointRPM = leftRpm; - rightSetpointRPM = rightRpm; - } - - @Override - public void setPID(double kP, double kI, double kD) { - leftController.setPID(kP, kI, kD); - rightController.setPID(kP, kI, kD); - } - - @Override - public void setFF(double kS, double kV, double kA) { - ff = new SimpleMotorFeedforward(kS, kV, kA); - } - - @Override - public void stop() { - runVelocity(0.0, 0.0); - } - - @Override - public void runCharacterizationLeftVolts(double volts) { - leftSetpointRPM = null; - rightSetpointRPM = null; - runVolts(volts, 0.0); - } - - @Override - public void runCharacterizationRightVolts(double volts) { - leftSetpointRPM = null; - rightSetpointRPM = null; - runVolts(0.0, volts); - } + inputs.leftPositionRads += + Units.radiansToRotations(leftSim.getAngularVelocityRadPerSec() * 0.02); + inputs.leftVelocityRpm = leftSim.getAngularVelocityRPM(); + inputs.leftAppliedVolts = leftAppliedVolts; + inputs.leftOutputCurrent = leftSim.getCurrentDrawAmps(); + + inputs.rightPositionRads += + Units.radiansToRotations(rightSim.getAngularVelocityRadPerSec() * 0.02); + inputs.rightVelocityRpm = rightSim.getAngularVelocityRPM(); + inputs.rightAppliedVolts = rightAppliedVolts; + inputs.rightOutputCurrent = rightSim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double leftVolts, double rightVolts) { + leftAppliedVolts = MathUtil.clamp(leftVolts, -12.0, 12.0); + rightAppliedVolts = MathUtil.clamp(rightVolts, -12.0, 12.0); + leftSim.setInputVoltage(leftAppliedVolts); + rightSim.setInputVoltage(rightAppliedVolts); + } + + @Override + public void runVelocity(double leftRpm, double rightRpm) { + leftSetpointRPM = leftRpm; + rightSetpointRPM = rightRpm; + } + + @Override + public void setPID(double kP, double kI, double kD) { + leftController.setPID(kP, kI, kD); + rightController.setPID(kP, kI, kD); + } + + @Override + public void setFF(double kS, double kV, double kA) { + ff = new SimpleMotorFeedforward(kS, kV, kA); + } + + @Override + public void stop() { + runVelocity(0.0, 0.0); + } + + @Override + public void runCharacterizationLeftVolts(double volts) { + leftSetpointRPM = null; + rightSetpointRPM = null; + runVolts(volts, 0.0); + } + + @Override + public void runCharacterizationRightVolts(double volts) { + leftSetpointRPM = null; + rightSetpointRPM = null; + runVolts(0.0, volts); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java index ba4235be..942a231b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java @@ -10,122 +10,122 @@ import edu.wpi.first.math.util.Units; public class FlywheelsIOSparkFlex implements FlywheelsIO { - // Hardware - private CANSparkFlex leftMotor; - private CANSparkFlex rightMotor; - private RelativeEncoder leftEncoder; - private RelativeEncoder rightEncoder; - - // Controllers - private SparkPIDController leftController; - private SparkPIDController rightController; - // Open loop - private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); - - public FlywheelsIOSparkFlex() { - // Init Hardware - leftMotor = new CANSparkFlex(leftID, CANSparkFlex.MotorType.kBrushless); - rightMotor = new CANSparkFlex(rightID, CANSparkFlex.MotorType.kBrushless); - leftEncoder = leftMotor.getEncoder(); - rightEncoder = rightMotor.getEncoder(); - - // Config Hardware - // Default - leftMotor.restoreFactoryDefaults(); - rightMotor.restoreFactoryDefaults(); - - // Limits - leftMotor.setSmartCurrentLimit(60); - rightMotor.setSmartCurrentLimit(60); - leftMotor.enableVoltageCompensation(12.0); - rightMotor.enableVoltageCompensation(12.0); - - // Reset encoders - leftEncoder.setPosition(0.0); - rightEncoder.setPosition(0.0); - leftEncoder.setMeasurementPeriod(10); - rightEncoder.setMeasurementPeriod(10); - leftEncoder.setAverageDepth(2); - rightEncoder.setAverageDepth(2); - - // Get controllers - leftController = leftMotor.getPIDController(); - rightController = rightMotor.getPIDController(); - setPID(gains.kP(), gains.kI(), gains.kD()); - setFF(gains.kS(), gains.kV(), gains.kA()); - - // Disable brake mode - leftMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - rightMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - - leftMotor.burnFlash(); - rightMotor.burnFlash(); - } - - @Override - public void updateInputs(FlywheelsIOInputs inputs) { - inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / reduction; - inputs.leftVelocityRpm = leftEncoder.getVelocity() / reduction; - inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); - inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); - inputs.leftTempCelsius = leftMotor.getMotorTemperature(); - - inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / reduction; - inputs.rightVelocityRpm = rightEncoder.getVelocity() / reduction; - inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); - inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); - inputs.rightTempCelsius = rightMotor.getMotorTemperature(); - } - - @Override - public void runVolts(double leftVolts, double rightVolts) { - leftMotor.setVoltage(leftVolts); - rightMotor.setVoltage(rightVolts); - } - - @Override - public void runVelocity(double leftRpm, double rightRpm) { - leftController.setReference( - leftRpm, - CANSparkBase.ControlType.kVelocity, - 0, - ff.calculate(leftRpm), - SparkPIDController.ArbFFUnits.kVoltage); - rightController.setReference( - rightRpm, - CANSparkBase.ControlType.kVelocity, - 0, - ff.calculate(rightRpm), - SparkPIDController.ArbFFUnits.kVoltage); - } - - @Override - public void setPID(double kP, double kI, double kD) { - leftController.setP(kP); - leftController.setI(kI); - leftController.setD(kD); - rightController.setP(kP); - rightController.setI(kI); - rightController.setD(kD); - } - - @Override - public void setFF(double kS, double kV, double kA) { - ff = new SimpleMotorFeedforward(kS, kV, kA); - } - - @Override - public void runCharacterizationLeftVolts(double volts) { - leftMotor.setVoltage(volts); - } - - @Override - public void runCharacterizationRightVolts(double volts) { - rightMotor.setVoltage(volts); - } - - @Override - public void stop() { - runVelocity(0.0, 0.0); - } + // Hardware + private CANSparkFlex leftMotor; + private CANSparkFlex rightMotor; + private RelativeEncoder leftEncoder; + private RelativeEncoder rightEncoder; + + // Controllers + private SparkPIDController leftController; + private SparkPIDController rightController; + // Open loop + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); + + public FlywheelsIOSparkFlex() { + // Init Hardware + leftMotor = new CANSparkFlex(leftID, CANSparkFlex.MotorType.kBrushless); + rightMotor = new CANSparkFlex(rightID, CANSparkFlex.MotorType.kBrushless); + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); + + // Config Hardware + // Default + leftMotor.restoreFactoryDefaults(); + rightMotor.restoreFactoryDefaults(); + + // Limits + leftMotor.setSmartCurrentLimit(60); + rightMotor.setSmartCurrentLimit(60); + leftMotor.enableVoltageCompensation(12.0); + rightMotor.enableVoltageCompensation(12.0); + + // Reset encoders + leftEncoder.setPosition(0.0); + rightEncoder.setPosition(0.0); + leftEncoder.setMeasurementPeriod(10); + rightEncoder.setMeasurementPeriod(10); + leftEncoder.setAverageDepth(2); + rightEncoder.setAverageDepth(2); + + // Get controllers + leftController = leftMotor.getPIDController(); + rightController = rightMotor.getPIDController(); + setPID(gains.kP(), gains.kI(), gains.kD()); + setFF(gains.kS(), gains.kV(), gains.kA()); + + // Disable brake mode + leftMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + rightMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + + leftMotor.burnFlash(); + rightMotor.burnFlash(); + } + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / reduction; + inputs.leftVelocityRpm = leftEncoder.getVelocity() / reduction; + inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); + inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); + inputs.leftTempCelsius = leftMotor.getMotorTemperature(); + + inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / reduction; + inputs.rightVelocityRpm = rightEncoder.getVelocity() / reduction; + inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); + inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); + inputs.rightTempCelsius = rightMotor.getMotorTemperature(); + } + + @Override + public void runVolts(double leftVolts, double rightVolts) { + leftMotor.setVoltage(leftVolts); + rightMotor.setVoltage(rightVolts); + } + + @Override + public void runVelocity(double leftRpm, double rightRpm) { + leftController.setReference( + leftRpm, + CANSparkBase.ControlType.kVelocity, + 0, + ff.calculate(leftRpm), + SparkPIDController.ArbFFUnits.kVoltage); + rightController.setReference( + rightRpm, + CANSparkBase.ControlType.kVelocity, + 0, + ff.calculate(rightRpm), + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setPID(double kP, double kI, double kD) { + leftController.setP(kP); + leftController.setI(kI); + leftController.setD(kD); + rightController.setP(kP); + rightController.setI(kI); + rightController.setD(kD); + } + + @Override + public void setFF(double kS, double kV, double kA) { + ff = new SimpleMotorFeedforward(kS, kV, kA); + } + + @Override + public void runCharacterizationLeftVolts(double volts) { + leftMotor.setVoltage(volts); + } + + @Override + public void runCharacterizationRightVolts(double volts) { + rightMotor.setVoltage(volts); + } + + @Override + public void stop() { + runVelocity(0.0, 0.0); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java deleted file mode 100644 index 7755b8a6..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.intake; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.frc2024.util.LoggedTunableNumber; -import org.littletonrobotics.junction.Logger; - -public class Intake extends SubsystemBase { - private final LoggedTunableNumber intakeVolts = - new LoggedTunableNumber("Intake/intakeVoltage", 8.0); - private final LoggedTunableNumber ejectVolts = new LoggedTunableNumber("Intake/ejectVolts", -2.0); - private final IntakeIO io; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - - public enum Goal { - IDLE, - INTAKE, - REVERSE_INTAKE - } - - @Setter @Getter Goal goal = Goal.IDLE; - - public Intake(IntakeIO io) { - System.out.println("[Init] Creating Intake"); - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); - - switch (goal) { - case IDLE -> io.stop(); - case INTAKE -> io.runVolts(intakeVolts.get()); - case REVERSE_INTAKE -> io.runVolts(ejectVolts.get()); - } - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java deleted file mode 100644 index 047c3fe7..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.intake; - -import org.littletonrobotics.junction.AutoLog; - -public interface IntakeIO { - @AutoLog - class IntakeIOInputs { - public double velocityRadsPerSec = 0.0; - public double positionRads = 0.0; - public double appliedVoltage = 0.0; - public double currentAmps = 0.0; - } - - /** Update inputs */ - default void updateInputs(IntakeIOInputs inputs) {} - - /** Set voltage of intake */ - default void runVolts(double volts) {} - - /** Stop intake */ - default void stop() {} -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java deleted file mode 100644 index 573f0e19..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java +++ /dev/null @@ -1,69 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.intake; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.IntakeConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; - -public class IntakeIOKrakenFOC implements IntakeIO { - private final TalonFX motor; - - private final TalonFX otherMotor; - private StatusSignal velocityRadsPerSec; - private StatusSignal positionRads; - private StatusSignal appliedVoltage; - private StatusSignal currentAmps; - - public IntakeIOKrakenFOC() { - motor = new TalonFX(2, "canivore"); - otherMotor = new TalonFX(3); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimit = 30.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.Voltage.PeakForwardVoltage = 12.0; - config.Voltage.PeakReverseVoltage = -12.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = - inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); - - velocityRadsPerSec = motor.getVelocity(); - positionRads = motor.getPosition(); - appliedVoltage = motor.getMotorVoltage(); - currentAmps = motor.getTorqueCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, velocityRadsPerSec, positionRads, appliedVoltage, currentAmps); - } - - @Override - public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll(velocityRadsPerSec, positionRads, appliedVoltage, currentAmps); - - inputs.velocityRadsPerSec = - Units.rotationsToRadians(velocityRadsPerSec.getValueAsDouble()) / reduction; - inputs.positionRads = Units.rotationsToRadians(positionRads.getValueAsDouble()) / reduction; - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - inputs.currentAmps = currentAmps.getValueAsDouble(); - } - - @Override - public void runVolts(double volts) { - motor.setControl(new VoltageOut(volts).withEnableFOC(true)); - otherMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); - } - - @Override - public void stop() { - motor.setControl(new NeutralOut()); - otherMotor.setControl(new NeutralOut()); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java deleted file mode 100644 index ef73c587..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.intake; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -public class IntakeIOSim implements IntakeIO { - private final FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.0, 0.01); - - private double appliedVoltage = 0.0; - - @Override - public void updateInputs(IntakeIOInputs inputs) { - sim.update(0.02); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02; - inputs.appliedVoltage = appliedVoltage; - inputs.currentAmps = sim.getCurrentDrawAmps(); - } - - @Override - public void runVolts(double volts) { - appliedVoltage = volts; - sim.setInputVoltage(volts); - } - - @Override - public void stop() { - appliedVoltage = 0.0; - sim.setInputVoltage(0.0); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java deleted file mode 100644 index 324a8358..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.intake; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.IntakeConstants.*; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; - -public class IntakeIOSparkMax implements IntakeIO { - private final CANSparkMax motor; - private final RelativeEncoder encoder; - - public IntakeIOSparkMax() { - motor = new CANSparkMax(id, CANSparkMax.MotorType.kBrushless); - motor.restoreFactoryDefaults(); - motor.setInverted(inverted); - motor.setSmartCurrentLimit(80); - motor.enableVoltageCompensation(12.0); - - encoder = motor.getEncoder(); - encoder.setPositionConversionFactor(1.0 / reduction * 2 * Math.PI); - encoder.setVelocityConversionFactor(1.0 / reduction * 2 * Math.PI); - } - - @Override - public void updateInputs(IntakeIOInputs inputs) { - inputs.velocityRadsPerSec = encoder.getVelocity(); - inputs.positionRads = encoder.getPosition(); - inputs.appliedVoltage = motor.getAppliedOutput(); - inputs.currentAmps = motor.getOutputCurrent(); - } - - @Override - public void runVolts(double volts) { - motor.setVoltage(volts); - } - - @Override - public void stop() { - motor.stopMotor(); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/util/Alert.java b/src/main/java/org/littletonrobotics/frc2024/util/Alert.java index a7143c28..a4d3b8a4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/Alert.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/Alert.java @@ -21,127 +21,127 @@ /** Class for managing persistent alerts to be sent over NetworkTables. */ public class Alert { - private static Map groups = new HashMap(); + private static Map groups = new HashMap(); - private final AlertType type; - private boolean active = false; - private double activeStartTime = 0.0; - private String text; + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, - * the appropriate entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) { - this("Alerts", text, type); - } + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } - /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate - * entries will be added to NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String group, String text, AlertType type) { - if (!groups.containsKey(group)) { - groups.put(group, new SendableAlerts()); - SmartDashboard.putData(group, groups.get(group)); - } - - this.text = text; - this.type = type; - groups.get(group).alerts.add(this); + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); } - /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. - */ - public void set(boolean active) { - if (active && !this.active) { - activeStartTime = Timer.getFPGATimestamp(); - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case INFO: - System.out.println(text); - break; - } - } - this.active = active; + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + if (active && !this.active) { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } } + this.active = active; + } - /** Updates current alert text. */ - public void setText(String text) { - if (active && !text.equals(this.text)) { - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case INFO: - System.out.println(text); - break; - } - } - this.text = text; + /** Updates current alert text. */ + public void setText(String text) { + if (active && !text.equals(this.text)) { + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } } + this.text = text; + } - private static class SendableAlerts implements Sendable { - public final List alerts = new ArrayList<>(); + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); - public String[] getStrings(AlertType type) { - Predicate activeFilter = (Alert x) -> x.type == type && x.active; - Comparator timeSorter = - (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); - return alerts.stream() - .filter(activeFilter) - .sorted(timeSorter) - .map((Alert a) -> a.text) - .toArray(String[]::new); - } + public String[] getStrings(AlertType type) { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = + (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); } + } - /** Represents an alert's level of urgency. */ - public static enum AlertType { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - */ - ERROR, + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - */ - WARNING, + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type - * for problems which are unlikely to affect the robot's functionality, or any other alerts - * which do not fall under "ERROR" or "WARNING". - */ - INFO - } + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java index 4be6f237..0117e500 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java @@ -11,71 +11,71 @@ /** Utility functions for flipping from the blue to red alliance. */ public class AllianceFlipUtil { - /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ - public static double apply(double xCoordinate) { - if (shouldFlip()) { - return FieldConstants.fieldLength - xCoordinate; - } else { - return xCoordinate; - } + /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ + public static double apply(double xCoordinate) { + if (shouldFlip()) { + return FieldConstants.fieldLength - xCoordinate; + } else { + return xCoordinate; } + } - /** Flips a translation to the correct side of the field based on the current alliance color. */ - public static Translation2d apply(Translation2d translation) { - if (shouldFlip()) { - return new Translation2d(apply(translation.getX()), translation.getY()); - } else { - return translation; - } + /** Flips a translation to the correct side of the field based on the current alliance color. */ + public static Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(apply(translation.getX()), translation.getY()); + } else { + return translation; } + } - /** Flips a rotation based on the current alliance color. */ - public static Rotation2d apply(Rotation2d rotation) { - if (shouldFlip()) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); - } else { - return rotation; - } + /** Flips a rotation based on the current alliance color. */ + public static Rotation2d apply(Rotation2d rotation) { + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else { + return rotation; } + } - /** Flips a pose to the correct side of the field based on the current alliance color. */ - public static Pose2d apply(Pose2d pose) { - if (shouldFlip()) { - return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); - } else { - return pose; - } + /** Flips a pose to the correct side of the field based on the current alliance color. */ + public static Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } else { + return pose; } + } - public static Translation3d apply(Translation3d translation3d) { - if (shouldFlip()) { - return new Translation3d( - apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); - } else { - return translation3d; - } + public static Translation3d apply(Translation3d translation3d) { + if (shouldFlip()) { + return new Translation3d( + apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); + } else { + return translation3d; } + } - /** - * Flips a trajectory state to the correct side of the field based on the current alliance color. - */ - public static VehicleState apply(VehicleState state) { - if (shouldFlip()) { - return VehicleState.newBuilder() - .setX(apply(state.getX())) - .setY(state.getY()) - .setTheta(apply(new Rotation2d(state.getTheta())).getRadians()) - .setVx(-state.getVx()) - .setVy(state.getVy()) - .setOmega(state.getOmega()) - .build(); - } else { - return state; - } + /** + * Flips a trajectory state to the correct side of the field based on the current alliance color. + */ + public static VehicleState apply(VehicleState state) { + if (shouldFlip()) { + return VehicleState.newBuilder() + .setX(apply(state.getX())) + .setY(state.getY()) + .setTheta(apply(new Rotation2d(state.getTheta())).getRadians()) + .setVx(-state.getVx()) + .setVy(state.getVy()) + .setOmega(state.getOmega()) + .build(); + } else { + return state; } + } - public static boolean shouldFlip() { - return DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - } + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/EqualsUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/EqualsUtil.java index b0fcee95..feb27d03 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/EqualsUtil.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/EqualsUtil.java @@ -3,20 +3,20 @@ import edu.wpi.first.math.geometry.Twist2d; public class EqualsUtil { - public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); - } + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } - public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, 1e-9); - } + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, 1e-9); + } - /** Extension methods for wpi geometry objects */ - public static class GeomExtensions { - public static boolean epsilonEquals(Twist2d twist, Twist2d other) { - return EqualsUtil.epsilonEquals(twist.dx, other.dx) - && EqualsUtil.epsilonEquals(twist.dy, other.dy) - && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); - } + /** Extension methods for wpi geometry objects */ + public static class GeomExtensions { + public static boolean epsilonEquals(Twist2d twist, Twist2d other) { + return EqualsUtil.epsilonEquals(twist.dx, other.dx) + && EqualsUtil.epsilonEquals(twist.dy, other.dy) + && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); } + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/GeomUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/GeomUtil.java index b2920b16..928b40c9 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/GeomUtil.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/GeomUtil.java @@ -18,135 +18,135 @@ /** Geometry utilities for working with translations, rotations, transforms, and poses. */ public class GeomUtil { - /** - * Creates a pure translating transform - * - * @param translation The translation to create the transform with - * @return The resulting transform - */ - public static Transform2d toTransform2d(Translation2d translation) { - return new Transform2d(translation, new Rotation2d()); - } + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } - /** - * Creates a pure rotating transform - * - * @param rotation The rotation to create the transform with - * @return The resulting transform - */ - public static Transform2d toTransform2d(Rotation2d rotation) { - return new Transform2d(new Translation2d(), rotation); - } + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } - /** - * Converts a Pose2d to a Transform2d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform2d toTransform2d(Pose2d pose) { - return new Transform2d(pose.getTranslation(), pose.getRotation()); - } + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } - public static Pose2d inverse(Pose2d pose) { - Rotation2d rotationInverse = pose.getRotation().unaryMinus(); - return new Pose2d( - pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); - } + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } - /** - * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose2d toPose2d(Transform2d transform) { - return new Pose2d(transform.getTranslation(), transform.getRotation()); - } + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } - /** - * Creates a pure translated pose - * - * @param translation The translation to create the pose with - * @return The resulting pose - */ - public static Pose2d toPose2d(Translation2d translation) { - return new Pose2d(translation, new Rotation2d()); - } + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } - /** - * Creates a pure rotated pose - * - * @param rotation The rotation to create the pose with - * @return The resulting pose - */ - public static Pose2d toPose2d(Rotation2d rotation) { - return new Pose2d(new Translation2d(), rotation); - } + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } - /** - * Multiplies a twist by a scaling factor - * - * @param twist The twist to multiply - * @param factor The scaling factor for the twist components - * @return The new twist - */ - public static Twist2d multiply(Twist2d twist, double factor) { - return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); - } + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } - /** - * Converts a Pose3d to a Transform3d to be used in a kinematic chain - * - * @param pose The pose that will represent the transform - * @return The resulting transform - */ - public static Transform3d toTransform3d(Pose3d pose) { - return new Transform3d(pose.getTranslation(), pose.getRotation()); - } + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } - /** - * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic - * chain - * - * @param transform The transform that will represent the pose - * @return The resulting pose - */ - public static Pose3d toPose3d(Transform3d transform) { - return new Pose3d(transform.getTranslation(), transform.getRotation()); - } + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } - /** - * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain - * - * @param speeds The original translation - * @return The resulting translation - */ - public static Twist2d toTwist2d(ChassisSpeeds speeds) { - return new Twist2d( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); - } + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } - /** - * Creates a new pose from an existing one using a different translation value. - * - * @param pose The original pose - * @param translation The new translation to use - * @return The new pose with the new translation and original rotation - */ - public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { - return new Pose2d(translation, pose.getRotation()); - } + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } - /** - * Creates a new pose from an existing one using a different rotation value. - * - * @param pose The original pose - * @param rotation The new rotation to use - * @return The new pose with the original translation and new rotation - */ - public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { - return new Pose2d(pose.getTranslation(), rotation); - } + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/LoggedTunableNumber.java b/src/main/java/org/littletonrobotics/frc2024/util/LoggedTunableNumber.java index ef0cc705..b6f3de9c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/LoggedTunableNumber.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/LoggedTunableNumber.java @@ -4,6 +4,7 @@ import java.util.HashMap; import java.util.Map; import java.util.function.Consumer; +import java.util.function.DoubleSupplier; import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -11,101 +12,106 @@ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ -public class LoggedTunableNumber { - private static final String tableKey = "TunableNumbers"; +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "TunableNumbers"; - private final String key; - private boolean hasDefault = false; - private double defaultValue; - private LoggedDashboardNumber dashboardNumber; - private Map lastHasChangedValues = new HashMap<>(); + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - */ - public LoggedTunableNumber(String dashboardKey) { - this.key = tableKey + "/" + dashboardKey; - } + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue) { - this(dashboardKey); - initDefault(defaultValue); + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } } + } - /** - * Set the default value of the number. The default value can only be set once. - * - * @param defaultValue The default value - */ - public void initDefault(double defaultValue) { - if (!hasDefault) { - hasDefault = true; - this.defaultValue = defaultValue; - if (Constants.tuningMode) { - dashboardNumber = new LoggedDashboardNumber(key, defaultValue); - } - } + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; } + } - /** - * Get the current value, from dashboard if available and in tuning mode. - * - * @return The current value - */ - public double get() { - if (!hasDefault) { - return 0.0; - } else { - return Constants.tuningMode ? dashboardNumber.get() : defaultValue; - } + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + if (!Constants.tuningMode) return false; + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; } - /** - * Checks whether the number has changed since our last check - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple - * objects. Recommended approach is to pass the result of "hashCode()" - * @return True if the number has changed since the last time this method was called, false - * otherwise. - */ - public boolean hasChanged(int id) { - if (!Constants.tuningMode) return false; - double currentValue = get(); - Double lastValue = lastHasChangedValues.get(id); - if (lastValue == null || currentValue != lastValue) { - lastHasChangedValues.put(id, currentValue); - return true; - } + return false; + } - return false; + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); } + } - /** - * Runs action if any of the tunableNumbers have changed - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * - * objects. Recommended approach is to pass the result of "hashCode()" - * @param action Callback to run when any of the tunable numbers have changed. Access tunable - * numbers in order inputted in method - * @param tunableNumbers All tunable numbers to check - */ - public static void ifChanged( - int id, Consumer action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { - action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); - } - } + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } - /** Runs action if any of the tunableNumbers have changed */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - ifChanged(id, values -> action.run(), tunableNumbers); - } + @Override + public double getAsDouble() { + return get(); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/PolynomialRegression.java b/src/main/java/org/littletonrobotics/frc2024/util/PolynomialRegression.java index d5a6528c..607f2f1c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/PolynomialRegression.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/PolynomialRegression.java @@ -23,183 +23,183 @@ * @author Kevin Wayne */ public class PolynomialRegression implements Comparable { - private final String variableName; // name of the predictor variable - private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error - private double sst; // total sum of squares - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name - * of the predictor variable. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree) { - this(x, y, degree, "n"); - } - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @param variableName the name of the predictor variable - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { - this.degree = degree; - this.variableName = variableName; - - int n = x.length; - QRDecomposition qr = null; - Matrix matrixX = null; - - // in case Vandermonde matrix does not have full rank, reduce degree until it - // does - while (true) { - - // build Vandermonde matrix - double[][] vandermonde = new double[n][this.degree + 1]; - for (int i = 0; i < n; i++) { - for (int j = 0; j <= this.degree; j++) { - vandermonde[i][j] = Math.pow(x[i], j); - } - } - matrixX = new Matrix(vandermonde); - - // find least squares solution - qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; - - // decrease degree and try again - this.degree--; + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); } + } + matrixX = new Matrix(vandermonde); - // create matrix from vector - Matrix matrixY = new Matrix(y, n); - - // linear regression coefficients - beta = qr.solve(matrixY); - - // mean of y[] values - double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; - double mean = sum / n; + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; - // total variation to be accounted for - for (int i = 0; i < n; i++) { - double dev = y[i] - mean; - sst += dev * dev; - } - - // variation not accounted for - Matrix residuals = matrixX.times(beta).minus(matrixY); - sse = residuals.norm2() * residuals.norm2(); + // decrease degree and try again + this.degree--; } - /** - * Returns the {@code j}th regression coefficient. - * - * @param j the index - * @return the {@code j}th regression coefficient - */ - public double beta(int j) { - // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; - return beta.get(j, 0); - } + // create matrix from vector + Matrix matrixY = new Matrix(y, n); - /** - * Returns the degree of the polynomial to fit. - * - * @return the degree of the polynomial to fit - */ - public int degree() { - return degree; - } + // linear regression coefficients + beta = qr.solve(matrixY); - /** - * Returns the coefficient of determination R2. - * - * @return the coefficient of determination R2, which is a real number between - * 0 and 1 - */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function - return 1.0 - sse / sst; - } + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; - /** - * Returns the expected response {@code y} given the value of the predictor variable {@code x}. - * - * @param x the value of the predictor variable - * @return the expected response {@code y} given the value of the predictor variable {@code x} - */ - public double predict(double x) { - // horner's method - double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); - return y; + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; } - /** - * Returns a string representation of the polynomial regression model. - * - * @return a string representation of the polynomial regression model, including the best-fit - * polynomial and the coefficient of determination R2 - */ - public String toString() { - StringBuilder s = new StringBuilder(); - int j = degree; - - // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; - - // create remaining terms - while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); - else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); - j--; - } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); - - // replace "+ -2n" with "- 2n" - return s.toString().replace("+ -", "- "); + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.10f ", beta(j))); + else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); + j--; } - - /** Compare lexicographically. */ - public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; - int maxDegree = Math.max(this.degree(), that.degree()); - for (int j = maxDegree; j >= 0; j--) { - double term1 = 0.0; - double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; - } - return 0; - } - - /** - * Unit tests the {@code PolynomialRegression} data type. - * - * @param args the command-line arguments - */ - public static void main(String[] args) { - double[] x = {10, 20, 40, 80, 160, 200}; - double[] y = {100, 350, 1500, 6700, 20160, 40000}; - PolynomialRegression regression = new PolynomialRegression(x, y, 3); - - System.out.println(regression); + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); + } + + /** Compare lexicographically. */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; } + return 0; + } + + /** + * Unit tests the {@code PolynomialRegression} data type. + * + * @param args the command-line arguments + */ + public static void main(String[] args) { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + System.out.println(regression); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/PoseEstimator.java b/src/main/java/org/littletonrobotics/frc2024/util/PoseEstimator.java index cc2ed16f..4f689d5d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/PoseEstimator.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/PoseEstimator.java @@ -24,157 +24,157 @@ @ExtensionMethod({GeomUtil.class}) public class PoseEstimator { - private static final double historyLengthSecs = 0.3; + private static final double historyLengthSecs = 0.3; - private Pose2d basePose = new Pose2d(); + private Pose2d basePose = new Pose2d(); - /** -- GETTER -- Returns the latest robot pose based on drive and vision data. */ - @Getter private Pose2d latestPose = new Pose2d(); + /** -- GETTER -- Returns the latest robot pose based on drive and vision data. */ + @Getter private Pose2d latestPose = new Pose2d(); - private final NavigableMap updates = new TreeMap<>(); - private final Matrix q = new Matrix<>(Nat.N3(), Nat.N1()); + private final NavigableMap updates = new TreeMap<>(); + private final Matrix q = new Matrix<>(Nat.N3(), Nat.N1()); - public PoseEstimator(Matrix stateStdDevs) { - for (int i = 0; i < 3; ++i) { - q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); - } + public PoseEstimator(Matrix stateStdDevs) { + for (int i = 0; i < 3; ++i) { + q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); } + } + + /** Resets the odometry to a known pose. */ + public void resetPose(Pose2d pose) { + basePose = pose; + updates.clear(); + update(); + } + + /** Records a new drive movement. */ + public void addDriveData(double timestamp, Twist2d twist) { + updates.put(timestamp, new PoseUpdate(twist, new ArrayList<>())); + update(); + } + + /** Records a new set of vision updates. */ + public void addVisionData(List visionData) { + for (var timestampedVisionUpdate : visionData) { + var timestamp = timestampedVisionUpdate.timestamp(); + var visionUpdate = + new VisionUpdate(timestampedVisionUpdate.pose(), timestampedVisionUpdate.stdDevs()); + + if (updates.containsKey(timestamp)) { + // There was already an update at this timestamp, add to it + var oldVisionUpdates = updates.get(timestamp).visionUpdates(); + oldVisionUpdates.add(visionUpdate); + oldVisionUpdates.sort(VisionUpdate.compareDescStdDev); + + } else { + // Insert a new update + var prevUpdate = updates.floorEntry(timestamp); + var nextUpdate = updates.ceilingEntry(timestamp); + if (prevUpdate == null || nextUpdate == null) { + // Outside the range of existing data + return; + } - /** Resets the odometry to a known pose. */ - public void resetPose(Pose2d pose) { - basePose = pose; - updates.clear(); - update(); + // Create partial twists (prev -> vision, vision -> next) + var twist0 = + nextUpdate + .getValue() + .twist() + .multiply( + (timestamp - prevUpdate.getKey()) + / (nextUpdate.getKey() - prevUpdate.getKey())); + var twist1 = + nextUpdate + .getValue() + .twist() + .multiply( + (nextUpdate.getKey() - timestamp) + / (nextUpdate.getKey() - prevUpdate.getKey())); + + // Add new pose updates + var newVisionUpdates = new ArrayList(); + newVisionUpdates.add(visionUpdate); + newVisionUpdates.sort(VisionUpdate.compareDescStdDev); + updates.put(timestamp, new PoseUpdate(twist0, newVisionUpdates)); + updates.put( + nextUpdate.getKey(), new PoseUpdate(twist1, nextUpdate.getValue().visionUpdates())); + } } - /** Records a new drive movement. */ - public void addDriveData(double timestamp, Twist2d twist) { - updates.put(timestamp, new PoseUpdate(twist, new ArrayList<>())); - update(); + // Recalculate latest pose once + update(); + } + + /** Clears old data and calculates the latest pose. */ + private void update() { + // Clear old data and update base pose + while (updates.size() > 1 + && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { + var update = updates.pollFirstEntry(); + basePose = update.getValue().apply(basePose, q); } - /** Records a new set of vision updates. */ - public void addVisionData(List visionData) { - for (var timestampedVisionUpdate : visionData) { - var timestamp = timestampedVisionUpdate.timestamp(); - var visionUpdate = - new VisionUpdate(timestampedVisionUpdate.pose(), timestampedVisionUpdate.stdDevs()); - - if (updates.containsKey(timestamp)) { - // There was already an update at this timestamp, add to it - var oldVisionUpdates = updates.get(timestamp).visionUpdates(); - oldVisionUpdates.add(visionUpdate); - oldVisionUpdates.sort(VisionUpdate.compareDescStdDev); - - } else { - // Insert a new update - var prevUpdate = updates.floorEntry(timestamp); - var nextUpdate = updates.ceilingEntry(timestamp); - if (prevUpdate == null || nextUpdate == null) { - // Outside the range of existing data - return; - } - - // Create partial twists (prev -> vision, vision -> next) - var twist0 = - nextUpdate - .getValue() - .twist() - .multiply( - (timestamp - prevUpdate.getKey()) - / (nextUpdate.getKey() - prevUpdate.getKey())); - var twist1 = - nextUpdate - .getValue() - .twist() - .multiply( - (nextUpdate.getKey() - timestamp) - / (nextUpdate.getKey() - prevUpdate.getKey())); - - // Add new pose updates - var newVisionUpdates = new ArrayList(); - newVisionUpdates.add(visionUpdate); - newVisionUpdates.sort(VisionUpdate.compareDescStdDev); - updates.put(timestamp, new PoseUpdate(twist0, newVisionUpdates)); - updates.put( - nextUpdate.getKey(), new PoseUpdate(twist1, nextUpdate.getValue().visionUpdates())); - } - } - - // Recalculate latest pose once - update(); + // Update latest pose + latestPose = basePose; + for (var updateEntry : updates.entrySet()) { + latestPose = updateEntry.getValue().apply(latestPose, q); } - - /** Clears old data and calculates the latest pose. */ - private void update() { - // Clear old data and update base pose - while (updates.size() > 1 - && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { - var update = updates.pollFirstEntry(); - basePose = update.getValue().apply(basePose, q); + } + + /** + * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of + * vision updates. + */ + private record PoseUpdate(Twist2d twist, ArrayList visionUpdates) { + public Pose2d apply(Pose2d lastPose, Matrix q) { + // Apply drive twist + var pose = lastPose.exp(twist); + + // Apply vision updates + for (var visionUpdate : visionUpdates) { + // Calculate Kalman gains based on std devs + // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) + Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); + var r = new double[3]; + for (int i = 0; i < 3; ++i) { + r[i] = visionUpdate.stdDevs().get(i, 0) * visionUpdate.stdDevs().get(i, 0); } - - // Update latest pose - latestPose = basePose; - for (var updateEntry : updates.entrySet()) { - latestPose = updateEntry.getValue().apply(latestPose, q); + for (int row = 0; row < 3; ++row) { + if (q.get(row, 0) == 0.0) { + visionK.set(row, row, 0.0); + } else { + visionK.set( + row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); + } } - } - /** - * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of - * vision updates. - */ - private record PoseUpdate(Twist2d twist, ArrayList visionUpdates) { - public Pose2d apply(Pose2d lastPose, Matrix q) { - // Apply drive twist - var pose = lastPose.exp(twist); - - // Apply vision updates - for (var visionUpdate : visionUpdates) { - // Calculate Kalman gains based on std devs - // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) - Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); - var r = new double[3]; - for (int i = 0; i < 3; ++i) { - r[i] = visionUpdate.stdDevs().get(i, 0) * visionUpdate.stdDevs().get(i, 0); - } - for (int row = 0; row < 3; ++row) { - if (q.get(row, 0) == 0.0) { - visionK.set(row, row, 0.0); - } else { - visionK.set( - row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); - } - } - - // Calculate twist between current and vision pose - var visionTwist = pose.log(visionUpdate.pose()); - - // Multiply by Kalman gain matrix - var twistMatrix = - visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); - - // Apply twist - pose = - pose.exp( - new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); - } - - return pose; - } - } + // Calculate twist between current and vision pose + var visionTwist = pose.log(visionUpdate.pose()); - /** Represents a single vision pose with associated standard deviations. */ - public record VisionUpdate(Pose2d pose, Matrix stdDevs) { - public static final Comparator compareDescStdDev = - (VisionUpdate a, VisionUpdate b) -> { - return -Double.compare( - a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), - b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); - }; - } + // Multiply by Kalman gain matrix + var twistMatrix = + visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); + + // Apply twist + pose = + pose.exp( + new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); + } - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - public record TimestampedVisionUpdate(double timestamp, Pose2d pose, Matrix stdDevs) {} + return pose; + } + } + + /** Represents a single vision pose with associated standard deviations. */ + public record VisionUpdate(Pose2d pose, Matrix stdDevs) { + public static final Comparator compareDescStdDev = + (VisionUpdate a, VisionUpdate b) -> { + return -Double.compare( + a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), + b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); + }; + } + + /** Represents a single vision pose with a timestamp and associated standard deviations. */ + public record TimestampedVisionUpdate(double timestamp, Pose2d pose, Matrix stdDevs) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/VirtualSubsystem.java b/src/main/java/org/littletonrobotics/frc2024/util/VirtualSubsystem.java index 358658a4..3f5a1b27 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/VirtualSubsystem.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/VirtualSubsystem.java @@ -4,17 +4,17 @@ import java.util.List; public abstract class VirtualSubsystem { - private static List subsystems = new ArrayList<>(); + private static List subsystems = new ArrayList<>(); - public VirtualSubsystem() { - subsystems.add(this); - } + public VirtualSubsystem() { + subsystems.add(this); + } - public static void periodicAll() { - for (VirtualSubsystem subsystem : subsystems) { - subsystem.periodic(); - } + public static void periodicAll() { + for (VirtualSubsystem subsystem : subsystems) { + subsystem.periodic(); } + } - public abstract void periodic(); + public abstract void periodic(); } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/shooting/ShotCalculator.java b/src/main/java/org/littletonrobotics/frc2024/util/shooting/ShotCalculator.java index 8fed1ca6..c6fb6d23 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/shooting/ShotCalculator.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/shooting/ShotCalculator.java @@ -16,43 +16,43 @@ */ @ExtensionMethod({GeomUtil.class}) public class ShotCalculator { - public record ShotData( - double effectiveRobotToSpeakerDist, - double radialFeedforward, // ff value due to radial velocity of robot to speaker - Rotation2d goalHeading) {} // heading of robot to match tangential velocity + public record ShotData( + double effectiveRobotToSpeakerDist, + double radialFeedforward, // ff value due to radial velocity of robot to speaker + Rotation2d goalHeading) {} // heading of robot to match tangential velocity - /** In theory we will aim at different locations inside speaker */ - public static ShotData calculate( - Translation2d speaker, Translation2d robot, Translation2d linearFieldVelocity) { - // Calculate radial and tangential velocity from speaker - Rotation2d speakerToRobotAngle = robot.minus(speaker).getAngle(); - Translation2d tangentialVelocity = - linearFieldVelocity.rotateBy(speakerToRobotAngle.unaryMinus()); - // Positive when velocity is away from speaker - double radialComponent = tangentialVelocity.getX(); - // Positive when traveling CCW about speaker - double tangentialComponent = tangentialVelocity.getY(); + /** In theory we will aim at different locations inside speaker */ + public static ShotData calculate( + Translation2d speaker, Translation2d robot, Translation2d linearFieldVelocity) { + // Calculate radial and tangential velocity from speaker + Rotation2d speakerToRobotAngle = robot.minus(speaker).getAngle(); + Translation2d tangentialVelocity = + linearFieldVelocity.rotateBy(speakerToRobotAngle.unaryMinus()); + // Positive when velocity is away from speaker + double radialComponent = tangentialVelocity.getX(); + // Positive when traveling CCW about speaker + double tangentialComponent = tangentialVelocity.getY(); - // TODO: what does this do - // Ig this is the estimated time of the note in the air - // later on this will be a function of the distance - final double shotTime = 1.05; + // TODO: what does this do + // Ig this is the estimated time of the note in the air + // later on this will be a function of the distance + final double shotTime = 1.05; - // Add robot velocity to raw shot speed - double rawDistToGoal = robot.getDistance(speaker); - double shotSpeed = rawDistToGoal / shotTime + radialComponent; - if (shotSpeed <= 0.0) shotSpeed = 0.0; - // Rotate back into field frame then add take opposite - Rotation2d goalHeading = - robot.toPose2d().inverse().transformBy(speaker.toTransform2d()).getTranslation().getAngle(); - // Aim opposite of tangentialComponent (negative lead when tangentialComponent is positive) - goalHeading = goalHeading.plus(new Rotation2d(shotSpeed, tangentialComponent)); - double effectiveDist = shotTime * Math.hypot(tangentialComponent, shotSpeed); + // Add robot velocity to raw shot speed + double rawDistToGoal = robot.getDistance(speaker); + double shotSpeed = rawDistToGoal / shotTime + radialComponent; + if (shotSpeed <= 0.0) shotSpeed = 0.0; + // Rotate back into field frame then add take opposite + Rotation2d goalHeading = + robot.toPose2d().inverse().transformBy(speaker.toTransform2d()).getTranslation().getAngle(); + // Aim opposite of tangentialComponent (negative lead when tangentialComponent is positive) + goalHeading = goalHeading.plus(new Rotation2d(shotSpeed, tangentialComponent)); + double effectiveDist = shotTime * Math.hypot(tangentialComponent, shotSpeed); - Logger.recordOutput("ShootWhileMoving/heading", goalHeading); - Logger.recordOutput("ShootWhileMoving/radialFF", radialComponent); - Logger.recordOutput("ShootWhileMoving/effectiveDistance", effectiveDist); - // Use radial component of velocity for ff value - return new ShotData(effectiveDist, radialComponent, goalHeading); - } + Logger.recordOutput("ShootWhileMoving/heading", goalHeading); + Logger.recordOutput("ShootWhileMoving/driveFeedVelocity", radialComponent); + Logger.recordOutput("ShootWhileMoving/effectiveDistance", effectiveDist); + // Use radial component of velocity for ff value + return new ShotData(effectiveDist, radialComponent, goalHeading); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/swerve/ModuleLimits.java b/src/main/java/org/littletonrobotics/frc2024/util/swerve/ModuleLimits.java index 896b259b..d2fb7193 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/swerve/ModuleLimits.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/swerve/ModuleLimits.java @@ -1,4 +1,4 @@ package org.littletonrobotics.frc2024.util.swerve; public record ModuleLimits( - double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} + double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/util/swerve/SwerveSetpointGenerator.java b/src/main/java/org/littletonrobotics/frc2024/util/swerve/SwerveSetpointGenerator.java index aa79f024..9ec77d4f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/swerve/SwerveSetpointGenerator.java @@ -30,366 +30,366 @@ @RequiredArgsConstructor @ExtensionMethod({GeomUtil.class, EqualsUtil.GeomExtensions.class}) public class SwerveSetpointGenerator { - private final SwerveDriveKinematics kinematics; - private final Translation2d[] moduleLocations; + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; - /** - * Check if it would be faster to go to the opposite of the goal heading (and reverse drive - * direction). - * - * @param prevToGoal The rotation from the previous state to the goal state (i.e. - * prev.inverse().rotateBy(goal)). - * @return True if the shortest path to achieve this rotation involves flipping the drive - * direction. - */ - private boolean flipHeading(Rotation2d prevToGoal) { - return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; - } + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } - private double unwrapAngle(double ref, double angle) { - double diff = angle - ref; - if (diff > Math.PI) { - return angle - 2.0 * Math.PI; - } else if (diff < -Math.PI) { - return angle + 2.0 * Math.PI; - } else { - return angle; - } + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; } + } - @FunctionalInterface - private interface Function2d { - double f(double x, double y); - } + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } - /** - * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - * This is a pretty naive way to do root finding, but it's usually faster than simple bisection - * while being robust in ways that e.g. the Newton-Raphson method isn't. - * - * @param func The Function2d to take the root of. - * @param x_0 x value of the lower bracket. - * @param y_0 y value of the lower bracket. - * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - * recursion) - * @param x_1 x value of the upper bracket. - * @param y_1 y value of the upper bracket. - * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - * recursion) - * @param iterations_left Number of iterations of root finding left. - * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the - * (approximate) root. - */ - private double findRoot( - Function2d func, - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - int iterations_left) { - if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { - return 1.0; - } - var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); - var x_guess = (x_1 - x_0) * s_guess + x_0; - var y_guess = (y_1 - y_0) * s_guess + y_0; - var f_guess = func.f(x_guess, y_guess); - if (Math.signum(f_0) == Math.signum(f_guess)) { - // 0 and guess on same side of root, so use upper bracket. - return s_guess - + (1.0 - s_guess) - * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); - } else { - // Use lower bracket. - return s_guess - * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); - } + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return 1.0; } - - protected double findSteeringMaxS( - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - double max_deviation, - int max_iterations) { - f_1 = unwrapAngle(f_0, f_1); - double diff = f_1 - f_0; - if (Math.abs(diff) <= max_deviation) { - // Can go all the way to s=1. - return 1.0; - } - double offset = f_0 + Math.signum(diff) * max_deviation; - Function2d func = - (x, y) -> { - return unwrapAngle(f_0, Math.atan2(y, x)) - offset; - }; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); } + } - protected double findDriveMaxS( - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - double max_vel_step, - int max_iterations) { - double diff = f_1 - f_0; - if (Math.abs(diff) <= max_vel_step) { - // Can go all the way to s=1. - return 1.0; - } - double offset = f_0 + Math.signum(diff) * max_vel_step; - Function2d func = - (x, y) -> { - return Math.hypot(x, y) - offset; - }; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = + (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } - protected double findDriveMaxS( - double x_0, double y_0, double x_1, double y_1, double max_vel_step) { - // Our drive velocity between s=0 and s=1 is quadratic in s: - // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 - // = a * s^2 + b * s + c - // Where: - // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 - // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) - // c = x_0^2 + y_0^2 - // We want to find where this quadratic results in a velocity that is > max_vel_step from our - // velocity at s=0: - // sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... - final double dx = x_1 - x_0; - final double dy = y_1 - y_0; - final double a = dx * dx + dy * dy; - final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; - final double c = x_0 * x_0 + y_0 * y_0; - final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); - final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); - return 0.0; + protected double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = + (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } - /** - * Generate a new setpoint. - * - * @param limits The kinematic limits to respect for this setpoint. - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredState The desired state of motion, such as from the driver sticks or a path - * following algorithm. - * @param dt The loop time. - * @return A Setpoint object that satisfies all of the KinematicLimits while converging to - * desiredState quickly. - */ - public SwerveSetpoint generateSetpoint( - final ModuleLimits limits, - final SwerveSetpoint prevSetpoint, - ChassisSpeeds desiredState, - double dt) { - final Translation2d[] modules = moduleLocations; + protected double findDriveMaxS( + double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Our drive velocity between s=0 and s=1 is quadratic in s: + // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 + // = a * s^2 + b * s + c + // Where: + // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 + // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) + // c = x_0^2 + y_0^2 + // We want to find where this quadratic results in a velocity that is > max_vel_step from our + // velocity at s=0: + // sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... + final double dx = x_1 - x_0; + final double dy = y_1 - y_0; + final double a = dx * dx + dy * dy; + final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; + final double c = x_0 * x_0 + y_0 * y_0; + final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); + final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); + return 0.0; + } - SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); - // Make sure desiredState respects velocity limits. - if (limits.maxDriveVelocity() > 0.0) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); - desiredState = kinematics.toChassisSpeeds(desiredModuleState); - } - - // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so - // just use the previous angle. - boolean need_to_steer = true; - if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { - need_to_steer = false; - for (int i = 0; i < modules.length; ++i) { - desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; - desiredModuleState[i].speedMetersPerSecond = 0.0; - } - } + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredState The desired state of motion, such as from the driver sticks or a path + * following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all of the KinematicLimits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final ModuleLimits limits, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt) { + final Translation2d[] modules = moduleLocations; - // For each module, compute local Vx and Vy vectors. - double[] prev_vx = new double[modules.length]; - double[] prev_vy = new double[modules.length]; - Rotation2d[] prev_heading = new Rotation2d[modules.length]; - double[] desired_vx = new double[modules.length]; - double[] desired_vy = new double[modules.length]; - Rotation2d[] desired_heading = new Rotation2d[modules.length]; - boolean all_modules_should_flip = true; - for (int i = 0; i < modules.length; ++i) { - prev_vx[i] = - prevSetpoint.moduleStates()[i].angle.getCos() - * prevSetpoint.moduleStates()[i].speedMetersPerSecond; - prev_vy[i] = - prevSetpoint.moduleStates()[i].angle.getSin() - * prevSetpoint.moduleStates()[i].speedMetersPerSecond; - prev_heading[i] = prevSetpoint.moduleStates()[i].angle; - if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { - prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); - } - desired_vx[i] = - desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; - desired_vy[i] = - desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; - desired_heading[i] = desiredModuleState[i].angle; - if (desiredModuleState[i].speedMetersPerSecond < 0.0) { - desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); - } - if (all_modules_should_flip) { - double required_rotation_rad = - Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); - if (required_rotation_rad < Math.PI / 2.0) { - all_modules_should_flip = false; - } - } - } - if (all_modules_should_flip - && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) - && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { - // It will (likely) be faster to stop the robot, rotate the modules in place to the complement - // of the desired - // angle, and accelerate again. - return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); - } + SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.maxDriveVelocity() > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); + desiredState = kinematics.toChassisSpeeds(desiredModuleState); + } - // Compute the deltas between start and goal. We can then interpolate from the start state to - // the goal state; then - // find the amount we can move from start towards goal in this cycle such that no kinematic - // limit is exceeded. - double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; - double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; - double dtheta = - desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } - // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at - // desiredState. - double min_s = 1.0; + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d[] prev_heading = new Rotation2d[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d[] desired_heading = new Rotation2d[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = + prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = + prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.moduleStates()[i].angle; + if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + desired_vx[i] = + desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) + && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired + // angle, and accelerate again. + return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); + } - // In cases where an individual module is stopped, we want to remember the right steering angle - // to command (since - // inverse kinematics doesn't care about angle, we can be opportunistically lazy). - List> overrideSteering = new ArrayList<>(modules.length); - // Enforce steering velocity limits. We do this by taking the derivative of steering angle at - // the current angle, - // and then backing out the maximum interpolant between start and goal states. We remember the - // minimum across all modules, since - // that is the active constraint. - final double max_theta_step = dt * limits.maxSteeringVelocity(); - for (int i = 0; i < modules.length; ++i) { - if (!need_to_steer) { - overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); - continue; - } - overrideSteering.add(Optional.empty()); - if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { - // If module is stopped, we know that we will need to move straight to the final steering - // angle, so limit based - // purely on rotation in place. - if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { - // Goal angle doesn't matter. Just leave module at its current angle. - overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); - continue; - } + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then + // find the amount we can move from start towards goal in this cycle such that no kinematic + // limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; - var necessaryRotation = - prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); - if (flipHeading(necessaryRotation)) { - necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); - } - // getRadians() bounds to +/- Pi. - final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; - if (numStepsNeeded <= 1.0) { - // Steer directly to goal angle. - overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); - // Don't limit the global min_s; - continue; - } else { - // Adjust steering by max_theta_step. - overrideSteering.set( - i, - Optional.of( - prevSetpoint.moduleStates()[i].angle.rotateBy( - Rotation2d.fromRadians( - Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); - min_s = 0.0; - continue; - } - } - if (min_s == 0.0) { - // s can't get any lower. Save some CPU. - continue; - } + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since + // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, + // and then backing out the maximum interpolant between start and goal states. We remember the + // minimum across all modules, since + // that is the active constraint. + final double max_theta_step = dt * limits.maxSteeringVelocity(); + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based + // purely on rotation in place. + if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } - final int kMaxIterations = 8; - double s = - findSteeringMaxS( - prev_vx[i], - prev_vy[i], - prev_heading[i].getRadians(), - desired_vx[i], - desired_vy[i], - desired_heading[i].getRadians(), - max_theta_step, - kMaxIterations); - min_s = Math.min(min_s, s); + var necessaryRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; - // Enforce drive wheel acceleration limits. - final double max_vel_step = dt * limits.maxDriveAcceleration(); - for (int i = 0; i < modules.length; ++i) { - if (min_s == 0.0) { - // No need to carry on. - break; - } - double vx_min_s = - min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; - double vy_min_s = - min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; - // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we - // already know we can't go faster - // than that. - final int kMaxIterations = 10; - double s = - min_s - * findDriveMaxS( - prev_vx[i], - prev_vy[i], - Math.hypot(prev_vx[i], prev_vy[i]), - vx_min_s, - vy_min_s, - Math.hypot(vx_min_s, vy_min_s), - max_vel_step, - kMaxIterations); - min_s = Math.min(min_s, s); + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of( + prevSetpoint.moduleStates()[i].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + continue; } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = + findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.maxDriveAcceleration(); + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster + // than that. + final int kMaxIterations = 10; + double s = + min_s + * findDriveMaxS( + prev_vx[i], + prev_vy[i], + Math.hypot(prev_vx[i], prev_vy[i]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + max_vel_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } - ChassisSpeeds retSpeeds = - new ChassisSpeeds( - prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, - prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); - var retStates = kinematics.toSwerveModuleStates(retSpeeds); - for (int i = 0; i < modules.length; ++i) { - final var maybeOverride = overrideSteering.get(i); - if (maybeOverride.isPresent()) { - var override = maybeOverride.get(); - if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { - retStates[i].speedMetersPerSecond *= -1.0; - } - retStates[i].angle = override; - } - final var deltaRotation = - prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); - if (flipHeading(deltaRotation)) { - retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); - retStates[i].speedMetersPerSecond *= -1.0; - } + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + var retStates = kinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; } - return new SwerveSetpoint(retSpeeds, retStates); + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); + retStates[i].speedMetersPerSecond *= -1.0; + } } + return new SwerveSetpoint(retSpeeds, retStates); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java index c65409a2..59272a65 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java @@ -9,67 +9,67 @@ import lombok.Getter; public class HolonomicDriveController { - private final PIDController linearController; - private final PIDController thetaController; + private final PIDController linearController; + private final PIDController thetaController; - @Getter private Pose2d poseError; + @Getter private Pose2d poseError; - public HolonomicDriveController( - double linearkP, double linearkD, double thetakP, double thetakD) { - linearController = new PIDController(linearkP, 0, linearkD); - thetaController = new PIDController(thetakP, 0, thetakD); + public HolonomicDriveController( + double linearkP, double linearkD, double thetakP, double thetakD) { + linearController = new PIDController(linearkP, 0, linearkD); + thetaController = new PIDController(thetakP, 0, thetakD); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - } + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } - /** Reset all controllers */ - public void resetControllers() { - linearController.reset(); - thetaController.reset(); - } + /** Reset all controllers */ + public void resetControllers() { + linearController.reset(); + thetaController.reset(); + } - public void resetThetaController() { - thetaController.reset(); - } + public void resetThetaController() { + thetaController.reset(); + } - public void setControllerTolerance(Pose2d controllerTolerance) { - linearController.setTolerance(controllerTolerance.getTranslation().getNorm()); - thetaController.setTolerance(controllerTolerance.getRotation().getRadians()); - } + public void setControllerTolerance(Pose2d controllerTolerance) { + linearController.setTolerance(controllerTolerance.getTranslation().getNorm()); + thetaController.setTolerance(controllerTolerance.getRotation().getRadians()); + } - /** Set PID values */ - public void setPID(double linearkP, double linearkD, double thetakP, double thetakD) { - linearController.setPID(linearkP, 0, linearkD); - thetaController.setPID(thetakP, 0, thetakD); - } + /** Set PID values */ + public void setPID(double linearkP, double linearkD, double thetakP, double thetakD) { + linearController.setPID(linearkP, 0, linearkD); + thetaController.setPID(thetakP, 0, thetakD); + } - /** Calculate robot relative chassis speeds */ - public ChassisSpeeds calculate(VehicleState currentState, VehicleState setpointState) { - Pose2d currentPose = - new Pose2d( - currentState.getX(), currentState.getY(), new Rotation2d(currentState.getTheta())); - Pose2d setpointPose = - new Pose2d( - setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta())); - poseError = setpointPose.relativeTo(currentPose); + /** Calculate robot relative chassis speeds */ + public ChassisSpeeds calculate(VehicleState currentState, VehicleState setpointState) { + Pose2d currentPose = + new Pose2d( + currentState.getX(), currentState.getY(), new Rotation2d(currentState.getTheta())); + Pose2d setpointPose = + new Pose2d( + setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta())); + poseError = setpointPose.relativeTo(currentPose); - // Calculate feedback velocities (based on position error). - double linearFeedback = - linearController.calculate( - 0, currentPose.getTranslation().getDistance(setpointPose.getTranslation())); - Rotation2d currentToStateAngle = - setpointPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); - double xFeedback = linearFeedback * currentToStateAngle.getCos(); - double yFeedback = linearFeedback * currentToStateAngle.getSin(); - double thetaFeedback = - thetaController.calculate( - currentPose.getRotation().getRadians(), setpointPose.getRotation().getRadians()); + // Calculate feedback velocities (based on position error). + double linearFeedback = + linearController.calculate( + 0, currentPose.getTranslation().getDistance(setpointPose.getTranslation())); + Rotation2d currentToStateAngle = + setpointPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double xFeedback = linearFeedback * currentToStateAngle.getCos(); + double yFeedback = linearFeedback * currentToStateAngle.getSin(); + double thetaFeedback = + thetaController.calculate( + currentPose.getRotation().getRadians(), setpointPose.getRotation().getRadians()); - // Return next output. - return ChassisSpeeds.fromFieldRelativeSpeeds( - setpointState.getVx() + xFeedback, - setpointState.getVy() + yFeedback, - setpointState.getOmega() + thetaFeedback, - currentPose.getRotation()); - } + // Return next output. + return ChassisSpeeds.fromFieldRelativeSpeeds( + setpointState.getVx() + xFeedback, + setpointState.getVy() + yFeedback, + setpointState.getOmega() + thetaFeedback, + currentPose.getRotation()); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java index 33d2b06a..d33c6d54 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java @@ -13,101 +13,101 @@ import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState; public class HolonomicTrajectory { - private final Trajectory trajectory; - - public HolonomicTrajectory(String name) { - File file = Path.of("src", "main", "deploy", "trajectories", name + ".pathblob").toFile(); - try { - InputStream fileStream = new FileInputStream(file); - trajectory = Trajectory.parseFrom(fileStream); - } catch (IOException e) { - System.err.println("Could not load trajectory \"" + name + "\""); - throw new RuntimeException(); - } + private final Trajectory trajectory; + + public HolonomicTrajectory(String name) { + File file = Path.of("src", "main", "deploy", "trajectories", name + ".pathblob").toFile(); + try { + InputStream fileStream = new FileInputStream(file); + trajectory = Trajectory.parseFrom(fileStream); + } catch (IOException e) { + System.err.println("Could not load trajectory \"" + name + "\""); + throw new RuntimeException(); } + } - public double getDuration() { - return trajectory.getStates(trajectory.getStatesCount() - 1).getTime(); - } - ; + public double getDuration() { + return trajectory.getStates(trajectory.getStatesCount() - 1).getTime(); + } + ; - public Pose2d getStartPose() { - VehicleState startState = getStartState(); - return new Pose2d(startState.getX(), startState.getY(), new Rotation2d(startState.getTheta())); - } + public Pose2d getStartPose() { + VehicleState startState = getStartState(); + return new Pose2d(startState.getX(), startState.getY(), new Rotation2d(startState.getTheta())); + } - public Pose2d[] getTrajectoryPoses() { - Pose2d[] poses = new Pose2d[trajectory.getStatesCount()]; + public Pose2d[] getTrajectoryPoses() { + Pose2d[] poses = new Pose2d[trajectory.getStatesCount()]; - for (int i = 0; i < trajectory.getStatesCount(); i++) { - VehicleState state = trajectory.getStates(i).getState(); - poses[i] = new Pose2d(state.getX(), state.getY(), new Rotation2d(state.getTheta())); - } - return poses; + for (int i = 0; i < trajectory.getStatesCount(); i++) { + VehicleState state = trajectory.getStates(i).getState(); + poses[i] = new Pose2d(state.getX(), state.getY(), new Rotation2d(state.getTheta())); } - ; - - public VehicleState getStartState() { - return trajectory.getStates(0).getState(); + return poses; + } + ; + + public VehicleState getStartState() { + return trajectory.getStates(0).getState(); + } + + public VehicleState getEndState() { + return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); + } + + public VehicleState sample(double timeSeconds) { + TimestampedVehicleState before = null; + TimestampedVehicleState after = null; + + for (TimestampedVehicleState state : trajectory.getStatesList()) { + if (state.getTime() == timeSeconds) { + return state.getState(); + } + + if (state.getTime() < timeSeconds) { + before = state; + } else { + after = state; + break; + } } - public VehicleState getEndState() { - return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); + if (before == null) { + return trajectory.getStates(0).getState(); } - public VehicleState sample(double timeSeconds) { - TimestampedVehicleState before = null; - TimestampedVehicleState after = null; - - for (TimestampedVehicleState state : trajectory.getStatesList()) { - if (state.getTime() == timeSeconds) { - return state.getState(); - } - - if (state.getTime() < timeSeconds) { - before = state; - } else { - after = state; - break; - } - } - - if (before == null) { - return trajectory.getStates(0).getState(); - } - - if (after == null) { - return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); - } - - double s = (timeSeconds - before.getTime()) / (after.getTime() - before.getTime()); - - Pose2d beforePose = - new Pose2d( - before.getState().getX(), - before.getState().getY(), - new Rotation2d(before.getState().getTheta())); - Pose2d afterPose = - new Pose2d( - after.getState().getX(), - after.getState().getY(), - new Rotation2d(after.getState().getTheta())); - - Pose2d interpolatedPose = beforePose.interpolate(afterPose, s); - double interpolatedVelocityX = - MathUtil.interpolate(before.getState().getVx(), after.getState().getVx(), s); - double interpolatedVelocityY = - MathUtil.interpolate(before.getState().getVy(), after.getState().getVy(), s); - double interpolatedAngularVelocity = - MathUtil.interpolate(before.getState().getOmega(), after.getState().getOmega(), s); - - return VehicleState.newBuilder() - .setX(interpolatedPose.getTranslation().getX()) - .setY(interpolatedPose.getTranslation().getY()) - .setTheta(interpolatedPose.getRotation().getRadians()) - .setVx(interpolatedVelocityX) - .setVy(interpolatedVelocityY) - .setOmega(interpolatedAngularVelocity) - .build(); + if (after == null) { + return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); } + + double s = (timeSeconds - before.getTime()) / (after.getTime() - before.getTime()); + + Pose2d beforePose = + new Pose2d( + before.getState().getX(), + before.getState().getY(), + new Rotation2d(before.getState().getTheta())); + Pose2d afterPose = + new Pose2d( + after.getState().getX(), + after.getState().getY(), + new Rotation2d(after.getState().getTheta())); + + Pose2d interpolatedPose = beforePose.interpolate(afterPose, s); + double interpolatedVelocityX = + MathUtil.interpolate(before.getState().getVx(), after.getState().getVx(), s); + double interpolatedVelocityY = + MathUtil.interpolate(before.getState().getVy(), after.getState().getVy(), s); + double interpolatedAngularVelocity = + MathUtil.interpolate(before.getState().getOmega(), after.getState().getOmega(), s); + + return VehicleState.newBuilder() + .setX(interpolatedPose.getTranslation().getX()) + .setY(interpolatedPose.getTranslation().getY()) + .setTheta(interpolatedPose.getRotation().getRadians()) + .setVx(interpolatedVelocityX) + .setVy(interpolatedVelocityY) + .setOmega(interpolatedAngularVelocity) + .build(); + } } diff --git a/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java b/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java index b07bb3a6..65658018 100644 --- a/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java +++ b/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java @@ -4,9 +4,9 @@ public class RobotContainerTest { - @Test - public void createRobotContainer() { - // Instantiate RobotContainer - new RobotContainer(); - } + @Test + public void createRobotContainer() { + // Instantiate RobotContainer + new RobotContainer(); + } }