From 3e8932967cf2b4d464777fad771f91d6cbb0c75b Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Thu, 14 Dec 2023 20:16:34 -0500 Subject: [PATCH 1/7] initial updates to tct for phoenix6 and 2024 beta --- bin/main/logback.xml | 22 + bin/main/org/strykeforce/tct/Main.class | Bin 0 -> 2199 bytes .../tct/device/NotImplementedError.class | Bin 0 -> 509 bytes .../strykeforce/tct/device/TalonBase.class | Bin 0 -> 15210 bytes .../org/strykeforce/tct/device/TalonFX.class | Bin 0 -> 2869 bytes .../strykeforce/tct/device/TalonFXBase.class | Bin 0 -> 6183 bytes .../org/strykeforce/tct/device/TalonSRX.class | Bin 0 -> 1804 bytes .../strykeforce/tct/device/TalonSRXBase.class | Bin 0 -> 4085 bytes bin/main/templates/hello.html | 10 + bin/test/junit-platform.properties | 1 + build.gradle | 21 +- gradle/wrapper/gradle-wrapper.properties | 4 +- gradlew | 282 ++++---- gradlew.bat | 15 +- settings.gradle | 5 +- .../kotlin/org/strykeforce/thirdcoast/Koin.kt | 7 +- .../strykeforce/thirdcoast/command/Command.kt | 23 +- .../thirdcoast/device/LegacyTalonFxService.kt | 71 ++ .../thirdcoast/device/TalonFxService.kt | 193 +++++- .../talon/FeedbackCoefficientCommand.kt | 12 +- .../thirdcoast/talon/HardLimitCommands.kt | 18 +- .../thirdcoast/talon/RunTalonsCommand.kt | 10 +- .../talon/SelectAbsoluteSensorRange.kt | 11 +- .../talon/SelectBrakeModeCommand.kt | 10 +- .../talon/SelectControlModeCommand.kt | 8 +- .../talon/SelectFeedbackSensorCommand.kt | 10 +- .../talon/SelectInitializationStrategy.kt | 11 +- .../talon/SelectMotorCommutationCommand.kt | 12 +- .../thirdcoast/talon/SelectSlotCommand.kt | 8 +- .../thirdcoast/talon/SelectTalonsCommand.kt | 10 +- .../SelectVelocityMeasurmentPeriodCommand.kt | 10 +- .../talon/SetSensorPositionCommand.kt | 10 +- .../thirdcoast/talon/TalonParameterCommand.kt | 54 +- .../thirdcoast/talon/TalonsStatusCommand.kt | 6 +- .../talon/phoenix6/P6HardLimitCommand.kt | 95 +++ .../talon/phoenix6/P6ModeStatusCommand.kt | 22 + .../thirdcoast/talon/phoenix6/P6RunCommand.kt | 372 +++++++++++ ...P6SelectDifferentialSensorSourceCommand.kt | 45 ++ .../P6SelectDifferentialTypeCommand.kt | 28 + .../phoenix6/P6SelectFeedbackSensorCommand.kt | 53 ++ .../phoenix6/P6SelectFollowerTypeCommand.kt | 27 + .../phoenix6/P6SelectGravityTypeCommand.kt | 58 ++ .../talon/phoenix6/P6SelectModeCommand.kt | 28 + .../P6SelectMotionMagicTypeCommand.kt | 27 + .../phoenix6/P6SelectMotorInvertCommand.kt | 29 + .../phoenix6/P6SelectNeutralModeCommand.kt | 38 ++ .../phoenix6/P6SelectNeutralOutputCommand.kt | 27 + .../talon/phoenix6/P6SelectSlotCommand.kt | 25 + .../talon/phoenix6/P6SelectTalonsCommand.kt | 45 ++ .../talon/phoenix6/P6SelectUnitCommand.kt | 27 + .../talon/phoenix6/P6TalonStatusCommand.kt | 27 + .../talon/phoenix6/Phoenix6Parameter.kt | 98 +++ .../phoenix6/Phoenix6ParameterCommand.kt | 614 ++++++++++++++++++ src/main/resources/commands.toml | 512 ++++++++++++++- src/main/resources/phoenix6.toml | 332 ++++++++++ vendordeps/NavX.json | 17 +- vendordeps/Phoenix5.json | 151 +++++ .../{PhoenixProAnd5.json => Phoenix6.json} | 241 ++----- vendordeps/thirdcoast.json | 5 +- 59 files changed, 3341 insertions(+), 456 deletions(-) create mode 100644 bin/main/logback.xml create mode 100644 bin/main/org/strykeforce/tct/Main.class create mode 100644 bin/main/org/strykeforce/tct/device/NotImplementedError.class create mode 100644 bin/main/org/strykeforce/tct/device/TalonBase.class create mode 100644 bin/main/org/strykeforce/tct/device/TalonFX.class create mode 100644 bin/main/org/strykeforce/tct/device/TalonFXBase.class create mode 100644 bin/main/org/strykeforce/tct/device/TalonSRX.class create mode 100644 bin/main/org/strykeforce/tct/device/TalonSRXBase.class create mode 100644 bin/main/templates/hello.html create mode 100644 bin/test/junit-platform.properties create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt create mode 100644 src/main/resources/phoenix6.toml create mode 100644 vendordeps/Phoenix5.json rename vendordeps/{PhoenixProAnd5.json => Phoenix6.json} (57%) diff --git a/bin/main/logback.xml b/bin/main/logback.xml new file mode 100644 index 0000000..20519e6 --- /dev/null +++ b/bin/main/logback.xml @@ -0,0 +1,22 @@ + + + + + System.err + + %-23(%d{HH:mm:ss.SSS} [%thread]) %highlight(%-5level) %logger{32} - %msg%n + + + + + + + + + + + + + + \ No newline at end of file diff --git a/bin/main/org/strykeforce/tct/Main.class b/bin/main/org/strykeforce/tct/Main.class new file mode 100644 index 0000000000000000000000000000000000000000..1fea9c15e5e6a6e964171d95789c0be817fb2d1d GIT binary patch literal 2199 zcma)7+g95~6y1XjzMyb1mjp-?LP=u-LFt9GjcL-vAq8qk8WU2Q-h@521+wHN4=nhV zKJ*{@LTIwu59sPQb#+U!K?aBRWkx!4uKVmc^VdIb{siy^>M?W)Tyflr-cau0t}Hul zQR=Fw^fkk@V~7e2>=^roZW(q(f4aRRi%Otp(X>rB@#$8cy(e}J z-II=LxQr_TiH20qTv;{`1V+-Ga~K!6+MYuz&XQdr5=o39;VtiJxQ6QjLv-3wX{92t zkoJMNppuodMuZ@V8+bp4DS>&@(LIbT)7D*Svs{97QkK3N9Bs6K(lCvi1lVh(z>TxP zTcLsy{6M#mi6PBqh5|LcYM8++iBWVVq4)Zg$)DHCCUFb5HOxa7=x!*(W#r50m5#b` zd?+CF6p8w=hEK3S_f^9#d32g+cVAVit}pv%t{>x54WHq2^0Oit;bDiqg|llj@0bf= zGTaKeI@E!NydvrhuLKtVtCttBlC9C4ldd{(xJW%YhKyvAzDC^2}>_!YvSg^;a;P#)K#Ia1psctJje0)@-d@DksWLqTwMs1qyvYEPWDaUtxBkVcA%BVp{FU=pE% zv&U)r#Pk@O@aE~f=bm(VkX8U{a;{b9=2vjsYLXLtm1p?ZNZA;g`I+FY(Ic}Z7`ry>)| zirK9(+W4q2G@tpBM=4iHR1_J!OR1zCFqAu|V}{C&o{1Xj49C|h)N^6Dm0E=|50$o| zTuTF#vDH8o4Tg3nUW-T8NR{-Rtr<%thKp<>%mbgKf}s`bDNo1TNSCjQ75gM}hV%G; zKk761naFdV6yiTGU`>I82Ex$9F6~a?Do!yV(y?CmY=farWKb9mf2oy$c^1oQanchUM5h4Ue`#Gl3Ns z8goX*GVGbm==6m&@3%oKffMsiDUP{Hb7A4R%oZ2~Oag1fFWVeLq<07@%dB<_$b(1P&e# zY<46jP0z$=(r4Qa+6+Ayjk9{F8tSSv7(9+um_321Mg}<7vM_rFbL4N2#MO58j=*Mp90C$#JAtJ( z4Sd(IM0ml>gjyRBkAxn!s)UKG?q&L0A`$h}7^N1wi>Nd*%<5SkrOHp8>g7dXkEdk%mK$Mj&C9=f%QVg z^%ZZwLyJW-Z*tMrqXViua7jK&A?;tgXK0LEPgaXpaav{ES2EBp+LA1W&n~QBP=ir= zb#@lKMrknDEEPzjZh<*Df&K*Bd9oPl?;BxNy{oPhm*fqbFCC{v0vqDWh(Qt&AFnrskpDXYCtjU`w2V2FO@UhAzXJq6zAj2+XUv6MAKfRkcLD-#6@_Fj@&+ z2{u$IEW!nzemt0=+lv=tj%TtWk7bSQCUm6ulCXxucdWCJePTWNV!;&RVO+p<{h~yu zX+h3bsVqvQtf!7q*sunSfO7Ll&!qMxRizUJ(m=M+WDJu&N2Z`QFeJ9ZUCK`(rbvtA*S zsWp+z74>ZmnH+P}?-nxC-7}@qgG8v`I7C4FXF$lMe4IGXP#96)#8ZH zSg`CdYF-UtEhgVc!H12#A#`6pACv;iz)2RC4W_|vE9H?uX7trc3tLFRUG$}xOQ1rO ziLQecaTC~_@=1X}>I?RcWe3I_3&&sn0&c*ITOoQ4e_a^#s@qDLR^!7&Y1nGMW&lSM zb66S8p%M@9c+?wn9W1J74y}WVWj!iZ!z*jxCkz+w(e?3vU!GA)+W8J zhFMuj3_C0^_q&1V(dx^UBF^r{QK5&0*!FGPN*HnF085ad^vG^lGGfqXNvnnc33D`G zv6vvs;hbcS=5TUp&-k-g2;g8UdcQ&FT`ZQWhzKXgOkL9~A*0L#vS?){TbH+};U-g> ztSnvEqsVz1P@lE*PI(BAxwSJW7Aejaad^Qy3Opcd%YiprySQQl0%s&gR#bH+B8_9( zey@V3Rv%%}_KAebO| zhGP#=_pD!G4I3MUhz}X;j&;n^$HfR}OHx3r@wv!y?Alt34olxST~1)GhOYht6rVpW zE8#)H?@G>~gxMBXX=4=pkilz}-Q|LqCwGW07Q+W7&}Ee`XIt4M8P)>^Yy4}$4{rth z#SdKge=*)=@R#6k%jWx`;~uM3${%*ZQoQaI3t$;6$2;_1H~{}M;xSy^jKA#< zLDzZr!-0?Db(DSur{65rNP1iit6(+9Y=?tj4N|aY2jdlE@eo)m(FgI(q6X-zIeNEP z6G1-|4wL8%vk9-)!Qm29#qrt{j=Q-E8J};6al8SJ(B!x~h2tuE$nlY|QDPp;B}mut zksMRlsJ&vNqo7BVr_CumZGz2+5L$+5@hCVNb$tvz1K5Ip%tyB3FY^SIt+u7^!W=NV z%Qo1KPjoO(NWnZ7EmVtn92_q(Rrc@Ym~oFi0Zx>dC&3%*JoaR5^uGN-KLvUt`e=08 z22=ELDt>Dh`q%-dac?>WPM4Tdc;CkKpK^lF=9tIxz!7>EvsM+ek7KfDJE32m^Z|U< zf;;TOr^1#hFCOFj)O+zDoFVB?MLv0)Na1rgr(Z4BL}J02kVOOnXTjN;NK+|DLom#d zMzoPGNI@E{AdPAAR7gQO2gW&1Ihc@0Dm|MVsb0^!VN%kwbKJwTO)o?TbL9vMC2^`Hf%2s$fZ({j~ADd={^pBOr?D0#KnjJOVzB|6K5 zjnE(C=8yk zaIq%(vnlA8z`GSj=~G)3DVs(4>$!4GR=cBNom8^gQ~KBDIp<)&%zBQQ zZkg-h1}QN+Weu_g*~pRNneO8}Rxyb`f%>dQLY9j@sf~X0exTo|jV^liedGNmZFJGK z?;HB3w9!vdYUa~1y?;g<{d6Vz%`x;_w9$7d(LWnQ|C~1ZSxR))8`XN`=i%0fzc;8_ z$fy$iwpi8j1?{sw!O`P+P;}DK`0+*E%uUWq;h5=I+cuacv}TJ0O!nAfIY-XFt!0KO@` zO);|~*K^Ezdp-ycY0}@ANdGN(Skf!{4stW6kDuj7;88>%@NM1c8@V-+lgF?c(3+Bx z+Y{*@hwo@ga%YMp--YjSNxlzHXd>O4g2X(A>Es96NDrhSvCf7e{ZQLa9!}KGlkg)% zVc!!}c78mClc(Tm&dHDAClZN$3t*YDiQLWpSHGAM(Y*^_X{yYjo8T$EjjOFDfzNIlQV?9&CF2OL$)DT9u1l=IHTU z#4xK9X_kv#fL}?>0`At}s|R_NWA4B&T&%XJP8wc>UrWqt_5Kp>)6CcX2CsNe!24$Q zb?gQBt>%uRD^J=nltOREm*IDEN5vd~h)z9jl~*b-UeJ0Pq@K7NC#792dS7OVK!{H7;CgNmp7P zJqhsVo)dDm^{D`t2@D-MCyo_5%(r$Aj;JdOaFxKC%OlH`f?D-MPFe8Hinv4?&ZOX> zW$&b`a<&So*kp$ebE!w*QcNY9m$uI*&=F{c9q!Tf%ECSY=U)sBzA7|wn77xcYaas` zqRmdIkZHiHU3$zc6iXWDG)hqm{3;DmGU{zn>K!=lah0-|3j!ayWNq42RAJU_d!aL# zO(9aPQ6d5p+HF#=hmCR~1Z8qNhyv97tI1`x!wBWa91`KkfU|Xz?=Kg1&}DAYNzYb~ zxpf%*@7xRy47p8Zb$df)XB7yv>~^>zFrL}!mW!eUzrlJD*fkZT-Nabpw>>i&@}4}+ zyFC^HE2X>>7q!lrb|dV7(h;cn1_j^?{WH30y{a`$Eu-Y ztNCeWs0@s1ZuiKSlUQr->ygu*#BQyoX6|gMlBZ`Y!%V4S@UfULl+6MeIAQQzoxG|{ zHS`+<_Rq;MQ!3xmC|%zCY?k6&Vnco-K6aogX_<%V=vIyN%6O*qebsJ?=W!@y45C)L zwXW$}?U1XOshb+$dYmlXF_j;A&Mpg=3{gpd*E%;*`PySm;6oUv&;tKRA9{ERN($9Pw=U? z6@)m=iu%khr4KxO(&5_-|E}N@aTcF_1-J(3^REEc752k`!XgYRya6{g4k;XlTN+0c zj_UnU$o9wd{upHY<8WJjHx3gDC*h99DTUK;SK~c}Gcc?1zQQ?}*Z4r;LwKa{F_aW8 iz!Qy66)wUvjY|qGc&_ngk;sCCHZKQDA{J5_7-%-S6JHGn0S+`}1!A*YR~1 z+8E9`ZY5pw+?^ME({W2Y?UlTAnQxmkTrn)i&aK}zYP<{W3}kOT9 zrfqsR7!IUHRvFqePMLS(0D5}Sg@X)-3#QE%>(veJt{58@XE<7LN`|#+xTbjbKel_% z%o;ir z7-Sf0;cLUB9 z_{AH5U^q#s-83sjZt)Tk$#GuZFiJ0q+^#t;!-IshcI~oqnat+%BVjHw@1mlq-ugFv zN6LU- z7gRB1L={ge)m_6g9eY+2TyKfoNne@G3d2oJXgY6iV(|=Gi;b~7)y5JsYK%pfDJ^>q zeiFGg5E{8ps7&AGURX&6y+E2|>jWBllWkcfwG}64S{6fqS13S+A;o$@wAYMNt=4G{ z)fU5&E~CvRvpR+YawRo5xumJdkPn8%ee=@f?f72an!PbxPDKSJ;~*=!7d0DESD8pf zxYQY)Q8e@5rE-FI71wpq5EO|bE%rEe7o9rY0%=dDZD1g9 z0UUd7Y#aC(bbF$I9dv^nrYGIg==Tu4rRmA&*)jSW-G9)a4ZZZ+DL?k0kH)=f0{U^7 z-UuE?=vNr$Xm&e25B`Ot4`1WGe`p-E@0a$&KKIH~yIBl^}s9!}x3?`Mwuw6*YaO!^s8a{}ClrI>V8#u*=J zndY=o;gm!g^_eIY1-rBOln}(|96s~?q-Z1(HG6z;F>`K9b0m%tTDK zMkG==Tjynw2%dtS$ks(%@-cNPj>WQ5g*T3ggzP3`*j<)!2HA}#)@~+-ox-I1Zes0j z#j+D|2H8y})-D&zPH8uV>4e(di(z*KSDV;P;hK-B%j|LtJB7(GgV}^g4`Pr+wTTLz z!JLmYA*+wnjH%#95=r%|-QK|HPi8lKd)*57CPt^5xJ3xTEMCXFkEzp%$3&+v8EzvJ zVAraTJda_QrQY_2o#-C7aVNm8)tYx=*eOg#9sX{BT`Qz|47<;f53m#6BZn^n>{|8U z?_$^~Oon?X_(;0S_+BD4SH=Ps6Wad}X}^SJ-(I%|{3z|id%%~tPY9y2!h66Hiaxro z#(s*?Nue{W;6Xy3KfeXf6|5%Y`AWlc4eLIVZl(Srk-~e`Lp&mc;7VP?R|(nu8pBRu Z(%CyPyWbMCdz_fv?;3VbDC#ym{U7glxnckS literal 0 HcmV?d00001 diff --git a/bin/main/org/strykeforce/tct/device/TalonSRX.class b/bin/main/org/strykeforce/tct/device/TalonSRX.class new file mode 100644 index 0000000000000000000000000000000000000000..36f7c9db33b0f97171d435c33087492264117471 GIT binary patch literal 1804 zcmb_dO>fgc5S=ZBKxv>KB(zXyOZiAISmHnrp$Dj2DN-vcT$Kt5RU3Pny4mbTn{`G0 z8V7y=KMFBxZ=$9}ECder^H`quX5QGI??1kN1At9`J%Q>wsh9=8iw z28Vy@%^)$%=Us0!e$m*r*0z1}oxy_B&AMID-^#Q`X?D|2N|XHT(~Pbp(M+j{Omk~% z-6LHsr1z1xM}=2v-m;f#zQz`>u!pX$q1jZeNoA(8-l+Z@O5PMWdULY&$-bo11#PBr zzY#Jx_IVU@IsE7-BSJ%}7d-NI>Yaj5!83(2y~R{mQB{vY_#ZkVpWgi|jWv6=F#D|# z{{HO`B3h-FX&PZP=LZMHRv4Td z@qIrqc`-D@qQ#GG47Ux(_Zr{7SvOidO({lqbd%N0aYnawGXcwJe8u+c@Cl>Qd|`)C zs^VKbL!&gAr8He&bh&PO{B_&i> zHiLTa+XoFUG;wQ_b8F8qj}$GVoupQG0;_WUlxwv!5|hm!8Xdfs_9G@0z-UQhL??k$ zL|92p+p1ab&>A&H6QS*Lza759qPQNV)^0YPlS(@X(2;f9wbA`Q>e6Zq1YsS(r*>e7 z%9S zi(`rfu&8yAVQ718zuoFrmE)e+sQO=~^1EWY8&}^@vt7U{%67oFYFOPr>uO^fUUzpw zM1zNr&{G?&8KdYJeL-|I3A#uJq3NQFSm;PlWe3Bn|AR9yfBnD0hzd@rqCImQ_>cM@ zK;N5x1AMAx8Y?n}<8*y^tXWFEw)fL8`eAsD8i}G>u%oMTb$F(laCl|-coCjy_-!0& zHf?csnCk4F;ta6xi*anw7lfk#{>E@3n#LWc4qPYjRK%U(J~scJGQVOlLRnnL<;w}W zh;dd;pi6WaPxwiqDO^SHCT6E_zwkTd-oB@g{=~R9J}cvAL5(?4ycm5fMTpTgx{e5e zc_YHy#dG=`%$&r0C{sBb^Am|F&_AUarH?+tyD>0)PB$Z-Mahd7@So@1Eg3%_=~v#1obJ$Pm^#m1EQ?Oq_WWt6PgvH|X7i#OgL;)OEajNZ%x^!pj(>C0Ygoqet{OLell} zcM@r^m%pVl5YF!9N3;^5>$+t-MyEn&_!sa=LY;4i(78%a6YBg)qjQa(MM%0eXG)~O yHMdR`Aar&FvF6sOno!+gj5>wMXal?bhh6`?9l=ekZiAjDR2OK}y+GC@RQm_A_K!pW literal 0 HcmV?d00001 diff --git a/bin/main/templates/hello.html b/bin/main/templates/hello.html new file mode 100644 index 0000000..a3c6b8d --- /dev/null +++ b/bin/main/templates/hello.html @@ -0,0 +1,10 @@ + + + + + Hello + + +

Hello, World

+ + \ No newline at end of file diff --git a/bin/test/junit-platform.properties b/bin/test/junit-platform.properties new file mode 100644 index 0000000..2af5bf8 --- /dev/null +++ b/bin/test/junit-platform.properties @@ -0,0 +1 @@ +junit.jupiter.testinstance.lifecycle.default=per_class diff --git a/build.gradle b/build.gradle index 281569b..f24d1a0 100644 --- a/build.gradle +++ b/build.gradle @@ -2,14 +2,14 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "org.jetbrains.kotlin.jvm" version "1.6.20" - id "edu.wpi.first.GradleRIO" version "2023.1.1" + id "org.jetbrains.kotlin.jvm" version "1.7.21" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } -version = "23.0.0" +version = "23.0.1-SNAPSHOT" -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +sourceCompatibility = JavaVersion.VERSION_17 +targetCompatibility = JavaVersion.VERSION_17 def ROBOT_MAIN_CLASS = "org.strykeforce.thirdcoast.DummyMain" @@ -69,8 +69,8 @@ dependencies { implementation("net.consensys.cava:cava-toml:0.5.0") // Logging - implementation('io.github.microutils:kotlin-logging-jvm:2.1.21') - implementation('ch.qos.logback:logback-classic:1.2.10') + implementation('io.github.microutils:kotlin-logging-jvm:3.0.5') //2.1.21 + implementation('ch.qos.logback:logback-classic:1.3.5') // Koin implementation("org.koin:koin-core:1.0.2") @@ -78,15 +78,16 @@ dependencies { testImplementation("org.koin:koin-test:1.0.2") // Testing - testImplementation(platform("org.junit:junit-bom:5.7.2")) + testImplementation(platform("org.junit:junit-bom:5.9.0")) testImplementation("org.junit.jupiter:junit-jupiter") - testImplementation("org.assertj:assertj-core:3.19.0") + testImplementation("org.assertj:assertj-core:3.23.1") testImplementation("com.nhaarman.mockitokotlin2:mockito-kotlin:2.2.0") + testRuntimeOnly('ch.qos.logback:logback-classic:1.4.4') } tasks.withType(org.jetbrains.kotlin.gradle.tasks.KotlinCompile).configureEach { kotlinOptions { - jvmTarget = "11" + jvmTarget = "17" } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index f959987..1058752 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index 4f906e0..1aa94a4 100755 --- a/gradlew +++ b/gradlew @@ -1,7 +1,7 @@ -#!/usr/bin/env sh +#!/bin/sh # -# Copyright 2015 the original author or authors. +# Copyright © 2015-2021 the original authors. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -17,67 +17,99 @@ # ############################################################################## -## -## Gradle start up script for UN*X -## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# ############################################################################## # Attempt to set APP_HOME + # Resolve links: $0 may be a link -PRG="$0" -# Need this for relative symlinks. -while [ -h "$PRG" ] ; do - ls=`ls -ld "$PRG"` - link=`expr "$ls" : '.*-> \(.*\)$'` - if expr "$link" : '/.*' > /dev/null; then - PRG="$link" - else - PRG=`dirname "$PRG"`"/$link" - fi +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac done -SAVED="`pwd`" -cd "`dirname \"$PRG\"`/" >/dev/null -APP_HOME="`pwd -P`" -cd "$SAVED" >/dev/null -APP_NAME="Gradle" -APP_BASE_NAME=`basename "$0"` - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. -MAX_FD="maximum" +MAX_FD=maximum warn () { echo "$*" -} +} >&2 die () { echo echo "$*" echo exit 1 -} +} >&2 # OS specific support (must be 'true' or 'false'). cygwin=false msys=false darwin=false nonstop=false -case "`uname`" in - CYGWIN* ) - cygwin=true - ;; - Darwin* ) - darwin=true - ;; - MINGW* ) - msys=true - ;; - NONSTOP* ) - nonstop=true - ;; +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; esac CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar @@ -87,9 +119,9 @@ CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar if [ -n "$JAVA_HOME" ] ; then if [ -x "$JAVA_HOME/jre/sh/java" ] ; then # IBM's JDK on AIX uses strange locations for the executables - JAVACMD="$JAVA_HOME/jre/sh/java" + JAVACMD=$JAVA_HOME/jre/sh/java else - JAVACMD="$JAVA_HOME/bin/java" + JAVACMD=$JAVA_HOME/bin/java fi if [ ! -x "$JAVACMD" ] ; then die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME @@ -98,88 +130,120 @@ Please set the JAVA_HOME variable in your environment to match the location of your Java installation." fi else - JAVACMD="java" - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. -if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then - MAX_FD_LIMIT=`ulimit -H -n` - if [ $? -eq 0 ] ; then - if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then - MAX_FD="$MAX_FD_LIMIT" - fi - ulimit -n $MAX_FD - if [ $? -ne 0 ] ; then - warn "Could not set maximum file descriptor limit: $MAX_FD" - fi - else - warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" - fi +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac fi -# For Darwin, add options to specify how the application appears in the dock -if $darwin; then - GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" -fi +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. # For Cygwin or MSYS, switch paths to Windows format before running java -if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then - APP_HOME=`cygpath --path --mixed "$APP_HOME"` - CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` - - JAVACMD=`cygpath --unix "$JAVACMD"` - - # We build the pattern for arguments to be converted via cygpath - ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` - SEP="" - for dir in $ROOTDIRSRAW ; do - ROOTDIRS="$ROOTDIRS$SEP$dir" - SEP="|" - done - OURCYGPATTERN="(^($ROOTDIRS))" - # Add a user-defined pattern to the cygpath arguments - if [ "$GRADLE_CYGPATTERN" != "" ] ; then - OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" - fi +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + # Now convert the arguments - kludge to limit ourselves to /bin/sh - i=0 - for arg in "$@" ; do - CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` - CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option - - if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition - eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` - else - eval `echo args$i`="\"$arg\"" + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) fi - i=`expr $i + 1` + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg done - case $i in - 0) set -- ;; - 1) set -- "$args0" ;; - 2) set -- "$args0" "$args1" ;; - 3) set -- "$args0" "$args1" "$args2" ;; - 4) set -- "$args0" "$args1" "$args2" "$args3" ;; - 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; - 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; - 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; - 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; - 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; - esac fi -# Escape application args -save () { - for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done - echo " " -} -APP_ARGS=`save "$@"` -# Collect all arguments for the java command, following the shell quoting and substitution rules -eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat index ac1b06f..6689b85 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -14,7 +14,7 @@ @rem limitations under the License. @rem -@if "%DEBUG%" == "" @echo off +@if "%DEBUG%"=="" @echo off @rem ########################################################################## @rem @rem Gradle startup script for Windows @@ -25,7 +25,8 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 -if "%DIRNAME%" == "" set DIRNAME=. +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% @@ -40,7 +41,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto execute +if %ERRORLEVEL% equ 0 goto execute echo. echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. @@ -75,13 +76,15 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar :end @rem End local scope for the variables with windows NT shell -if "%ERRORLEVEL%"=="0" goto mainEnd +if %ERRORLEVEL% equ 0 goto mainEnd :fail rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of rem the _cmd.exe /c_ return code! -if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 -exit /b 1 +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% :mainEnd if "%OS%"=="Windows_NT" endlocal diff --git a/settings.gradle b/settings.gradle index 48c039e..d94f73c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index 904c9be..60e84ac 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -1,11 +1,8 @@ package org.strykeforce.thirdcoast import com.ctre.phoenix.CANifier -import com.ctre.phoenix.motorcontrol.FeedbackDevice -import com.ctre.phoenix.motorcontrol.NeutralMode import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonSRX -import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration import com.ctre.phoenix.sensors.PigeonIMU import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.wpilibj.DigitalOutput @@ -46,7 +43,9 @@ val tctModule = module { single { TalonService(get()) { id -> TalonSRX(id) } } - single { TalonFxService(get()) { id -> TalonFX(id) } } + single { LegacyTalonFxService(get()) { id -> TalonFX(id) } } + + single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id)} } single { ServoService { id -> Servo(id) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt index a84c0a7..3084c0a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt @@ -22,6 +22,7 @@ import org.strykeforce.thirdcoast.swerve.SaveZeroCommand import org.strykeforce.thirdcoast.swerve.SelectAzimuthCommand import org.strykeforce.thirdcoast.swerve.SetAzimuthCommand import org.strykeforce.thirdcoast.talon.* +import org.strykeforce.thirdcoast.talon.phoenix6.* private val logger = KotlinLogging.logger {} @@ -40,10 +41,11 @@ interface Command { const val DEVICE_KEY = "device" fun createFromToml(toml: TomlTable, parent: MenuCommand? = null, key: String = "ROOT"): Command { - + logger.info{"key: $key, toml: ${toml.keySet()}"} val type = toml.getString(TYPE_KEY) ?: throw Exception("$key: $TYPE_KEY missing") logger.info { "type: $type, key: $key" } + return when (type) { "menu" -> createMenuCommand(parent, key, toml) "talon.select" -> SelectTalonsCommand(parent, key, toml) @@ -81,6 +83,25 @@ interface Command { "swerve.azimuth.adjust" -> AdjustAzimuthCommand(parent, key, toml) "pigeon.select" -> SelectPigeonCommand(parent, key, toml) "pigeon.param" -> PigeonParameterCommand(parent, key, toml) + "p6.run" -> P6RunCommand(parent, key, toml) + "p6.select" -> P6SelectTalonsCommand(parent, key, toml) + "p6.status" -> P6TalonStatusCommand(parent, key, toml) + "p6.modeStatus" -> P6ModeStatusCommand(parent, key, toml) + "p6.mode" -> P6SelectModeCommand(parent, key, toml) + "p6.param" -> Phoenix6ParameterCommand(parent, key, toml) + "p6.mmType" -> P6SelectMotionMagicTypeCommand(parent, key, toml) + "p6.diffType" -> P6SelectDifferentialTypeCommand(parent, key, toml) + "p6.followType" -> P6SelectFollowerTypeCommand(parent, key, toml) + "p6.neutralOut" -> P6SelectNeutralOutputCommand(parent, key, toml) + "p6.units" -> P6SelectUnitCommand(parent, key, toml) + "p6.slot" -> P6SelectSlotCommand(parent, key, toml) + "p6.gravity" -> P6SelectGravityTypeCommand(parent, key, toml) + "p6.invert" -> P6SelectMotorInvertCommand(parent, key, toml) + "p6.neutral" -> P6SelectNeutralModeCommand(parent, key, toml) + "p6.fwdNorm" -> P6SelectFwdHardLimitNormalCommand(parent, key, toml) + "p6.fwdSource" -> P6SelectFwdHardLimitSourceCommand(parent, key, toml) + "p6.revNorm" -> P6SelectRevHardLimitNormalCommand(parent, key, toml) + "p6.revSource" -> P6SelectRevHardLimitSourceCommand(parent, key, toml) else -> DefaultCommand(parent, key, toml) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt new file mode 100644 index 0000000..e5ae51d --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt @@ -0,0 +1,71 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.NeutralMode +import com.ctre.phoenix.motorcontrol.can.SlotConfiguration +import com.ctre.phoenix.motorcontrol.can.TalonFX +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private val logger = KotlinLogging.logger {} + +private val CONTROL_MODE_DEFAULT = ControlMode.PercentOutput +private const val ACTIVE_SLOT_DEFAULT = 0 +private val NEUTRAL_MODE_DEFAULT = NeutralMode.EEPROMSetting +private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true +private const val CURRENT_LIMIT_ENABLED_DEFAULT = false +private const val SENSOR_PHASE_INVERTED_DEFAULT = false +private const val OUTPUT_INVERTED_DEFAULT = false + + +class LegacyTalonFxService( + private val telemetryService: TelemetryService, factory: (id: Int) -> TalonFX +) : AbstractDeviceService(factory) { + + val timeout = 10 + var dirty = true + var neutralMode = NEUTRAL_MODE_DEFAULT + var controlMode = CONTROL_MODE_DEFAULT + var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT + var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT + var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT + + var activeConfiguration = TalonFXConfiguration() + get() { + if (!dirty) return field + active.firstOrNull()?.getAllConfigs(field, timeout) + ?: logger.debug("no active talon fx's, returning default config") + dirty = false + return field + } + + val activeSlot: SlotConfiguration + get() = when (activeSlotIndex) { + 0 -> activeConfiguration.slot0 + 1 -> activeConfiguration.slot1 + 2 -> activeConfiguration.slot2 + 3 -> activeConfiguration.slot3 + else -> throw IllegalStateException("invalid slot: $activeSlotIndex") + } + + val outputInverted: Boolean + get() = active.firstOrNull()?.inverted ?: OUTPUT_INVERTED_DEFAULT + + override fun activate(ids: Collection): Set { + dirty = true + + val new = super.activate(ids) + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach { + it.setNeutralMode(neutralMode) + it.selectProfileSlot(activeSlotIndex, 0) + it.enableVoltageCompensation(voltageCompensation) + it.setSensorPhase(sensorPhase) + it.inverted = OUTPUT_INVERTED_DEFAULT + telemetryService.register(it) + } + telemetryService.start() + return new + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt index 919f327..2d5373b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt @@ -1,71 +1,198 @@ package org.strykeforce.thirdcoast.device -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.NeutralMode -import com.ctre.phoenix.motorcontrol.can.SlotConfiguration -import com.ctre.phoenix.motorcontrol.can.TalonFX -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.DynamicMotionMagicDutyCycle +import com.ctre.phoenix6.controls.StrictFollower +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.ControlModeValue +import com.ctre.phoenix6.signals.NeutralModeValue import mu.KotlinLogging import org.strykeforce.telemetry.TelemetryService private val logger = KotlinLogging.logger {} -private val CONTROL_MODE_DEFAULT = ControlMode.PercentOutput +private val CONTROL_MODE_DEFAULT = ControlModeValue.DutyCycleOut private const val ACTIVE_SLOT_DEFAULT = 0 -private val NEUTRAL_MODE_DEFAULT = NeutralMode.EEPROMSetting +private val NEUTRAL_MODE_DEFAULT = NeutralModeValue.Coast private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true private const val CURRENT_LIMIT_ENABLED_DEFAULT = false private const val SENSOR_PHASE_INVERTED_DEFAULT = false private const val OUTPUT_INVERTED_DEFAULT = false +enum class Units{ + PERCENT, VOLTAGE, TORQUE_CURRENT +} + +enum class MM_Type { + STANDARD, VELOCITY, DYNAMIC +} + +enum class FollowerType { + STRICT, STANDARD +} + +enum class SetpointType { + OPEN_LOOP, POSITION, VELOCITY, MOTION_MAGIC, DIFFERENTIAL, FOLLOWER, NEUTRAL, MUSIC +} +enum class DifferentialType { + OPEN_LOOP, POSITION, VELOCITY, MOTION_MAGIC, FOLLOWER +} + class TalonFxService( - private val telemetryService: TelemetryService, factory: (id: Int) -> TalonFX + private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ) : AbstractDeviceService(factory) { val timeout = 10 var dirty = true - var neutralMode = NEUTRAL_MODE_DEFAULT - var controlMode = CONTROL_MODE_DEFAULT - var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT - var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT + //var neutralMode = NEUTRAL_MODE_DEFAULT + //var controlMode = CONTROL_MODE_DEFAULT + // var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT + //var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT + var activeUnits: Units = Units.PERCENT + var active_MM_type: MM_Type = MM_Type.STANDARD + var activeFeedForward: Double = 0.0 + var activeTorqueCurrentDeadband: Double = 0.0 + var activeTorqueCurrentMaxOut: Double = 1.0 + var activeVelocity: Double = 0.0 + var activeAcceleration: Double = 0.0 + var activeJerk: Double = 0.0 + var activeDifferentialTarget: Double = 0.0 + var activeFollowerType: FollowerType = FollowerType.STRICT + var activeOpposeMain: Boolean = false + var activeDifferentialSlot: Int = 0 + var activeFOC: Boolean = false + var setpointType: SetpointType = SetpointType.OPEN_LOOP + var differentialType: DifferentialType = DifferentialType.OPEN_LOOP + var activeNeutralOut: NeutralModeValue = NeutralModeValue.Coast + var activeOverrideNeutral: Boolean = false + + var controlMode: String = "Duty Cycle Out" + get() { + when(setpointType){ + SetpointType.OPEN_LOOP -> { + when(activeUnits){ + Units.PERCENT -> return "Duty Cycle Out" + Units.VOLTAGE -> return "Voltage Out" + Units.TORQUE_CURRENT -> return "Torque Current FOC" + } + } + SetpointType.POSITION -> { + when(activeUnits){ + Units.PERCENT -> return "Position: Duty Cycle" + Units.VOLTAGE -> return "Position: Voltage" + Units.TORQUE_CURRENT -> return "Position: Torque Current FOC" + } + } + SetpointType.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Velocity: Duty Cycle" + Units.VOLTAGE -> return "Velocity: Voltage" + Units.TORQUE_CURRENT -> return "Velocity: Torque Current FOC" + } + } + SetpointType.MOTION_MAGIC -> { + when(active_MM_type) { + MM_Type.STANDARD -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "Motion Magic: Torque Current FOC" + } + } MM_Type.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Velocity: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Velocity: Voltage" + Units.TORQUE_CURRENT -> return "Motion Magic Velocity: Torque Current FOC" + } + } MM_Type.DYNAMIC -> { + when(activeUnits) { + Units.PERCENT -> return "Dynamic Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Dynamic Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "Dynamic Motion Magic: Torque Current FOC" + } + } + } + } + SetpointType.DIFFERENTIAL -> { + when(differentialType) { + DifferentialType.OPEN_LOOP -> { + when(activeUnits) { + Units.PERCENT -> return "Differential: Duty Cycle" + Units.VOLTAGE -> return "Differential: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.POSITION -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Position: Duty Cycle" + Units.VOLTAGE -> return "Differential Position: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.VELOCITY -> { + when(activeUnits){ + Units.PERCENT -> return "Differential Velocity: Duty Cycle" + Units.VOLTAGE -> return "Differential Velocity: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.MOTION_MAGIC -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Differential Motion Magic: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Differential Follower: Non-Strict" + FollowerType.STRICT -> return "Differential Follower: Strict" + } + } + } + } + SetpointType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Follower: Non-Strict" + FollowerType.STRICT -> return "Follower: Strict" + } + } + SetpointType.NEUTRAL -> { + when(activeNeutralOut){ + NeutralModeValue.Brake -> return "Brake Mode" + NeutralModeValue.Coast -> return "Coast Mode" + } + } + SetpointType.MUSIC -> { + return "Music Tone" + + } + } + } + + var activeConfiguration = TalonFXConfiguration() get() { - if (!dirty) return field - active.firstOrNull()?.getAllConfigs(field, timeout) - ?: logger.debug("no active talon fx's, returning default config") + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?:logger.debug("no active talon fx's, returning default config") dirty = false return field } - val activeSlot: SlotConfiguration - get() = when (activeSlotIndex) { - 0 -> activeConfiguration.slot0 - 1 -> activeConfiguration.slot1 - 2 -> activeConfiguration.slot2 - 3 -> activeConfiguration.slot3 - else -> throw IllegalStateException("invalid slot: $activeSlotIndex") - } - - val outputInverted: Boolean - get() = active.firstOrNull()?.inverted ?: OUTPUT_INVERTED_DEFAULT override fun activate(ids: Collection): Set { dirty = true val new = super.activate(ids) telemetryService.stop() - active.filter { new.contains(it.deviceID) }.forEach { - it.setNeutralMode(neutralMode) - it.selectProfileSlot(activeSlotIndex, 0) - it.enableVoltageCompensation(voltageCompensation) - it.setSensorPhase(sensorPhase) - it.inverted = OUTPUT_INVERTED_DEFAULT - telemetryService.register(it) + active.filter { new.contains(it.deviceID) }.forEach{ + activeSlotIndex = it.closedLoopSlot.value + telemetryService.register(it,false) } + telemetryService.start() + active.firstOrNull() return new } + } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt index fe7a5c6..2a20560 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt @@ -6,7 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -18,7 +18,7 @@ class FeedbackCoefficientCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout @@ -30,12 +30,12 @@ class FeedbackCoefficientCommand( when (pidIndex) { 0 -> { if(type == "srx") talonService.activeConfiguration.primaryPID.selectedFeedbackCoefficient - else if(type == "fx") talonFxService.activeConfiguration.primaryPID.selectedFeedbackCoefficient + else if(type == "fx") legacyTalonFxService.activeConfiguration.primaryPID.selectedFeedbackCoefficient else throw IllegalArgumentException() } else -> { if(type == "srx") talonService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient - else if(type == "fx") talonFxService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient + else if(type == "fx") legacyTalonFxService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient else throw IllegalArgumentException() } } @@ -44,7 +44,7 @@ class FeedbackCoefficientCommand( override fun execute(): Command { var config: BaseTalonConfiguration if(type == "srx") config = talonService.activeConfiguration - else if(type == "fx") config = talonFxService.activeConfiguration + else if(type == "fx") config = legacyTalonFxService.activeConfiguration else throw IllegalArgumentException() val default = when (pidIndex) { @@ -54,7 +54,7 @@ class FeedbackCoefficientCommand( val paramValue = param.readDouble(reader, default) if(type == "srx") talonService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonService.timeout) } - else if(type == "fx") talonFxService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonFxService.timeout) } + else if(type == "fx") legacyTalonFxService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, legacyTalonFxService.timeout) } logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } return super.execute() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt index ee377d3..fee82fd 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt @@ -12,7 +12,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -30,7 +30,7 @@ class SelectHardLimitSourceCommand( ) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") val isForward = toml.getBoolean("forward") ?: true @@ -43,7 +43,7 @@ class SelectHardLimitSourceCommand( get() { var config: BaseTalonConfiguration = when (type) { "srx" -> talonService.activeConfiguration - "fx" -> talonFxService.activeConfiguration + "fx" -> legacyTalonFxService.activeConfiguration else -> throw IllegalArgumentException() } return values.indexOf( @@ -62,8 +62,8 @@ class SelectHardLimitSourceCommand( config = talonService.activeConfiguration } else if(type == "fx") { - active = talonFxService.active - config = talonFxService.activeConfiguration + active = legacyTalonFxService.active + config = legacyTalonFxService.activeConfiguration } else throw IllegalArgumentException() val source = values[index] @@ -102,7 +102,7 @@ class SelectHardLimitNormalCommand( ) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") val isForward = toml.getBoolean("forward") ?: true @@ -114,7 +114,7 @@ class SelectHardLimitNormalCommand( override val activeIndex: Int get() { if(type == "srx") activeConfig = talonService.activeConfiguration - else if(type == "fx") activeConfig = talonFxService.activeConfiguration + else if(type == "fx") activeConfig = legacyTalonFxService.activeConfiguration else throw IllegalArgumentException() return values.indexOf( if (isForward) @@ -130,8 +130,8 @@ class SelectHardLimitNormalCommand( activeConfig = talonService.activeConfiguration active = talonService.active } else if(type == "fx"){ - activeConfig = talonFxService.activeConfiguration - active = talonFxService.active + activeConfig = legacyTalonFxService.activeConfiguration + active = legacyTalonFxService.active } else throw IllegalArgumentException() val source = getSource() val normal = values[index] diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt index 2d4f7ea..6031386 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt @@ -10,7 +10,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.warn import java.lang.IllegalArgumentException @@ -26,7 +26,7 @@ class RunTalonsCommand( val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() override fun execute(): Command { @@ -43,7 +43,7 @@ class RunTalonsCommand( if (type == "srx") { mode = talonService.controlMode } else if (type == "fx") { - mode = talonFxService.controlMode + mode = legacyTalonFxService.controlMode } else throw IllegalArgumentException() // sanity checks @@ -61,7 +61,7 @@ class RunTalonsCommand( if (type == "srx"){ talonService.active.forEach { it.set(mode, setpoint) } } else if(type == "fx"){ - talonFxService.active.forEach { it.set(mode, setpoint) } + legacyTalonFxService.active.forEach { it.set(mode, setpoint) } } @@ -72,7 +72,7 @@ class RunTalonsCommand( if(type == "srx"){ talonService.active.forEach { it.set(mode, 0.0) } } else if(type == "fx"){ - talonFxService.active.forEach { it.set(mode, 0.0) } + legacyTalonFxService.active.forEach { it.set(mode, 0.0) } } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt index 5f8864e..3304552 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt @@ -1,12 +1,11 @@ package org.strykeforce.thirdcoast.talon import com.ctre.phoenix.sensors.AbsoluteSensorRange -import com.ctre.phoenix.sensors.SensorInitializationStrategy import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService private val RANGE = listOf( AbsoluteSensorRange.Signed_PlusMinus180, @@ -24,14 +23,14 @@ class SelectAbsoluteSensorRange( toml: TomlTable ) : AbstractSelectCommand(parent, key, toml, RANGE, LABELS) { - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() override val activeIndex: Int - get() = values.indexOf(talonFxService.activeConfiguration.absoluteSensorRange) + get() = values.indexOf(legacyTalonFxService.activeConfiguration.absoluteSensorRange) override fun setActive(index: Int) { val range = values[index] - talonFxService.active.forEach { it.configIntegratedSensorAbsoluteRange(range, talonFxService.timeout) } - talonFxService.activeConfiguration.absoluteSensorRange = range + legacyTalonFxService.active.forEach { it.configIntegratedSensorAbsoluteRange(range, legacyTalonFxService.timeout) } + legacyTalonFxService.activeConfiguration.absoluteSensorRange = range } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt index 6368b6e..331890f 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt @@ -6,7 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class SelectBrakeModeCommand( @@ -21,14 +21,14 @@ class SelectBrakeModeCommand( listOf("B/C CAL Button", "Brake", "Coast") ) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { if(type == "srx") return values.indexOf(talonService.neutralMode) - else if(type == "fx") return values.indexOf(talonFxService.neutralMode) + else if(type == "fx") return values.indexOf(legacyTalonFxService.neutralMode) else throw IllegalArgumentException() } @@ -37,8 +37,8 @@ class SelectBrakeModeCommand( talonService.neutralMode = values[index] talonService.active.forEach { it.setNeutralMode(talonService.neutralMode) } } else if(type == "fx"){ - talonFxService.neutralMode = values[index] - talonFxService.active.forEach { it.setNeutralMode(talonFxService.neutralMode) } + legacyTalonFxService.neutralMode = values[index] + legacyTalonFxService.active.forEach { it.setNeutralMode(legacyTalonFxService.neutralMode) } } } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt index 94a6631..cf82478 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt @@ -6,7 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService @@ -22,19 +22,19 @@ class SelectControlModeCommand( listOf("Percent Output", "Motion Magic", "Velocity", "Position", "Follower") ) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { if(type == "srx") return values.indexOf(talonService.controlMode) - else if(type == "fx") return values.indexOf(talonFxService.controlMode) + else if(type == "fx") return values.indexOf(legacyTalonFxService.controlMode) else throw IllegalArgumentException() } override fun setActive(index: Int) { if(type == "srx") talonService.controlMode = values[index] - else if(type == "fx") talonFxService.controlMode = values[index] + else if(type == "fx") legacyTalonFxService.controlMode = values[index] } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt index d176f13..8197fec 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt @@ -7,7 +7,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val SENSORS = listOf( @@ -44,7 +44,7 @@ class SelectFeedbackSensorCommand( ) : AbstractSelectCommand(parent, key, toml, SENSORS, LABELS) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val pidIndex = toml.getLong("pid")?.toInt() ?: 0 @@ -52,7 +52,7 @@ class SelectFeedbackSensorCommand( get() { val config: BaseTalonConfiguration if (type == "srx") config = talonService.activeConfiguration - else if (type == "fx") config = talonFxService.activeConfiguration + else if (type == "fx") config = legacyTalonFxService.activeConfiguration else throw IllegalArgumentException() val sensor = when (pidIndex) { @@ -76,8 +76,8 @@ class SelectFeedbackSensorCommand( config = talonService.activeConfiguration talonService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, talonService.timeout) } } else if (type == "fx") { - config = talonFxService.activeConfiguration - talonFxService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, talonFxService.timeout) } + config = legacyTalonFxService.activeConfiguration + legacyTalonFxService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, legacyTalonFxService.timeout) } } else throw IllegalArgumentException() when (pidIndex) { 0 -> config.primaryPID.selectedFeedbackSensor = sensor diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt index 23444cd..8f68291 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt @@ -1,12 +1,11 @@ package org.strykeforce.thirdcoast.talon -import com.ctre.phoenix.motorcontrol.MotorCommutation import com.ctre.phoenix.sensors.SensorInitializationStrategy import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService private val STRATEGY = listOf( SensorInitializationStrategy.BootToAbsolutePosition, @@ -24,14 +23,14 @@ class SelectInitializationStrategy( toml: TomlTable ) : AbstractSelectCommand(parent, key, toml, STRATEGY, LABELS) { - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() override val activeIndex: Int - get() = values.indexOf(talonFxService.activeConfiguration.initializationStrategy) + get() = values.indexOf(legacyTalonFxService.activeConfiguration.initializationStrategy) override fun setActive(index: Int) { val mode = values[index] - talonFxService.active.forEach { it.configIntegratedSensorInitializationStrategy(mode, talonFxService.timeout) } - talonFxService.activeConfiguration.initializationStrategy = mode + legacyTalonFxService.active.forEach { it.configIntegratedSensorInitializationStrategy(mode, legacyTalonFxService.timeout) } + legacyTalonFxService.activeConfiguration.initializationStrategy = mode } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt index c40d508..5aa2a54 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt @@ -1,13 +1,11 @@ package org.strykeforce.thirdcoast.talon -import com.ctre.phoenix.motorcontrol.FeedbackDevice import com.ctre.phoenix.motorcontrol.MotorCommutation import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject -import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService private val COMMUTATION = listOf( MotorCommutation.Trapezoidal @@ -23,14 +21,14 @@ class SelectMotorCommutationCommand( toml: TomlTable ) : AbstractSelectCommand(parent, key, toml, COMMUTATION, LABELS) { - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() override val activeIndex: Int - get() = values.indexOf(talonFxService.activeConfiguration.motorCommutation) + get() = values.indexOf(legacyTalonFxService.activeConfiguration.motorCommutation) override fun setActive(index: Int) { val mode = values[index] - talonFxService.active.forEach { it.configMotorCommutation(mode,talonFxService.timeout) } - talonFxService.activeConfiguration.motorCommutation = mode + legacyTalonFxService.active.forEach { it.configMotorCommutation(mode,legacyTalonFxService.timeout) } + legacyTalonFxService.activeConfiguration.motorCommutation = mode } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt index 4eda3d6..9746677 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt @@ -4,7 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val SLOTS = listOf(0, 1, 2, 3) @@ -18,17 +18,17 @@ class SelectSlotCommand( val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() override val activeIndex: Int get() { if(type == "srx") return talonService.activeSlotIndex - else if(type == "fx") return talonFxService.activeSlotIndex + else if(type == "fx") return legacyTalonFxService.activeSlotIndex else throw IllegalArgumentException() } override fun setActive(index: Int) { if(type == "srx") talonService.activeSlotIndex = index - else if(type == "fx") talonFxService.activeSlotIndex = index + else if(type == "fx") legacyTalonFxService.activeSlotIndex = index } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt index 8c44d91..c526ceb 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt @@ -7,7 +7,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.info import org.strykeforce.thirdcoast.readIntList @@ -19,7 +19,7 @@ class SelectTalonsCommand( toml: TomlTable ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") @@ -30,7 +30,7 @@ class SelectTalonsCommand( return formatMenu(talonService.active.map(TalonSRX::getDeviceID).joinToString()) } "fx" -> { - return formatMenu(talonFxService.active.map(TalonFX::getDeviceID).joinToString()) + return formatMenu(legacyTalonFxService.active.map(TalonFX::getDeviceID).joinToString()) } else -> throw IllegalArgumentException() } @@ -46,8 +46,8 @@ class SelectTalonsCommand( ids = reader.readIntList(this.prompt("ids"), talonService.active.map(TalonSRX::getDeviceID)) new = talonService.activate(ids) } else if (type == "fx") { - ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) - new = talonFxService.activate(ids) + ids = reader.readIntList(this.prompt("ids"), legacyTalonFxService.active.map(TalonFX::getDeviceID)) + new = legacyTalonFxService.activate(ids) } else throw IllegalArgumentException() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt index 8b5497f..a91b3d3 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt @@ -6,7 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class SelectVelocityMeasurmentPeriodCommand( @@ -21,12 +21,12 @@ class SelectVelocityMeasurmentPeriodCommand( listOf("1 ms", "2 ms", "5 ms", "10 ms", "20 ms", "50 ms", "100 ms") ) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { if(type == "srx") return values.indexOf(talonService.activeConfiguration.velocityMeasurementPeriod) - else if(type == "fx") return values.indexOf(talonFxService.activeConfiguration.velocityMeasurementPeriod) + else if(type == "fx") return values.indexOf(legacyTalonFxService.activeConfiguration.velocityMeasurementPeriod) else throw IllegalArgumentException() } @@ -36,8 +36,8 @@ class SelectVelocityMeasurmentPeriodCommand( talonService.active.forEach { it.configVelocityMeasurementPeriod(period, talonService.timeout) } talonService.activeConfiguration.velocityMeasurementPeriod = period }else if(type == "fx"){ - talonFxService.active.forEach { it.configVelocityMeasurementPeriod(period, talonFxService.timeout) } - talonFxService.activeConfiguration.velocityMeasurementPeriod = period + legacyTalonFxService.active.forEach { it.configVelocityMeasurementPeriod(period, legacyTalonFxService.timeout) } + legacyTalonFxService.activeConfiguration.velocityMeasurementPeriod = period } } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt index 8b9e288..426eb93 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt @@ -5,7 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -17,7 +17,7 @@ class SetSensorPositionCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN") @@ -26,7 +26,7 @@ class SetSensorPositionCommand( override val menu: String get() { if(type == "srx") return formatMenu(talonService.active.map { it.getSelectedSensorPosition(pidIndex) }.joinToString()) - else if(type == "fx") return formatMenu(talonFxService.active.map{ it.getSelectedSensorPosition(pidIndex)}.joinToString()) + else if(type == "fx") return formatMenu(legacyTalonFxService.active.map{ it.getSelectedSensorPosition(pidIndex)}.joinToString()) else throw IllegalArgumentException() } @@ -39,8 +39,8 @@ class SetSensorPositionCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } else if(type == "fx") { - talonFxService.active.forEach { it.setSelectedSensorPosition(paramValue.toDouble(), pidIndex, talonFxService.timeout) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { it.setSelectedSensorPosition(paramValue.toDouble(), pidIndex, legacyTalonFxService.timeout) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt index ed1fe35..e600135 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt @@ -9,7 +9,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.talon.TalonParameter.Enum.* @@ -22,7 +22,7 @@ class TalonParameterCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN") @@ -37,13 +37,13 @@ class TalonParameterCommand( slot = talonService.activeSlot } "fx" -> { - baseConfig = talonFxService.activeConfiguration - slot = talonFxService.activeSlot + baseConfig = legacyTalonFxService.activeConfiguration + slot = legacyTalonFxService.activeSlot } else -> throw IllegalArgumentException() } val srxConfig = talonService.activeConfiguration - val fxConfig = talonFxService.activeConfiguration + val fxConfig = legacyTalonFxService.activeConfiguration return when (param.enum) { SLOT_P -> formatMenu(slot.kP, DOUBLE_FORMAT_4) SLOT_I -> formatMenu(slot.kI, DOUBLE_FORMAT_4) @@ -55,7 +55,7 @@ class TalonParameterCommand( SLOT_PEAK_OUTPUT -> formatMenu(slot.closedLoopPeakOutput) OUTPUT_REVERSED -> when(type){ "srx" -> formatMenu(talonService.outputInverted) - "fx" -> formatMenu(talonFxService.outputInverted) + "fx" -> formatMenu(legacyTalonFxService.outputInverted) else -> throw IllegalArgumentException() } OPEN_LOOP_RAMP -> formatMenu(baseConfig.openloopRamp) @@ -67,7 +67,7 @@ class TalonParameterCommand( NEUTRAL_DEADBAND -> formatMenu(baseConfig.neutralDeadband) VOLTAGE_COMP_ENABLE -> when (type) { "srx" -> formatMenu(talonService.voltageCompensation) - "fx" -> formatMenu(talonFxService.voltageCompensation) + "fx" -> formatMenu(legacyTalonFxService.voltageCompensation) else -> throw IllegalArgumentException() } VOLTAGE_COMP_SATURATION -> formatMenu(baseConfig.voltageCompSaturation) @@ -76,7 +76,7 @@ class TalonParameterCommand( MOTION_ACCELERATION -> formatMenu(baseConfig.motionAcceleration) SENSOR_PHASE -> when (type) { "srx" -> formatMenu(talonService.sensorPhase) - "fx" -> formatMenu(talonFxService.sensorPhase) + "fx" -> formatMenu(legacyTalonFxService.sensorPhase) else -> throw IllegalArgumentException() } CURRENT_LIMIT_ENABLE -> formatMenu(talonService.currentLimit) @@ -145,15 +145,15 @@ class TalonParameterCommand( timeout = talonService.timeout } "fx" -> { - config = talonFxService.activeConfiguration - slot = talonFxService.activeSlot - activeSlotIndex = talonFxService.activeSlotIndex - timeout = talonFxService.timeout + config = legacyTalonFxService.activeConfiguration + slot = legacyTalonFxService.activeSlot + activeSlotIndex = legacyTalonFxService.activeSlotIndex + timeout = legacyTalonFxService.timeout } else -> throw IllegalArgumentException() } val srxConfig = talonService.activeConfiguration - val fxConfig = talonFxService.activeConfiguration + val fxConfig = legacyTalonFxService.activeConfiguration when (param.enum) { SLOT_P -> configDoubleParam(slot.kP) { baseTalon, value -> @@ -224,7 +224,7 @@ class TalonParameterCommand( baseTalon.enableVoltageCompensation(value) when (type) { "srx" -> talonService.voltageCompensation = value - "fx" -> talonFxService.voltageCompensation = value + "fx" -> legacyTalonFxService.voltageCompensation = value else -> throw IllegalArgumentException() } } @@ -248,7 +248,7 @@ class TalonParameterCommand( baseTalon.setSensorPhase(value) when (type) { "srx" -> talonService.sensorPhase = value - "fx" -> talonFxService.sensorPhase = value + "fx" -> legacyTalonFxService.sensorPhase = value else -> throw IllegalArgumentException() } } @@ -399,7 +399,7 @@ class TalonParameterCommand( // Let the Talon round down then number to a legal value when (type) { "srx" -> talonService.dirty = true - "fx" -> talonFxService.dirty = true + "fx" -> legacyTalonFxService.dirty = true else -> throw IllegalArgumentException() } } @@ -423,8 +423,8 @@ class TalonParameterCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } "fx" -> { - talonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { config(it, paramValue) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } else -> throw IllegalArgumentException() } @@ -438,8 +438,8 @@ class TalonParameterCommand( private fun configBooleanFxParam(default: Boolean, config: (TalonFX, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) - talonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { config(it, paramValue) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } private fun configIntParam(default: Int, config: (BaseTalon, Int) -> Unit) { @@ -450,8 +450,8 @@ class TalonParameterCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } "fx" -> { - talonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { config(it, paramValue) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } else -> throw IllegalArgumentException() } @@ -471,8 +471,8 @@ class TalonParameterCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } "fx" -> { - talonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { config(it, paramValue) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } else -> throw IllegalArgumentException() } @@ -486,14 +486,14 @@ class TalonParameterCommand( private fun configDoubleFxParam(default: Double, config: (TalonFX, Double) -> Unit) { val paramValue = param.readDouble(reader, default) - talonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonfx ${param.name}: $paramValue" } + legacyTalonFxService.active.forEach { config(it, paramValue) } + logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } } private fun defaultFor(frame: StatusFrameEnhanced): Int { when (type) { "srx" -> return talonService.active.first().getStatusFramePeriod(frame) - "fx" -> return talonFxService.active.first().getStatusFramePeriod(frame) + "fx" -> return legacyTalonFxService.active.first().getStatusFramePeriod(frame) else -> throw IllegalArgumentException() } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt index 60a8344..b53078c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt @@ -6,7 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class TalonsStatusCommand( @@ -16,7 +16,7 @@ class TalonsStatusCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val talonFxService: TalonFxService by inject() + private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { @@ -28,7 +28,7 @@ class TalonsStatusCommand( writer.println(config.toString(it.deviceID.toString())) } } else if(type == "fx"){ - talonFxService.active.forEach { + legacyTalonFxService.active.forEach { val config = TalonFXConfiguration() it.getAllConfigs(config) writer.println(config.toString(it.deviceID.toString())) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt new file mode 100644 index 0000000..081a026 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt @@ -0,0 +1,95 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.ForwardLimitSourceValue +import com.ctre.phoenix6.signals.ForwardLimitSourceValue.* +import com.ctre.phoenix6.signals.ForwardLimitTypeValue +import com.ctre.phoenix6.signals.ForwardLimitTypeValue.* +import com.ctre.phoenix6.signals.ReverseLimitSourceValue +import com.ctre.phoenix6.signals.ReverseLimitTypeValue +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectFwdHardLimitSourceCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(LimitSwitchPin, RemoteCANifier, RemoteTalonFX), + listOf("Talon FX pin", "Remote CANifier", "Remote TalonFx")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + + override fun setActive(index: Int) { + val source = values[index] + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } +} + +class P6SelectRevHardLimitSourceCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(ReverseLimitSourceValue.LimitSwitchPin, ReverseLimitSourceValue.RemoteCANifier, ReverseLimitSourceValue.RemoteTalonFX), + listOf("Talon FX pin", "Remote CANifier", "Remote TalonFX")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + + override fun setActive(index: Int) { + val source = values[index] + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } + +} + +class P6SelectFwdHardLimitNormalCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(NormallyClosed, NormallyOpen), + listOf("Normally Closed", "Normally Open")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + + override fun setActive(index: Int) { + val type = values[index] + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } +} + +class P6SelectRevHardLimitNormalCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(ReverseLimitTypeValue.NormallyClosed, ReverseLimitTypeValue.NormallyOpen), + listOf("Normally Closed", "Normally Open")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + + override fun setActive(index: Int) { + val type = values[index] + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt new file mode 100644 index 0000000..011c7be --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt @@ -0,0 +1,22 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6ModeStatusCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override fun execute(): Command { + val writer = terminal.writer() + writer.println(talonFxService.controlMode) + return super.execute() + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt new file mode 100644 index 0000000..6b6ff4c --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -0,0 +1,372 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.controls.CoastOut +import com.ctre.phoenix6.controls.ControlRequest +import com.ctre.phoenix6.controls.DifferentialDutyCycle +import com.ctre.phoenix6.controls.DifferentialFollower +import com.ctre.phoenix6.controls.DifferentialMotionMagicDutyCycle +import com.ctre.phoenix6.controls.DifferentialMotionMagicVoltage +import com.ctre.phoenix6.controls.DifferentialPositionDutyCycle +import com.ctre.phoenix6.controls.DifferentialPositionVoltage +import com.ctre.phoenix6.controls.DifferentialStrictFollower +import com.ctre.phoenix6.controls.DifferentialVelocityDutyCycle +import com.ctre.phoenix6.controls.DifferentialVelocityVoltage +import com.ctre.phoenix6.controls.DifferentialVoltage +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.DynamicMotionMagicDutyCycle +import com.ctre.phoenix6.controls.DynamicMotionMagicTorqueCurrentFOC +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage +import com.ctre.phoenix6.controls.Follower +import com.ctre.phoenix6.controls.MotionMagicDutyCycle +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC +import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle +import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage +import com.ctre.phoenix6.controls.MotionMagicVoltage +import com.ctre.phoenix6.controls.MusicTone +import com.ctre.phoenix6.controls.PositionDutyCycle +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC +import com.ctre.phoenix6.controls.PositionVoltage +import com.ctre.phoenix6.controls.StaticBrake +import com.ctre.phoenix6.controls.StrictFollower +import com.ctre.phoenix6.controls.TorqueCurrentFOC +import com.ctre.phoenix6.controls.VelocityDutyCycle +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC +import com.ctre.phoenix6.controls.VelocityVoltage +import com.ctre.phoenix6.controls.VoltageOut +import com.ctre.phoenix6.signals.NeutralModeValue +import edu.wpi.first.wpilibj.Timer +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.jline.reader.EndOfFileException +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.* +import org.strykeforce.thirdcoast.warn + +private val logger = KotlinLogging.logger{} + +class P6RunCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override fun execute(): Command { + var done = false + while(!done){ + try { + val line = reader.readLine(prompt()) + if (line.isEmpty()) throw EndOfFileException() + val setpoints = line.split(',') + val setpoint = setpoints[0].toDouble() + val duration = if (setpoints.size > 1) setpoints[1].toDouble() else 0.0 + val setpointType = talonFxService.setpointType + val units = talonFxService.activeUnits + val motionType = talonFxService.active_MM_type + val differentialType = talonFxService.differentialType + val followerType = talonFxService.activeFollowerType + val enableFOC = talonFxService.activeFOC + val overrideNeutral = talonFxService.activeOverrideNeutral + val velocity = talonFxService.activeVelocity + val acceleration = talonFxService.activeAcceleration + val jerk = talonFxService.activeJerk + val feedFwd = talonFxService.activeFeedForward + val slot = talonFxService.activeSlotIndex + val diffSlot = talonFxService.activeDifferentialSlot + val diffPos = talonFxService.activeDifferentialTarget + var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false) + + //sanity checks + if (units == Units.PERCENT && !(-1.0..1.0).contains(setpoint)) { + terminal.warn("setpoint must be in range -1.0 to 1.0 for percent modes") + continue + } + + if (((setpointType == SetpointType.MOTION_MAGIC && motionType != MM_Type.VELOCITY) || setpointType == SetpointType.POSITION) && duration > 0.0) { + terminal.warn("specifying a duration in position modes not allowed") + continue + } + + //make control request + when (setpointType) { + SetpointType.OPEN_LOOP -> { + when (units) { + Units.PERCENT -> controlRequest = DutyCycleOut(setpoint, enableFOC, overrideNeutral) + Units.VOLTAGE -> controlRequest = VoltageOut(setpoint, enableFOC, overrideNeutral) + Units.TORQUE_CURRENT -> controlRequest = TorqueCurrentFOC( + setpoint, + talonFxService.activeTorqueCurrentMaxOut, + talonFxService.activeTorqueCurrentDeadband, + overrideNeutral + ) + } + } + + SetpointType.POSITION -> { + when (units) { + Units.PERCENT -> controlRequest = + PositionDutyCycle(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral) + + Units.VOLTAGE -> controlRequest = + PositionVoltage(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral) + + Units.TORQUE_CURRENT -> controlRequest = + PositionTorqueCurrentFOC(setpoint, velocity, feedFwd, slot, overrideNeutral) + } + } + + SetpointType.VELOCITY -> { + when (units) { + Units.PERCENT -> controlRequest = + VelocityDutyCycle(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral) + + Units.VOLTAGE -> controlRequest = + VelocityVoltage(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral) + + Units.TORQUE_CURRENT -> controlRequest = + VelocityTorqueCurrentFOC(setpoint, acceleration, feedFwd, slot, overrideNeutral) + } + } + + SetpointType.MOTION_MAGIC -> { + when (motionType) { + MM_Type.STANDARD -> { + when (units) { + Units.PERCENT -> controlRequest = + MotionMagicDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral) + + Units.VOLTAGE -> controlRequest = + MotionMagicVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral) + + Units.TORQUE_CURRENT -> controlRequest = + MotionMagicTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral) + } + } + + MM_Type.VELOCITY -> { + when (units) { + Units.PERCENT -> controlRequest = MotionMagicVelocityDutyCycle( + setpoint, + acceleration, + enableFOC, + feedFwd, + slot, + overrideNeutral + ) + + Units.VOLTAGE -> controlRequest = MotionMagicVelocityVoltage( + setpoint, + acceleration, + enableFOC, + feedFwd, + slot, + overrideNeutral + ) + + Units.TORQUE_CURRENT -> controlRequest = MotionMagicVelocityTorqueCurrentFOC( + setpoint, + acceleration, + enableFOC, + feedFwd, + slot, + overrideNeutral + ) + } + } + + MM_Type.DYNAMIC -> { + when (units) { + Units.PERCENT -> controlRequest = DynamicMotionMagicDutyCycle( + setpoint, + velocity, + acceleration, + jerk, + enableFOC, + feedFwd, + slot, + overrideNeutral + ) + + Units.VOLTAGE -> controlRequest = DynamicMotionMagicVoltage( + setpoint, + velocity, + acceleration, + jerk, + enableFOC, + feedFwd, + slot, + overrideNeutral + ) + + Units.TORQUE_CURRENT -> controlRequest = DynamicMotionMagicTorqueCurrentFOC( + setpoint, + velocity, + acceleration, + jerk, + feedFwd, + slot, + overrideNeutral + ) + } + } + } + } + + SetpointType.DIFFERENTIAL -> { + when (differentialType) { + DifferentialType.OPEN_LOOP -> { + when (units) { + Units.PERCENT -> controlRequest = + DifferentialDutyCycle(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral) + + Units.VOLTAGE -> controlRequest = + DifferentialVoltage(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral) + + else -> { + terminal.warn("Units chosen not valid for this control mode") + continue + } + } + } + + DifferentialType.POSITION -> { + when (units) { + Units.PERCENT -> controlRequest = DifferentialPositionDutyCycle( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + Units.VOLTAGE -> controlRequest = DifferentialPositionVoltage( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + else -> { + terminal.warn("Units chosen not valid for this control mode") + continue + } + } + } + + DifferentialType.VELOCITY -> { + when (units) { + Units.PERCENT -> controlRequest = DifferentialVelocityDutyCycle( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + Units.VOLTAGE -> controlRequest = DifferentialVelocityVoltage( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + else -> { + terminal.warn("Units chosen not valid for this control mode") + continue + } + } + } + + DifferentialType.MOTION_MAGIC -> { + when (units) { + Units.PERCENT -> controlRequest = DifferentialMotionMagicDutyCycle( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + Units.VOLTAGE -> controlRequest = DifferentialMotionMagicVoltage( + setpoint, + diffPos, + enableFOC, + slot, + diffSlot, + overrideNeutral + ) + + else -> { + terminal.warn("Units chosen not valid for this control mode") + continue + } + } + } + + DifferentialType.FOLLOWER -> { + when (followerType) { + FollowerType.STANDARD -> controlRequest = + DifferentialFollower(setpoint.toInt(), talonFxService.activeOpposeMain) + + FollowerType.STRICT -> controlRequest = DifferentialStrictFollower(setpoint.toInt()) + } + } + + } + + } + + SetpointType.FOLLOWER -> { + when (followerType) { + FollowerType.STANDARD -> controlRequest = + Follower(setpoint.toInt(), talonFxService.activeOpposeMain) + + FollowerType.STRICT -> controlRequest = StrictFollower(setpoint.toInt()) + } + } + + SetpointType.NEUTRAL -> { + when (talonFxService.activeNeutralOut) { + NeutralModeValue.Coast -> controlRequest = CoastOut() + NeutralModeValue.Brake -> controlRequest = StaticBrake() + } + } + + SetpointType.MUSIC -> { + controlRequest = MusicTone(setpoint) + } + + } + + //run Talon + talonFxService.active.forEach { + it.setControl(controlRequest) + } + + //Check Timeout + if (duration > 0.0) { + logger.debug { "run duration = $duration seconds" } + Timer.delay(duration) + logger.debug { "run duration expired, setting output = 0.0" } + talonFxService.active.forEach { it.set(0.0) } + } + } catch (e: Exception) { + done = true + } + } + + return super.execute() + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt new file mode 100644 index 0000000..0e2e7c6 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt @@ -0,0 +1,45 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.DifferentialSensorSourceValue +import com.ctre.phoenix6.signals.DifferentialSensorSourceValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +private val SENSOR = listOf( + Disabled, + RemoteCANcoder, + RemotePigeon2_Pitch, + RemotePigeon2_Roll, + RemotePigeon2_Yaw, + RemoteTalonFX_Diff +) + +private val LABELS = listOf( + "Disabled", + "Remote CANcoder", + "Remote Pigeon2 Pitch", + "Remote Pigeon2 Roll", + "Remote Pigeon2 Yaw", + "Remote TalonFX Differential" +) + +class P6SelectDifferentialSensorSourceCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, SENSOR, LABELS) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + + override fun setActive(index: Int) { + val sensor = values[index] + talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt new file mode 100644 index 0000000..974134b --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt @@ -0,0 +1,28 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.DifferentialType +import org.strykeforce.thirdcoast.device.DifferentialType.* +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectDifferentialTypeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(OPEN_LOOP, POSITION, VELOCITY, MOTION_MAGIC, FOLLOWER), + listOf("Open Loop", "Position", "Velocity", "Motion Magic", "Follower")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.differentialType.ordinal + + override fun setActive(index: Int) { + val type = values[index] + talonFxService.differentialType = type + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt new file mode 100644 index 0000000..94e5aec --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt @@ -0,0 +1,53 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue.* +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.device.TalonFxService + +private val SENSORS = listOf( + FusedCANcoder, + RemoteCANcoder, + RemotePigeon2_Pitch, + RemotePigeon2_Roll, + RemotePigeon2_Yaw, + RotorSensor, + SyncCANcoder +) + +private val LABELS = listOf( + "(Pro) Fused CANcoder", + "Remote CANcoder", + "Remote Pigeon2 Pitch", + "Remote Pigeon2 Roll", + "Remote Pigeon2 Yaw", + "Rotor Sensor", + "(Pro) Sync CANcoder" +) + +class P6SelectFeedbackSensorCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, SENSORS, LABELS) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() { + val sensor = talonFxService.activeConfiguration.Feedback.FeedbackSensorSource + return values.indexOf(sensor) + } + + override fun setActive(index: Int) { + val sensor = values[index] + talonFxService.activeConfiguration.Feedback.FeedbackSensorSource = sensor + talonFxService.active.forEach { + it.configurator.apply(talonFxService.activeConfiguration) + } + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt new file mode 100644 index 0000000..832b3c2 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.FollowerType +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectFollowerTypeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(FollowerType.STRICT, FollowerType.STANDARD), + listOf("Strict Follower", "Standard Follower")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeFollowerType.ordinal + + override fun setActive(index: Int) { + val type = values[index] + talonFxService.activeFollowerType = type + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt new file mode 100644 index 0000000..d1c92dc --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt @@ -0,0 +1,58 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.GravityTypeValue +import com.ctre.phoenix6.signals.GravityTypeValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.warn + +private val GRAVITY = listOf( + Elevator_Static, + Arm_Cosine +) + +private val LABELS = listOf( + "Static - Elevator", + "Cosine - Arm" +) + +class P6SelectGravityTypeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, GRAVITY, LABELS) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() { + val slot = talonFxService.activeSlotIndex + when(slot){ + 0 -> return talonFxService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } + + override fun setActive(index: Int) { + val gravity = values[index] + val slot = talonFxService.activeSlotIndex + when(slot) { + 0 -> talonFxService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxService.active.forEach { + it.configurator.apply(talonFxService.activeConfiguration) + } + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt new file mode 100644 index 0000000..4133864 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt @@ -0,0 +1,28 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.SetpointType +import org.strykeforce.thirdcoast.device.SetpointType.* +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectModeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(OPEN_LOOP, POSITION, VELOCITY, MOTION_MAGIC, DIFFERENTIAL, FOLLOWER, NEUTRAL, MUSIC), + listOf("Open Loop", "Position", "Velocity", "Motion Magic", "Differential", "Follower", "Neutral Output", "Music Tone")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.setpointType.ordinal + + override fun setActive(index: Int) { + val mode = values[index] + talonFxService.setpointType = mode + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt new file mode 100644 index 0000000..2805df0 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.MM_Type +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectMotionMagicTypeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(MM_Type.STANDARD, MM_Type.VELOCITY, MM_Type.DYNAMIC), + listOf("Standard (Position)", "Velocity", "(FD) Dynamic")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.active_MM_type.ordinal + + override fun setActive(index: Int) { + val type = values[index] + talonFxService.active_MM_type = type + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt new file mode 100644 index 0000000..f7db425 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt @@ -0,0 +1,29 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.InvertedValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectMotorInvertCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(CounterClockwise_Positive, Clockwise_Positive), + listOf("CCW positive", "CW positive")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.MotorOutput.Inverted.ordinal + + override fun setActive(index: Int) { + val invert = values[index] + talonFxService.activeConfiguration.MotorOutput.Inverted = invert + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt new file mode 100644 index 0000000..a17fd2a --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt @@ -0,0 +1,38 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.NeutralModeValue +import com.ctre.phoenix6.signals.NeutralModeValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + + +private val MODE = listOf( + Brake, + Coast +) + +private val LABELS = listOf( + "Brake", + "Coast" +) + +class P6SelectNeutralModeCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, MODE, LABELS) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeConfiguration.MotorOutput.NeutralMode.ordinal + + override fun setActive(index: Int) { + val neutral = values[index] + talonFxService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt new file mode 100644 index 0000000..a223c32 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.NeutralModeValue +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6SelectNeutralOutputCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(NeutralModeValue.Brake, NeutralModeValue.Coast), + listOf("Brake", "Coast")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeNeutralOut.ordinal + + override fun setActive(index: Int) { + val neutral = values[index] + talonFxService.activeNeutralOut = neutral + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt new file mode 100644 index 0000000..a461896 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt @@ -0,0 +1,25 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +private val SLOTS = listOf(0,1,2) + +class P6SelectSlotCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, SLOTS, SLOTS.map(Int::toString)) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeSlotIndex + + override fun setActive(index: Int) { + talonFxService.activeSlotIndex = index + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt new file mode 100644 index 0000000..324236f --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt @@ -0,0 +1,45 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.hardware.TalonFX +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.info +import org.strykeforce.thirdcoast.readIntList +import org.strykeforce.thirdcoast.warn + +class P6SelectTalonsCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override val menu: String + get() = formatMenu(talonFxService.active.map(TalonFX::getDeviceID).joinToString()) + + override fun execute(): Command { + while(true){ + try { + var ids: List + var new: Set + + ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) + new = talonFxService.activate(ids) + + if(new.isNotEmpty()) { + terminal.info("Activating talons: ${new.joinToString()}") + } + return super.execute() + } catch(e: IllegalArgumentException) { + terminal.warn("Please enter a list of Talon FX ids separated by ','") + } + } + + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt new file mode 100644 index 0000000..0966f86 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.Units + +class P6SelectUnitCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, + listOf(Units.PERCENT, Units.VOLTAGE, Units.TORQUE_CURRENT), + listOf("Percent", "Voltage", "(Pro) TorqueCurrent")) { + + private val talonFxService: TalonFxService by inject() + + override val activeIndex: Int + get() = talonFxService.activeUnits.ordinal + + override fun setActive(index: Int) { + val units = values[index] + talonFxService.activeUnits = units + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt new file mode 100644 index 0000000..fe71960 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6TalonStatusCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override fun execute(): Command { + val writer = terminal.writer() + talonFxService.active.forEach { + var config = TalonFXConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + return super.execute() + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt new file mode 100644 index 0000000..6292edb --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt @@ -0,0 +1,98 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractParameter +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.parseResource + +class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): AbstractParameter(command, toml) { + + enum class P6Enum { + ROTOR_OFFSET, + SENSOR_TO_MECH_RATIO, + ROTOR_TO_SENSOR_RATIO, + REMOTE_SENSOR_ID, + + SLOT_KP, + SLOT_KI, + SLOT_KD, + SLOT_KS, + SLOT_KV, + SLOT_KA, + SLOT_KG, + + MM_ACCEL, + MM_CRUISE_VEL, + MM_JERK, + + PEAK_DIFF_DC, + PEAK_DIFF_TORQUE, + PEAK_DIFF_VOLT, + DIFF_SENSOR_REMOTE_ID, + DIFF_FX_ID, + + STATOR_LIM_EN, + STATOR_LIM, + SUPP_LIM_EN, + SUPP_LIM, + SUPP_TRIP_THRES, + SUPP_TRIP_TIME, + + CLOSED_LOOP_RAMP_DC, + PEAK_FWD_DC, + PEAK_REV_DC, + NEUTRAL_DEADBAND_DC, + OPEN_LOOP_RAMP_DC, + PEAK_FWD_V, + PEAK_REV_V, + SUPPLY_V_TIME_CONST, + OPEN_LOOP_RAMP_V, + CLOSED_LOOP_RAMP_V, + PEAK_FWD_I, + PEAK_REV_I, + TORQUE_NEUTRAL_DEADBAND, + OPEN_LOOP_RAMP_I, + CLOSED_LOOP_RAMP_I, + CONTINUOUS_WRAP, + + FWD_SOFT_EN, + FWD_SOFT_THRES, + REV_SOFT_EN, + REV_SOFT_THRES, + + FWD_HARD_EN, + FWD_REMOTE_ID, + FWD_AUTOSET_POS, + FWD_AUTOSET_POS_VALUE, + REV_HARD_EN, + REV_REMOTE_ID, + REV_AUTOSET_POS, + REV_AUTOSET_POS_VALUE, + + ALLOW_MUSIC_DIS, + BEEP_ON_BOOT, + BEEP_ON_CONFIG, + + VELOCITY, + ACCELERATION, + JERK, + TORQUE_CURRENT_DEADBAND, + TORQUE_CURRENT_MAX, + FEED_FORWARD, + OPPOSE_MAIN, + DIFFERENTIAL_SLOT, + DIFFERENTIAL_TARGET, + FOC, + OVERRIDE_NEUTRAL + + } + + companion object { + private val tomlTable by lazy { parseResource("/phoenix6.toml") } + + fun create(command: Command, param: String): Phoenix6Parameter { + val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param") + return Phoenix6Parameter(command, toml, P6Enum.valueOf(param)) + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt new file mode 100644 index 0000000..6475ed0 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt @@ -0,0 +1,614 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.configs.SlotConfigs +import com.ctre.phoenix6.hardware.TalonFX +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.talon.phoenix6.Phoenix6Parameter.P6Enum.* +import org.strykeforce.thirdcoast.warn + +private val logger = KotlinLogging.logger { } + +class Phoenix6ParameterCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService : TalonFxService by inject() + val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + private val timeout = talonFxService.timeout + private val param = Phoenix6Parameter.create(this, toml.getString("param") ?: "UNKNOWN") + + override val menu: String + get() { + val config = talonFxService.activeConfiguration + + return when(param.enum) { + ROTOR_OFFSET -> formatMenu(config.Feedback.FeedbackRotorOffset) + SENSOR_TO_MECH_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio) + ROTOR_TO_SENSOR_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio) + REMOTE_SENSOR_ID -> formatMenu(config.Feedback.FeedbackRemoteSensorID) + + SLOT_KP -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kP + 1 -> talonFxService.activeConfiguration.Slot1.kP + 2 -> talonFxService.activeConfiguration.Slot2.kP + else -> SlotConfigs().kP + } + ) + SLOT_KI -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kI + 1 -> talonFxService.activeConfiguration.Slot1.kI + 2 -> talonFxService.activeConfiguration.Slot2.kI + else -> SlotConfigs().kI + } + ) + SLOT_KD -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kD + 1 -> talonFxService.activeConfiguration.Slot1.kD + 2 -> talonFxService.activeConfiguration.Slot2.kD + else -> SlotConfigs().kD + } + ) + SLOT_KS -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kS + 1 -> talonFxService.activeConfiguration.Slot1.kS + 2 -> talonFxService.activeConfiguration.Slot2.kS + else -> SlotConfigs().kS + } + ) + SLOT_KV -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kV + 1 -> talonFxService.activeConfiguration.Slot1.kV + 2 -> talonFxService.activeConfiguration.Slot2.kV + else -> SlotConfigs().kV + } + ) + SLOT_KG -> formatMenu( + when(talonFxService.activeSlotIndex) { + 0 -> talonFxService.activeConfiguration.Slot0.kG + 1 -> talonFxService.activeConfiguration.Slot1.kG + 2 -> talonFxService.activeConfiguration.Slot2.kG + else -> SlotConfigs().kG + } + ) + + MM_CRUISE_VEL -> formatMenu(config.MotionMagic.MotionMagicCruiseVelocity) + MM_ACCEL -> formatMenu(config.MotionMagic.MotionMagicAcceleration) + MM_JERK -> formatMenu(config.MotionMagic.MotionMagicJerk) + + PEAK_DIFF_DC -> formatMenu(config.DifferentialConstants.PeakDifferentialDutyCycle) + PEAK_DIFF_VOLT -> formatMenu(config.DifferentialConstants.PeakDifferentialVoltage) + PEAK_DIFF_TORQUE -> formatMenu(config.DifferentialConstants.PeakDifferentialTorqueCurrent) + DIFF_SENSOR_REMOTE_ID -> formatMenu(config.DifferentialSensors.DifferentialRemoteSensorID) + DIFF_FX_ID -> formatMenu(config.DifferentialSensors.DifferentialTalonFXSensorID) + + STATOR_LIM_EN -> formatMenu(config.CurrentLimits.StatorCurrentLimitEnable) + STATOR_LIM -> formatMenu(config.CurrentLimits.StatorCurrentLimit) + SUPP_LIM_EN -> formatMenu(config.CurrentLimits.SupplyCurrentLimitEnable) + SUPP_LIM -> formatMenu(config.CurrentLimits.SupplyCurrentLimit) + SUPP_TRIP_THRES -> formatMenu(config.CurrentLimits.SupplyCurrentThreshold) + SUPP_TRIP_TIME -> formatMenu(config.CurrentLimits.SupplyTimeThreshold) + + CLOSED_LOOP_RAMP_DC -> formatMenu(config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) + PEAK_FWD_DC -> formatMenu(config.MotorOutput.PeakForwardDutyCycle) + PEAK_REV_DC -> formatMenu(config.MotorOutput.PeakReverseDutyCycle) + NEUTRAL_DEADBAND_DC -> formatMenu(config.MotorOutput.DutyCycleNeutralDeadband) + OPEN_LOOP_RAMP_DC -> formatMenu(config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) + PEAK_FWD_V -> formatMenu(config.Voltage.PeakForwardVoltage) + PEAK_REV_V -> formatMenu(config.Voltage.PeakReverseVoltage) + SUPPLY_V_TIME_CONST -> formatMenu(config.Voltage.SupplyVoltageTimeConstant) + OPEN_LOOP_RAMP_V -> formatMenu(config.OpenLoopRamps.VoltageOpenLoopRampPeriod) + CLOSED_LOOP_RAMP_V -> formatMenu(config.ClosedLoopRamps.VoltageClosedLoopRampPeriod) + PEAK_FWD_I -> formatMenu(config.TorqueCurrent.PeakForwardTorqueCurrent) + PEAK_REV_I -> formatMenu(config.TorqueCurrent.PeakReverseTorqueCurrent) + TORQUE_NEUTRAL_DEADBAND -> formatMenu(config.TorqueCurrent.TorqueNeutralDeadband) + OPEN_LOOP_RAMP_I -> formatMenu(config.OpenLoopRamps.TorqueOpenLoopRampPeriod) + CLOSED_LOOP_RAMP_I -> formatMenu(config.ClosedLoopRamps.TorqueClosedLoopRampPeriod) + + FWD_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitEnable) + FWD_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) + REV_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ReverseSoftLimitEnable) + REV_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) + + FWD_HARD_EN -> formatMenu(config.HardwareLimitSwitch.ForwardLimitEnable) + FWD_REMOTE_ID -> formatMenu(config.HardwareLimitSwitch.ForwardLimitRemoteSensorID) + FWD_AUTOSET_POS -> formatMenu(config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) + FWD_AUTOSET_POS_VALUE -> formatMenu(config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) + REV_HARD_EN -> formatMenu(config.HardwareLimitSwitch.ReverseLimitEnable) + REV_REMOTE_ID -> formatMenu(config.HardwareLimitSwitch.ReverseLimitRemoteSensorID) + REV_AUTOSET_POS -> formatMenu(config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) + REV_AUTOSET_POS_VALUE -> formatMenu(config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) + + ALLOW_MUSIC_DIS -> formatMenu(config.Audio.AllowMusicDurDisable) + BEEP_ON_BOOT -> formatMenu(config.Audio.BeepOnBoot) + BEEP_ON_CONFIG -> formatMenu(config.Audio.BeepOnConfig) + + else -> TODO("${param.enum} not implemented") + } + } + + override fun execute(): Command { + val config = talonFxService.activeConfiguration + val activeSlotIndex = talonFxService.activeSlotIndex + val timeout = talonFxService.timeout + + when(param.enum) { + ROTOR_OFFSET -> configDoubleParam(config.Feedback.FeedbackRotorOffset) { + talonFx, value -> + config.Feedback.FeedbackRotorOffset = value + talonFx.configurator.apply(config.Feedback) + } + SENSOR_TO_MECH_RATIO -> configDoubleParam(config.Feedback.SensorToMechanismRatio) { + talonFx, value -> + config.Feedback.SensorToMechanismRatio = value + talonFx.configurator.apply(config.Feedback) + } + ROTOR_TO_SENSOR_RATIO -> configDoubleParam(config.Feedback.RotorToSensorRatio) { + talonFx, value -> + config.Feedback.RotorToSensorRatio = value + talonFx.configurator.apply(config.Feedback) + } + REMOTE_SENSOR_ID -> configIntParam(config.Feedback.FeedbackRemoteSensorID) { + talonFx, value -> + config.Feedback.FeedbackRemoteSensorID = value + talonFx.configurator.apply(config.Feedback) + } + + SLOT_KP -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kP) { + talonFx, value -> + config.Slot0.kP = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kP) { + talonFx, value -> + config.Slot1.kP = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kP) { + talonFx, value -> + config.Slot2.kP = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KI -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kI) { + talonFx, value -> + config.Slot0.kI = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kI) { + talonFx, value -> + config.Slot1.kI = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot0.kI) { + talonFx, value -> + config.Slot2.kI = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KD -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kD) { + talonFx, value -> + config.Slot0.kD = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kD) { + talonFx, value -> + config.Slot1.kD = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kD) { + talonFx, value -> + config.Slot2.kD = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KS -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kS) { + talonFx, value -> + config.Slot0.kS = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kS) { + talonFx, value -> + config.Slot1.kS = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kS) { + talonFx, value -> + config.Slot2.kS = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KV -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kV) { + talonFx, value -> + config.Slot0.kV = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kV) { + talonFx, value -> + config.Slot1.kV = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kV) { + talonFx, value -> + config.Slot2.kV = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KA -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kA) { + talonFx, value -> + config.Slot0.kA = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kA) { + talonFx, value -> + config.Slot1.kA = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kA) { + talonFx, value -> + config.Slot2.kA = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + SLOT_KG -> { + when(activeSlotIndex) { + 0 -> configDoubleParam(config.Slot0.kG) { + talonFx, value -> + config.Slot0.kG = value + talonFx.configurator.apply(config.Slot0) + } + 1 -> configDoubleParam(config.Slot1.kG) { + talonFx, value -> + config.Slot1.kG = value + talonFx.configurator.apply(config.Slot1) + } + 2 -> configDoubleParam(config.Slot2.kG) { + talonFx, value -> + config.Slot2.kG = value + talonFx.configurator.apply(config.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + + MM_ACCEL -> configDoubleParam(config.MotionMagic.MotionMagicAcceleration) { + talonFx, value -> + config.MotionMagic.MotionMagicAcceleration = value + talonFx.configurator.apply(config.MotionMagic) + } + MM_CRUISE_VEL -> configDoubleParam(config.MotionMagic.MotionMagicCruiseVelocity) { + talonFx, value -> + config.MotionMagic.MotionMagicCruiseVelocity = value + talonFx.configurator.apply(config.MotionMagic) + } + MM_JERK -> configDoubleParam(config.MotionMagic.MotionMagicJerk) { + talonFx, value -> + config.MotionMagic.MotionMagicJerk = value + talonFx.configurator.apply(config.MotionMagic) + } + + PEAK_DIFF_DC -> configDoubleParam(config.DifferentialConstants.PeakDifferentialDutyCycle) { + talonFx, value -> + config.DifferentialConstants.PeakDifferentialDutyCycle = value + talonFx.configurator.apply(config.DifferentialConstants) + } + PEAK_DIFF_TORQUE -> configDoubleParam(config.DifferentialConstants.PeakDifferentialTorqueCurrent) { + talonFx, value -> + config.DifferentialConstants.PeakDifferentialTorqueCurrent = value + talonFx.configurator.apply(config.DifferentialConstants) + } + PEAK_DIFF_VOLT -> configDoubleParam(config.DifferentialConstants.PeakDifferentialVoltage) { + talonFx, value -> + config.DifferentialConstants.PeakDifferentialVoltage - value + talonFx.configurator.apply(config.DifferentialConstants) + } + DIFF_SENSOR_REMOTE_ID -> configIntParam(config.DifferentialSensors.DifferentialRemoteSensorID) { + talonFx, value -> + config.DifferentialSensors.DifferentialRemoteSensorID = value + talonFx.configurator.apply(config.DifferentialSensors) + } + DIFF_FX_ID -> configIntParam(config.DifferentialSensors.DifferentialTalonFXSensorID) { + talonFx, value -> + config.DifferentialSensors.DifferentialTalonFXSensorID = value + talonFx.configurator.apply(config.DifferentialSensors) + } + + STATOR_LIM_EN -> configBooleanParam(config.CurrentLimits.StatorCurrentLimitEnable){ + talonFx, value -> + config.CurrentLimits.StatorCurrentLimitEnable = value + talonFx.configurator.apply(config.CurrentLimits) + } + STATOR_LIM -> configDoubleParam(config.CurrentLimits.StatorCurrentLimit) { + talonFx, value -> + config.CurrentLimits.StatorCurrentLimit = value + talonFx.configurator.apply(config.CurrentLimits) + } + SUPP_LIM_EN -> configBooleanParam(config.CurrentLimits.SupplyCurrentLimitEnable) { + talonFx, value -> + config.CurrentLimits.SupplyCurrentLimitEnable = value + talonFx.configurator.apply(config.CurrentLimits) + } + SUPP_LIM -> configDoubleParam(config.CurrentLimits.SupplyCurrentLimit) { + talonFx, value -> + config.CurrentLimits.SupplyCurrentLimit = value + talonFx.configurator.apply(config.CurrentLimits) + } + SUPP_TRIP_THRES -> configDoubleParam(config.CurrentLimits.SupplyCurrentThreshold) { + talonFx, value -> + config.CurrentLimits.SupplyCurrentThreshold = value + talonFx.configurator.apply(config.CurrentLimits) + } + SUPP_TRIP_TIME -> configDoubleParam(config.CurrentLimits.SupplyTimeThreshold) { + talonFx, value -> + config.CurrentLimits.SupplyTimeThreshold + talonFx.configurator.apply(config.CurrentLimits) + } + + CLOSED_LOOP_RAMP_DC -> configDoubleParam(config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) { + talonFx, value -> + config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = value + talonFx.configurator.apply(config.ClosedLoopRamps) + } + PEAK_FWD_DC -> configDoubleParam(config.MotorOutput.PeakForwardDutyCycle) { + talonFx, value -> + config.MotorOutput.PeakForwardDutyCycle = value + talonFx.configurator.apply(config.MotorOutput) + } + PEAK_REV_DC -> configDoubleParam(config.MotorOutput.PeakReverseDutyCycle) { + talonFx, value -> + config.MotorOutput.PeakReverseDutyCycle = value + talonFx.configurator.apply(config.MotorOutput) + } + NEUTRAL_DEADBAND_DC -> configDoubleParam(config.MotorOutput.DutyCycleNeutralDeadband) { + talonFx, value -> + config.MotorOutput.DutyCycleNeutralDeadband = value + talonFx.configurator.apply(config.MotorOutput) + } + OPEN_LOOP_RAMP_DC -> configDoubleParam(config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) { + talonFx, value -> + config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = value + talonFx.configurator.apply(config.OpenLoopRamps) + } + PEAK_FWD_V -> configDoubleParam(config.Voltage.PeakForwardVoltage) { + talonFx, value -> + config.Voltage.PeakForwardVoltage = value + talonFx.configurator.apply(config.Voltage) + } + PEAK_REV_V -> configDoubleParam(config.Voltage.PeakReverseVoltage) { + talonFx, value -> + config.Voltage.PeakReverseVoltage = value + talonFx.configurator.apply(config.Voltage) + } + SUPPLY_V_TIME_CONST -> configDoubleParam(config.Voltage.SupplyVoltageTimeConstant) { + talonFx, value -> + config.Voltage.SupplyVoltageTimeConstant = value + talonFx.configurator.apply(config.Voltage) + } + OPEN_LOOP_RAMP_V -> configDoubleParam(config.OpenLoopRamps.VoltageOpenLoopRampPeriod) { + talonFx, value -> + config.OpenLoopRamps.VoltageOpenLoopRampPeriod = value + talonFx.configurator.apply(config.OpenLoopRamps) + } + CLOSED_LOOP_RAMP_V -> configDoubleParam(config.ClosedLoopRamps.VoltageClosedLoopRampPeriod) { + talonFx, value -> + config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = value + talonFx.configurator.apply(config.ClosedLoopRamps) + } + PEAK_FWD_I -> configDoubleParam(config.TorqueCurrent.PeakForwardTorqueCurrent) { + talonFx, value -> + config.TorqueCurrent.PeakForwardTorqueCurrent = value + talonFx.configurator.apply(config.TorqueCurrent) + } + PEAK_REV_I -> configDoubleParam(config.TorqueCurrent.PeakReverseTorqueCurrent) { + talonFx, value -> + config.TorqueCurrent.PeakReverseTorqueCurrent = value + talonFx.configurator.apply(config.TorqueCurrent) + } + TORQUE_NEUTRAL_DEADBAND -> configDoubleParam(config.TorqueCurrent.TorqueNeutralDeadband) { + talonFx, value -> + config.TorqueCurrent.TorqueNeutralDeadband = value + talonFx.configurator.apply(config.TorqueCurrent) + } + OPEN_LOOP_RAMP_I -> configDoubleParam(config.OpenLoopRamps.TorqueOpenLoopRampPeriod) { + talonFx, value -> + config.OpenLoopRamps.TorqueOpenLoopRampPeriod = value + talonFx.configurator.apply(config.OpenLoopRamps) + } + CLOSED_LOOP_RAMP_I -> configDoubleParam(config.ClosedLoopRamps.TorqueClosedLoopRampPeriod) { + talonFx, value -> + config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = value + talonFx.configurator.apply(config.ClosedLoopRamps) + } + CONTINUOUS_WRAP -> configBooleanParam(config.ClosedLoopGeneral.ContinuousWrap){ + talonFx, value -> + config.ClosedLoopGeneral.ContinuousWrap = value + talonFx.configurator.apply(config.ClosedLoopGeneral) + } + + FWD_SOFT_EN -> configBooleanParam(config.SoftwareLimitSwitch.ForwardSoftLimitEnable) { + talonFx, value -> + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = value + talonFx.configurator.apply(config.SoftwareLimitSwitch) + } + FWD_SOFT_THRES -> configDoubleParam(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) { + talonFx, value -> + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = value + talonFx.configurator.apply(config.SoftwareLimitSwitch) + } + REV_SOFT_EN -> configBooleanParam(config.SoftwareLimitSwitch.ReverseSoftLimitEnable) { + talonFx, value -> + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = value + talonFx.configurator.apply(config.SoftwareLimitSwitch) + } + REV_SOFT_THRES -> configDoubleParam(config.SoftwareLimitSwitch.ReverseSoftLimitThreshold) { + talonFx, value -> + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = value + talonFx.configurator.apply(config.SoftwareLimitSwitch) + } + + FWD_HARD_EN -> configBooleanParam(config.HardwareLimitSwitch.ForwardLimitEnable) { + talonFx, value -> + config.HardwareLimitSwitch.ForwardLimitEnable = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + FWD_REMOTE_ID -> configIntParam(config.HardwareLimitSwitch.ForwardLimitRemoteSensorID) { + talonFx, value -> + config.HardwareLimitSwitch.ForwardLimitRemoteSensorID = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + FWD_AUTOSET_POS -> configBooleanParam(config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) { + talonFx, value -> + config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + FWD_AUTOSET_POS_VALUE -> configDoubleParam(config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) { + talonFx, value -> + config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + REV_HARD_EN -> configBooleanParam(config.HardwareLimitSwitch.ReverseLimitEnable) { + talonFx, value -> + config.HardwareLimitSwitch.ReverseLimitEnable = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + REV_REMOTE_ID -> configIntParam(config.HardwareLimitSwitch.ReverseLimitRemoteSensorID) { + talonFx, value -> + config.HardwareLimitSwitch.ReverseLimitRemoteSensorID = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + REV_AUTOSET_POS -> configBooleanParam(config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) { + talonFx, value -> + config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + REV_AUTOSET_POS_VALUE -> configDoubleParam(config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) { + talonFx, value -> + config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = value + talonFx.configurator.apply(config.HardwareLimitSwitch) + } + + ALLOW_MUSIC_DIS -> configBooleanParam(config.Audio.AllowMusicDurDisable) { + talonFx, value -> + config.Audio.AllowMusicDurDisable = value + talonFx.configurator.apply(config.Audio) + } + BEEP_ON_BOOT -> configBooleanParam(config.Audio.BeepOnBoot) { + talonFx, value -> + config.Audio.BeepOnBoot = value + talonFx.configurator.apply(config.Audio) + } + BEEP_ON_CONFIG -> configBooleanParam(config.Audio.BeepOnConfig) { + talonFx, value -> + config.Audio.BeepOnConfig = value + talonFx.configurator.apply(config.Audio) + } + VELOCITY -> configDoubleParam(talonFxService.activeVelocity) { + talonFx, value -> + talonFxService.activeVelocity = value + } + ACCELERATION -> configDoubleParam(talonFxService.activeAcceleration){ + talonFx, value -> + talonFxService.activeAcceleration = value + } + JERK -> configDoubleParam(talonFxService.activeJerk){ + talonFx, value -> + talonFxService.activeJerk = value + } + TORQUE_CURRENT_DEADBAND -> configDoubleParam(talonFxService.activeTorqueCurrentDeadband){ + talonFx, value -> + talonFxService.activeTorqueCurrentDeadband = value + } + TORQUE_CURRENT_MAX -> configDoubleParam(talonFxService.activeTorqueCurrentMaxOut){ + talonFx, value -> + talonFxService.activeTorqueCurrentMaxOut = value + } + FEED_FORWARD -> configDoubleParam(talonFxService.activeFeedForward){ + talonFx, value -> + talonFxService.activeFeedForward = value + } + OPPOSE_MAIN -> configBooleanParam(talonFxService.activeOpposeMain){ + talonFx, value -> + talonFxService.activeOpposeMain = value + } + DIFFERENTIAL_SLOT -> configIntParam(talonFxService.activeDifferentialSlot){ + talonFx, value -> + if(value >= 0 && value <= 2){ + talonFxService.activeDifferentialSlot = value + } else { + terminal.warn("${value} is not a valid slot index, must be 0-2") + } + } + DIFFERENTIAL_TARGET -> configDoubleParam(talonFxService.activeDifferentialTarget){ + talonFx, value -> + talonFxService.activeDifferentialTarget = value + } + FOC -> configBooleanParam(talonFxService.activeFOC){ + talonFx, value -> + talonFxService.activeFOC = value + } + OVERRIDE_NEUTRAL -> configBooleanParam(talonFxService.activeOverrideNeutral){ + talonFx, value -> + talonFxService.activeOverrideNeutral = value + } + else -> TODO("${param.name} not implemented") + + } + return super.execute() + } + + private fun configDoubleParam(default: Double, configure: (TalonFX, Double) -> Unit) { + val paramValue = param.readDouble(reader, default) + + talonFxService.active.forEach { configure(it, paramValue) } + logger.debug { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } + + private fun configIntParam(default: Int, configure: (TalonFX, Int) -> Unit) { + val paramValue = param.readInt(reader, default) + talonFxService.active.forEach { configure(it, paramValue) } + logger.debug { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } + + private fun configBooleanParam(default: Boolean, configure: (TalonFX, Boolean) -> Unit) { + val paramValue = param.readBoolean(reader, default) + talonFxService.active.forEach { configure(it, paramValue) } + logger.debug { "Set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } + + +} \ No newline at end of file diff --git a/src/main/resources/commands.toml b/src/main/resources/commands.toml index 267023d..5e1c908 100644 --- a/src/main/resources/commands.toml +++ b/src/main/resources/commands.toml @@ -465,7 +465,7 @@ type = "menu" [talonFx] type = "menu" order = 15 - menu = "work with talonFXs" + menu = "work with legacy talonFXs" [talonFx.run] type = "talon.run" order = 5 @@ -940,6 +940,516 @@ type = "menu" param = "FACTORY_DEFAULTS" device = "fx" +[FxP6] + type = "menu" + order = 17 + menu = "work with talonFX's" + [FxP6.run] + type = "p6.run" + order = 10 + menu = "run active talon fx's" + [FxP6.select] + type = "p6.select" + order = 20 + menu = "active fx's" + [FxP6.status] + type = "p6.status" + order = 30 + menu = "status of active talonFx's" + [FxP6.mode] + type = "menu" + order = 40 + menu = "config active mode" + [FXP6.mode.status] + type = "p6.modeStatus" + order = 10 + menu = "active mode" + [FxP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + [FxP6.mode.openLoop] + type = "menu" + order = 20 + menu = "open loop modes" + [FxP6.mode.openLoop.deadband] + type = "p6.param" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" + [FxP6.mode.position] + type = "menu" + order = 30 + menu = "position modes" + [FxP6.mode.position.velocity] + type = "p6.param" + order = 20 + menu = "velocity target" + param = "VELOCITY" + [FxP6.mode.position.feedFwd] + type = "p6.param" + order = 30 + menu = "velocity feed forward" + param = "FEED_FORWARD" + [FxP6.mode.velocity] + type = "menu" + order = 40 + menu = "velocity modes" + [FxP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + [FxP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + [FxP6.mode.motion] + type = "menu" + order = 50 + menu = "motion magic modes" + [FxP6.mode.motion.motionType] + type = "p6.mmType" + order = 10 + menu = "select motion magic type" + [FxP6.mode.motion.feedFwd] + type = "p6.param" + order = 20 + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" + [FxP6.mode.motion.velocity] + type = "p6.param" + order = 30 + menu = "select velocity target (rot/s)" + param = "VELOCITY" + [FxP6.mode.motion.acceleration] + type = "p6.param" + order = 40 + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" + [FxP6.mode.motion.jerk] + type = "p6.param" + order = 50 + menu = "select jerk target (rot/s^3)" + param = "JERK" + [FxP6.mode.diff] + type = "menu" + order = 60 + menu = "differential modes" + [FxP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" + [FxP6.mode.diff.slot] + type = "p6.param" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" + [Fx.P6.mode.diff.target] + type = "p6.param" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" + [FxP6.mode.follow] + type = "menu" + order = 70 + menu = "follower modes" + [FxP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" + [FxP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + [FxP6.mode.neutral] + type = "menu" + order = 80 + memnu = "neutral modes" + [FxP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + [FxP6.mode.foc] + type = "p6.param" + order = 100 + menu = "enable FOC" + param = "FOC" + [FxP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + [FxP6.units] + type = "p6.units" + order = 50 + menu = "select active units" + [FxP6.feedback] + type = "menu" + order = 60 + menu = "setup feedback configs" + [FxP6.feedback.offset] + type = "p6.param" + order = 10 + menu = "set rotor offset" + param = "ROTOR_OFFSET" + [FxP6.feedback.sense2mech] + type = "p6.param" + order = 20 + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + [FxP6.feedback.rotor2sense] + type = "p6.param" + order = 30 + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + [FxP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + [FxP6.slot] + type = "menu" + order = 70 + menu = "setup slot configs" + [FxP6.slot.active] + type = "p6.slot" + order = 10 + menu = "active slot" + [FxP6.slot.kP] + type = "p6.param" + order = 20 + menu = "kP" + param = "SLOT_KP" + [FxP6.slot.kI] + type = "p6.param" + order = 30 + menu = "kI" + param = "SLOT_KI" + [FxP6.slot.kD] + type = "p6.param" + order = 40 + menu = "kD" + param = "SLOT_KD" + [FxP6.slot.kS] + type = "p6.param" + order = 50 + menu = "kS" + param = "SLOT_KS" + [FxP6.slot.kV] + type = "p6.param" + order = 60 + menu = "kV" + param = "SLOT_KV" + [FxP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + [FxP6.slot.kG] + type = "p6.param" + order = 70 + menu = "kG" + param = "SLOT_KG" + [FxP6.slot.gravity] + type = "p6.gravity" + order = 80 + menu = "gravity type" + [FxP6.motion] + type = "menu" + order = 80 + menu = "setup motion magic configs" + [FxP6.motion.accel] + type = "p6.param" + order = 10 + menu = "acceleration" + param = "MM_ACCEL" + [FxP6.motion.cruise] + type = "p6.param" + order = 20 + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + [FxP6.motion.jerk] + type = "p6.param" + order = 30 + menu = "jerk" + param = "MM_JERK" + [FxP6.diff] + type = "menu" + order = 90 + menu = "setup differential configs" + [FxP6.diff.duty] + type = "p6.param" + order = 10 + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + [FxP6.diff.torque] + type = "p6.param" + order = 20 + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + [FxP6.diff.volt] + type = "p6.param" + order = 30 + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + [FxP6.diff.remote] + type = "p6.param" + order = 40 + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + [FxP6.diff.fx] + type = "p6.param" + order = 50 + menu = "differential fx id" + param = "DIFF_FX_ID" + [FxP6.current] + type = "menu" + order = 100 + menu = "setup current limit configs" + [FxP6.current.statorEn] + type = "p6.param" + order = 10 + menu = "stator limit enable" + param = "STATOR_LIM_EN" + [FxP6.current.statorLim] + type = "p6.param" + order = 20 + menu = "stator current limit" + param = "STATOR_LIM" + [FxP6.current.suppEn] + type = "p6.param" + order = 30 + menu = "supply limit enable" + param = "SUPP_LIM_EN" + [FxP6.current.suppLim] + type = "p6.param" + order = 40 + menu = "supply current limit" + param = "SUPP_LIM" + [FxP6.current.tripI] + type = "p6.param" + order = 50 + menu = "supply current trip threshold (A)" + param = "SUPP_TRIP_THRES" + [FxP6.current.tripT] + type = "p6.param" + order = 60 + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" + [FxP6.motor] + type = "menu" + order = 110 + menu = "setup motor output and ramp configs" + [FxP6.motor.invert] + type = "p6.invert" + order = 10 + menu = "motor inversion" + [FxP6.motor.neutral] + type = "p6.neutral" + order = 20 + menu = "neutral mode" + [FxP6.motor.duty] + type = "menu" + order = 30 + menu = "duty cycle configs" + [FxP6.motor.duty.peakFwd] + type= "p6.param" + order = 20 + menu = "peak forward %" + param = "PEAK_FWD_DC" + [FxP6.motor.duty.peakRev] + type = "p6.param" + order = 30 + menu = "peak reverse %" + param = "PEAK_REV_DC" + [FxP6.motor.duty.dead] + type = "p6.param" + order = 40 + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" + [FxP6.motor.duty.openRamp] + type = "p6.param" + order = 50 + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" + [FxP6.motor.duty.closedRamp] + type = "p6.param" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" + [FxP6.motor.volt] + type = "menu" + order = 40 + menu = "voltage configs" + [FxP6.motor.volt.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (V)" + param = "PEAK_FWD_V" + [FxP6.motor.volt.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (V)" + param = "PEAK_REV_V" + [FxP6.motor.volt.supplyConst] + type = "p6.param" + order = 30 + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" + [FxP6.motor.volt.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" + [FxP6.motor.volt.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" + [FxP6.motor.torque] + type = "menu" + order = 50 + menu = "torque current configs" + [FxP6.motor.torque.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (A)" + param = "PEAK_FWD_I" + [FxP6.motor.torque.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (A)" + param = "PEAK_REV_I" + [FxP6.motor.torque.dead] + type = "p6.param" + order = 30 + menu = "neutral deadband (A)" + param = "TORQUE_NEUTRAL_DEADBAND" + [FxP6.motor.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" + [FxP6.motor.closedRamp] + type = "p6.param" + order = 50 + menuu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + [FxP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [FxP6.limits.hard] + type = "menu" + order = 10 + menu = "hard limits" + [FxP6.limits.hard.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_HARD_EN" + [FxP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 10 + menu = "FWD limit normal" + [FxP6.limits.hard.fwdSource] + type = "p6.fwdSource" + order = 20 + menu = "FWD limit source" + [FxP6.limits.hard.fwdRemote] + type = "p6.param" + order = 30 + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" + [FxP6.limits.hard.fwdAutoSet] + type = "p6.param" + order = 40 + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + [FxP6.limits.hard.fwdAutoSetValue] + type = "p6.param" + order = 50 + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + [FxP6.limits.hard.revEn] + type = "p6.param" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" + [FxP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + [FxP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + [FxP6.limits.hard.revRemote] + type = "p6.param" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" + [FxP6.limits.hard.revAutoSet] + type = "p6.param" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" + [FxP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + [FxP6.limits.soft] + type = "menu" + order = 20 + menu = "soft limits" + [FxP6.limits.soft.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_SOFT_EN" + [FxP6.limits.soft.fwdThres] + type = "p6.param" + order = 20 + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" + [FxP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + [FxP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + [FxP6.audio] + type = "menu" + order = 130 + menu = "setup audio configs" + [FxP6.audio.disable] + type = "p6.param" + order = 10 + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" + [FxP6.audio.beepBoot] + type = "p6.param" + order = 20 + menu = "beep on boot" + param = "BEEP_ON_BOOT" + [FxP6.audio.beepConfig] + type = "p6.param" + order = 30 + menu = "beep on config" + param = "BEEP_ON_CONFIG" + [FxP6.factory] + type = "menu" + order = 140 + menu = "config factory default" + [servo] type = "menu" order = 20 diff --git a/src/main/resources/phoenix6.toml b/src/main/resources/phoenix6.toml new file mode 100644 index 0000000..4ee8f3e --- /dev/null +++ b/src/main/resources/phoenix6.toml @@ -0,0 +1,332 @@ +# Feedback Configs +[ROTOR_OFFSET] + name = "Rotor Offset" + type = "DOUBLE" + help = "This offset is applied to the absolute integrated rotor sensor. This can be used to zero the rotor in applications that are within one rotor rotation.Units: rotations" + range = [-1.0, 1.0] +[SENSOR_TO_MECH_RATIO] + name = "Sensor to Mechanism Ratio" + type = "DOUBLE" + help = "This is the ratio of sensor rotations to the mechanism's output. This is equivalent to the mechanism's gear ratio if the sensor is located on the input of a gearbox. If sensor is on the output of a gearbox, then this is typically set to 1. Note if this is set to zero, device will reset back to one." + range = [-1000.0, 1000.0] +[ROTOR_TO_SENSOR_RATIO] + name = "Rotor to Sensor Ratio" + type = "DOUBLE" + help = "Talon FX is capable of fusing a remote CANcoder with its rotor sensor to produce a high-bandwidth sensor source. This feature requires specifying the ratio between the remote sensor and the motor rotor. Note if this is set to zero, device will reset back to one." + range = [-1000.0, 1000.0] +[REMOTE_SENSOR_ID] + name = "Remote Sensor ID" + type = "INTEGER" + help = "Device ID of which remote device to use. This is not used if the Sensor Source is the internal rotor sensor." + range = [0,62] + +#Slot Configs +[SLOT_KP] + name = "Proportional Gain" + type = "DOUBLE" + help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by error in the input, the units should be defined as units of output per unit of input error. For example, when controlling velocity using a duty cycle closed loop, the units for the proportional gain will be duty cycle per rps of error, or 1/rps." + range = [0, 3.4e38] +[SLOT_KI] + name = "Integral Gain" + type = "DOUBLE" + help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by error in the input integrated over time (in units of seconds), the units should be defined as units of output per unit of integrated input error. For example, when controlling velocity using a duty cycle closed loop, integrating velocity over time results in rps * s = rotations. Therefore, the units for the integral gain will be duty cycle per rotation of accumulated error, or 1/rot." + range = [0, 3.4e38] +[SLOT_KD] + name = "Derivitave Gain" + type = "DOUBLE" + help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the derivative of error in the input with respect to time (in units of seconds), the units should be defined as units of output per unit of the differentiated input error. For example, when controlling velocity using a duty cycle closed loop, the derivative of velocity with respect to time is rps/s, which is acceleration. Therefore, the units for the derivative gain will be duty cycle per unit of acceleration error, or 1/(rps/s)." + range = [0, 3.4e38] +[SLOT_KS] + name = "Static FWD Gain" + type = "DOUBLE" + help = "This is added to the closed loop output. The sign is determined by target velocity. The unit for this constant is dependent on the control mode, typically fractional duty cycle, voltage, or torque current." + range = [-512, 511] +[SLOT_KV] + name = "Velocity Feedforward Gain" + type = "DOUBLE" + help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the requested velocity, the units should be defined as units of output per unit of requested input velocity. For example, when controlling velocity using a duty cycle closed loop, the units for the velocity feedfoward gain will be duty cycle per requested rps, or 1/rps." + range = [0, 3.4e38] +[SLOT_KA] + name = "Acceleration Feedforward Gain" + type = "DOUBLE" + help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the requested acceleration, the units should be defined as units of output per unit of requested input acceleration. For example, when controlling velocity using a duty cycle closed loop, the units for the acceleration feedfoward gain will be duty cycle per requested rps/s, or 1/(rps/s)." + range = [0, 3.4e38] +[SLOT_KG] + name = "Gravity Feedforward Gain" + type = "DOUBLE" + help = "This is added to the closed loop output. The sign is determined by the type of gravity feedforward. The unit for this constant is dependent on the control mode, typically fractional duty cycle, voltage, or torque current." + range = "-512, 511" + + +#Motion Magic Configs +[MM_CRUISE_VEL] + name = "Motion Magic Cruise Velocity" + type = "DOUBLE" + help = "This is the maximum velocity Motion Magic® based control modes are allowed to use. Units: rps" + range = [0, 9999] +[MM_ACCEL] + name = "Motion Magic Acceleration" + type = "DOUBLE" + help = "This is the target acceleration Motion Magic® based control modes are allowed to use. Units: rps^2" + range = [0, 9999] +[MM_JERK] + name = "Motion Magic Jerk" + type = "DOUBLE" + help = "This is the target jerk (acceleration derivative) Motion Magic® based control modes are allowed to use. This allows Motion Magic® support of S-Curves. If this is set to zero, then Motion Magic® will not apply a Jerk limit. Units: rot/s^3" + range = [0, 9999] + +#Differential Configs +[PEAK_DIFF_DC] + name = "Peak Differntial Duty Cycle" + type = "DOUBLE" + help = "Maximum differential output during duty cycle based differential control modes." + range = [0.0, 2.0] +[PEAK_DIFF_VOLT] + name = "Peak Differential Voltage" + type = "DOUBLE" + help = "Maximum differential output during voltage based differential control modes." + range = [0.0, 32.0] +[PEAK_DIFF_TORQUE] + name = "Peak Differential Torque Current" + type = "DOUBLE" + help = "Maximum differential output during torque current based differential control modes." + range = [0.0, 1600.0] +[DIFF_SENSOR_REMOTE_ID] + name = "Differential Remote Sensor ID" + type = "INTEGER" + help = "Device ID of which remote sensor to use on the differential axis. This is used when the Differential Sensor Source is not RemoteTalonFX_Diff." + range = [0, 62] +[DIFF_FX_ID] + name = "Differential Talon FX ID" + type = "INTEGER" + help = "Device ID of which remote Talon FX to use. This is used when the Differential Sensor Source is not disabled." + range = [0, 62] + +#Current Limit Configs +[STATOR_LIM_EN] + name = "Stator Current Limit Enable" + type = "BOOLEAN" + help = "Enable motor stator current limiting." +[STATOR_LIM] + name = "Stator Current Limit" + type = "DOUBLE" + help = "The amount of current allowed in the motor (motoring and regen current). This is only applicable for non-torque current control modes. Note this requires the corresponding enable to be true. Units: A" + range = [0.0, 800.0] +[SUPP_LIM_EN] + name = "Supply Current Limit Enable" + type = "BOOLEAN" + help = "Enable motor supply current limiting." +[SUPP_LIM_] + name = "Supply Current Limit" + type = "DOUBLE" + help = "The amount of supply current allowed. This is only applicable for non-torque current control modes. Note this requires the corresponding enable to be true. Use SupplyCurrentThreshold and SupplyTimeThreshold to allow brief periods of high-current before limiting occurs. Units: A" + range = [0.0, 800.0] +[SUPP_TRIP_THRES] + name = "Supply Current Trip Threshold" + type = "DOUBLE" + help = "Delay supply current limiting until current exceeds this threshold for longer than SupplyTimeThreshold. This allows current draws above SupplyCurrentLimit for a fixed period of time. This has no effect if SupplyCurrentLimit is greater than this value. Units: A" + range = [0.0, 511.0] +[SUPP_TRIP_TIME] + name = "Supply Current Trip Time" + type = "DOUBLE" + help = "Allows unlimited current for a period of time before current limiting occurs. Current threshold is the maximum of SupplyCurrentThreshold and SupplyCurrentLimit. Units: sec" + range = [0.0, 1.275] + + +#Motor & Ramp Configs +[PEAK_FWD_DC] + name = "Peak Forward Duty Cycle" + type = "DOUBLE" + help = "Maximum (forward) output during duty cycle based control modes." + range = [-1.0, 1.0] +[PEAK_REV_DC] + name = "Peak Reverse Duty Cycle" + type = "DOUBLE" + help = "Minimum (reverse) output during duty cycle based control modes." + range = [-1.0, 1.0] +[NEUTRAL_DEADBAND_DC] + name = "Neutral Deadband: Duty Cycle" + type = "DOUBLE" + help = "Configures the output deadband percentage." + range = [0.0, 0.25] +[OPEN_LOOP_RAMP_DC] + name = "Open Loop Ramp: Duty Cycle" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0% output to 100% during open-loop modes. Units: sec" + range = [0.0, 1.0] +[CLOSED_LOOP_RAMP_DC] + name = "Closed Loop Ramp: Duty Cycle" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0% output to 100% during closed-loop modes. Units: sec" + range = [0.0, 1.0] +[PEAK_FWD_V] + name = "Peak FWD Voltage" + type = "DOUBLE" + help = "Maximum (forward) output during voltage based control modes." + range = [-16.0, 16.0] +[PEAK_REV_V] + name = "Peak REV Voltage" + type = "DOUBLE" + help = "Minimum (reverse) output during voltage based control modes." + range = [-16.0, 16.0] +[SUPPLY_V_TIME_CONST] + name = "Supply Voltage Time Constant" + type = "DOUBLE" + help = "This impacts the filtering for the reported supply voltage, and any control strategies that use the supply voltage (such as voltage control on a motor controller). Units: sec" + range = [0.0, 0.1] +[OPEN_LOOP_RAMP_V] + name = "Open Loop Ramp: Voltage" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0V output to 12V during open-loop modes. Units: sec" + range = [0.0, 1.0] +[CLOSED_LOOP_RAMP_V] + name = "Closed Loop Ramp: Voltage" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0V output to 12V during closed-loop modes. Units: sec" + range = [0.0, 1.0] +[PEAK_FWD_I] + name = "Peak FWD Torque Current" + type = "DOUBLE" + help = "Maximum (forward) output during torque current based control modes." + range = [-800.0, 800.0] +[PEAK_REV_I] + name = "Peak REV Torque Current" + type = "DOUBLE" + help = "Minimum (reverse) output during torque current based control modes." + range = [-800.0, 800.0] +[TORQUE_NEUTRAL_DEADBAND] + name = "Torque Current Neutral Deadband" + type = "DOUBLE" + help = "Configures the output deadband during torque current based control modes." + range = [0.0, 25.0] +[OPEN_LOOP_RAMP_I] + name = "Open Loop Ramp: Torque Current" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0A output to 300A during open-loop modes. Units: sec" + range = [0.0, 10.0] +[CLOSED_LOOP_RAMP_I] + name = "Closed Loop Ramp: Torque Current" + type = "DOUBLE" + help = "If non-zero, this determines how much time to ramp from 0A output to 300A during closed-loop modes. Units: sec" + range = [0.0, 10.0] +[CONTINUOUS_WRAP] + +#Soft Limits +[FWD_SOFT_EN] + name = "Forward Soft Limit Enable" + type = "BOOLEAN" + help = "If enabled, the motor output is set to neutral if position exceeds ForwardSoftLimitThreshold and forward output is requested." +[FWD_SOFT_THRES] + name = "Forward Soft Limit" + type = "DOUBLE" + help = "Position threshold for forward soft limit features. ForwardSoftLimitEnable must be enabled for this to take effect. Units: Rotations" + range = [-3.4e38, 3.4e38] +[REV_SOFT_EN] + name = "Reverse Soft Limit Enable" + type = "BOOLEAN" + help = "If enabled, the motor output is set to neutral if position exceeds ReverseSoftLimitThreshold and reverse output is requested." +[REV_SOFT_THRES] + name = "Reverse Soft Limit" + type = "DOUBLE" + help = "Position threshold for reverse soft limit features. ReverseSoftLimitEnable must be enabled for this to take effect. Units: Rotations" + range = [-3.4e38, 3.4e38] + +#Hard Limits +[FWD_HARD_EN] + name = "Forward Limit Enable" + type = "BOOLEAN" + help = "If enabled, motor output is set to neutral when forward limit switch is asseted and positive output is requested." +[FWD_REMOTE_ID] + name = "FWD Remote sensor ID" + type = "INTEGER" + help = "Device ID of the device if using remote limit switch features." + range = [0, 62] +[FWD_AUTOSET_POS] + name = "FWD limit autoset position enable" + type = "BOOLEAN" + help = "If enabled, the position is auto-set to a specific value, specified by ForwardLimitAutosetPositionValue" +[FWD_AUTOSET_POS_VALUE] + name = "FWD limit autoset position" + type = "DOUBLE" + help = "The value to auto-set the position to. This has no effect if ForwardLimitAutosetPositionEnable is false." + range = [-3.4e38, 3.4e38] +[REV_HARD_EN] + name = "Reverse Limit Enable" + type = "BOOLEAN" + help = "If enabled, motor output is set to neutral when reverse limit switch is asseted and positive output is requested." +[REV_REMOTE_ID] + name = "REV Remote Sensor ID" + type = "INTEGER" + help = "Device ID of the device if using remote limit switch features." + range = [0,62] +[REV_AUTOSET_POS] + name = "REV limit autoset position enable" + type = "BOOLEAN" + help = "If enabled, the position is auto-set to a specific value, specified by ReverseLimitAutosetPositionValue" +[REV_AUTOSET_POS_VALUE] + name = "REV limit autoset position" + type = "DOUBLE" + help = "The value to auto-set the position to. This has no effect if ReverseLimitAutosetPositionEnable is false. Units: Rotations" + range = [-3.4e38, 3.4e38] + +#Music Configs +[ALLOW_MUSIC_DIS] + name = "Allow Music During Disable" + type = "BOOLEAN" + help = "If true, the TalonFX will allow Orchestra and MusicTone requests during disabled state. This can be used to address corner cases when music features are needed when disabled. This setting defaults to false. Note that if the rotor is moving, music features are always disabled regardless of this setting." +[BEEP_ON_BOOT] + name = "Beep on Boot" + type = "BOOLEAN" + help = "If true, the TalonFX will beep during boot-up. This is useful for general debugging, and defaults to true. If rotor is moving during boot-up, the beep will not occur regardless of this setting." +[BEEP_ON_CONFIG] + name = "Beep on Config" + type = "BOOLEAN" + help = "If true, the TalonFX will beep during configuration API calls if device is disabled. This is useful for general debugging, and defaults to true. Note that if the rotor is moving, the beep will not occur regardless of this setting." + +#Saved Setpoints +[VELOCITY] + name = "Velocity" + type = "DOUBLE" + help = "Target velocity to drive toward in rotations per second. This can be changed on-the fly." +[ACCELERATION] + name = "Acceleration" + type = "DOUBLE" + help = "This is the absolute Acceleration to use generating the profile. If this parameter is zero, the Acceleration persistent configuration parameter is used instead. Acceleration is in rotations per second squared. If nonzero, the signage does not matter as the absolute value is used." +[JERK] + name = "Jerk" + type = "DOUBLE" + help = "Jerk for profiling. The signage does not matter as the device will use the absolute value for profile generation." +[TORQUE_CURRENT_DEADBAND] + name = "Torque Current Deadband" + type = "DOUBLE" + help = "Deadband in Amperes. If torque request is within deadband, the bridge output is neutral. If deadband is set to zero then there is effectively no deadband. Note if deadband is zero, a free spinning motor will spin for quite a while as the firmware attempts to hold the motor's bemf. If user expects motor to cease spinning quickly with a demand of zero, we recommend a deadband of one Ampere. This value will be converted to an integral value of amps." +[TORQUE_CURRENT_MAX] + name = "Torque Current MAX absolute duty cycle" + type = "DOUBLE" + help = "The maximum absolute motor output that can be applied, which effectively limits the velocity. For example, 0.50 means no more than 50% output in either direction. This is useful for preventing the motor from spinning to its terminal velocity when there is no external torque applied unto the rotor. Note this is absolute maximum, so the value should be between zero and one." +[FEED_FORWARD] + name = "Feed Forward" + type = "DOUBLE" + help = "Feedforward to apply. Units: Selected Units" +[OPPOSE_MAIN] + name = "Oppose Leader Direction" + type = "BOOLEAN" + help = "Set to false for motor invert to match the leader's configured Invert - which is typical when leader and follower are mechanically linked and spin in the same direction. Set to true for motor invert to oppose the master's configured Invert - this is typical where the the leader and follower mechanically spin in opposite directions." +[DIFFERENTIAL_SLOT] + name = "Differential Slot" + type = "INTEGER" + help = "Select which gains are applied to the differential controller by selecting the slot. Use the configuration api to set the gain values for the selected slot before enabling this feature. Slot must be within [0,2]." + range = [0,2] +[DIFFERENTIAL_TARGET] + name = "Differential Position" + type = "DOUBLE" + help = "Differential position to drive toward in rotations." +[FOC] + name = "Enable FOC" + type = "BOOLEAN" + help = "Set to true to use FOC commutation (requires Phoenix Pro), which increases peak power by ~15%. Set to false to use trapezoidal commutation. FOC improves motor performance by leveraging torque (current) control. However, this may be inconvenient for applications that require specifying duty cycle or voltage. CTR-Electronics has developed a hybrid method that combines the performances gains of FOC while still allowing applications to provide duty cycle or voltage demand. This not to be confused with simple sinusoidal control or phase voltage control which lacks the performance gains." +[OVERRIDE_NEUTRAL] + name = "Override Neutral Setting" + type = "BOOLEAN" + help = "Set to true to static-brake (if Voltage/Duty Cycle) or coast (if Torque Current) the rotor when output is zero (or within deadband). Set to false to use the NeutralMode configuration setting (default). This flag exists to provide the fundamental behavior of this control when output is zero, which is to provide 0V to the motor." + diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 250544a..2a29835 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,17 +1,18 @@ { "fileName": "NavX.json", - "name": "KauaiLabs_navX_FRC", - "version": "2023.0.1", + "name": "NavX", + "version": "2024.0.1-beta-4", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", "mavenUrls": [ - "https://dev.studica.com/maven/release/2023/" + "https://dev.studica.com/maven/release/2024/" ], - "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json", + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", "javaDependencies": [ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2023.0.1" + "version": "2024.0.1-beta-4" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2023.0.1", + "version": "2024.0.1-beta-4", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, @@ -28,6 +29,10 @@ "binaryPlatforms": [ "linuxathena", "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", "windowsx86-64" ] } diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88ca81d --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.32.0-beta-1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-beta-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.32.0-beta-1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.32.0-beta-1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.32.0-beta-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.32.0-beta-1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.32.0-beta-1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.32.0-beta-1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.32.0-beta-1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.32.0-beta-1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.32.0-beta-1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.32.0-beta-1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/PhoenixProAnd5.json b/vendordeps/Phoenix6.json similarity index 57% rename from vendordeps/PhoenixProAnd5.json rename to vendordeps/Phoenix6.json index d4b63d1..c1d8140 100644 --- a/vendordeps/PhoenixProAnd5.json +++ b/vendordeps/Phoenix6.json @@ -1,61 +1,32 @@ { - "fileName": "PhoenixProAnd5.json", - "name": "CTRE-Phoenix (Pro And v5)", - "version": "23.0.1", - "frcYear": 2023, - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-2", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.2" - }, + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.2" - }, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "23.0.1" + "version": "24.0.0-beta-2" } ], "jniDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -66,9 +37,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -79,9 +50,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -92,9 +63,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -105,9 +76,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,9 +89,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -131,9 +102,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -144,9 +115,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -157,9 +128,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -170,9 +141,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.1", + "version": "24.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -185,40 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCI", + "version": "24.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -230,9 +171,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -245,40 +186,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCISim", + "version": "24.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -290,9 +201,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -305,9 +216,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,9 +231,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -335,9 +246,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -350,9 +261,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -365,9 +276,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -380,9 +291,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -395,9 +306,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -410,9 +321,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.1", + "version": "24.0.0-beta-2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -423,36 +334,6 @@ "osxuniversal" ], "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "wpiapi-cpp", - "version": "23.0.1", - "libName": "CTRE_PhoenixPro_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "23.0.1", - "libName": "CTRE_PhoenixPro_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index b9c0fb7..7cd2c56 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,8 +1,9 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "23.0.0", + "version": "23.3.1-SNAPSHOT", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", + "frcYear": "2024", "mavenUrls": [ "https://dl9vkdsodilqp.cloudfront.net/repo" ], @@ -11,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "23.0.0" + "version": "23.3.1-SNAPSHOT" } ], "jniDependencies": [], From 31acd90980a43f91198a3863ba525dc74d0e5fe0 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 14 Jan 2024 13:16:37 -0500 Subject: [PATCH 2/7] update to kickoff releases for 2024, initial testing complete on roborio --- build.gradle | 2 +- .../kotlin/org/strykeforce/thirdcoast/Koin.kt | 6 +- .../strykeforce/thirdcoast/command/Command.kt | 9 +- .../thirdcoast/device/TalonFxService.kt | 76 +++++++++++-- .../swerve/AdjustAzimuthCommands.kt | 10 +- .../thirdcoast/swerve/SaveZeroCommand.kt | 4 +- .../thirdcoast/swerve/SetAzimuthCommand.kt | 10 +- .../thirdcoast/talon/TalonParameterCommand.kt | 1 + .../phoenix6/P6DefaultStatusFrameCommand.kt | 66 +++++++++++ .../talon/phoenix6/P6FactoryDefaultCommand.kt | 26 +++++ .../talon/phoenix6/P6HardLimitCommand.kt | 16 +-- .../thirdcoast/talon/phoenix6/P6RunCommand.kt | 66 ++++++----- .../P6SelectMotionMagicTypeCommand.kt | 4 +- .../phoenix6/P6SelectNeutralModeCommand.kt | 8 +- .../phoenix6/P6SelectNeutralOutputCommand.kt | 4 +- .../talon/phoenix6/Phoenix6Parameter.kt | 8 +- .../phoenix6/Phoenix6ParameterCommand.kt | 103 ++++++++++++++++-- src/main/resources/commands.toml | 31 +++++- src/main/resources/phoenix6.toml | 47 +++++--- vendordeps/NavX.json | 6 +- vendordeps/Phoenix5.json | 26 ++--- vendordeps/Phoenix6.json | 50 ++++----- vendordeps/thirdcoast.json | 4 +- 23 files changed, 436 insertions(+), 147 deletions(-) create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt diff --git a/build.gradle b/build.gradle index f24d1a0..eb72d81 100644 --- a/build.gradle +++ b/build.gradle @@ -6,7 +6,7 @@ plugins { id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" } -version = "23.0.1-SNAPSHOT" +version = "24.0.0" sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index 60e84ac..e58046f 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -73,17 +73,17 @@ val swerveModule = module { private fun getSwerveModules() = Array(4) { i -> getSwerveModule(i) } -private val moduleBuilder = TalonSwerveModule.Builder().driveGearRatio(1.0).wheelDiameterInches(1.0) +private val moduleBuilder = V6TalonSwerveModule.V6Builder().driveGearRatio(1.0).wheelDiameterInches(1.0) .driveMaximumMetersPerSecond(1.0) -private fun getSwerveModule(i: Int) : TalonSwerveModule { +private fun getSwerveModule(i: Int) : V6TalonSwerveModule { val location = when (i) { 0 -> Translation2d(1.0, 1.0) 1 -> Translation2d(1.0, -1.0) 2 -> Translation2d(-1.0, 1.0) else -> Translation2d(-1.0, -1.0) } - return moduleBuilder.azimuthTalon(TalonSRX(i)).driveTalon(TalonFX(i + 10)) + return moduleBuilder.azimuthTalon(TalonSRX(i)).driveTalon(com.ctre.phoenix6.hardware.TalonFX(i + 10)) .wheelLocationMeters(location).build() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt index 3084c0a..4663253 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt @@ -41,8 +41,11 @@ interface Command { const val DEVICE_KEY = "device" fun createFromToml(toml: TomlTable, parent: MenuCommand? = null, key: String = "ROOT"): Command { - logger.info{"key: $key, toml: ${toml.keySet()}"} + //logger.info{"key: $key, toml: ${toml.keySet()}"} val type = toml.getString(TYPE_KEY) ?: throw Exception("$key: $TYPE_KEY missing") + if(type == "p6.param"){ + logger.info { "key: $key, toml: ${toml.keySet()}" } + } logger.info { "type: $type, key: $key" } @@ -88,7 +91,7 @@ interface Command { "p6.status" -> P6TalonStatusCommand(parent, key, toml) "p6.modeStatus" -> P6ModeStatusCommand(parent, key, toml) "p6.mode" -> P6SelectModeCommand(parent, key, toml) - "p6.param" -> Phoenix6ParameterCommand(parent, key, toml) + "p6.param" -> Phoenix6ParameterCommand(parent, key, toml) "p6.mmType" -> P6SelectMotionMagicTypeCommand(parent, key, toml) "p6.diffType" -> P6SelectDifferentialTypeCommand(parent, key, toml) "p6.followType" -> P6SelectFollowerTypeCommand(parent, key, toml) @@ -102,6 +105,8 @@ interface Command { "p6.fwdSource" -> P6SelectFwdHardLimitSourceCommand(parent, key, toml) "p6.revNorm" -> P6SelectRevHardLimitNormalCommand(parent, key, toml) "p6.revSource" -> P6SelectRevHardLimitSourceCommand(parent, key, toml) + "p6.factory" -> P6FactoryDefaultCommand(parent, key, toml) + "p6.graph" -> P6DefaultStatusFrameCommand(parent, key, toml) else -> DefaultCommand(parent, key, toml) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt index 2d5373b..8a9fcdd 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt @@ -24,7 +24,7 @@ enum class Units{ } enum class MM_Type { - STANDARD, VELOCITY, DYNAMIC + STANDARD, VELOCITY, DYNAMIC, EXPONENTIAL } enum class FollowerType { @@ -43,7 +43,7 @@ class TalonFxService( private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ) : AbstractDeviceService(factory) { - val timeout = 10 + val timeout = 10.0 var dirty = true //var neutralMode = NEUTRAL_MODE_DEFAULT //var controlMode = CONTROL_MODE_DEFAULT @@ -67,6 +67,9 @@ class TalonFxService( var differentialType: DifferentialType = DifferentialType.OPEN_LOOP var activeNeutralOut: NeutralModeValue = NeutralModeValue.Coast var activeOverrideNeutral: Boolean = false + var limFwdMotion : Boolean = false; + var limRevMotion : Boolean = false; + var grapherStatusFrameHz : Double = 20.0; var controlMode: String = "Duty Cycle Out" get() { @@ -75,21 +78,21 @@ class TalonFxService( when(activeUnits){ Units.PERCENT -> return "Duty Cycle Out" Units.VOLTAGE -> return "Voltage Out" - Units.TORQUE_CURRENT -> return "Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Torque Current FOC" } } SetpointType.POSITION -> { when(activeUnits){ Units.PERCENT -> return "Position: Duty Cycle" Units.VOLTAGE -> return "Position: Voltage" - Units.TORQUE_CURRENT -> return "Position: Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Position: Torque Current FOC" } } SetpointType.VELOCITY -> { when(activeUnits) { Units.PERCENT -> return "Velocity: Duty Cycle" Units.VOLTAGE -> return "Velocity: Voltage" - Units.TORQUE_CURRENT -> return "Velocity: Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Velocity: Torque Current FOC" } } SetpointType.MOTION_MAGIC -> { @@ -98,19 +101,25 @@ class TalonFxService( when(activeUnits) { Units.PERCENT -> return "Motion Magic: Duty Cycle" Units.VOLTAGE -> return "Motion Magic: Voltage" - Units.TORQUE_CURRENT -> return "Motion Magic: Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic: Torque Current FOC" } } MM_Type.VELOCITY -> { when(activeUnits) { Units.PERCENT -> return "Motion Magic Velocity: Duty Cycle" Units.VOLTAGE -> return "Motion Magic Velocity: Voltage" - Units.TORQUE_CURRENT -> return "Motion Magic Velocity: Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Velocity: Torque Current FOC" } } MM_Type.DYNAMIC -> { when(activeUnits) { Units.PERCENT -> return "Dynamic Motion Magic: Duty Cycle" Units.VOLTAGE -> return "Dynamic Motion Magic: Voltage" - Units.TORQUE_CURRENT -> return "Dynamic Motion Magic: Torque Current FOC" + Units.TORQUE_CURRENT -> return "(pro) Dynamic Motion Magic: Torque Current FOC" + } + } MM_Type.EXPONENTIAL -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Exponential: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Exponential: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Exponential: Torque Current FOC" } } } @@ -187,9 +196,58 @@ class TalonFxService( telemetryService.stop() active.filter { new.contains(it.deviceID) }.forEach{ activeSlotIndex = it.closedLoopSlot.value - telemetryService.register(it,false) + telemetryService.register(it,true) + + //Update Freq Log + /* + logger.info("Accel update: {}", it.acceleration.appliedUpdateFrequency) + logger.info("Bridge Out update: {}", it.bridgeOutput.appliedUpdateFrequency) + logger.info("Device Temp update: {}", it.deviceTemp.appliedUpdateFrequency) + logger.info("Diff Avg Pos update: {}", it.differentialAveragePosition.appliedUpdateFrequency) + logger.info("Diff Avg Vel update: {}", it.differentialAverageVelocity.appliedUpdateFrequency) + logger.info("Diff Diff Pos update: {}", it.differentialDifferencePosition.appliedUpdateFrequency) + logger.info("Diff Diff Vel update: {}", it.differentialDifferenceVelocity.appliedUpdateFrequency) + logger.info("Duty Cycle update: {}", it.dutyCycle.appliedUpdateFrequency) + logger.info("Fwd Lim update: {}", it.forwardLimit.appliedUpdateFrequency) + logger.info("Rev Lim update: {}", it.reverseLimit.appliedUpdateFrequency) + logger.info("Is Pro Lic update: {}", it.isProLicensed.appliedUpdateFrequency) + logger.info("Is MM Running update: {}", it.motionMagicIsRunning.appliedUpdateFrequency) + logger.info("Motor Voltage update: {}", it.motorVoltage.appliedUpdateFrequency) + logger.info("Position update: {}", it.position.appliedUpdateFrequency) + logger.info("Processor Temp update: {}", it.processorTemp.appliedUpdateFrequency) + logger.info("Rotor Pos update: {}", it.rotorPosition.appliedUpdateFrequency) + logger.info("Rotor Vel update: {}", it.rotorVelocity.appliedUpdateFrequency) + logger.info("Stator I update: {}", it.statorCurrent.appliedUpdateFrequency) + logger.info("Supply I update: {}", it.supplyCurrent.appliedUpdateFrequency) + logger.info("Supply V update: {}", it.supplyVoltage.appliedUpdateFrequency) + logger.info("Torque I update: {}", it.torqueCurrent.appliedUpdateFrequency) + logger.info("Velocity update: {}", it.velocity.appliedUpdateFrequency) + + logger.info("Closed D Out update: {}", it.closedLoopDerivativeOutput.appliedUpdateFrequency) + logger.info("Closed Loop Err update: {}", it.closedLoopError.appliedUpdateFrequency) + logger.info("Closed FF update: {}", it.closedLoopFeedForward.appliedUpdateFrequency) + logger.info("Closed I update: {}", it.closedLoopIntegratedOutput.appliedUpdateFrequency) + logger.info("Closed Loop Out: {}", it.closedLoopOutput.appliedUpdateFrequency) + logger.info("Closed P update: {}", it.closedLoopProportionalOutput.appliedUpdateFrequency) + logger.info("Closed Loop Ref update: {}", it.closedLoopReference.appliedUpdateFrequency) + logger.info("Closed Loop Ref Slope: {}", it.closedLoopReferenceSlope.appliedUpdateFrequency) + logger.info("Closed Loop Slot update: {}", it.closedLoopSlot.appliedUpdateFrequency) + logger.info("Diff Closed D update: {}", it.differentialClosedLoopDerivativeOutput.appliedUpdateFrequency) + logger.info("Diff Closed Err update: {}", it.differentialClosedLoopError.appliedUpdateFrequency) + logger.info("Diff Closed FF update: {}", it.differentialClosedLoopFeedForward.appliedUpdateFrequency) + logger.info("Diff Closed I update: {}", it.differentialClosedLoopIntegratedOutput.appliedUpdateFrequency) + logger.info("Diff Closed Out update: {}", it.differentialClosedLoopOutput.appliedUpdateFrequency) + logger.info("Diff Closed P update: {}", it.differentialClosedLoopProportionalOutput.appliedUpdateFrequency) + logger.info("Diff Closed Ref update: {}", it.differentialClosedLoopReference.appliedUpdateFrequency) + logger.info("Diff Closed Ref Slope update: {}", it.differentialClosedLoopReferenceSlope.appliedUpdateFrequency) + logger.info("Diff Closed Slot update: {}", it.differentialClosedLoopSlot.appliedUpdateFrequency) + */ + + + } + telemetryService.start() active.firstOrNull() return new diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt index a0b3f64..5d49b5a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt @@ -6,7 +6,7 @@ import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.swerve.SwerveDrive -import org.strykeforce.swerve.TalonSwerveModule +import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt @@ -54,11 +54,11 @@ class AdjustAzimuthCommand( override val menu: String get() = formatMenu( - (swerve.swerveModules[active] as TalonSwerveModule).azimuthTalon.getSelectedSensorPosition(0) + (swerve.swerveModules[active] as V6TalonSwerveModule).azimuthTalon.getSelectedSensorPosition(0) ) override fun execute(): Command { - val swerveModule = swerve.swerveModules[active] as TalonSwerveModule + val swerveModule = swerve.swerveModules[active] as V6TalonSwerveModule var position = swerveModule.azimuthTalon.getSelectedSensorPosition(0).toInt() while (true) { try { @@ -78,7 +78,7 @@ class AdjustAzimuthCommand( } } -private fun TalonSwerveModule.jogAround(position: Double) { +private fun V6TalonSwerveModule.jogAround(position: Double) { val positions = listOf(position - JOG, position + JOG, position) positions.forEach { this.azimuthTalon.set(TalonSRXControlMode.MotionMagic, it) @@ -86,5 +86,5 @@ private fun TalonSwerveModule.jogAround(position: Double) { } } -internal fun TalonSwerveModule.onTarget(target: Double, goodEnough: Int = GOOD_ENOUGH) = +internal fun V6TalonSwerveModule.onTarget(target: Double, goodEnough: Int = GOOD_ENOUGH) = abs(target - this.azimuthTalon.getSelectedSensorPosition(0)) < goodEnough diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt index 9c94b8e..1dc013e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt @@ -4,7 +4,7 @@ import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.swerve.SwerveDrive -import org.strykeforce.swerve.TalonSwerveModule +import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt @@ -25,7 +25,7 @@ class SaveZeroCommand( try { if (reader.readBoolean(prompt(), false)) { swerve.swerveModules.forEach { - val module = it as TalonSwerveModule + val module = it as V6TalonSwerveModule logger.debug { "azimuth ${module.azimuthTalon.deviceID}: store zero, before=${module.azimuthTalon.selectedSensorPosition}" } module.storeAzimuthZeroReference() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt index 7b6b9e8..1777214 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt @@ -7,7 +7,7 @@ import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.swerve.SwerveDrive -import org.strykeforce.swerve.TalonSwerveModule +import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt @@ -29,7 +29,7 @@ class SetAzimuthCommand( override val menu: String get() = formatMenu(swerve.swerveModules.map { - (it as TalonSwerveModule).azimuthTalon.getSelectedSensorPosition(0) + (it as V6TalonSwerveModule).azimuthTalon.getSelectedSensorPosition(0) }.joinToString()) override fun execute(): Command { @@ -44,17 +44,17 @@ class SetAzimuthCommand( try { val setpoint = reader.readInt(prompt()).toDouble() swerve.swerveModules.forEach { - (it as TalonSwerveModule).azimuthTalon.set( + (it as V6TalonSwerveModule).azimuthTalon.set( TalonSRXControlMode.MotionMagic, setpoint ) } - val swerveModule = swerve.swerveModules[0] as TalonSwerveModule + val swerveModule = swerve.swerveModules[0] as V6TalonSwerveModule while (!swerveModule.onTarget(setpoint)) Timer.delay(DELAY) Timer.delay(5 * DELAY) logger.info { swerve.swerveModules.map { - (it as TalonSwerveModule).azimuthTalon.getSelectedSensorPosition( + (it as V6TalonSwerveModule).azimuthTalon.getSelectedSensorPosition( 0 ) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt index e600135..5b1df1e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt @@ -15,6 +15,7 @@ import org.strykeforce.thirdcoast.talon.TalonParameter.Enum.* private val logger = KotlinLogging.logger {} + class TalonParameterCommand( parent: Command?, key: String, diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt new file mode 100644 index 0000000..285295d --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt @@ -0,0 +1,66 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6DefaultStatusFrameCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override fun execute(): Command { + val timeout = talonFxService.timeout + val value = 10.0 + talonFxService.active.forEach { + talonFxService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) + } + return super.execute() + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt new file mode 100644 index 0000000..f32474a --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt @@ -0,0 +1,26 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxService + +class P6FactoryDefaultCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val talonFxService: TalonFxService by inject() + + override fun execute(): Command { + val config = TalonFXConfiguration() + talonFxService.activeConfiguration = config + talonFxService.active.forEach { it.configurator.apply(config) } + + return super.execute() + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt index 081a026..8910062 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt @@ -17,8 +17,8 @@ class P6SelectFwdHardLimitSourceCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(LimitSwitchPin, RemoteCANifier, RemoteTalonFX), - listOf("Talon FX pin", "Remote CANifier", "Remote TalonFx")) { + listOf(LimitSwitchPin, RemoteTalonFX, RemoteCANifier), + listOf("Talon FX pin", "Remote TalonFx", "Remote CANifier")) { private val talonFxService: TalonFxService by inject() @@ -37,8 +37,8 @@ class P6SelectRevHardLimitSourceCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(ReverseLimitSourceValue.LimitSwitchPin, ReverseLimitSourceValue.RemoteCANifier, ReverseLimitSourceValue.RemoteTalonFX), - listOf("Talon FX pin", "Remote CANifier", "Remote TalonFX")) { + listOf(ReverseLimitSourceValue.LimitSwitchPin, ReverseLimitSourceValue.RemoteTalonFX, ReverseLimitSourceValue.RemoteCANifier), + listOf("Talon FX pin", "Remote TalonFX", "Remote CANifier")) { private val talonFxService: TalonFxService by inject() @@ -58,8 +58,8 @@ class P6SelectFwdHardLimitNormalCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(NormallyClosed, NormallyOpen), - listOf("Normally Closed", "Normally Open")) { + listOf(NormallyOpen, NormallyClosed), + listOf("Normally Open", "Normally Closed")) { private val talonFxService: TalonFxService by inject() @@ -78,8 +78,8 @@ class P6SelectRevHardLimitNormalCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(ReverseLimitTypeValue.NormallyClosed, ReverseLimitTypeValue.NormallyOpen), - listOf("Normally Closed", "Normally Open")) { + listOf(ReverseLimitTypeValue.NormallyOpen, ReverseLimitTypeValue.NormallyClosed), + listOf("Normally Open", "Normally Closed")) { private val talonFxService: TalonFxService by inject() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt index 6b6ff4c..03ef224 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -18,6 +18,9 @@ import com.ctre.phoenix6.controls.DynamicMotionMagicTorqueCurrentFOC import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage import com.ctre.phoenix6.controls.Follower import com.ctre.phoenix6.controls.MotionMagicDutyCycle +import com.ctre.phoenix6.controls.MotionMagicExpoDutyCycle +import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC @@ -72,6 +75,8 @@ class P6RunCommand( val followerType = talonFxService.activeFollowerType val enableFOC = talonFxService.activeFOC val overrideNeutral = talonFxService.activeOverrideNeutral + val limFwdMotion = talonFxService.limFwdMotion + val limRevMotion = talonFxService.limRevMotion val velocity = talonFxService.activeVelocity val acceleration = talonFxService.activeAcceleration val jerk = talonFxService.activeJerk @@ -79,7 +84,7 @@ class P6RunCommand( val slot = talonFxService.activeSlotIndex val diffSlot = talonFxService.activeDifferentialSlot val diffPos = talonFxService.activeDifferentialTarget - var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false) + var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false,limFwdMotion,limRevMotion) //sanity checks if (units == Units.PERCENT && !(-1.0..1.0).contains(setpoint)) { @@ -96,13 +101,13 @@ class P6RunCommand( when (setpointType) { SetpointType.OPEN_LOOP -> { when (units) { - Units.PERCENT -> controlRequest = DutyCycleOut(setpoint, enableFOC, overrideNeutral) - Units.VOLTAGE -> controlRequest = VoltageOut(setpoint, enableFOC, overrideNeutral) + Units.PERCENT -> controlRequest = DutyCycleOut(setpoint, enableFOC, overrideNeutral,limFwdMotion, limRevMotion) + Units.VOLTAGE -> controlRequest = VoltageOut(setpoint, enableFOC, overrideNeutral, limFwdMotion, limRevMotion) Units.TORQUE_CURRENT -> controlRequest = TorqueCurrentFOC( setpoint, talonFxService.activeTorqueCurrentMaxOut, talonFxService.activeTorqueCurrentDeadband, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) } } @@ -110,26 +115,26 @@ class P6RunCommand( SetpointType.POSITION -> { when (units) { Units.PERCENT -> controlRequest = - PositionDutyCycle(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral) + PositionDutyCycle(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.VOLTAGE -> controlRequest = - PositionVoltage(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral) + PositionVoltage(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - PositionTorqueCurrentFOC(setpoint, velocity, feedFwd, slot, overrideNeutral) + PositionTorqueCurrentFOC(setpoint, velocity, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) } } SetpointType.VELOCITY -> { when (units) { Units.PERCENT -> controlRequest = - VelocityDutyCycle(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral) + VelocityDutyCycle(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.VOLTAGE -> controlRequest = - VelocityVoltage(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral) + VelocityVoltage(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - VelocityTorqueCurrentFOC(setpoint, acceleration, feedFwd, slot, overrideNeutral) + VelocityTorqueCurrentFOC(setpoint, acceleration, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) } } @@ -138,13 +143,13 @@ class P6RunCommand( MM_Type.STANDARD -> { when (units) { Units.PERCENT -> controlRequest = - MotionMagicDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral) + MotionMagicDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.VOLTAGE -> controlRequest = - MotionMagicVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral) + MotionMagicVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - MotionMagicTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral) + MotionMagicTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) } } @@ -156,7 +161,7 @@ class P6RunCommand( enableFOC, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.VOLTAGE -> controlRequest = MotionMagicVelocityVoltage( @@ -165,7 +170,7 @@ class P6RunCommand( enableFOC, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.TORQUE_CURRENT -> controlRequest = MotionMagicVelocityTorqueCurrentFOC( @@ -174,7 +179,7 @@ class P6RunCommand( enableFOC, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) } } @@ -189,7 +194,7 @@ class P6RunCommand( enableFOC, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.VOLTAGE -> controlRequest = DynamicMotionMagicVoltage( @@ -200,7 +205,7 @@ class P6RunCommand( enableFOC, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.TORQUE_CURRENT -> controlRequest = DynamicMotionMagicTorqueCurrentFOC( @@ -210,10 +215,17 @@ class P6RunCommand( jerk, feedFwd, slot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) } } + MM_Type.EXPONENTIAL -> { + when(units) { + Units.PERCENT -> controlRequest = MotionMagicExpoDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) + Units.VOLTAGE -> controlRequest = MotionMagicExpoVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) + Units.TORQUE_CURRENT -> controlRequest = MotionMagicExpoTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) + } + } } } @@ -222,10 +234,10 @@ class P6RunCommand( DifferentialType.OPEN_LOOP -> { when (units) { Units.PERCENT -> controlRequest = - DifferentialDutyCycle(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral) + DifferentialDutyCycle(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral,limFwdMotion,limRevMotion) Units.VOLTAGE -> controlRequest = - DifferentialVoltage(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral) + DifferentialVoltage(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral,limFwdMotion,limRevMotion) else -> { terminal.warn("Units chosen not valid for this control mode") @@ -242,7 +254,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.VOLTAGE -> controlRequest = DifferentialPositionVoltage( @@ -251,7 +263,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) else -> { @@ -269,7 +281,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.VOLTAGE -> controlRequest = DifferentialVelocityVoltage( @@ -278,7 +290,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) else -> { @@ -296,7 +308,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) Units.VOLTAGE -> controlRequest = DifferentialMotionMagicVoltage( @@ -305,7 +317,7 @@ class P6RunCommand( enableFOC, slot, diffSlot, - overrideNeutral + overrideNeutral,limFwdMotion,limRevMotion ) else -> { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt index 2805df0..61fdb16 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt @@ -12,8 +12,8 @@ class P6SelectMotionMagicTypeCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(MM_Type.STANDARD, MM_Type.VELOCITY, MM_Type.DYNAMIC), - listOf("Standard (Position)", "Velocity", "(FD) Dynamic")) { + listOf(MM_Type.STANDARD, MM_Type.VELOCITY, MM_Type.DYNAMIC, MM_Type.EXPONENTIAL), + listOf("Standard (Position)", "Velocity", "(FD) Dynamic", "Exponential")) { private val talonFxService: TalonFxService by inject() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt index a17fd2a..26cc38f 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt @@ -10,13 +10,13 @@ import org.strykeforce.thirdcoast.device.TalonFxService private val MODE = listOf( - Brake, - Coast + Coast, + Brake ) private val LABELS = listOf( - "Brake", - "Coast" + "Coast", + "Brake" ) class P6SelectNeutralModeCommand( diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt index a223c32..4885350 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt @@ -12,8 +12,8 @@ class P6SelectNeutralOutputCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(NeutralModeValue.Brake, NeutralModeValue.Coast), - listOf("Brake", "Coast")) { + listOf(NeutralModeValue.Coast, NeutralModeValue.Brake), + listOf("Coast", "Brake")) { private val talonFxService: TalonFxService by inject() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt index 6292edb..3394e2f 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt @@ -1,10 +1,12 @@ package org.strykeforce.thirdcoast.talon.phoenix6 +import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.strykeforce.thirdcoast.command.AbstractParameter import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.parseResource +private val logger = KotlinLogging.logger { } class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): AbstractParameter(command, toml) { enum class P6Enum { @@ -83,7 +85,10 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab DIFFERENTIAL_SLOT, DIFFERENTIAL_TARGET, FOC, - OVERRIDE_NEUTRAL + OVERRIDE_NEUTRAL, + LIM_FWD_MOT, + LIM_REV_MOT, + GRAPHER_FRAME } @@ -91,6 +96,7 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab private val tomlTable by lazy { parseResource("/phoenix6.toml") } fun create(command: Command, param: String): Phoenix6Parameter { + logger.info { "Creating P6 Param: ${param}: ${P6Enum.valueOf(param)}" } val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param") return Phoenix6Parameter(command, toml, P6Enum.valueOf(param)) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt index 6475ed0..eb36527 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt @@ -7,6 +7,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.talon.phoenix6.Phoenix6Parameter.P6Enum.* import org.strykeforce.thirdcoast.warn @@ -19,19 +20,22 @@ class Phoenix6ParameterCommand( toml: TomlTable ): AbstractCommand(parent, key, toml) { + + private val talonFxService : TalonFxService by inject() - val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + //val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonFxService.timeout private val param = Phoenix6Parameter.create(this, toml.getString("param") ?: "UNKNOWN") override val menu: String get() { val config = talonFxService.activeConfiguration + logger.info { "Current Param: ${param.name}, ${param.prompt}" } return when(param.enum) { - ROTOR_OFFSET -> formatMenu(config.Feedback.FeedbackRotorOffset) - SENSOR_TO_MECH_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio) - ROTOR_TO_SENSOR_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio) + ROTOR_OFFSET -> formatMenu(config.Feedback.FeedbackRotorOffset, DOUBLE_FORMAT_4) + SENSOR_TO_MECH_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio, DOUBLE_FORMAT_4) + ROTOR_TO_SENSOR_RATIO -> formatMenu(config.Feedback.RotorToSensorRatio, DOUBLE_FORMAT_4) REMOTE_SENSOR_ID -> formatMenu(config.Feedback.FeedbackRemoteSensorID) SLOT_KP -> formatMenu( @@ -74,6 +78,14 @@ class Phoenix6ParameterCommand( else -> SlotConfigs().kV } ) + SLOT_KA -> formatMenu( + when(talonFxService.activeSlotIndex){ + 0 -> talonFxService.activeConfiguration.Slot0.kA + 1 -> talonFxService.activeConfiguration.Slot1.kA + 2 -> talonFxService.activeConfiguration.Slot2.kA + else -> SlotConfigs().kA + } + ) SLOT_KG -> formatMenu( when(talonFxService.activeSlotIndex) { 0 -> talonFxService.activeConfiguration.Slot0.kG @@ -107,7 +119,7 @@ class Phoenix6ParameterCommand( OPEN_LOOP_RAMP_DC -> formatMenu(config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) PEAK_FWD_V -> formatMenu(config.Voltage.PeakForwardVoltage) PEAK_REV_V -> formatMenu(config.Voltage.PeakReverseVoltage) - SUPPLY_V_TIME_CONST -> formatMenu(config.Voltage.SupplyVoltageTimeConstant) + SUPPLY_V_TIME_CONST -> formatMenu(config.Voltage.SupplyVoltageTimeConstant, DOUBLE_FORMAT_4) OPEN_LOOP_RAMP_V -> formatMenu(config.OpenLoopRamps.VoltageOpenLoopRampPeriod) CLOSED_LOOP_RAMP_V -> formatMenu(config.ClosedLoopRamps.VoltageClosedLoopRampPeriod) PEAK_FWD_I -> formatMenu(config.TorqueCurrent.PeakForwardTorqueCurrent) @@ -119,7 +131,7 @@ class Phoenix6ParameterCommand( FWD_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitEnable) FWD_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) REV_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ReverseSoftLimitEnable) - REV_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) + REV_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ReverseSoftLimitThreshold) FWD_HARD_EN -> formatMenu(config.HardwareLimitSwitch.ForwardLimitEnable) FWD_REMOTE_ID -> formatMenu(config.HardwareLimitSwitch.ForwardLimitRemoteSensorID) @@ -133,6 +145,20 @@ class Phoenix6ParameterCommand( ALLOW_MUSIC_DIS -> formatMenu(config.Audio.AllowMusicDurDisable) BEEP_ON_BOOT -> formatMenu(config.Audio.BeepOnBoot) BEEP_ON_CONFIG -> formatMenu(config.Audio.BeepOnConfig) + VELOCITY -> formatMenu(talonFxService.activeVelocity) + ACCELERATION -> formatMenu(talonFxService.activeAcceleration) + JERK -> formatMenu(talonFxService.activeJerk) + TORQUE_CURRENT_DEADBAND -> formatMenu(talonFxService.activeTorqueCurrentDeadband) + TORQUE_CURRENT_MAX -> formatMenu(talonFxService.activeTorqueCurrentMaxOut) + FEED_FORWARD -> formatMenu(talonFxService.activeFeedForward) + OPPOSE_MAIN -> formatMenu(talonFxService.activeOpposeMain) + DIFFERENTIAL_SLOT -> formatMenu(talonFxService.activeDifferentialSlot) + DIFFERENTIAL_TARGET -> formatMenu(talonFxService.activeDifferentialTarget) + FOC -> formatMenu(talonFxService.activeFOC) + OVERRIDE_NEUTRAL -> formatMenu(talonFxService.activeOverrideNeutral) + LIM_FWD_MOT -> formatMenu(talonFxService.limFwdMotion) + LIM_REV_MOT -> formatMenu(talonFxService.limRevMotion) + GRAPHER_FRAME -> formatMenu(talonFxService.grapherStatusFrameHz) else -> TODO("${param.enum} not implemented") } @@ -334,7 +360,7 @@ class Phoenix6ParameterCommand( } PEAK_DIFF_VOLT -> configDoubleParam(config.DifferentialConstants.PeakDifferentialVoltage) { talonFx, value -> - config.DifferentialConstants.PeakDifferentialVoltage - value + config.DifferentialConstants.PeakDifferentialVoltage = value talonFx.configurator.apply(config.DifferentialConstants) } DIFF_SENSOR_REMOTE_ID -> configIntParam(config.DifferentialSensors.DifferentialRemoteSensorID) { @@ -375,7 +401,7 @@ class Phoenix6ParameterCommand( } SUPP_TRIP_TIME -> configDoubleParam(config.CurrentLimits.SupplyTimeThreshold) { talonFx, value -> - config.CurrentLimits.SupplyTimeThreshold + config.CurrentLimits.SupplyTimeThreshold = value talonFx.configurator.apply(config.CurrentLimits) } @@ -585,6 +611,60 @@ class Phoenix6ParameterCommand( talonFx, value -> talonFxService.activeOverrideNeutral = value } + LIM_FWD_MOT -> configBooleanParam(talonFxService.limFwdMotion) { + talonFx, value -> + talonFxService.limFwdMotion = value; + } + LIM_REV_MOT -> configBooleanParam(talonFxService.limRevMotion) { + talonFx, value -> + talonFxService.limRevMotion = value; + } + GRAPHER_FRAME -> configDoubleParam(talonFxService.grapherStatusFrameHz) { + talonFx, value -> + talonFxService.grapherStatusFrameHz = value; + talonFx.acceleration.setUpdateFrequency(value, timeout) + talonFx.bridgeOutput.setUpdateFrequency(value, timeout) + talonFx.deviceTemp.setUpdateFrequency(value, timeout) + talonFx.differentialAveragePosition.setUpdateFrequency(value, timeout) + talonFx.differentialAverageVelocity.setUpdateFrequency(value, timeout) + talonFx.differentialDifferencePosition.setUpdateFrequency(value, timeout) + talonFx.differentialDifferenceVelocity.setUpdateFrequency(value, timeout) + talonFx.dutyCycle.setUpdateFrequency(value, timeout) + talonFx.forwardLimit.setUpdateFrequency(value, timeout) + talonFx.reverseLimit.setUpdateFrequency(value, timeout) + talonFx.isProLicensed.setUpdateFrequency(value, timeout) + talonFx.motionMagicIsRunning.setUpdateFrequency(value, timeout) + talonFx.motorVoltage.setUpdateFrequency(value, timeout) + talonFx.position.setUpdateFrequency(value, timeout) + talonFx.processorTemp.setUpdateFrequency(value, timeout) + talonFx.rotorPosition.setUpdateFrequency(value, timeout) + talonFx.rotorVelocity.setUpdateFrequency(value, timeout) + talonFx.statorCurrent.setUpdateFrequency(value, timeout) + talonFx.supplyCurrent.setUpdateFrequency(value, timeout) + talonFx.supplyVoltage.setUpdateFrequency(value, timeout) + talonFx.torqueCurrent.setUpdateFrequency(value, timeout) + talonFx.velocity.setUpdateFrequency(value, timeout) + + talonFx.closedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopError.setUpdateFrequency(value, timeout) + talonFx.closedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFx.closedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopReference.setUpdateFrequency(value, timeout) + talonFx.closedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFx.closedLoopSlot.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopError.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopReference.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopSlot.setUpdateFrequency(value, timeout) + + } else -> TODO("${param.name} not implemented") } @@ -595,20 +675,21 @@ class Phoenix6ParameterCommand( val paramValue = param.readDouble(reader, default) talonFxService.active.forEach { configure(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } } private fun configIntParam(default: Int, configure: (TalonFX, Int) -> Unit) { val paramValue = param.readInt(reader, default) talonFxService.active.forEach { configure(it, paramValue) } - logger.debug { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } } private fun configBooleanParam(default: Boolean, configure: (TalonFX, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) talonFxService.active.forEach { configure(it, paramValue) } - logger.debug { "Set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + logger.info { "Set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } } + } \ No newline at end of file diff --git a/src/main/resources/commands.toml b/src/main/resources/commands.toml index 5e1c908..8e9b5d5 100644 --- a/src/main/resources/commands.toml +++ b/src/main/resources/commands.toml @@ -960,7 +960,7 @@ type = "menu" type = "menu" order = 40 menu = "config active mode" - [FXP6.mode.status] + [FxP6.mode.status] type = "p6.modeStatus" order = 10 menu = "active mode" @@ -1046,7 +1046,7 @@ type = "menu" order = 20 menu = "select differential slot" param = "DIFFERENTIAL_SLOT" - [Fx.P6.mode.diff.target] + [FxP6.mode.diff.target] type = "p6.param" order = 30 menu = "select differential target (rots)" @@ -1075,13 +1075,23 @@ type = "menu" [FxP6.mode.foc] type = "p6.param" order = 100 - menu = "enable FOC" + menu = "(pro) enable FOC" param = "FOC" [FxP6.mode.overrideNeut] type = "p6.param" order = 110 menu = "override neutral mode" param = "OVERRIDE_NEUTRAL" + [FxP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + [FxP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" [FxP6.units] type = "p6.units" order = 50 @@ -1446,9 +1456,22 @@ type = "menu" menu = "beep on config" param = "BEEP_ON_CONFIG" [FxP6.factory] - type = "menu" + type = "p6.factory" order = 140 menu = "config factory default" + [FxP6.grapher] + type = "menu" + order = 150 + menu = "set status frames" + [FxP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + [FxP6.grapher.rate] + type = "p6.param" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" [servo] type = "menu" diff --git a/src/main/resources/phoenix6.toml b/src/main/resources/phoenix6.toml index 4ee8f3e..31c6c2a 100644 --- a/src/main/resources/phoenix6.toml +++ b/src/main/resources/phoenix6.toml @@ -18,44 +18,44 @@ name = "Remote Sensor ID" type = "INTEGER" help = "Device ID of which remote device to use. This is not used if the Sensor Source is the internal rotor sensor." - range = [0,62] + range = [0.0,62.0] #Slot Configs [SLOT_KP] name = "Proportional Gain" type = "DOUBLE" help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by error in the input, the units should be defined as units of output per unit of input error. For example, when controlling velocity using a duty cycle closed loop, the units for the proportional gain will be duty cycle per rps of error, or 1/rps." - range = [0, 3.4e38] + range = [0.0, 3.4e38] [SLOT_KI] name = "Integral Gain" type = "DOUBLE" help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by error in the input integrated over time (in units of seconds), the units should be defined as units of output per unit of integrated input error. For example, when controlling velocity using a duty cycle closed loop, integrating velocity over time results in rps * s = rotations. Therefore, the units for the integral gain will be duty cycle per rotation of accumulated error, or 1/rot." - range = [0, 3.4e38] + range = [0.0, 3.4e38] [SLOT_KD] name = "Derivitave Gain" type = "DOUBLE" help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the derivative of error in the input with respect to time (in units of seconds), the units should be defined as units of output per unit of the differentiated input error. For example, when controlling velocity using a duty cycle closed loop, the derivative of velocity with respect to time is rps/s, which is acceleration. Therefore, the units for the derivative gain will be duty cycle per unit of acceleration error, or 1/(rps/s)." - range = [0, 3.4e38] + range = [0.0, 3.4e38] [SLOT_KS] name = "Static FWD Gain" type = "DOUBLE" help = "This is added to the closed loop output. The sign is determined by target velocity. The unit for this constant is dependent on the control mode, typically fractional duty cycle, voltage, or torque current." - range = [-512, 511] + range = [-512.0, 511.0] [SLOT_KV] name = "Velocity Feedforward Gain" type = "DOUBLE" help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the requested velocity, the units should be defined as units of output per unit of requested input velocity. For example, when controlling velocity using a duty cycle closed loop, the units for the velocity feedfoward gain will be duty cycle per requested rps, or 1/rps." - range = [0, 3.4e38] + range = [0.0, 3.4e38] [SLOT_KA] name = "Acceleration Feedforward Gain" type = "DOUBLE" help = "The units for this gain is dependent on the control mode. Since this gain is multiplied by the requested acceleration, the units should be defined as units of output per unit of requested input acceleration. For example, when controlling velocity using a duty cycle closed loop, the units for the acceleration feedfoward gain will be duty cycle per requested rps/s, or 1/(rps/s)." - range = [0, 3.4e38] + range = [0.0, 3.4e38] [SLOT_KG] name = "Gravity Feedforward Gain" type = "DOUBLE" help = "This is added to the closed loop output. The sign is determined by the type of gravity feedforward. The unit for this constant is dependent on the control mode, typically fractional duty cycle, voltage, or torque current." - range = "-512, 511" + range = [-512.0, 511.0] #Motion Magic Configs @@ -63,17 +63,17 @@ name = "Motion Magic Cruise Velocity" type = "DOUBLE" help = "This is the maximum velocity Motion Magic® based control modes are allowed to use. Units: rps" - range = [0, 9999] + range = [0.0, 9999.0] [MM_ACCEL] name = "Motion Magic Acceleration" type = "DOUBLE" help = "This is the target acceleration Motion Magic® based control modes are allowed to use. Units: rps^2" - range = [0, 9999] + range = [0.0, 9999.0] [MM_JERK] name = "Motion Magic Jerk" type = "DOUBLE" help = "This is the target jerk (acceleration derivative) Motion Magic® based control modes are allowed to use. This allows Motion Magic® support of S-Curves. If this is set to zero, then Motion Magic® will not apply a Jerk limit. Units: rot/s^3" - range = [0, 9999] + range = [0.0, 9999.0] #Differential Configs [PEAK_DIFF_DC] @@ -95,12 +95,12 @@ name = "Differential Remote Sensor ID" type = "INTEGER" help = "Device ID of which remote sensor to use on the differential axis. This is used when the Differential Sensor Source is not RemoteTalonFX_Diff." - range = [0, 62] + range = [0.0, 62.0] [DIFF_FX_ID] name = "Differential Talon FX ID" type = "INTEGER" help = "Device ID of which remote Talon FX to use. This is used when the Differential Sensor Source is not disabled." - range = [0, 62] + range = [0.0, 62.0] #Current Limit Configs [STATOR_LIM_EN] @@ -116,7 +116,7 @@ name = "Supply Current Limit Enable" type = "BOOLEAN" help = "Enable motor supply current limiting." -[SUPP_LIM_] +[SUPP_LIM] name = "Supply Current Limit" type = "DOUBLE" help = "The amount of supply current allowed. This is only applicable for non-torque current control modes. Note this requires the corresponding enable to be true. Use SupplyCurrentThreshold and SupplyTimeThreshold to allow brief periods of high-current before limiting occurs. Units: A" @@ -240,7 +240,7 @@ name = "FWD Remote sensor ID" type = "INTEGER" help = "Device ID of the device if using remote limit switch features." - range = [0, 62] + range = [0.0, 62.0] [FWD_AUTOSET_POS] name = "FWD limit autoset position enable" type = "BOOLEAN" @@ -258,7 +258,7 @@ name = "REV Remote Sensor ID" type = "INTEGER" help = "Device ID of the device if using remote limit switch features." - range = [0,62] + range = [0.0,62.0] [REV_AUTOSET_POS] name = "REV limit autoset position enable" type = "BOOLEAN" @@ -316,7 +316,7 @@ name = "Differential Slot" type = "INTEGER" help = "Select which gains are applied to the differential controller by selecting the slot. Use the configuration api to set the gain values for the selected slot before enabling this feature. Slot must be within [0,2]." - range = [0,2] + range = [0.0,2.0] [DIFFERENTIAL_TARGET] name = "Differential Position" type = "DOUBLE" @@ -329,4 +329,15 @@ name = "Override Neutral Setting" type = "BOOLEAN" help = "Set to true to static-brake (if Voltage/Duty Cycle) or coast (if Torque Current) the rotor when output is zero (or within deadband). Set to false to use the NeutralMode configuration setting (default). This flag exists to provide the fundamental behavior of this control when output is zero, which is to provide 0V to the motor." - +[LIM_FWD_MOT] + name = "Limit Forward Motion" + type = "BOOLEAN" + help = "Set to true to force forward limiting. This allows users to use other limit switch sensors connected to robot controller. This also allows use of active sensors that require external power." +[LIM_REV_MOT] + name = "Limit Reverse Motion" + type = "BOOLEAN" + help = "Set to true to force reverse limiting. This allows users to use other limit switch sensors connected to robot controller. This also allows use of active sensors that require external power." +[GRAPHER_FRAME] + name = "Grapher Update Frequency (Hz)" + type = "DOUBLE" + help = "Input update frequncy for all singals on the grapher in Hz, if this value shows 0 it is using the default update frequency" \ No newline at end of file diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 2a29835..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,7 +1,7 @@ { "fileName": "NavX.json", "name": "NavX", - "version": "2024.0.1-beta-4", + "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2024.0.1-beta-4" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2024.0.1-beta-4", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index 88ca81d..88a68dd 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,38 +1,38 @@ { "fileName": "Phoenix5.json", "name": "CTRE-Phoenix (v5)", - "version": "5.32.0-beta-1", + "version": "5.33.0", "frcYear": 2024, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json" + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.32.0-beta-1" + "version": "5.33.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.32.0-beta-1" + "version": "5.33.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.32.0-beta-1", + "version": "5.33.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.32.0-beta-1", + "version": "5.33.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.32.0-beta-1", + "version": "5.33.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index c1d8140..69a4079 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-2", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-2" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-2", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-2", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index 7cd2c56..a050a90 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "23.3.1-SNAPSHOT", + "version": "24.0.0", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "23.3.1-SNAPSHOT" + "version": "24.0.0" } ], "jniDependencies": [], From 595f36d878459c6bcd28173e7bfc5a16511eeaa5 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Tue, 6 Feb 2024 22:09:34 -0500 Subject: [PATCH 3/7] add CANcoder, update talonFX options --- build.gradle | 2 +- .../kotlin/org/strykeforce/thirdcoast/Koin.kt | 3 + .../thirdcoast/cancoder/CancoderParameter.kt | 26 +++++++ .../cancoder/CancoderParameterCommand.kt | 74 +++++++++++++++++++ .../cancoder/CancoderStatusCommand.kt | 27 +++++++ .../cancoder/SelectAbsRangeValueCommand.kt | 36 +++++++++ .../cancoder/SelectCancoderCommand.kt | 34 +++++++++ .../SelectCancoderSensorDirectionCommand.kt | 37 ++++++++++ .../cancoder/SetCancoderPositionCommand.kt | 10 +++ .../strykeforce/thirdcoast/command/Command.kt | 18 +++++ .../thirdcoast/command/MenuCommand.kt | 2 +- .../thirdcoast/device/CancoderService.kt | 40 ++++++++++ .../talon/phoenix6/P6ModeStatusCommand.kt | 7 +- .../talon/phoenix6/P6ModeStatusMenuCommand.kt | 56 ++++++++++++++ .../thirdcoast/talon/phoenix6/P6RunCommand.kt | 4 + .../talon/phoenix6/Phoenix6Parameter.kt | 3 + .../phoenix6/Phoenix6ParameterCommand.kt | 19 +++++ src/main/resources/cancoder.toml | 11 +++ src/main/resources/commands.toml | 47 +++++++++++- src/main/resources/phoenix6.toml | 14 ++++ vendordeps/thirdcoast.json | 4 +- 21 files changed, 467 insertions(+), 7 deletions(-) create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SetCancoderPositionCommand.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderService.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt create mode 100644 src/main/resources/cancoder.toml diff --git a/build.gradle b/build.gradle index eb72d81..234275a 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" id "org.jetbrains.kotlin.jvm" version "1.7.21" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2024.1.1" } version = "24.0.0" diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index e58046f..d36dbba 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -4,6 +4,7 @@ import com.ctre.phoenix.CANifier import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix.sensors.PigeonIMU +import com.ctre.phoenix6.hardware.CANcoder import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.wpilibj.DigitalOutput import edu.wpi.first.wpilibj.PneumaticsModuleType @@ -55,6 +56,8 @@ val tctModule = module { single { CanifierService(get()) { id -> CANifier(id) } } + single { CancoderService(get()) {id -> CANcoder(id)} } + single { PigeonService(get()) { id -> PigeonIMU(id) } } single { (command: Command) -> Shell(command, get()) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt new file mode 100644 index 0000000..edd1f77 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt @@ -0,0 +1,26 @@ +package org.strykeforce.thirdcoast.cancoder + +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractParameter +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.parseResource + +class CancoderParameter( + command: Command, + toml: TomlTable, + val enum: Enum +): AbstractParameter(command, toml) { + enum class Enum { + POSITION, + MAG_OFFSET + } + + companion object { + private val tomlTable by lazy { parseResource("/cancoder.toml")} + + fun create(command: Command, param: String): CancoderParameter { + val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param $param") + return CancoderParameter(command, toml, Enum.valueOf(param)) + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt new file mode 100644 index 0000000..31eb449 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt @@ -0,0 +1,74 @@ +package org.strykeforce.thirdcoast.cancoder + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.hardware.CANcoder +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderService +import org.strykeforce.thirdcoast.cancoder.CancoderParameter.Enum.* +import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 + +private val logger = KotlinLogging.logger { } +class CancoderParameterCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val cancoderService: CancoderService by inject() + private val timeout = cancoderService.timeout + private val param = CancoderParameter.create(this, toml.getString("param")?: "UNKNOWN") + + override val menu: String + get() { + return when (param.enum) { + POSITION -> formatMenu(cancoderService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0, DOUBLE_FORMAT_4) + MAG_OFFSET -> formatMenu(cancoderService.activeConfiguration.MagnetSensor.MagnetOffset) + } + } + + override fun execute(): Command { + var activeConfig = cancoderService.activeConfiguration + when(param.enum) { + POSITION -> configDoubleParam(cancoderService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0) { + cancoder, value -> + cancoder.setPosition(value) + } + MAG_OFFSET -> configDoubleParam(activeConfig.MagnetSensor.MagnetOffset) { + cancoder, value -> + activeConfig.MagnetSensor.MagnetOffset = value + cancoder.configurator.apply(activeConfig) + } + else -> TODO("${param.name} not implemented") + } + return super.execute() + } + + + private fun configDoubleParam(default: Double, config: (CANcoder, Double) -> Unit) { + val paramValue = param.readDouble(reader, default) + cancoderService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } + + private fun configIntParameter(default: Int, config: (CANcoder, Int) -> Unit) { + val paramValue = param.readInt(reader, default) + cancoderService.active.forEach{ + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } + + private fun configBooleanParam(default: Boolean, config: (CANcoder, Boolean) -> Unit) { + val paramValue = param.readBoolean(reader, default) + cancoderService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt new file mode 100644 index 0000000..6e998d8 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt @@ -0,0 +1,27 @@ +package org.strykeforce.thirdcoast.cancoder + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderService + +class CancoderStatusCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + + private val cancoderService: CancoderService by inject() + + override fun execute(): Command { + val writer = terminal.writer() + cancoderService.active.forEach{ + val config = CANcoderConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + return super.execute() + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt new file mode 100644 index 0000000..8ad984b --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt @@ -0,0 +1,36 @@ +package org.strykeforce.thirdcoast.cancoder + +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderService + +private val RANGE_VALUE = listOf( + Unsigned_0To1, + Signed_PlusMinusHalf +) + +private val LABELS = listOf( + "0 to 1", + "-0.5 to 0.5" +) + +class SelectAbsRangeValueCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, RANGE_VALUE, LABELS) { + private val cancoderService: CancoderService by inject() + + override val activeIndex: Int + get() = cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal + + override fun setActive(index: Int) { + val range = values[index] + cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range + cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt new file mode 100644 index 0000000..42f2441 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt @@ -0,0 +1,34 @@ +package org.strykeforce.thirdcoast.cancoder + +import com.ctre.phoenix6.hardware.CANcoder +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.CancoderService +import org.strykeforce.thirdcoast.readInt +import org.strykeforce.thirdcoast.warn + +class SelectCancoderCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + private val cancoderService: CancoderService by inject() + + override val menu: String + get() = formatMenu(cancoderService.active.map(CANcoder::getDeviceID).joinToString()) + + override fun execute(): Command { + while(true) { + try { + val id = reader.readInt(this.prompt("id")) + cancoderService.activate(listOf(id)) + return super.execute() + } catch (e: IllegalArgumentException) { + terminal.warn("Please enter a CANcoder id") + } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt new file mode 100644 index 0000000..4c03a40 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt @@ -0,0 +1,37 @@ +package org.strykeforce.thirdcoast.cancoder + +import com.ctre.phoenix6.signals.SensorDirectionValue +import com.ctre.phoenix6.signals.SensorDirectionValue.* +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderService + +private val DIRECTION = listOf( + Clockwise_Positive, + CounterClockwise_Positive +) + +private val LABELS = listOf( + "Clockwise Positive", + "Counter-clockwise Positive" +) + +class SelectCancoderSensorDirectionCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, DIRECTION, LABELS) { + + private val cancoderService: CancoderService by inject() + + override val activeIndex: Int + get() = cancoderService.activeConfiguration.MagnetSensor.SensorDirection.ordinal + + override fun setActive(index: Int) { + val direction = values[index] + cancoderService.activeConfiguration.MagnetSensor.SensorDirection = direction + cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SetCancoderPositionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SetCancoderPositionCommand.kt new file mode 100644 index 0000000..65c3950 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SetCancoderPositionCommand.kt @@ -0,0 +1,10 @@ +package org.strykeforce.thirdcoast.cancoder + +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderService + +private val logger = KotlinLogging.logger { } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt index 4663253..b20d2c4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt @@ -8,6 +8,7 @@ import org.jline.utils.AttributedString import org.jline.utils.AttributedStyle import org.koin.standalone.KoinComponent import org.koin.standalone.inject +import org.strykeforce.thirdcoast.cancoder.* import org.strykeforce.thirdcoast.canifier.* import org.strykeforce.thirdcoast.dio.RunDigitalOutputsCommand import org.strykeforce.thirdcoast.dio.SelectDigitalOutputsCommand @@ -90,6 +91,7 @@ interface Command { "p6.select" -> P6SelectTalonsCommand(parent, key, toml) "p6.status" -> P6TalonStatusCommand(parent, key, toml) "p6.modeStatus" -> P6ModeStatusCommand(parent, key, toml) + "p6.modeMenu" -> createModeMenuCommand(parent, key, toml) "p6.mode" -> P6SelectModeCommand(parent, key, toml) "p6.param" -> Phoenix6ParameterCommand(parent, key, toml) "p6.mmType" -> P6SelectMotionMagicTypeCommand(parent, key, toml) @@ -107,6 +109,11 @@ interface Command { "p6.revSource" -> P6SelectRevHardLimitSourceCommand(parent, key, toml) "p6.factory" -> P6FactoryDefaultCommand(parent, key, toml) "p6.graph" -> P6DefaultStatusFrameCommand(parent, key, toml) + "cancoder.select" -> SelectCancoderCommand(parent, key, toml) + "cancoder.status" -> CancoderStatusCommand(parent, key, toml) + "cancoder.param" -> CancoderParameterCommand(parent, key, toml) + "cancoder.absRange" -> SelectAbsRangeValueCommand(parent, key, toml) + "cancoder.sensorDirection" -> SelectCancoderSensorDirectionCommand(parent, key, toml) else -> DefaultCommand(parent, key, toml) } } @@ -121,6 +128,17 @@ interface Command { } return command } + + private fun createModeMenuCommand(parent: MenuCommand?, key: String, toml: TomlTable): P6ModeStatusMenuCommand { + val command = P6ModeStatusMenuCommand(parent, key, toml) + toml.keySet().filter(toml::isTable) + .forEach{ k -> + val child = createFromToml(toml.getTable(k)!!, command, k) + command.children.add(child) + logger.info { "Create Menu: $k, ${command.validMenuChoices}" } + } + return command + } } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/MenuCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/MenuCommand.kt index 5f093da..834dce2 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/MenuCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/MenuCommand.kt @@ -6,7 +6,7 @@ import org.jline.utils.AttributedStyle import org.strykeforce.thirdcoast.* import java.util.* -class MenuCommand( +open class MenuCommand( parent: Command?, key: String, toml: TomlTable diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderService.kt new file mode 100644 index 0000000..dee7bdc --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderService.kt @@ -0,0 +1,40 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.hardware.CANcoder +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private val logger = KotlinLogging.logger {} + +class CancoderService( + private val telemetryService: TelemetryService, factory: (id: Int) -> CANcoder +): AbstractDeviceService(factory) { + + val timeout = 10.0 + var dirty = true + + var activeConfiguration = CANcoderConfiguration() + get() { + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?: logger.debug("no active CANcoder's, returning default config") + dirty = false + return field + } + + override fun activate(ids: Collection): Set { + dirty = true + + val new = super.activate(ids) + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach{ + telemetryService.register(it) + } + telemetryService.start() + active.firstOrNull() + return new + } + + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt index 011c7be..bbbefe8 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt @@ -14,9 +14,12 @@ class P6ModeStatusCommand( private val talonFxService: TalonFxService by inject() + override val menu: String + get() = formatMenu(talonFxService.controlMode) + override fun execute(): Command { - val writer = terminal.writer() - writer.println(talonFxService.controlMode) + //val writer = terminal.writer() + //writer.println(talonFxService.controlMode) return super.execute() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt new file mode 100644 index 0000000..d75920d --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt @@ -0,0 +1,56 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.koin.standalone.inject +import org.strykeforce.thirdcoast.* +import org.strykeforce.thirdcoast.command.* +import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.invalidMenuChoice +import java.util.* + +private val logger = KotlinLogging.logger {} + +class P6ModeStatusMenuCommand( + parent: Command?, + key: String, + toml: TomlTable +): MenuCommand(parent, key, toml) { + private val talonFxService: TalonFxService by inject() + + override val children = TreeSet(compareBy(Command::order, Command::key)) + + override val menu: String + get() { + logger.info { "Getting Menu Option..." } + return formatMenu(talonFxService.controlMode) + } + + override fun execute(): Command { + while(true){ + printMenu() + val choice = readRawMenuChoice() + when(choice) { + INVALID -> terminal.invalidMenuChoice() + BACK -> return parent ?: this + QUIT -> return QuitCommand() + else -> return children.elementAt(choice) + } + } + } + + private fun printMenu() { + logger.info { "Printing menu... " + children.size } + val writer = terminal.writer() + writer.println() + children.forEachIndexed{ index, cmd -> + writer.println(cmd.menu.toRawMenu(index)) + logger.info { "Child: " + cmd.menu + " " + cmd.key } + } + if(parent != null) + writer.println("back to previous menu".toMenu("b")) + writer.println("quit TCT".toMenu("Q")) + } + + private fun readRawMenuChoice() = terminal.readRawMenu(children.size, prompt(), quit = true) +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt index 03ef224..9474ff8 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -97,6 +97,8 @@ class P6RunCommand( continue } + logger.info { "Setpoint: $setpoint" } + //make control request when (setpointType) { SetpointType.OPEN_LOOP -> { @@ -141,6 +143,7 @@ class P6RunCommand( SetpointType.MOTION_MAGIC -> { when (motionType) { MM_Type.STANDARD -> { + logger.info { "Motion Magic: $motionType, $units" } when (units) { Units.PERCENT -> controlRequest = MotionMagicDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) @@ -364,6 +367,7 @@ class P6RunCommand( //run Talon talonFxService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } it.setControl(controlRequest) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt index 3394e2f..3e61dfc 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt @@ -26,6 +26,8 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab MM_ACCEL, MM_CRUISE_VEL, MM_JERK, + MM_EXPO_KA, + MM_EXPO_KV, PEAK_DIFF_DC, PEAK_DIFF_TORQUE, @@ -75,6 +77,7 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab BEEP_ON_BOOT, BEEP_ON_CONFIG, + POSITION, VELOCITY, ACCELERATION, JERK, diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt index eb36527..7a2c2d9 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt @@ -98,6 +98,8 @@ class Phoenix6ParameterCommand( MM_CRUISE_VEL -> formatMenu(config.MotionMagic.MotionMagicCruiseVelocity) MM_ACCEL -> formatMenu(config.MotionMagic.MotionMagicAcceleration) MM_JERK -> formatMenu(config.MotionMagic.MotionMagicJerk) + MM_EXPO_KA -> formatMenu(config.MotionMagic.MotionMagicExpo_kA) + MM_EXPO_KV -> formatMenu(config.MotionMagic.MotionMagicExpo_kV) PEAK_DIFF_DC -> formatMenu(config.DifferentialConstants.PeakDifferentialDutyCycle) PEAK_DIFF_VOLT -> formatMenu(config.DifferentialConstants.PeakDifferentialVoltage) @@ -145,6 +147,7 @@ class Phoenix6ParameterCommand( ALLOW_MUSIC_DIS -> formatMenu(config.Audio.AllowMusicDurDisable) BEEP_ON_BOOT -> formatMenu(config.Audio.BeepOnBoot) BEEP_ON_CONFIG -> formatMenu(config.Audio.BeepOnConfig) + POSITION -> formatMenu(talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4) VELOCITY -> formatMenu(talonFxService.activeVelocity) ACCELERATION -> formatMenu(talonFxService.activeAcceleration) JERK -> formatMenu(talonFxService.activeJerk) @@ -347,6 +350,16 @@ class Phoenix6ParameterCommand( config.MotionMagic.MotionMagicJerk = value talonFx.configurator.apply(config.MotionMagic) } + MM_EXPO_KA -> configDoubleParam(config.MotionMagic.MotionMagicExpo_kA) { + talonFx, value -> + config.MotionMagic.MotionMagicExpo_kA = value + talonFx.configurator.apply(config.MotionMagic) + } + MM_EXPO_KV -> configDoubleParam(config.MotionMagic.MotionMagicExpo_kV) { + talonFx, value -> + config.MotionMagic.MotionMagicExpo_kV = value + talonFx.configurator.apply(config.MotionMagic) + } PEAK_DIFF_DC -> configDoubleParam(config.DifferentialConstants.PeakDifferentialDutyCycle) { talonFx, value -> @@ -563,6 +576,12 @@ class Phoenix6ParameterCommand( config.Audio.BeepOnConfig = value talonFx.configurator.apply(config.Audio) } + POSITION -> configDoubleParam(talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0) { + talonFx, value -> + talonFxService.active.forEach{ + talonFx.setPosition(value, timeout) + } + } VELOCITY -> configDoubleParam(talonFxService.activeVelocity) { talonFx, value -> talonFxService.activeVelocity = value diff --git a/src/main/resources/cancoder.toml b/src/main/resources/cancoder.toml new file mode 100644 index 0000000..15279d4 --- /dev/null +++ b/src/main/resources/cancoder.toml @@ -0,0 +1,11 @@ +## Params +[POSITION] + name = "Position" + help = "Sets cancoder position. Units in rotations." + type = "DOUBLE" + +[MAG_OFFSET] + name = "Magnet Offset" + help = "This offset is added to the reported position, allowing the application to trim the zero position. When set to the default value of zero, position reports zero when magnet north pole aligns with the LED. Units in Rotations" + type = "DOUBLE" + range = [-1.0, 1.0] \ No newline at end of file diff --git a/src/main/resources/commands.toml b/src/main/resources/commands.toml index 8e9b5d5..921827b 100644 --- a/src/main/resources/commands.toml +++ b/src/main/resources/commands.toml @@ -957,7 +957,7 @@ type = "menu" order = 30 menu = "status of active talonFx's" [FxP6.mode] - type = "menu" + type = "p6.modeMenu" order = 40 menu = "config active mode" [FxP6.mode.status] @@ -1100,6 +1100,11 @@ type = "menu" type = "menu" order = 60 menu = "setup feedback configs" + [FxP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" [FxP6.feedback.offset] type = "p6.param" order = 10 @@ -1186,6 +1191,16 @@ type = "menu" order = 30 menu = "jerk" param = "MM_JERK" + [FxP6.motion.kA] + type = "p6.param" + order = 40 + menu = "Expo kA" + param = "MM_EXPO_KA" + [FxP6.motion.kV] + type = "p6.param" + order = 50 + menu = "Expo kV" + param = "MM_EXPO_KV" [FxP6.diff] type = "menu" order = 90 @@ -1756,6 +1771,36 @@ type = "menu" menu = "config factory defaults" param = "FACTORY_DEFAULT" +[cancoder] + type = "menu" + order = 70 + menu = "work with CANcoders" + [cancoder.select] + type = "cancoder.select" + order = 10 + menu = "select active CANcoders" + [cancoder.status] + type = "cancoder.status" + order = 20 + menu = "status of active CANcoders" + [cancoder.position] + type = "cancoder.param" + param = "POSITION" + order = 30 + menu = "set CANcoder position" + [cancoder.magOffset] + type = "cancoder.param" + param = "MAG_OFFSET" + order = 40 + [cancoder.absRange] + type = "cancoder.absRange" + order = 50 + [cancoder.sensorDirection] + type = "cancoder.sensorDirection" + order = 60 + + + # [dio] # type = "menu" # order = 20 diff --git a/src/main/resources/phoenix6.toml b/src/main/resources/phoenix6.toml index 31c6c2a..d2a8d80 100644 --- a/src/main/resources/phoenix6.toml +++ b/src/main/resources/phoenix6.toml @@ -74,6 +74,16 @@ type = "DOUBLE" help = "This is the target jerk (acceleration derivative) Motion Magic® based control modes are allowed to use. This allows Motion Magic® support of S-Curves. If this is set to zero, then Motion Magic® will not apply a Jerk limit. Units: rot/s^3" range = [0.0, 9999.0] +[MM_EXPO_KA] + name = "Motion Magic Expo kA" + type = "DOUBLE" + help = "This is the target kA used only by Motion Magic® Expo control modes, in units of V/rps². This represents the amount of voltage necessary to achieve an acceleration. In terms of the Motion Magic® Expo profile, a higher kA results in a slower acceleration. A kA of 0 will be promoted to a reasonable default of 0.1." + range = [1e-5, 100.0] +[MM_EXPO_KV] + name = "Motion Magic Expo kV" + type = "DOUBLE" + help = "This is the target kV used only by Motion Magic® Expo control modes, in units of V/rps. This represents the amount of voltage necessary to hold a velocity. In terms of the Motion Magic® Expo profile, a higher kV results in a slower maximum velocity. A kV of 0 will be promoted to a reasonable default of 0.12." + range = [0.001, 100] #Differential Configs [PEAK_DIFF_DC] @@ -284,6 +294,10 @@ help = "If true, the TalonFX will beep during configuration API calls if device is disabled. This is useful for general debugging, and defaults to true. Note that if the rotor is moving, the beep will not occur regardless of this setting." #Saved Setpoints +[POSITION] + name = "Position" + type = "DOUBLE" + help = "Sets the mechanism position of the device in mechanism rotations." [VELOCITY] name = "Velocity" type = "DOUBLE" diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index a050a90..d106902 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "24.0.0", + "version": "24.0.2", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "24.0.0" + "version": "24.0.2" } ], "jniDependencies": [], From 657036f9b489f4364779f113ead86f0b5bc796b0 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sat, 10 Feb 2024 13:38:06 -0500 Subject: [PATCH 4/7] fix duplicate grapher entries --- .../org/strykeforce/thirdcoast/device/TalonFxService.kt | 6 ++++++ .../org/strykeforce/thirdcoast/device/TalonService.kt | 6 ++++++ src/main/resources/phoenix6.toml | 2 +- vendordeps/thirdcoast.json | 4 ++-- 4 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt index 8a9fcdd..867f8aa 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt @@ -191,10 +191,16 @@ class TalonFxService( override fun activate(ids: Collection): Set { dirty = true + logger.info { "Number Active: ${active.size}" } + active.forEach{ + logger.info { "Active TalonFX: ${it.deviceID}" } + } val new = super.activate(ids) + logger.info { "Number New: ${new.size}" } telemetryService.stop() active.filter { new.contains(it.deviceID) }.forEach{ + logger.info { "New TalonFX: ${it.deviceID}" } activeSlotIndex = it.closedLoopSlot.value telemetryService.register(it,true) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt index 4755976..a8d6d70 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt @@ -85,10 +85,16 @@ class TalonService(private val telemetryService: TelemetryService, factory: (id: override fun activate(ids: Collection): Set { dirty = true + logger.info { "Number Active: ${active.size}" } + active.forEach{ + logger.info { "Active TalonFX: ${it.deviceID}" } + } val new = super.activate(ids) + logger.info { "Number New: ${new.size}" } telemetryService.stop() active.filter { new.contains(it.deviceID) }.forEach { + logger.info { "New TalonFX: ${it.deviceID}" } it.setNeutralMode(neutralMode) it.selectProfileSlot(activeSlotIndex, 0) it.enableVoltageCompensation(voltageCompensation) diff --git a/src/main/resources/phoenix6.toml b/src/main/resources/phoenix6.toml index d2a8d80..91996c6 100644 --- a/src/main/resources/phoenix6.toml +++ b/src/main/resources/phoenix6.toml @@ -83,7 +83,7 @@ name = "Motion Magic Expo kV" type = "DOUBLE" help = "This is the target kV used only by Motion Magic® Expo control modes, in units of V/rps. This represents the amount of voltage necessary to hold a velocity. In terms of the Motion Magic® Expo profile, a higher kV results in a slower maximum velocity. A kV of 0 will be promoted to a reasonable default of 0.12." - range = [0.001, 100] + range = [0.001, 100.0] #Differential Configs [PEAK_DIFF_DC] diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index d106902..308dddd 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "24.0.2", + "version": "24.0.3", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "24.0.2" + "version": "24.0.3" } ], "jniDependencies": [], From 8453f40c3c67bd3948f75feeab2c1c4047b58e8e Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 18 Feb 2024 18:55:32 -0500 Subject: [PATCH 5/7] allow non percent units on closed-loop percent modes --- .../org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt index 9474ff8..3b3c42c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -87,7 +87,7 @@ class P6RunCommand( var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false,limFwdMotion,limRevMotion) //sanity checks - if (units == Units.PERCENT && !(-1.0..1.0).contains(setpoint)) { + if (units == Units.PERCENT && setpointType == SetpointType.OPEN_LOOP && !(-1.0..1.0).contains(setpoint)) { terminal.warn("setpoint must be in range -1.0 to 1.0 for percent modes") continue } From 0fedf95102a7d10e136500911d0b5cc99207f347 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Tue, 12 Mar 2024 22:34:41 -0400 Subject: [PATCH 6/7] add canivore options to tct --- build.gradle | 2 +- .../kotlin/org/strykeforce/thirdcoast/Koin.kt | 8 +- .../cancoder/CancoderParameterCommand.kt | 56 +- .../cancoder/CancoderStatusCommand.kt | 22 +- .../cancoder/SelectAbsRangeValueCommand.kt | 20 +- .../cancoder/SelectCancoderCommand.kt | 15 +- .../SelectCancoderSensorDirectionCommand.kt | 20 +- .../strykeforce/thirdcoast/command/Command.kt | 2 + .../thirdcoast/device/CancoderFDService.kt | 38 + .../thirdcoast/device/DeviceService.kt | 13 + .../thirdcoast/device/TalonFxFDService.kt | 188 + .../thirdcoast/device/TalonFxService.kt | 8 +- .../phoenix6/P6DefaultStatusFrameCommand.kt | 135 +- .../talon/phoenix6/P6FactoryDefaultCommand.kt | 13 +- .../talon/phoenix6/P6HardLimitCommand.kt | 79 +- .../talon/phoenix6/P6ModeStatusCommand.kt | 11 +- .../talon/phoenix6/P6ModeStatusMenuCommand.kt | 10 +- .../thirdcoast/talon/phoenix6/P6RunCommand.kt | 63 +- ...P6SelectDifferentialSensorSourceCommand.kt | 20 +- .../P6SelectDifferentialTypeCommand.kt | 17 +- .../phoenix6/P6SelectFeedbackSensorCommand.kt | 29 +- .../phoenix6/P6SelectFollowerTypeCommand.kt | 16 +- .../phoenix6/P6SelectGravityTypeCommand.kt | 65 +- .../talon/phoenix6/P6SelectModeCommand.kt | 16 +- .../P6SelectMotionMagicTypeCommand.kt | 15 +- .../phoenix6/P6SelectMotorInvertCommand.kt | 21 +- .../phoenix6/P6SelectNeutralModeCommand.kt | 20 +- .../phoenix6/P6SelectNeutralOutputCommand.kt | 15 +- .../talon/phoenix6/P6SelectSlotCommand.kt | 16 +- .../talon/phoenix6/P6SelectTalonsCommand.kt | 17 +- .../talon/phoenix6/P6SelectUnitCommand.kt | 16 +- .../talon/phoenix6/P6TalonStatusCommand.kt | 23 +- .../phoenix6/Phoenix6ParameterCommand.kt | 179 +- src/main/resources/commands.toml | 4118 ++++++++++------- .../strykeforce/thirdcoast/CheckTomlTest.kt | 2 +- vendordeps/Phoenix6.json | 48 +- 36 files changed, 3451 insertions(+), 1905 deletions(-) create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderFDService.kt create mode 100644 src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxFDService.kt diff --git a/build.gradle b/build.gradle index 234275a..899ab51 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" id "org.jetbrains.kotlin.jvm" version "1.7.21" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" } version = "24.0.0" diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index d36dbba..c14f775 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -46,7 +46,9 @@ val tctModule = module { single { LegacyTalonFxService(get()) { id -> TalonFX(id) } } - single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id)} } + single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id, "rio")} } + + single {TalonFxFDService(get()) {id -> com.ctre.phoenix6.hardware.TalonFX(id, "*")} } single { ServoService { id -> Servo(id) } } @@ -56,7 +58,9 @@ val tctModule = module { single { CanifierService(get()) { id -> CANifier(id) } } - single { CancoderService(get()) {id -> CANcoder(id)} } + single { CancoderService(get()) {id -> CANcoder(id, "rio")} } + + single { CancoderFDService(get()) {id -> CANcoder(id, "*")} } single { PigeonService(get()) { id -> PigeonIMU(id) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt index 31eb449..208c442 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt @@ -10,6 +10,7 @@ import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.device.CancoderService import org.strykeforce.thirdcoast.cancoder.CancoderParameter.Enum.* import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 +import org.strykeforce.thirdcoast.device.CancoderFDService private val logger = KotlinLogging.logger { } class CancoderParameterCommand( @@ -19,19 +20,26 @@ class CancoderParameterCommand( ): AbstractCommand(parent, key, toml) { private val cancoderService: CancoderService by inject() + private val cancoderFDService: CancoderFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") private val timeout = cancoderService.timeout private val param = CancoderParameter.create(this, toml.getString("param")?: "UNKNOWN") override val menu: String get() { return when (param.enum) { - POSITION -> formatMenu(cancoderService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0, DOUBLE_FORMAT_4) - MAG_OFFSET -> formatMenu(cancoderService.activeConfiguration.MagnetSensor.MagnetOffset) + POSITION -> formatMenu( + if(bus == "rio") cancoderService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0 + else cancoderFDService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0, DOUBLE_FORMAT_4) + MAG_OFFSET -> formatMenu( + if(bus == "rio") cancoderService.activeConfiguration.MagnetSensor.MagnetOffset + else cancoderFDService.activeConfiguration.MagnetSensor.MagnetOffset) } } override fun execute(): Command { - var activeConfig = cancoderService.activeConfiguration + var activeConfig = if(bus == "rio") cancoderService.activeConfiguration else cancoderFDService.activeConfiguration when(param.enum) { POSITION -> configDoubleParam(cancoderService.active.firstOrNull()?.position?.valueAsDouble ?: 0.0) { cancoder, value -> @@ -50,25 +58,43 @@ class CancoderParameterCommand( private fun configDoubleParam(default: Double, config: (CANcoder, Double) -> Unit) { val paramValue = param.readDouble(reader, default) - cancoderService.active.forEach { - config(it, paramValue) - logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } - } + if(bus == "rio") { + cancoderService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } else if(bus == "canivore") { + cancoderFDService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderFDService.active.size} CANcoder's ${param.name}: $paramValue" }} + } else throw IllegalArgumentException() } private fun configIntParameter(default: Int, config: (CANcoder, Int) -> Unit) { val paramValue = param.readInt(reader, default) - cancoderService.active.forEach{ - config(it, paramValue) - logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } - } + if(bus == "rio") { + cancoderService.active.forEach{ + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } else if(bus == "canivore") { + cancoderFDService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderFDService.active.size} CANcoder's ${param.name}: $paramValue" }} + } else throw IllegalArgumentException() } private fun configBooleanParam(default: Boolean, config: (CANcoder, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) - cancoderService.active.forEach { - config(it, paramValue) - logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } - } + if(bus == "rio") { + cancoderService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } else if(bus == "canivore") { + cancoderFDService.active.forEach { + config(it, paramValue) + logger.info { "set ${cancoderFDService.active.size} CANcoder's ${param.name}: $paramValue" }} + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt index 6e998d8..d5f7b14 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderStatusCommand.kt @@ -5,6 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderFDService import org.strykeforce.thirdcoast.device.CancoderService class CancoderStatusCommand( @@ -14,14 +15,25 @@ class CancoderStatusCommand( ): AbstractCommand(parent, key, toml) { private val cancoderService: CancoderService by inject() + private val cancoderFDService: CancoderFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override fun execute(): Command { val writer = terminal.writer() - cancoderService.active.forEach{ - val config = CANcoderConfiguration() - it.configurator.refresh(config) - writer.println(config.toString()) - } + if(bus == "rio") { + cancoderService.active.forEach{ + val config = CANcoderConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + } else if(bus == "canivore") { + cancoderFDService.active.forEach { + val config = CANcoderConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + } else throw IllegalArgumentException() return super.execute() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt index 8ad984b..833d238 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderFDService import org.strykeforce.thirdcoast.device.CancoderService private val RANGE_VALUE = listOf( @@ -24,13 +25,26 @@ class SelectAbsRangeValueCommand( toml: TomlTable ): AbstractSelectCommand(parent, key, toml, RANGE_VALUE, LABELS) { private val cancoderService: CancoderService by inject() + private val cancoderFDService: CancoderFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override val activeIndex: Int - get() = cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal + get() { + if(bus == "rio") return cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal + else if(bus == "canivore") return cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val range = values[index] - cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range - cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + if(bus == "rio") { + cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range + cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + } else if(bus == "caniover") { + cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range + cancoderFDService.active.forEach { it.configurator.apply(cancoderFDService.activeConfiguration.MagnetSensor) } + } } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt index 42f2441..348c3d4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderCommand.kt @@ -6,6 +6,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.CancoderFDService import org.strykeforce.thirdcoast.device.CancoderService import org.strykeforce.thirdcoast.readInt import org.strykeforce.thirdcoast.warn @@ -16,15 +17,25 @@ class SelectCancoderCommand( toml: TomlTable ): AbstractCommand(parent, key, toml) { private val cancoderService: CancoderService by inject() + private val cancoderFDService: CancoderFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override val menu: String - get() = formatMenu(cancoderService.active.map(CANcoder::getDeviceID).joinToString()) + get() = formatMenu( + if(bus == "rio") cancoderService.active.map(CANcoder::getDeviceID).joinToString() + else cancoderFDService.active.map(CANcoder::getDeviceID).joinToString() + ) override fun execute(): Command { while(true) { try { val id = reader.readInt(this.prompt("id")) - cancoderService.activate(listOf(id)) + if(bus == "rio") { + cancoderService.activate(listOf(id)) + } else if(bus == "canivore") { + cancoderFDService.activate(listOf(id)) + } else throw IllegalArgumentException() return super.execute() } catch (e: IllegalArgumentException) { terminal.warn("Please enter a CANcoder id") diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt index 4c03a40..3cc3acb 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.CancoderFDService import org.strykeforce.thirdcoast.device.CancoderService private val DIRECTION = listOf( @@ -25,13 +26,26 @@ class SelectCancoderSensorDirectionCommand( ): AbstractSelectCommand(parent, key, toml, DIRECTION, LABELS) { private val cancoderService: CancoderService by inject() + private val cancoderFDService: CancoderFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override val activeIndex: Int - get() = cancoderService.activeConfiguration.MagnetSensor.SensorDirection.ordinal + get() { + if(bus == "rio") return cancoderService.activeConfiguration.MagnetSensor.SensorDirection.ordinal + else if(bus == "canivore") return cancoderFDService.activeConfiguration.MagnetSensor.SensorDirection.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val direction = values[index] - cancoderService.activeConfiguration.MagnetSensor.SensorDirection = direction - cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + if(bus == "rio") { + cancoderService.activeConfiguration.MagnetSensor.SensorDirection = direction + cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} + } else if(bus == "canivore") { + cancoderFDService.activeConfiguration.MagnetSensor.SensorDirection = direction + cancoderFDService.active.forEach { it.configurator.apply(cancoderFDService.activeConfiguration.MagnetSensor) } + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt index b20d2c4..bc38b06 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt @@ -40,6 +40,7 @@ interface Command { const val TYPE_KEY = "type" const val ORDER_KEY = "order" const val DEVICE_KEY = "device" + const val BUS_KEY = "bus" fun createFromToml(toml: TomlTable, parent: MenuCommand? = null, key: String = "ROOT"): Command { //logger.info{"key: $key, toml: ${toml.keySet()}"} @@ -94,6 +95,7 @@ interface Command { "p6.modeMenu" -> createModeMenuCommand(parent, key, toml) "p6.mode" -> P6SelectModeCommand(parent, key, toml) "p6.param" -> Phoenix6ParameterCommand(parent, key, toml) + "p6.feedback" -> P6SelectFeedbackSensorCommand(parent, key, toml) "p6.mmType" -> P6SelectMotionMagicTypeCommand(parent, key, toml) "p6.diffType" -> P6SelectDifferentialTypeCommand(parent, key, toml) "p6.followType" -> P6SelectFollowerTypeCommand(parent, key, toml) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderFDService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderFDService.kt new file mode 100644 index 0000000..8e44ac9 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/CancoderFDService.kt @@ -0,0 +1,38 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.hardware.CANcoder +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private val logger = KotlinLogging.logger {} + + +class CancoderFDService( + private val telemetryService: TelemetryService, factory: (id: Int) -> CANcoder +): AbstractDeviceService (factory) { + val timeout = 10.0 + var dirty = true + + var activeConfiguration = CANcoderConfiguration() + get() { + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?: logger.debug("no active CANcoder's, returning default config") + dirty = false + return field + } + + override fun activate(ids: Collection): Set { + dirty = true + + val new = super.activate(ids) + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach{ + telemetryService.register(it) + } + telemetryService.start() + active.firstOrNull() + return new + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt index 4447a92..a650404 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt @@ -36,6 +36,12 @@ open class AbstractDeviceService(private val factory: (id: Int) -> T) : return device } + fun get(id: Int, bus: String): T { + val device = _all[id] ?: factory(id) + if(_all.put(id, device) == null) logger.debug("_all add: {}", device) + return device + } + override fun activate(ids: Collection): Set { val new = ids.toSet().minus(_active.keys) _active.clear() @@ -43,6 +49,13 @@ open class AbstractDeviceService(private val factory: (id: Int) -> T) : return new } + open fun activate(ids: Collection, bus: String): Set { + val new = ids.toSet().minus(_active.keys) + _active.clear() + ids.associateTo(_active) { id -> id to get(id,bus)} + return new + } + fun idsInAll(ids: Collection): Set = _all.keys.intersect(ids) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxFDService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxFDService.kt new file mode 100644 index 0000000..6f96bce --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxFDService.kt @@ -0,0 +1,188 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.ControlModeValue +import com.ctre.phoenix6.signals.NeutralModeValue +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private val logger = KotlinLogging.logger {} + +private val CONTROL_MODE_DEFAULT = ControlModeValue.DutyCycleOut +private const val ACTIVE_SLOT_DEFAULT = 0 +private val NEUTRAL_MODE_DEFAULT = NeutralModeValue.Coast +private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true +private const val CURRENT_LIMIT_ENABLED_DEFAULT = false +private const val SENSOR_PHASE_INVERTED_DEFAULT = false +private const val OUTPUT_INVERTED_DEFAULT = false +class TalonFxFDService( + private val telemetryService: TelemetryService, + factory: (id: Int) -> TalonFX +) : AbstractDeviceService(factory) { + + var dirty = true + //var neutralMode = NEUTRAL_MODE_DEFAULT + //var controlMode = CONTROL_MODE_DEFAULT + // var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT + //var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT + var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT + var activeUnits: Units = Units.PERCENT + var active_MM_type: MM_Type = MM_Type.STANDARD + var activeFeedForward: Double = 0.0 + var activeTorqueCurrentDeadband: Double = 0.0 + var activeTorqueCurrentMaxOut: Double = 1.0 + var activeVelocity: Double = 0.0 + var activeAcceleration: Double = 0.0 + var activeJerk: Double = 0.0 + var activeDifferentialTarget: Double = 0.0 + var activeFollowerType: FollowerType = FollowerType.STRICT + var activeOpposeMain: Boolean = false + var activeDifferentialSlot: Int = 0 + var activeFOC: Boolean = false + var setpointType: SetpointType = SetpointType.OPEN_LOOP + var differentialType: DifferentialType = DifferentialType.OPEN_LOOP + var activeNeutralOut: NeutralModeValue = NeutralModeValue.Coast + var activeOverrideNeutral: Boolean = false + var limFwdMotion : Boolean = false; + var limRevMotion : Boolean = false; + var grapherStatusFrameHz : Double = 20.0; + + var controlMode: String = "Duty Cycle Out" + get() { + when(setpointType){ + SetpointType.OPEN_LOOP -> { + when(activeUnits){ + Units.PERCENT -> return "Duty Cycle Out" + Units.VOLTAGE -> return "Voltage Out" + Units.TORQUE_CURRENT -> return "(pro) Torque Current FOC" + } + } + SetpointType.POSITION -> { + when(activeUnits){ + Units.PERCENT -> return "Position: Duty Cycle" + Units.VOLTAGE -> return "Position: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Position: Torque Current FOC" + } + } + SetpointType.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Velocity: Duty Cycle" + Units.VOLTAGE -> return "Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Velocity: Torque Current FOC" + } + } + SetpointType.MOTION_MAGIC -> { + when(active_MM_type) { + MM_Type.STANDARD -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic: Torque Current FOC" + } + } MM_Type.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Velocity: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Velocity: Torque Current FOC" + } + } MM_Type.DYNAMIC -> { + when(activeUnits) { + Units.PERCENT -> return "Dynamic Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Dynamic Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Dynamic Motion Magic: Torque Current FOC" + } + } MM_Type.EXPONENTIAL -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Exponential: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Exponential: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Exponential: Torque Current FOC" + } + } + } + } + SetpointType.DIFFERENTIAL -> { + when(differentialType) { + DifferentialType.OPEN_LOOP -> { + when(activeUnits) { + Units.PERCENT -> return "Differential: Duty Cycle" + Units.VOLTAGE -> return "Differential: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.POSITION -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Position: Duty Cycle" + Units.VOLTAGE -> return "Differential Position: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.VELOCITY -> { + when(activeUnits){ + Units.PERCENT -> return "Differential Velocity: Duty Cycle" + Units.VOLTAGE -> return "Differential Velocity: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.MOTION_MAGIC -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Differential Motion Magic: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Differential Follower: Non-Strict" + FollowerType.STRICT -> return "Differential Follower: Strict" + } + } + } + } + SetpointType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Follower: Non-Strict" + FollowerType.STRICT -> return "Follower: Strict" + } + } + SetpointType.NEUTRAL -> { + when(activeNeutralOut){ + NeutralModeValue.Brake -> return "Brake Mode" + NeutralModeValue.Coast -> return "Coast Mode" + } + } + SetpointType.MUSIC -> { + return "Music Tone" + + } + } + } + + + var activeConfiguration = TalonFXConfiguration() + get() { + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?:logger.debug("no active talon fx's, returning default config") + dirty = false + return field + } + + override fun activate(ids: Collection): Set { + dirty = true + logger.info { "Number Active: ${active.size}" } + active.forEach{ + logger.info { "Active TalonFX: ${it.deviceID}" } + } + + val new = super.activate(ids) + logger.info { "Number New: ${new.size}" } + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach{ + logger.info { "New TalonFX: ${it.deviceID}" } + activeSlotIndex = it.closedLoopSlot.value + telemetryService.register(it,true) + } + + + telemetryService.start() + active.firstOrNull() + return new + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt index 867f8aa..754a9f8 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt @@ -39,7 +39,7 @@ enum class DifferentialType { } -class TalonFxService( +open class TalonFxService( private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ) : AbstractDeviceService(factory) { @@ -70,6 +70,7 @@ class TalonFxService( var limFwdMotion : Boolean = false; var limRevMotion : Boolean = false; var grapherStatusFrameHz : Double = 20.0; + var isRioBus : Boolean = true; var controlMode: String = "Duty Cycle Out" get() { @@ -188,15 +189,14 @@ class TalonFxService( return field } - - override fun activate(ids: Collection): Set { + override fun activate(ids: Collection, bus: String): Set { dirty = true logger.info { "Number Active: ${active.size}" } active.forEach{ logger.info { "Active TalonFX: ${it.deviceID}" } } - val new = super.activate(ids) + val new = super.activate(ids, bus) logger.info { "Number New: ${new.size}" } telemetryService.stop() active.filter { new.contains(it.deviceID) }.forEach{ diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt index 285295d..9099f94 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt @@ -4,6 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6DefaultStatusFrameCommand( @@ -13,53 +14,103 @@ class P6DefaultStatusFrameCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override fun execute(): Command { val timeout = talonFxService.timeout val value = 10.0 - talonFxService.active.forEach { - talonFxService.grapherStatusFrameHz = 0.0; - it.acceleration.setUpdateFrequency(50.0, timeout) - it.bridgeOutput.setUpdateFrequency(100.0, timeout) - it.deviceTemp.setUpdateFrequency(4.0, timeout) - it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) - it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) - it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) - it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) - it.dutyCycle.setUpdateFrequency(100.0, timeout) - it.forwardLimit.setUpdateFrequency(100.0, timeout) - it.reverseLimit.setUpdateFrequency(100.0, timeout) - it.isProLicensed.setUpdateFrequency(4.0, timeout) - it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) - it.motorVoltage.setUpdateFrequency(100.0, timeout) - it.position.setUpdateFrequency(50.0, timeout) - it.processorTemp.setUpdateFrequency(4.0, timeout) - it.rotorPosition.setUpdateFrequency(4.0, timeout) - it.rotorVelocity.setUpdateFrequency(4.0, timeout) - it.statorCurrent.setUpdateFrequency(4.0, timeout) - it.supplyCurrent.setUpdateFrequency(4.0, timeout) - it.supplyVoltage.setUpdateFrequency(4.0, timeout) - it.torqueCurrent.setUpdateFrequency(100.0, timeout) - it.velocity.setUpdateFrequency(50.0, timeout) + if(bus == "rio") { + talonFxService.active.forEach { + talonFxService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) + } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { + talonFxFDService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) - it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopError.setUpdateFrequency(4.0, timeout) - it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) - it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopReference.setUpdateFrequency(4.0, timeout) - it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) - it.closedLoopSlot.setUpdateFrequency(4.0, timeout) - it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) + } } return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt index f32474a..c262b87 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt @@ -5,6 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6FactoryDefaultCommand( @@ -14,11 +15,19 @@ class P6FactoryDefaultCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFdService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") override fun execute(): Command { val config = TalonFXConfiguration() - talonFxService.activeConfiguration = config - talonFxService.active.forEach { it.configurator.apply(config) } + if(bus == "rio") { + talonFxService.activeConfiguration = config + talonFxService.active.forEach { it.configurator.apply(config) } + } else if(bus == "canivore") { + talonFxFdService.activeConfiguration = config + talonFxFdService.active.forEach { it.configurator.apply(config)} + } else throw IllegalArgumentException() return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt index 8910062..eb3ed1a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt @@ -10,6 +10,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectFwdHardLimitSourceCommand( @@ -21,14 +22,27 @@ class P6SelectFwdHardLimitSourceCommand( listOf("Talon FX pin", "Remote TalonFx", "Remote CANifier")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val source = values[index] - talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} + } else throw IllegalArgumentException() } } @@ -41,14 +55,28 @@ class P6SelectRevHardLimitSourceCommand( listOf("Talon FX pin", "Remote TalonFX", "Remote CANifier")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val source = values[index] - talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } else throw IllegalArgumentException() } } @@ -62,14 +90,27 @@ class P6SelectFwdHardLimitNormalCommand( listOf("Normally Open", "Normally Closed")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val type = values[index] - talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if (bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } else throw IllegalArgumentException() } } @@ -82,14 +123,28 @@ class P6SelectRevHardLimitNormalCommand( listOf("Normally Open", "Normally Closed")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val type = values[index] - talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt index bbbefe8..b431bd4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt @@ -4,6 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6ModeStatusCommand( @@ -13,9 +14,17 @@ class P6ModeStatusCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val menu: String - get() = formatMenu(talonFxService.controlMode) + get() { + if(bus == "rio") return formatMenu(talonFxService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) + else throw IllegalArgumentException() + } override fun execute(): Command { //val writer = terminal.writer() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt index d75920d..19aa83e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt @@ -5,6 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.* import org.strykeforce.thirdcoast.command.* +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.invalidMenuChoice import java.util.* @@ -17,13 +18,20 @@ class P6ModeStatusMenuCommand( toml: TomlTable ): MenuCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val children = TreeSet(compareBy(Command::order, Command::key)) override val menu: String get() { logger.info { "Getting Menu Option..." } - return formatMenu(talonFxService.controlMode) + if(bus == "rio") return formatMenu(talonFxService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) + else throw IllegalArgumentException() + } override fun execute(): Command { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt index 3b3c42c..cf4e40c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -58,6 +58,10 @@ class P6RunCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override fun execute(): Command { var done = false @@ -68,23 +72,25 @@ class P6RunCommand( val setpoints = line.split(',') val setpoint = setpoints[0].toDouble() val duration = if (setpoints.size > 1) setpoints[1].toDouble() else 0.0 - val setpointType = talonFxService.setpointType - val units = talonFxService.activeUnits - val motionType = talonFxService.active_MM_type - val differentialType = talonFxService.differentialType - val followerType = talonFxService.activeFollowerType - val enableFOC = talonFxService.activeFOC - val overrideNeutral = talonFxService.activeOverrideNeutral - val limFwdMotion = talonFxService.limFwdMotion - val limRevMotion = talonFxService.limRevMotion - val velocity = talonFxService.activeVelocity - val acceleration = talonFxService.activeAcceleration - val jerk = talonFxService.activeJerk - val feedFwd = talonFxService.activeFeedForward - val slot = talonFxService.activeSlotIndex - val diffSlot = talonFxService.activeDifferentialSlot - val diffPos = talonFxService.activeDifferentialTarget + val setpointType = if(bus == "rio") talonFxService.setpointType else talonFxFDService.setpointType + val units = if(bus == "rio") talonFxService.activeUnits else talonFxFDService.activeUnits + val motionType = if(bus == "rio") talonFxService.active_MM_type else talonFxFDService.active_MM_type + val differentialType = if(bus == "rio") talonFxService.differentialType else talonFxFDService.differentialType + val followerType = if(bus == "rio") talonFxService.activeFollowerType else talonFxFDService.activeFollowerType + val enableFOC = if(bus == "rio") talonFxService.activeFOC else talonFxFDService.activeFOC + val overrideNeutral = if(bus == "rio") talonFxService.activeOverrideNeutral else talonFxFDService.activeOverrideNeutral + val limFwdMotion = if(bus == "rio") talonFxService.limFwdMotion else talonFxFDService.limFwdMotion + val limRevMotion = if(bus == "rio") talonFxService.limRevMotion else talonFxFDService.limRevMotion + val velocity = if(bus == "rio") talonFxService.activeVelocity else talonFxFDService.activeVelocity + val acceleration = if(bus == "rio") talonFxService.activeAcceleration else talonFxFDService.activeAcceleration + val jerk = if(bus == "rio") talonFxService.activeJerk else talonFxFDService.activeJerk + val feedFwd = if(bus == "rio") talonFxService.activeFeedForward else talonFxFDService.activeFeedForward + val slot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex + val diffSlot = if(bus == "rio") talonFxService.activeDifferentialSlot else talonFxFDService.activeDifferentialSlot + val diffPos = if(bus == "rio") talonFxService.activeDifferentialTarget else talonFxFDService.activeDifferentialTarget var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false,limFwdMotion,limRevMotion) + val torqueCurrentMaxOut = if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut else talonFxFDService.activeTorqueCurrentMaxOut + val torqueCurrentDeadband = if(bus == "rio") talonFxService.activeTorqueCurrentDeadband else talonFxFDService.activeTorqueCurrentDeadband //sanity checks if (units == Units.PERCENT && setpointType == SetpointType.OPEN_LOOP && !(-1.0..1.0).contains(setpoint)) { @@ -107,8 +113,8 @@ class P6RunCommand( Units.VOLTAGE -> controlRequest = VoltageOut(setpoint, enableFOC, overrideNeutral, limFwdMotion, limRevMotion) Units.TORQUE_CURRENT -> controlRequest = TorqueCurrentFOC( setpoint, - talonFxService.activeTorqueCurrentMaxOut, - talonFxService.activeTorqueCurrentDeadband, + torqueCurrentMaxOut, + torqueCurrentDeadband, overrideNeutral,limFwdMotion,limRevMotion ) } @@ -366,17 +372,28 @@ class P6RunCommand( } //run Talon - talonFxService.active.forEach { - logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } - it.setControl(controlRequest) - } + if(bus == "rio") { + talonFxService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else throw IllegalArgumentException() //Check Timeout if (duration > 0.0) { logger.debug { "run duration = $duration seconds" } Timer.delay(duration) logger.debug { "run duration expired, setting output = 0.0" } - talonFxService.active.forEach { it.set(0.0) } + if(bus == "rio") { + talonFxService.active.forEach { it.set(0.0) } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { it.set(0.0) } + } else throw IllegalArgumentException() } } catch (e: Exception) { done = true diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt index 0e2e7c6..0c984dc 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService private val SENSOR = listOf( @@ -33,13 +34,26 @@ class P6SelectDifferentialSensorSourceCommand( ): AbstractSelectCommand(parent, key, toml, SENSOR, LABELS) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else if(bus == "canivoer") return talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val sensor = values[index] - talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivoer") { + talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt index 974134b..4762d14 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt @@ -6,6 +6,7 @@ import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.device.DifferentialType import org.strykeforce.thirdcoast.device.DifferentialType.* +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectDifferentialTypeCommand( @@ -17,12 +18,24 @@ class P6SelectDifferentialTypeCommand( listOf("Open Loop", "Position", "Velocity", "Motion Magic", "Follower")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.differentialType.ordinal + get() { + if(bus == "rio") return talonFxService.differentialType.ordinal + else if(bus == "canivore") return talonFxFDService.differentialType.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val type = values[index] - talonFxService.differentialType = type + if(bus == "rio") { + talonFxService.differentialType = type + } else if(bus == "canivore") { + talonFxFDService.differentialType = type + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt index 94e5aec..ad631f6 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt @@ -6,6 +6,7 @@ import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command import com.ctre.phoenix6.signals.FeedbackSensorSourceValue.* import org.koin.standalone.inject +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService private val SENSORS = listOf( @@ -35,19 +36,35 @@ class P6SelectFeedbackSensorCommand( ): AbstractSelectCommand(parent, key, toml, SENSORS, LABELS) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int get() { - val sensor = talonFxService.activeConfiguration.Feedback.FeedbackSensorSource - return values.indexOf(sensor) + if(bus == "rio") { + val sensor = talonFxService.activeConfiguration.Feedback.FeedbackSensorSource + return values.indexOf(sensor) + } else if(bus == "canivore") { + val sensor = talonFxFDService.activeConfiguration.Feedback.FeedbackSensorSource + return values.indexOf(sensor) + }else throw IllegalArgumentException() } override fun setActive(index: Int) { val sensor = values[index] - talonFxService.activeConfiguration.Feedback.FeedbackSensorSource = sensor - talonFxService.active.forEach { - it.configurator.apply(talonFxService.activeConfiguration) - } + if(bus == "rio") { + talonFxService.activeConfiguration.Feedback.FeedbackSensorSource = sensor + talonFxService.active.forEach { + it.configurator.apply(talonFxService.activeConfiguration) + } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.Feedback.FeedbackSensorSource = sensor + talonFxService.active.forEach { + it.configurator.apply(talonFxFDService.activeConfiguration) + } + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt index 832b3c2..16c28dc 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt @@ -5,6 +5,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.device.FollowerType +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectFollowerTypeCommand( @@ -16,12 +17,23 @@ class P6SelectFollowerTypeCommand( listOf("Strict Follower", "Standard Follower")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeFollowerType.ordinal + get() { + if(bus == "rio") return talonFxService.activeFollowerType.ordinal + else if(bus == "canivore") return talonFxFDService.activeFollowerType.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val type = values[index] - talonFxService.activeFollowerType = type + if(bus == "rio") talonFxService.activeFollowerType = type + else if(bus == "canivore") talonFxFDService.activeFollowerType = type + else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt index d1c92dc..ec681a0 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.warn @@ -26,32 +27,60 @@ class P6SelectGravityTypeCommand( ): AbstractSelectCommand(parent, key, toml, GRAVITY, LABELS) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int get() { - val slot = talonFxService.activeSlotIndex - when(slot){ - 0 -> return talonFxService.activeConfiguration.Slot0.GravityType.ordinal - 1 -> return talonFxService.activeConfiguration.Slot1.GravityType.ordinal - 2 -> return talonFxService.activeConfiguration.Slot2.GravityType.ordinal - else -> { - terminal.warn("slot value ${slot} is invalid") - return 2767 + if(bus == "rio") { + val slot = talonFxService.activeSlotIndex + when(slot){ + 0 -> return talonFxService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } } - } + } else if(bus == "canivore") { + val slot = talonFxFDService.activeSlotIndex + when(slot) { + 0 -> return talonFxFDService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxFDService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxFDService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } else throw IllegalArgumentException() } override fun setActive(index: Int) { val gravity = values[index] - val slot = talonFxService.activeSlotIndex - when(slot) { - 0 -> talonFxService.activeConfiguration.Slot0.GravityType = gravity - 1 -> talonFxService.activeConfiguration.Slot1.GravityType = gravity - 2 -> talonFxService.activeConfiguration.Slot2.GravityType = gravity - else -> terminal.warn("slot value ${slot} is invalid") - } - talonFxService.active.forEach { - it.configurator.apply(talonFxService.activeConfiguration) + if(bus == "rio") { + val slot = talonFxService.activeSlotIndex + when(slot) { + 0 -> talonFxService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxService.active.forEach { + it.configurator.apply(talonFxService.activeConfiguration) + } + } else if(bus == "canivore") { + val slot = talonFxFDService.activeSlotIndex + when(slot) { + 0 -> talonFxFDService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxFDService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxFDService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt index 4133864..40b02bf 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt @@ -6,6 +6,7 @@ import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.device.SetpointType import org.strykeforce.thirdcoast.device.SetpointType.* +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectModeCommand( @@ -17,12 +18,23 @@ class P6SelectModeCommand( listOf("Open Loop", "Position", "Velocity", "Motion Magic", "Differential", "Follower", "Neutral Output", "Music Tone")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.setpointType.ordinal + get() { + if(bus == "rio") return talonFxService.setpointType.ordinal + if(bus == "canivore") return talonFxFDService.setpointType.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val mode = values[index] - talonFxService.setpointType = mode + if(bus == "rio") talonFxService.setpointType = mode + else if(bus == "canivore") talonFxFDService.setpointType = mode + else throw IllegalArgumentException() + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt index 61fdb16..576da76 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt @@ -5,6 +5,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.device.MM_Type +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectMotionMagicTypeCommand( @@ -16,12 +17,22 @@ class P6SelectMotionMagicTypeCommand( listOf("Standard (Position)", "Velocity", "(FD) Dynamic", "Exponential")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.active_MM_type.ordinal + get() { + if(bus == "rio") return talonFxService.active_MM_type.ordinal + else if(bus == "canivore") return talonFxFDService.active_MM_type.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val type = values[index] - talonFxService.active_MM_type = type + if(bus == "rio") talonFxService.active_MM_type = type + else if(bus == "canivore") talonFxFDService.active_MM_type = type + else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt index f7db425..47822de 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectMotorInvertCommand( @@ -17,13 +18,27 @@ class P6SelectMotorInvertCommand( listOf("CCW positive", "CW positive")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.MotorOutput.Inverted.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.Inverted.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.Inverted.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val invert = values[index] - talonFxService.activeConfiguration.MotorOutput.Inverted = invert - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } + if(bus == "rio") { + talonFxService.activeConfiguration.MotorOutput.Inverted = invert + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.MotorOutput.Inverted = invert + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt index 26cc38f..4aa505b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt @@ -6,6 +6,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService @@ -26,13 +27,26 @@ class P6SelectNeutralModeCommand( ): AbstractSelectCommand(parent, key, toml, MODE, LABELS) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeConfiguration.MotorOutput.NeutralMode.ordinal + get() { + if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val neutral = values[index] - talonFxService.activeConfiguration.MotorOutput.NeutralMode = neutral - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + if(bus == "rio") { + talonFxService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt index 4885350..1f1f4d4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt @@ -5,6 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectNeutralOutputCommand( @@ -16,12 +17,22 @@ class P6SelectNeutralOutputCommand( listOf("Coast", "Brake")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeNeutralOut.ordinal + get() { + if(bus == "rio") return talonFxService.activeNeutralOut.ordinal + else if(bus == "canivore") return talonFxFDService.activeNeutralOut.ordinal + else throw IllegalArgumentException() + } override fun setActive(index: Int) { val neutral = values[index] - talonFxService.activeNeutralOut = neutral + if(bus == "rio") talonFxService.activeNeutralOut = neutral + else if(bus == "canivore") talonFxFDService.activeNeutralOut = neutral + else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt index a461896..eacc751 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt @@ -4,6 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService private val SLOTS = listOf(0,1,2) @@ -15,11 +16,22 @@ class P6SelectSlotCommand( ): AbstractSelectCommand(parent, key, toml, SLOTS, SLOTS.map(Int::toString)) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeSlotIndex + get() { + if(bus == "rio") return talonFxService.activeSlotIndex + else if(bus == "canivore") return talonFxFDService.activeSlotIndex + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { - talonFxService.activeSlotIndex = index + if(bus == "rio") talonFxService.activeSlotIndex = index + else if(bus == "canivore") talonFxFDService.activeSlotIndex = index + else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt index 324236f..f7b29c1 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt @@ -6,6 +6,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.info import org.strykeforce.thirdcoast.readIntList @@ -18,9 +19,17 @@ class P6SelectTalonsCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val menu: String - get() = formatMenu(talonFxService.active.map(TalonFX::getDeviceID).joinToString()) + get() = formatMenu( + if(bus == "rio") talonFxService.active.map(TalonFX::getDeviceID).joinToString() + else talonFxFDService.active.map(TalonFX::getDeviceID).joinToString() + ) + override fun execute(): Command { while(true){ @@ -29,7 +38,11 @@ class P6SelectTalonsCommand( var new: Set ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) - new = talonFxService.activate(ids) + + new = + if(bus == "rio") talonFxService.activate(ids) + else if(bus == "canivore") talonFxFDService.activate(ids) + else throw IllegalArgumentException() if(new.isNotEmpty()) { terminal.info("Activating talons: ${new.joinToString()}") diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt index 0966f86..a472fe7 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt @@ -4,6 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.device.Units @@ -16,12 +17,23 @@ class P6SelectUnitCommand( listOf("Percent", "Voltage", "(Pro) TorqueCurrent")) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override val activeIndex: Int - get() = talonFxService.activeUnits.ordinal + get() { + if(bus == "rio") return talonFxService.activeUnits.ordinal + else if(bus == "canivore") return talonFxFDService.activeUnits.ordinal + else throw IllegalArgumentException() + + } override fun setActive(index: Int) { val units = values[index] - talonFxService.activeUnits = units + if(bus == "rio") talonFxService.activeUnits = units + else if(bus == "canivore") talonFxFDService.activeUnits = units + else throw IllegalArgumentException() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt index fe71960..83a4bba 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt @@ -5,6 +5,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService class P6TalonStatusCommand( @@ -14,14 +15,26 @@ class P6TalonStatusCommand( ): AbstractCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + override fun execute(): Command { val writer = terminal.writer() - talonFxService.active.forEach { - var config = TalonFXConfiguration() - it.configurator.refresh(config) - writer.println(config.toString()) - } + if(bus == "rio") { + talonFxService.active.forEach { + var config = TalonFXConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else if(bus == "canivore") { + talonFxFDService.active.forEach { + var config = TalonFXConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else throw IllegalArgumentException() return super.execute() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt index 7a2c2d9..9f40e18 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt @@ -8,6 +8,7 @@ import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 +import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService import org.strykeforce.thirdcoast.talon.phoenix6.Phoenix6Parameter.P6Enum.* import org.strykeforce.thirdcoast.warn @@ -23,13 +24,18 @@ class Phoenix6ParameterCommand( private val talonFxService : TalonFxService by inject() + private val talonFxFDService: TalonFxFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + //val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonFxService.timeout private val param = Phoenix6Parameter.create(this, toml.getString("param") ?: "UNKNOWN") override val menu: String get() { - val config = talonFxService.activeConfiguration + val config = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration + val activeSlot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex logger.info { "Current Param: ${param.name}, ${param.prompt}" } return when(param.enum) { @@ -39,58 +45,58 @@ class Phoenix6ParameterCommand( REMOTE_SENSOR_ID -> formatMenu(config.Feedback.FeedbackRemoteSensorID) SLOT_KP -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kP - 1 -> talonFxService.activeConfiguration.Slot1.kP - 2 -> talonFxService.activeConfiguration.Slot2.kP + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kP else talonFxFDService.activeConfiguration.Slot0.kP + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kP else talonFxFDService.activeConfiguration.Slot1.kP + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kP else talonFxFDService.activeConfiguration.Slot2.kP else -> SlotConfigs().kP } ) SLOT_KI -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kI - 1 -> talonFxService.activeConfiguration.Slot1.kI - 2 -> talonFxService.activeConfiguration.Slot2.kI + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kI else talonFxFDService.activeConfiguration.Slot0.kI + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kI else talonFxFDService.activeConfiguration.Slot1.kI + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kI else talonFxFDService.activeConfiguration.Slot2.kI else -> SlotConfigs().kI } ) SLOT_KD -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kD - 1 -> talonFxService.activeConfiguration.Slot1.kD - 2 -> talonFxService.activeConfiguration.Slot2.kD + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kD else talonFxFDService.activeConfiguration.Slot0.kD + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kD else talonFxFDService.activeConfiguration.Slot1.kD + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kD else talonFxFDService.activeConfiguration.Slot2.kD else -> SlotConfigs().kD } ) SLOT_KS -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kS - 1 -> talonFxService.activeConfiguration.Slot1.kS - 2 -> talonFxService.activeConfiguration.Slot2.kS + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kS else talonFxFDService.activeConfiguration.Slot0.kS + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kS else talonFxFDService.activeConfiguration.Slot1.kS + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kS else talonFxFDService.activeConfiguration.Slot2.kS else -> SlotConfigs().kS } ) SLOT_KV -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kV - 1 -> talonFxService.activeConfiguration.Slot1.kV - 2 -> talonFxService.activeConfiguration.Slot2.kV + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kV else talonFxFDService.activeConfiguration.Slot0.kV + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kV else talonFxFDService.activeConfiguration.Slot1.kV + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kV else talonFxFDService.activeConfiguration.Slot2.kV else -> SlotConfigs().kV } ) SLOT_KA -> formatMenu( - when(talonFxService.activeSlotIndex){ - 0 -> talonFxService.activeConfiguration.Slot0.kA - 1 -> talonFxService.activeConfiguration.Slot1.kA - 2 -> talonFxService.activeConfiguration.Slot2.kA + when(activeSlot){ + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kA else talonFxFDService.activeConfiguration.Slot0.kA + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kA else talonFxFDService.activeConfiguration.Slot1.kA + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kA else talonFxFDService.activeConfiguration.Slot2.kA else -> SlotConfigs().kA } ) SLOT_KG -> formatMenu( - when(talonFxService.activeSlotIndex) { - 0 -> talonFxService.activeConfiguration.Slot0.kG - 1 -> talonFxService.activeConfiguration.Slot1.kG - 2 -> talonFxService.activeConfiguration.Slot2.kG + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kG else talonFxFDService.activeConfiguration.Slot0.kG + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kG else talonFxFDService.activeConfiguration.Slot1.kG + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kG else talonFxFDService.activeConfiguration.Slot2.kG else -> SlotConfigs().kG } ) @@ -147,29 +153,82 @@ class Phoenix6ParameterCommand( ALLOW_MUSIC_DIS -> formatMenu(config.Audio.AllowMusicDurDisable) BEEP_ON_BOOT -> formatMenu(config.Audio.BeepOnBoot) BEEP_ON_CONFIG -> formatMenu(config.Audio.BeepOnConfig) - POSITION -> formatMenu(talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4) - VELOCITY -> formatMenu(talonFxService.activeVelocity) - ACCELERATION -> formatMenu(talonFxService.activeAcceleration) - JERK -> formatMenu(talonFxService.activeJerk) - TORQUE_CURRENT_DEADBAND -> formatMenu(talonFxService.activeTorqueCurrentDeadband) - TORQUE_CURRENT_MAX -> formatMenu(talonFxService.activeTorqueCurrentMaxOut) - FEED_FORWARD -> formatMenu(talonFxService.activeFeedForward) - OPPOSE_MAIN -> formatMenu(talonFxService.activeOpposeMain) - DIFFERENTIAL_SLOT -> formatMenu(talonFxService.activeDifferentialSlot) - DIFFERENTIAL_TARGET -> formatMenu(talonFxService.activeDifferentialTarget) - FOC -> formatMenu(talonFxService.activeFOC) - OVERRIDE_NEUTRAL -> formatMenu(talonFxService.activeOverrideNeutral) - LIM_FWD_MOT -> formatMenu(talonFxService.limFwdMotion) - LIM_REV_MOT -> formatMenu(talonFxService.limRevMotion) - GRAPHER_FRAME -> formatMenu(talonFxService.grapherStatusFrameHz) + POSITION -> formatMenu( + if(bus == "rio") talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 + ) + VELOCITY -> formatMenu( + if(bus == "rio") talonFxService.activeVelocity + else talonFxFDService.activeVelocity + ) + ACCELERATION -> formatMenu( + if(bus == "rio") talonFxService.activeAcceleration + else talonFxFDService.activeAcceleration + ) + JERK -> formatMenu( + if(bus == "rio") talonFxService.activeJerk + else talonFxFDService.activeJerk + ) + TORQUE_CURRENT_DEADBAND -> formatMenu( + if(bus == "rio") talonFxService.activeTorqueCurrentDeadband + else talonFxFDService.activeTorqueCurrentDeadband + ) + TORQUE_CURRENT_MAX -> formatMenu( + if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut + else talonFxFDService.activeTorqueCurrentMaxOut + ) + FEED_FORWARD -> formatMenu( + if(bus == "rio") talonFxService.activeFeedForward + else talonFxFDService.activeFeedForward + ) + OPPOSE_MAIN -> formatMenu( + if(bus == "rio") talonFxService.activeOpposeMain + else talonFxFDService.activeOpposeMain + + ) + DIFFERENTIAL_SLOT -> formatMenu( + if(bus == "rio") talonFxService.activeDifferentialSlot + else talonFxFDService.activeDifferentialSlot + + ) + DIFFERENTIAL_TARGET -> formatMenu( + if(bus == "rio") talonFxService.activeDifferentialTarget + else talonFxFDService.activeDifferentialTarget + + ) + FOC -> formatMenu( + if(bus == "rio") talonFxService.activeFOC + else talonFxFDService.activeFOC + + ) + OVERRIDE_NEUTRAL -> formatMenu( + if(bus == "rio") talonFxService.activeOverrideNeutral + else talonFxFDService.activeOverrideNeutral + + ) + LIM_FWD_MOT -> formatMenu( + if(bus == "rio") talonFxService.limFwdMotion + else talonFxFDService.limFwdMotion + + ) + LIM_REV_MOT -> formatMenu( + if(bus == "rio") talonFxService.limRevMotion + else talonFxFDService.limRevMotion + + ) + GRAPHER_FRAME -> formatMenu( + if(bus == "rio") talonFxService.grapherStatusFrameHz + else talonFxFDService.grapherStatusFrameHz + + ) else -> TODO("${param.enum} not implemented") } } override fun execute(): Command { - val config = talonFxService.activeConfiguration - val activeSlotIndex = talonFxService.activeSlotIndex + val config = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration + val activeSlotIndex = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex val timeout = talonFxService.timeout when(param.enum) { @@ -692,21 +751,35 @@ class Phoenix6ParameterCommand( private fun configDoubleParam(default: Double, configure: (TalonFX, Double) -> Unit) { val paramValue = param.readDouble(reader, default) - - talonFxService.active.forEach { configure(it, paramValue) } - logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + if(bus == "rio") { + talonFxService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxFDService.active.size} talonFx's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() } private fun configIntParam(default: Int, configure: (TalonFX, Int) -> Unit) { val paramValue = param.readInt(reader, default) - talonFxService.active.forEach { configure(it, paramValue) } - logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + if(bus == "rio") { + talonFxService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxFDService.active.size} talonFx's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() } private fun configBooleanParam(default: Boolean, configure: (TalonFX, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) - talonFxService.active.forEach { configure(it, paramValue) } - logger.info { "Set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + if(bus == "rio") { + talonFxService.active.forEach { configure(it, paramValue) } + logger.info { "Set ${talonFxService.active.size} talonFx's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { configure(it, paramValue) } + logger.info { "Set ${talonFxFDService.active.size} talonFx's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() } diff --git a/src/main/resources/commands.toml b/src/main/resources/commands.toml index 921827b..7ab269b 100644 --- a/src/main/resources/commands.toml +++ b/src/main/resources/commands.toml @@ -1,1803 +1,2607 @@ type = "menu" -[talon] +[rio] type = "menu" - order = 10 - menu = "work with talonSRXs" - [talon.run] - type = "talon.run" - order = 5 - menu = "run active talons" - device = "srx" - [talon.select] - type = "talon.select" - order = 10 - menu = "active talons" - device = "srx" - [talon.status] - type = "talon.status" - order = 12 - menu = "status of active talons" - device = "srx" - [talon.mode] - type = "talon.mode" - order = 15 - menu = "control mode" - device = "srx" - help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." - [talon.sensor] - type = "menu" - order = 17 - menu = "configure feedback sensors" - device = "srx" - [talon.sensor.primary] - type = "talon.sensor" - order = 10 - menu = "primary closed-loop sensor" - pid = 0 - device = "srx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [talon.sensor.auxiliary] - type = "talon.sensor" - order = 20 - menu = "auxiliary closed-loop sensor" - pid = 1 - device = "srx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [talon.sensor.primary_position] - type = "talon.sensor.position" - order = 30 - menu = "primary sensor feedback position" - param = "SENSOR_POSITION" - pid = 0 - device = "srx" - [talon.sensor.auxiliary_position] - type = "talon.sensor.position" - order = 40 - menu = "auxiliary sensor feedback position" - param = "SENSOR_POSITION" - pid = 1 - device = "srx" - [talon.sensor.primary_coefficient] - type = "talon.sensor.coefficient" - order = 50 - menu = "primary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 0 - device = "srx" - [talon.sensor.auxiliary_coefficient] - type = "talon.sensor.coefficient" - order = 60 - menu = "auxiliary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 1 - device = "srx" - [talon.sensor.phase] - type = "talon.param" - order = 70 - menu = "sensor phase reversed" - param = "SENSOR_PHASE" - device = "srx" - [talon.slot] - type = "menu" - order = 20 - menu = "configure PID slots" - device = "srx" - [talon.slot.select] - type = "talon.slot.select" - order = 10 - menu = "active slot" - device = "srx" - help = "The Talon SRX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " - [talon.slot.P] - type ="talon.param" - order = 20 - param = "SLOT_P" - device = "srx" - [talon.slot.I] - type ="talon.param" - order = 30 - param = "SLOT_I" - device = "srx" - [talon.slot.D] - type ="talon.param" - order = 40 - param = "SLOT_D" - device = "srx" - [talon.slot.F] - type ="talon.param" - order = 50 - param = "SLOT_F" - device = "srx" - [talon.slot.IZone] - type ="talon.param" - menu = "I Zone" - order = 60 - param = "SLOT_I_ZONE" - device = "srx" - [talon.slot.AllowableErr] - type ="talon.param" - menu = "Allowable Error" - order = 70 - param = "SLOT_ALLOWABLE_ERR" - device = "srx" - [talon.slot.MaxIAccum] - type ="talon.param" - menu = "Max I Accum" - order = 80 - param = "SLOT_MAX_I_ACCUM" - device = "srx" - [talon.slot.PeakOutput] - type ="talon.param" - menu = "Peak output" - order = 90 - param = "SLOT_PEAK_OUTPUT" - device = "srx" - [talon.motion_magic] - type = "menu" - order = 22 - menu = "configure motion magic" - device = "srx" - [talon.motion_magic.cruise_velocity] - type = "talon.param" - order = 10 - menu = "motion magic cruise velocity" - param = "MOTION_CRUISE_VELOCITY" - device = "srx" - [talon.motion_magic.acceleration] - type = "talon.param" - order = 20 - menu = "motion magic acceleration" - param = "MOTION_ACCELERATION" - device = "srx" - [talon.current] - type = "menu" - order = 23 - menu = "configure current limits" - [talon.current.enabled] - type = "talon.param" - order = 10 - menu = "current limit enabled" - param = "CURRENT_LIMIT_ENABLE" - device = "srx" - [talon.current.continuous] - type = "talon.param" - order = 20 - menu = "continuous current limit" - param = "CURRENT_LIMIT_CONT" - device = "srx" - [talon.current.peak] - type = "talon.param" - order = 30 - menu = "peak current limit" - param = "CURRENT_LIMIT_PEAK" - device = "srx" - [talon.current.peak_duration] - type = "talon.param" - order = 40 - menu = "peak current limit duration" - param = "CURRENT_LIMIT_PEAK_DURATION" - device = "srx" - [talon.current.supply_enabled] - type = "talon.param" - order = 50 - menu = "supply current: limit enabled" - param = "SUPPLY_CURRENT_LIMIT_ENABLE" - device = "srx" - [talon.current.supply_limit] - type = "talon.param" - order = 60 - menu = "supply current: current limit (A)" - param = "SUPPLY_CURRENT_LIMIT" - device = "srx" - [talon.current.supply_triggerCurrent] - type = "talon.param" - order = 70 - menu = "supply current: trigger current (A)" - param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" - device = "srx" - help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" - [talon.current.supply_triggerTime] - type = "talon.param" - order = 80 - menu = "supply current: trigger time (s)" - param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" - device = "srx" - help = "current must exceed the threshold for this long before limiting occurs" - [talon.output] + order = 5 + menu = "work on roborio" + [rio.talon] type = "menu" - order = 25 - menu = "configure motor output" - [talon.output.brake] - type = "talon.brake" - order = 10 - menu = "brake mode" - help = "Mode of operation during Neutral output, brake or coast." - device = "srx" - [talon.output.reversed] - type = "talon.param" - order = 20 - menu = "motor output reversed" - param = "OUTPUT_REVERSED" - device = "srx" - [talon.output.open-loop_ramp] - type = "talon.param" - order = 30 - menu = "open-loop ramp rate" - param = "OPEN_LOOP_RAMP" - device = "srx" - [talon.output.closed-loop_ramp] - type = "talon.param" - order = 40 - menu = "closed-loop ramp rate" - param = "CLOSED_LOOP_RAMP" - device = "srx" - [talon.output.forward_peak_output] - type = "talon.param" - order = 50 - menu = "forward peak output" - param = "PEAK_OUTPUT_FORWARD" - device = "srx" - [talon.output.reverse_peak_output] - type = "talon.param" - order = 60 - menu = "reverse peak output" - param = "PEAK_OUTPUT_REVERSE" - device = "srx" - [talon.output.forward_nominal_output] - type = "talon.param" - order = 70 - menu = "forward nominal output" - param = "NOMINAL_OUTPUT_FORWARD" - device = "srx" - [talon.output.reverse_nominal_output] - type = "talon.param" - order = 80 - menu = "reverse nominal output" - param = "NOMINAL_OUTPUT_REVERSE" - device = "srx" - [talon.output.neutral_deadband] - type = "talon.param" - order = 90 - menu = "neutral deadband" - param = "NEUTRAL_DEADBAND" - device = "srx" - [talon.velocity_measurement] - type = "menu" - order = 26 - menu = "configure velocity measurement" - [talon.velocity_measurement.period] - type = "talon.velocity.period" - order = 10 - menu = "velocity measurement period" - device = "srx" - [talon.velocity_measurement.window] - type = "talon.param" - order = 20 - menu = "velocity measurement window" - param = "VELOCITY_MEASUREMENT_WINDOW" - device = "srx" - [talon.voltage_compensation] - type = "menu" - order = 27 - menu = "configure voltage compensation" - [talon.voltage_compensation.enabled] - type = "talon.param" - order = 10 - menu = "voltage compensation enabled" - param = "VOLTAGE_COMP_ENABLE" - device = "srx" - [talon.voltage_compensation.saturation_voltage] - type = "talon.param" - order = 20 - menu = "voltage compensation saturation voltage" - param = "VOLTAGE_COMP_SATURATION" - device = "srx" - [talon.voltage_compensation.filter] - type = "talon.param" - order = 30 - menu = "voltage compensation measurement filter" - param = "VOLTAGE_MEASUREMENT_FILTER" + order = 10 + menu = "work with talonSRXs" + [rio.talon.run] + type = "talon.run" + order = 5 + menu = "run active talons" device = "srx" - [talon.frame] - type = "menu" - order = 60 - menu = "configure CAN bus frames" - [talon.frame.grapher] - type = "default" + [rio.talon.select] + type = "talon.select" order = 10 - menu = "set grapher defaults" - device = "srx" - [talon.frame.general] - type = "talon.param" - order = 20 - menu = "general status frame" - param = "STATUS_GENERAL" - device = "srx" - [talon.frame.feedback_0] - type = "talon.param" - order = 30 - menu = "feedback 0 status frame" - param = "STATUS_FEEDBACK0" + menu = "active talons" device = "srx" - [talon.frame.quad_encoder] - type = "talon.param" - order = 40 - menu = "quad encoder status frame" - param = "STATUS_QUAD_ENCODER" + [rio.talon.status] + type = "talon.status" + order = 12 + menu = "status of active talons" device = "srx" - [talon.frame.ain_temp_vbat] - type = "talon.param" - order = 50 - menu = "ain/temp/vbat status frame" - param = "STATUS_AIN_TEMP_VBAT" - device = "srx" - [talon.frame.pulse_width] - type = "talon.param" - order = 60 - menu = "pulse width status frame" - param = "STATUS_PULSE_WIDTH" - device = "srx" - [talon.frame.motion] - type = "talon.param" - order = 70 - menu = "motion status frame" - param = "STATUS_MOTION" - device = "srx" - [talon.frame.pidf_0] - type = "talon.param" - order = 80 - menu = "PIDF 0 status frame" - param = "STATUS_PIDF0" - device = "srx" - [talon.frame.misc] - type = "talon.param" - order = 90 - menu = "Misc. status frame" - param = "STATUS_MISC" - device = "srx" - [talon.frame.comm] - type = "talon.param" - order = 100 - menu = "Communication status frame" - param = "STATUS_COMM" - device = "srx" - [talon.frame.motionBuff] - type = "talon.param" - order = 110 - menu = "Motion Profile Buffer status frame" - param = "STATUS_MOTION_BUFF" - device = "srx" - [talon.frame.feedback1] - type = "talon.param" - order = 120 - menu = "Feedback1 status frame" - param = "STATUS_FEEDBACK1" - device = "srx" - [talon.frame.pidf1] - type = "talon.param" - order = 130 - menu = "PIDF1 status frame" - param = "STATUS_PIDF1" - device = "srx" - [talon.frame.firmware] - type = "talon.param" - order = 140 - menu = "Firmware and API status frame" - param = "STATUS_FIRMWARE_API" - device = "srx" - [talon.frame.gadgeteer] - type = "talon.param" - order = 150 - menu = "UART Gadgeteer status frame" - param = "STATUS_UART_GADGETEER" + [rio.talon.mode] + type = "talon.mode" + order = 15 + menu = "control mode" device = "srx" - [talon.limit] - type = "menu" - order = 70 - menu = "configure soft and hard limits" - [talon.limit.forward_hard_source] - type = "talon.hard.source" - order = 10 - forward = true - menu = "forward hard limit source" + help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." + [rio.talon.sensor] + type = "menu" + order = 17 + menu = "configure feedback sensors" device = "srx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [talon.limit.forward_hard_normal] - type = "talon.hard.normal" + [rio.talon.sensor.primary] + type = "talon.sensor" + order = 10 + menu = "primary closed-loop sensor" + pid = 0 + device = "srx" + help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." + [rio.talon.sensor.auxiliary] + type = "talon.sensor" + order = 20 + menu = "auxiliary closed-loop sensor" + pid = 1 + device = "srx" + help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." + [rio.talon.sensor.primary_position] + type = "talon.sensor.position" + order = 30 + menu = "primary sensor feedback position" + param = "SENSOR_POSITION" + pid = 0 + device = "srx" + [rio.talon.sensor.auxiliary_position] + type = "talon.sensor.position" + order = 40 + menu = "auxiliary sensor feedback position" + param = "SENSOR_POSITION" + pid = 1 + device = "srx" + [rio.talon.sensor.primary_coefficient] + type = "talon.sensor.coefficient" + order = 50 + menu = "primary sensor feedback coefficient" + param = "FEEDBACK_COEFFICIENT" + pid = 0 + device = "srx" + [rio.talon.sensor.auxiliary_coefficient] + type = "talon.sensor.coefficient" + order = 60 + menu = "auxiliary sensor feedback coefficient" + param = "FEEDBACK_COEFFICIENT" + pid = 1 + device = "srx" + [rio.talon.sensor.phase] + type = "talon.param" + order = 70 + menu = "sensor phase reversed" + param = "SENSOR_PHASE" + device = "srx" + [rio.talon.slot] + type = "menu" order = 20 - forward = true - menu = "forward hard limit normal" + menu = "configure PID slots" device = "srx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [talon.limit.reverse_hard_source] - type = "talon.hard.source" - order = 30 - forward = false - menu = "reverse hard limit source" - device = "srx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [talon.limit.reverse_hard_normal] - type = "talon.hard.normal" - order = 40 - forward = false - menu = "reverse hard limit normal" - device = "srx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [talon.limit.forward_soft_enabled] - type = "talon.param" - order = 50 - menu = "forward soft limit enabled" - param = "SOFT_LIMIT_ENABLE_FORWARD" + [rio.talon.slot.select] + type = "talon.slot.select" + order = 10 + menu = "active slot" + device = "srx" + help = "The Talon SRX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " + [rio.talon.slot.P] + type ="talon.param" + order = 20 + param = "SLOT_P" + device = "srx" + [rio.talon.slot.I] + type ="talon.param" + order = 30 + param = "SLOT_I" + device = "srx" + [rio.talon.slot.D] + type ="talon.param" + order = 40 + param = "SLOT_D" + device = "srx" + [rio.talon.slot.F] + type ="talon.param" + order = 50 + param = "SLOT_F" + device = "srx" + [rio.talon.slot.IZone] + type ="talon.param" + menu = "I Zone" + order = 60 + param = "SLOT_I_ZONE" + device = "srx" + [rio.talon.slot.AllowableErr] + type ="talon.param" + menu = "Allowable Error" + order = 70 + param = "SLOT_ALLOWABLE_ERR" + device = "srx" + [rio.talon.slot.MaxIAccum] + type ="talon.param" + menu = "Max I Accum" + order = 80 + param = "SLOT_MAX_I_ACCUM" + device = "srx" + [rio.talon.slot.PeakOutput] + type ="talon.param" + menu = "Peak output" + order = 90 + param = "SLOT_PEAK_OUTPUT" + device = "srx" + [rio.talon.motion_magic] + type = "menu" + order = 22 + menu = "configure motion magic" device = "srx" - [talon.limit.forward_soft_threshold] - type = "talon.param" + [rio.talon.motion_magic.cruise_velocity] + type = "talon.param" + order = 10 + menu = "motion magic cruise velocity" + param = "MOTION_CRUISE_VELOCITY" + device = "srx" + [rio.talon.motion_magic.acceleration] + type = "talon.param" + order = 20 + menu = "motion magic acceleration" + param = "MOTION_ACCELERATION" + device = "srx" + [rio.talon.current] + type = "menu" + order = 23 + menu = "configure current limits" + [rio.talon.current.enabled] + type = "talon.param" + order = 10 + menu = "current limit enabled" + param = "CURRENT_LIMIT_ENABLE" + device = "srx" + [rio.talon.current.continuous] + type = "talon.param" + order = 20 + menu = "continuous current limit" + param = "CURRENT_LIMIT_CONT" + device = "srx" + [rio.talon.current.peak] + type = "talon.param" + order = 30 + menu = "peak current limit" + param = "CURRENT_LIMIT_PEAK" + device = "srx" + [rio.talon.current.peak_duration] + type = "talon.param" + order = 40 + menu = "peak current limit duration" + param = "CURRENT_LIMIT_PEAK_DURATION" + device = "srx" + [rio.talon.current.supply_enabled] + type = "talon.param" + order = 50 + menu = "supply current: limit enabled" + param = "SUPPLY_CURRENT_LIMIT_ENABLE" + device = "srx" + [rio.talon.current.supply_limit] + type = "talon.param" + order = 60 + menu = "supply current: current limit (A)" + param = "SUPPLY_CURRENT_LIMIT" + device = "srx" + [rio.talon.current.supply_triggerCurrent] + type = "talon.param" + order = 70 + menu = "supply current: trigger current (A)" + param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" + device = "srx" + help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" + [rio.talon.current.supply_triggerTime] + type = "talon.param" + order = 80 + menu = "supply current: trigger time (s)" + param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" + device = "srx" + help = "current must exceed the threshold for this long before limiting occurs" + [rio.talon.output] + type = "menu" + order = 25 + menu = "configure motor output" + [rio.talon.output.brake] + type = "talon.brake" + order = 10 + menu = "brake mode" + help = "Mode of operation during Neutral output, brake or coast." + device = "srx" + [rio.talon.output.reversed] + type = "talon.param" + order = 20 + menu = "motor output reversed" + param = "OUTPUT_REVERSED" + device = "srx" + [rio.talon.output.open-loop_ramp] + type = "talon.param" + order = 30 + menu = "open-loop ramp rate" + param = "OPEN_LOOP_RAMP" + device = "srx" + [rio.talon.output.closed-loop_ramp] + type = "talon.param" + order = 40 + menu = "closed-loop ramp rate" + param = "CLOSED_LOOP_RAMP" + device = "srx" + [rio.talon.output.forward_peak_output] + type = "talon.param" + order = 50 + menu = "forward peak output" + param = "PEAK_OUTPUT_FORWARD" + device = "srx" + [rio.talon.output.reverse_peak_output] + type = "talon.param" + order = 60 + menu = "reverse peak output" + param = "PEAK_OUTPUT_REVERSE" + device = "srx" + [rio.talon.output.forward_nominal_output] + type = "talon.param" + order = 70 + menu = "forward nominal output" + param = "NOMINAL_OUTPUT_FORWARD" + device = "srx" + [rio.talon.output.reverse_nominal_output] + type = "talon.param" + order = 80 + menu = "reverse nominal output" + param = "NOMINAL_OUTPUT_REVERSE" + device = "srx" + [rio.talon.output.neutral_deadband] + type = "talon.param" + order = 90 + menu = "neutral deadband" + param = "NEUTRAL_DEADBAND" + device = "srx" + [rio.talon.velocity_measurement] + type = "menu" + order = 26 + menu = "configure velocity measurement" + [rio.talon.velocity_measurement.period] + type = "talon.velocity.period" + order = 10 + menu = "velocity measurement period" + device = "srx" + [rio.talon.velocity_measurement.window] + type = "talon.param" + order = 20 + menu = "velocity measurement window" + param = "VELOCITY_MEASUREMENT_WINDOW" + device = "srx" + [rio.talon.voltage_compensation] + type = "menu" + order = 27 + menu = "configure voltage compensation" + [rio.talon.voltage_compensation.enabled] + type = "talon.param" + order = 10 + menu = "voltage compensation enabled" + param = "VOLTAGE_COMP_ENABLE" + device = "srx" + [rio.talon.voltage_compensation.saturation_voltage] + type = "talon.param" + order = 20 + menu = "voltage compensation saturation voltage" + param = "VOLTAGE_COMP_SATURATION" + device = "srx" + [rio.talon.voltage_compensation.filter] + type = "talon.param" + order = 30 + menu = "voltage compensation measurement filter" + param = "VOLTAGE_MEASUREMENT_FILTER" + device = "srx" + [rio.talon.frame] + type = "menu" order = 60 - menu = "forward soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_FORWARD" - device = "srx" - [talon.limit.reverse_soft_enabled] - type = "talon.param" + menu = "configure CAN bus frames" + [rio.talon.frame.grapher] + type = "default" + order = 10 + menu = "set grapher defaults" + device = "srx" + [rio.talon.frame.general] + type = "talon.param" + order = 20 + menu = "general status frame" + param = "STATUS_GENERAL" + device = "srx" + [rio.talon.frame.feedback_0] + type = "talon.param" + order = 30 + menu = "feedback 0 status frame" + param = "STATUS_FEEDBACK0" + device = "srx" + [rio.talon.frame.quad_encoder] + type = "talon.param" + order = 40 + menu = "quad encoder status frame" + param = "STATUS_QUAD_ENCODER" + device = "srx" + [rio.talon.frame.ain_temp_vbat] + type = "talon.param" + order = 50 + menu = "ain/temp/vbat status frame" + param = "STATUS_AIN_TEMP_VBAT" + device = "srx" + [rio.talon.frame.pulse_width] + type = "talon.param" + order = 60 + menu = "pulse width status frame" + param = "STATUS_PULSE_WIDTH" + device = "srx" + [rio.talon.frame.motion] + type = "talon.param" + order = 70 + menu = "motion status frame" + param = "STATUS_MOTION" + device = "srx" + [rio.talon.frame.pidf_0] + type = "talon.param" + order = 80 + menu = "PIDF 0 status frame" + param = "STATUS_PIDF0" + device = "srx" + [rio.talon.frame.misc] + type = "talon.param" + order = 90 + menu = "Misc. status frame" + param = "STATUS_MISC" + device = "srx" + [rio.talon.frame.comm] + type = "talon.param" + order = 100 + menu = "Communication status frame" + param = "STATUS_COMM" + device = "srx" + [rio.talon.frame.motionBuff] + type = "talon.param" + order = 110 + menu = "Motion Profile Buffer status frame" + param = "STATUS_MOTION_BUFF" + device = "srx" + [rio.talon.frame.feedback1] + type = "talon.param" + order = 120 + menu = "Feedback1 status frame" + param = "STATUS_FEEDBACK1" + device = "srx" + [rio.talon.frame.pidf1] + type = "talon.param" + order = 130 + menu = "PIDF1 status frame" + param = "STATUS_PIDF1" + device = "srx" + [rio.talon.frame.firmware] + type = "talon.param" + order = 140 + menu = "Firmware and API status frame" + param = "STATUS_FIRMWARE_API" + device = "srx" + [rio.talon.frame.gadgeteer] + type = "talon.param" + order = 150 + menu = "UART Gadgeteer status frame" + param = "STATUS_UART_GADGETEER" + device = "srx" + [rio.talon.limit] + type = "menu" order = 70 - menu = "reverse soft limit enabled" - param = "SOFT_LIMIT_ENABLE_REVERSE" - device = "srx" - [talon.limit.reverse_soft_threshold] + menu = "configure soft and hard limits" + [rio.talon.limit.forward_hard_source] + type = "talon.hard.source" + order = 10 + forward = true + menu = "forward hard limit source" + device = "srx" + help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." + [rio.talon.limit.forward_hard_normal] + type = "talon.hard.normal" + order = 20 + forward = true + menu = "forward hard limit normal" + device = "srx" + help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." + [rio.talon.limit.reverse_hard_source] + type = "talon.hard.source" + order = 30 + forward = false + menu = "reverse hard limit source" + device = "srx" + help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." + [rio.talon.limit.reverse_hard_normal] + type = "talon.hard.normal" + order = 40 + forward = false + menu = "reverse hard limit normal" + device = "srx" + help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." + [rio.talon.limit.forward_soft_enabled] + type = "talon.param" + order = 50 + menu = "forward soft limit enabled" + param = "SOFT_LIMIT_ENABLE_FORWARD" + device = "srx" + [rio.talon.limit.forward_soft_threshold] + type = "talon.param" + order = 60 + menu = "forward soft limit threshold" + param = "SOFT_LIMIT_THRESHOLD_FORWARD" + device = "srx" + [rio.talon.limit.reverse_soft_enabled] + type = "talon.param" + order = 70 + menu = "reverse soft limit enabled" + param = "SOFT_LIMIT_ENABLE_REVERSE" + device = "srx" + [rio.talon.limit.reverse_soft_threshold] + type = "talon.param" + order = 80 + menu = "reverse soft limit threshold" + param = "SOFT_LIMIT_THRESHOLD_REVERSE" + device = "srx" + # [talon.limit.forward_remote] + # type = "default" + # order = 90 + # menu = "forward remote limit switch" + # [talon.limit.reverse_remote] + # type = "default" + # order = 100 + # menu = "reverse remote limit switch" + [rio.talon.factory_defaults] type = "talon.param" order = 80 - menu = "reverse soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_REVERSE" + menu = "configure factory defaults" + param = "FACTORY_DEFAULTS" device = "srx" - # [talon.limit.forward_remote] - # type = "default" - # order = 90 - # menu = "forward remote limit switch" - # [talon.limit.reverse_remote] - # type = "default" - # order = 100 - # menu = "reverse remote limit switch" - [talon.factory_defaults] - type = "talon.param" - order = 80 - menu = "configure factory defaults" - param = "FACTORY_DEFAULTS" - device = "srx" -[talonFx] - type = "menu" - order = 15 - menu = "work with legacy talonFXs" - [talonFx.run] - type = "talon.run" - order = 5 - menu = "run active talonFXs" - device = "fx" - [talonFx.select] - type = "talon.select" - order = 10 - menu = "active talonFXs" - device = "fx" - [talonFx.status] - type = "talon.status" - order = 12 - menu = "status of active talonFXs" - device = "fx" - [talonFx.mode] - type = "talon.mode" - order = 15 - menu = "control mode" - device = "fx" - help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." - [talonFx.sensor] - type = "menu" - order = 17 - menu = "configure feedback sensors" - device = "fx" - [talonFx.sensor.primary] - type = "talon.sensor" - order = 10 - menu = "primary closed-loop sensor" - pid = 0 - device = "fx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [talonFx.sensor.auxiliary] - type = "talon.sensor" - order = 20 - menu = "auxiliary closed-loop sensor" - pid = 1 - device = "fx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [talonFx.sensor.primary_position] - type = "talon.sensor.position" - order = 30 - menu = "primary sensor feedback position" - param = "SENSOR_POSITION" - pid = 0 - device = "fx" - [talonFx.sensor.auxiliary_position] - type = "talon.sensor.position" - order = 40 - menu = "auxiliary sensor feedback position" - param = "SENSOR_POSITION" - pid = 1 - device = "fx" - [talonFx.sensor.primary_coefficient] - type = "talon.sensor.coefficient" - order = 50 - menu = "primary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 0 - device = "fx" - [talonFx.sensor.auxiliary_coefficient] - type = "talon.sensor.coefficient" - order = 60 - menu = "auxiliary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 1 - device = "fx" - [talonFx.sensor.phase] - type = "talon.param" - order = 70 - menu = "sensor phase reversed" - param = "SENSOR_PHASE" - device = "fx" - [talonFx.sensor.offset] - type = "talon.param" - order = 80 - menu = "integrated sensor offset degrees" - param = "INTEGRATED_SENSOR_OFFSET_DEGREES" - device = "fx" - [talonFx.sensor.absolute_range] - type = "talon.absoluteRange" - order = 90 - menu = "absolute sensor range" - device = "fx" - [talonFx.sensor.initialization_strategy] - type = "talon.initStrategy" - order = 100 - menu = "integrated sensor initialization strategy" - device = "fx" - [talonFx.slot] - type = "menu" - order = 20 - menu = "configure PID slots" - device = "fx" - [talonFx.slot.select] - type = "talon.slot.select" - order = 10 - menu = "active slot" - device = "fx" - help = "The Talon FX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " - [talonFx.slot.P] - type ="talon.param" - order = 20 - param = "SLOT_P" - device = "fx" - [talonFx.slot.I] - type ="talon.param" - order = 30 - param = "SLOT_I" - device = "fx" - [talonFx.slot.D] - type ="talon.param" - order = 40 - param = "SLOT_D" - device = "fx" - [talonFx.slot.F] - type ="talon.param" - order = 50 - param = "SLOT_F" - device = "fx" - [talonFx.slot.IZone] - type ="talon.param" - menu = "I Zone" - order = 60 - param = "SLOT_I_ZONE" - device = "fx" - [talonFx.slot.AllowableErr] - type ="talon.param" - menu = "Allowable Error" - order = 70 - param = "SLOT_ALLOWABLE_ERR" - device = "fx" - [talonFx.slot.MaxIAccum] - type ="talon.param" - menu = "Max I Accum" - order = 80 - param = "SLOT_MAX_I_ACCUM" - device = "fx" - [talonFx.slot.PeakOutput] - type = "talon.param" - menu = "Peak output" - order = 90 - param = "SLOT_PEAK_OUTPUT" - device = "fx" - [talonFx.motion_magic] - type = "menu" - order = 22 - menu = "configure motion magic" - device = "fx" - [talonFx.motion_magic.cruise_velocity] - type = "talon.param" - order = 10 - menu = "motion magic cruise velocity" - param = "MOTION_CRUISE_VELOCITY" - device = "fx" - [talonFx.motion_magic.acceleration] - type = "talon.param" - order = 20 - menu = "motion magic acceleration" - param = "MOTION_ACCELERATION" - device = "fx" - [talonFx.current] - type = "menu" - order = 23 - menu = "configure current limits" - [talonFx.current.stator_enabled] - type = "talon.param" - order = 10 - menu = "stator current: limit enabled" - param = "STATOR_CURRENT_LIMIT_ENABLE" - device = "fx" - [talonFx.current.stator_limit] - type = "talon.param" - order = 20 - menu = "stator current: current limit (A)" - param = "STATOR_CURRENT_LIMIT" - device = "fx" - [talonFx.current.stator_triggerCurrent] - type = "talon.param" - order = 30 - menu = "stator current: trigger current (A)" - param = "STATOR_CURRENT_LIMIT_THRES_CURRENT" - device = "fx" - help = "current must exceed this threshold before current limiting occurs. If this number is less than currentLimit than that limit is used as the threshold" - [talonFx.current.stator_triggerTime] - type = "talon.param" - order = 40 - menu = "stator current: trigger time (s)" - param = "STATOR_CURRENT_LIMIT_THRES_TIME" - device = "fx" - help = "current must exceed the threshold for this long before limiting occurs" - [talonFx.current.supply_enabled] - type = "talon.param" - order = 50 - menu = "supply current: limit enabled" - param = "SUPPLY_CURRENT_LIMIT_ENABLE" - device = "fx" - [talonFx.current.supply_limit] - type = "talon.param" - order = 60 - menu = "supply current: current limit (A)" - param = "SUPPLY_CURRENT_LIMIT" - device = "fx" - [talonFx.current.supply_triggerCurrent] - type = "talon.param" - order = 70 - menu = "supply current: trigger current (A)" - param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" - device = "fx" - help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" - [talonFx.current.supply_triggerTime] - type = "talon.param" - order = 80 - menu = "supply current: trigger time (s)" - param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" - device = "fx" - help = "current must exceed the threshold for this long before limiting occurs" - [talonFx.output] - type = "menu" - order = 25 - menu = "configure motor output" - [talonFx.output.brake] - type = "talon.brake" - order = 10 - menu = "brake mode" - help = "Mode of operation during Neutral output, brake or coast." - device = "fx" - [talonFx.output.reversed] - type = "talon.param" - order = 20 - menu = "motor output reversed" - param = "OUTPUT_REVERSED" - device = "fx" - [talonFx.output.open-loop_ramp] - type = "talon.param" - order = 30 - menu = "open-loop ramp rate" - param = "OPEN_LOOP_RAMP" - device = "fx" - [talonFx.output.closed-loop_ramp] - type = "talon.param" - order = 40 - menu = "closed-loop ramp rate" - param = "CLOSED_LOOP_RAMP" - device = "fx" - [talonFx.output.forward_peak_output] - type = "talon.param" - order = 50 - menu = "forward peak output" - param = "PEAK_OUTPUT_FORWARD" - device = "fx" - [talonFx.output.reverse_peak_output] - type = "talon.param" - order = 60 - menu = "reverse peak output" - param = "PEAK_OUTPUT_REVERSE" - device = "fx" - [talonFx.output.forward_nominal_output] - type = "talon.param" - order = 70 - menu = "forward nominal output" - param = "NOMINAL_OUTPUT_FORWARD" - device = "fx" - [talonFx.output.reverse_nominal_output] - type = "talon.param" - order = 80 - menu = "reverse nominal output" - param = "NOMINAL_OUTPUT_REVERSE" - device = "fx" - [talonFx.output.neutral_deadband] - type = "talon.param" - order = 90 - menu = "neutral deadband" - param = "NEUTRAL_DEADBAND" - device = "fx" - [talonFx.output.motor_commutation] - type = "talon.commutation" - order = 100 - menu = "motor commutation" - device = "fx" - [talonFx.velocity_measurement] + [rio.talonFx] type = "menu" - order = 26 - menu = "configure velocity measurement" - [talonFx.velocity_measurement.period] - type = "talon.velocity.period" - order = 10 - menu = "velocity measurement period" - device = "fx" - [talonFx.velocity_measurement.window] - type = "talon.param" - order = 20 - menu = "velocity measurement window" - param = "VELOCITY_MEASUREMENT_WINDOW" + order = 15 + menu = "work with legacy talonFXs" + [rio.talonFx.run] + type = "talon.run" + order = 5 + menu = "run active talonFXs" device = "fx" - [talonFx.voltage_compensation] - type = "menu" - order = 27 - menu = "configure voltage compensation" - [talonFx.voltage_compensation.enabled] - type = "talon.param" + [rio.talonFx.select] + type = "talon.select" order = 10 - menu = "voltage compensation enabled" - param = "VOLTAGE_COMP_ENABLE" + menu = "active talonFXs" device = "fx" - [talonFx.voltage_compensation.saturation_voltage] - type = "talon.param" - order = 20 - menu = "voltage compensation saturation voltage" - param = "VOLTAGE_COMP_SATURATION" + [rio.talonFx.status] + type = "talon.status" + order = 12 + menu = "status of active talonFXs" device = "fx" - [talonFx.voltage_compensation.filter] - type = "talon.param" - order = 30 - menu = "voltage compensation measurement filter" - param = "VOLTAGE_MEASUREMENT_FILTER" + [rio.talonFx.mode] + type = "talon.mode" + order = 15 + menu = "control mode" device = "fx" - [talonFx.frame] - type = "menu" - order = 60 - menu = "configure CAN bus frames" - [talonFx.frame.grapher] - type = "default" - order = 10 - menu = "set grapher defaults" + help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." + [rio.talonFx.sensor] + type = "menu" + order = 17 + menu = "configure feedback sensors" device = "fx" - [talonFx.frame.general] - type = "talon.param" + [rio.talonFx.sensor.primary] + type = "talon.sensor" + order = 10 + menu = "primary closed-loop sensor" + pid = 0 + device = "fx" + help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." + [rio.talonFx.sensor.auxiliary] + type = "talon.sensor" + order = 20 + menu = "auxiliary closed-loop sensor" + pid = 1 + device = "fx" + help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." + [rio.talonFx.sensor.primary_position] + type = "talon.sensor.position" + order = 30 + menu = "primary sensor feedback position" + param = "SENSOR_POSITION" + pid = 0 + device = "fx" + [rio.talonFx.sensor.auxiliary_position] + type = "talon.sensor.position" + order = 40 + menu = "auxiliary sensor feedback position" + param = "SENSOR_POSITION" + pid = 1 + device = "fx" + [rio.talonFx.sensor.primary_coefficient] + type = "talon.sensor.coefficient" + order = 50 + menu = "primary sensor feedback coefficient" + param = "FEEDBACK_COEFFICIENT" + pid = 0 + device = "fx" + [rio.talonFx.sensor.auxiliary_coefficient] + type = "talon.sensor.coefficient" + order = 60 + menu = "auxiliary sensor feedback coefficient" + param = "FEEDBACK_COEFFICIENT" + pid = 1 + device = "fx" + [rio.talonFx.sensor.phase] + type = "talon.param" + order = 70 + menu = "sensor phase reversed" + param = "SENSOR_PHASE" + device = "fx" + [rio.talonFx.sensor.offset] + type = "talon.param" + order = 80 + menu = "integrated sensor offset degrees" + param = "INTEGRATED_SENSOR_OFFSET_DEGREES" + device = "fx" + [rio.talonFx.sensor.absolute_range] + type = "talon.absoluteRange" + order = 90 + menu = "absolute sensor range" + device = "fx" + [rio.talonFx.sensor.initialization_strategy] + type = "talon.initStrategy" + order = 100 + menu = "integrated sensor initialization strategy" + device = "fx" + [rio.talonFx.slot] + type = "menu" order = 20 - menu = "general status frame" - param = "STATUS_GENERAL" - device = "fx" - [talonFx.frame.feedback_0] - type = "talon.param" - order = 30 - menu = "feedback 0 status frame" - param = "STATUS_FEEDBACK0" - device = "fx" - [talonFx.frame.quad_encoder] - type = "talon.param" - order = 40 - menu = "quad encoder status frame" - param = "STATUS_QUAD_ENCODER" + menu = "configure PID slots" device = "fx" - [talonFx.frame.ain_temp_vbat] - type = "talon.param" - order = 50 - menu = "ain/temp/vbat status frame" - param = "STATUS_AIN_TEMP_VBAT" + [rio.talonFx.slot.select] + type = "talon.slot.select" + order = 10 + menu = "active slot" + device = "fx" + help = "The Talon FX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " + [rio.talonFx.slot.P] + type ="talon.param" + order = 20 + param = "SLOT_P" + device = "fx" + [rio.talonFx.slot.I] + type ="talon.param" + order = 30 + param = "SLOT_I" + device = "fx" + [rio.talonFx.slot.D] + type ="talon.param" + order = 40 + param = "SLOT_D" + device = "fx" + [rio.talonFx.slot.F] + type ="talon.param" + order = 50 + param = "SLOT_F" + device = "fx" + [rio.talonFx.slot.IZone] + type ="talon.param" + menu = "I Zone" + order = 60 + param = "SLOT_I_ZONE" + device = "fx" + [rio.talonFx.slot.AllowableErr] + type ="talon.param" + menu = "Allowable Error" + order = 70 + param = "SLOT_ALLOWABLE_ERR" + device = "fx" + [rio.talonFx.slot.MaxIAccum] + type ="talon.param" + menu = "Max I Accum" + order = 80 + param = "SLOT_MAX_I_ACCUM" + device = "fx" + [rio.talonFx.slot.PeakOutput] + type = "talon.param" + menu = "Peak output" + order = 90 + param = "SLOT_PEAK_OUTPUT" + device = "fx" + [rio.talonFx.motion_magic] + type = "menu" + order = 22 + menu = "configure motion magic" device = "fx" - [talonFx.frame.pulse_width] - type = "talon.param" + [rio.talonFx.motion_magic.cruise_velocity] + type = "talon.param" + order = 10 + menu = "motion magic cruise velocity" + param = "MOTION_CRUISE_VELOCITY" + device = "fx" + [rio.talonFx.motion_magic.acceleration] + type = "talon.param" + order = 20 + menu = "motion magic acceleration" + param = "MOTION_ACCELERATION" + device = "fx" + [rio.talonFx.current] + type = "menu" + order = 23 + menu = "configure current limits" + [rio.talonFx.current.stator_enabled] + type = "talon.param" + order = 10 + menu = "stator current: limit enabled" + param = "STATOR_CURRENT_LIMIT_ENABLE" + device = "fx" + [rio.talonFx.current.stator_limit] + type = "talon.param" + order = 20 + menu = "stator current: current limit (A)" + param = "STATOR_CURRENT_LIMIT" + device = "fx" + [rio.talonFx.current.stator_triggerCurrent] + type = "talon.param" + order = 30 + menu = "stator current: trigger current (A)" + param = "STATOR_CURRENT_LIMIT_THRES_CURRENT" + device = "fx" + help = "current must exceed this threshold before current limiting occurs. If this number is less than currentLimit than that limit is used as the threshold" + [rio.talonFx.current.stator_triggerTime] + type = "talon.param" + order = 40 + menu = "stator current: trigger time (s)" + param = "STATOR_CURRENT_LIMIT_THRES_TIME" + device = "fx" + help = "current must exceed the threshold for this long before limiting occurs" + [rio.talonFx.current.supply_enabled] + type = "talon.param" + order = 50 + menu = "supply current: limit enabled" + param = "SUPPLY_CURRENT_LIMIT_ENABLE" + device = "fx" + [rio.talonFx.current.supply_limit] + type = "talon.param" + order = 60 + menu = "supply current: current limit (A)" + param = "SUPPLY_CURRENT_LIMIT" + device = "fx" + [rio.talonFx.current.supply_triggerCurrent] + type = "talon.param" + order = 70 + menu = "supply current: trigger current (A)" + param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" + device = "fx" + help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" + [rio.talonFx.current.supply_triggerTime] + type = "talon.param" + order = 80 + menu = "supply current: trigger time (s)" + param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" + device = "fx" + help = "current must exceed the threshold for this long before limiting occurs" + [rio.talonFx.output] + type = "menu" + order = 25 + menu = "configure motor output" + [rio.talonFx.output.brake] + type = "talon.brake" + order = 10 + menu = "brake mode" + help = "Mode of operation during Neutral output, brake or coast." + device = "fx" + [rio.talonFx.output.reversed] + type = "talon.param" + order = 20 + menu = "motor output reversed" + param = "OUTPUT_REVERSED" + device = "fx" + [rio.talonFx.output.open-loop_ramp] + type = "talon.param" + order = 30 + menu = "open-loop ramp rate" + param = "OPEN_LOOP_RAMP" + device = "fx" + [rio.talonFx.output.closed-loop_ramp] + type = "talon.param" + order = 40 + menu = "closed-loop ramp rate" + param = "CLOSED_LOOP_RAMP" + device = "fx" + [rio.talonFx.output.forward_peak_output] + type = "talon.param" + order = 50 + menu = "forward peak output" + param = "PEAK_OUTPUT_FORWARD" + device = "fx" + [rio.talonFx.output.reverse_peak_output] + type = "talon.param" + order = 60 + menu = "reverse peak output" + param = "PEAK_OUTPUT_REVERSE" + device = "fx" + [rio.talonFx.output.forward_nominal_output] + type = "talon.param" + order = 70 + menu = "forward nominal output" + param = "NOMINAL_OUTPUT_FORWARD" + device = "fx" + [rio.talonFx.output.reverse_nominal_output] + type = "talon.param" + order = 80 + menu = "reverse nominal output" + param = "NOMINAL_OUTPUT_REVERSE" + device = "fx" + [rio.talonFx.output.neutral_deadband] + type = "talon.param" + order = 90 + menu = "neutral deadband" + param = "NEUTRAL_DEADBAND" + device = "fx" + [rio.talonFx.output.motor_commutation] + type = "talon.commutation" + order = 100 + menu = "motor commutation" + device = "fx" + [rio.talonFx.velocity_measurement] + type = "menu" + order = 26 + menu = "configure velocity measurement" + [rio.talonFx.velocity_measurement.period] + type = "talon.velocity.period" + order = 10 + menu = "velocity measurement period" + device = "fx" + [rio.talonFx.velocity_measurement.window] + type = "talon.param" + order = 20 + menu = "velocity measurement window" + param = "VELOCITY_MEASUREMENT_WINDOW" + device = "fx" + [rio.talonFx.voltage_compensation] + type = "menu" + order = 27 + menu = "configure voltage compensation" + [rio.talonFx.voltage_compensation.enabled] + type = "talon.param" + order = 10 + menu = "voltage compensation enabled" + param = "VOLTAGE_COMP_ENABLE" + device = "fx" + [rio.talonFx.voltage_compensation.saturation_voltage] + type = "talon.param" + order = 20 + menu = "voltage compensation saturation voltage" + param = "VOLTAGE_COMP_SATURATION" + device = "fx" + [rio.talonFx.voltage_compensation.filter] + type = "talon.param" + order = 30 + menu = "voltage compensation measurement filter" + param = "VOLTAGE_MEASUREMENT_FILTER" + device = "fx" + [rio.talonFx.frame] + type = "menu" order = 60 - menu = "pulse width status frame" - param = "STATUS_PULSE_WIDTH" - device = "fx" - [talonFx.frame.motion] - type = "talon.param" + menu = "configure CAN bus frames" + [rio.talonFx.frame.grapher] + type = "default" + order = 10 + menu = "set grapher defaults" + device = "fx" + [rio.talonFx.frame.general] + type = "talon.param" + order = 20 + menu = "general status frame" + param = "STATUS_GENERAL" + device = "fx" + [rio.talonFx.frame.feedback_0] + type = "talon.param" + order = 30 + menu = "feedback 0 status frame" + param = "STATUS_FEEDBACK0" + device = "fx" + [rio.talonFx.frame.quad_encoder] + type = "talon.param" + order = 40 + menu = "quad encoder status frame" + param = "STATUS_QUAD_ENCODER" + device = "fx" + [rio.talonFx.frame.ain_temp_vbat] + type = "talon.param" + order = 50 + menu = "ain/temp/vbat status frame" + param = "STATUS_AIN_TEMP_VBAT" + device = "fx" + [rio.talonFx.frame.pulse_width] + type = "talon.param" + order = 60 + menu = "pulse width status frame" + param = "STATUS_PULSE_WIDTH" + device = "fx" + [rio.talonFx.frame.motion] + type = "talon.param" + order = 70 + menu = "motion status frame" + param = "STATUS_MOTION" + device = "fx" + [rio.talonFx.frame.pidf_0] + type = "talon.param" + order = 80 + menu = "PIDF 0 status frame" + param = "STATUS_PIDF0" + device = "fx" + [rio.talonFx.frame.misc] + type = "talon.param" + order = 90 + menu = "Misc. status frame" + param = "STATUS_MISC" + device = "fx" + [rio.talonFx.frame.comm] + type = "talon.param" + order = 100 + menu = "Communication status frame" + param = "STATUS_COMM" + device = "fx" + [rio.talonFx.frame.motionBuff] + type = "talon.param" + order = 110 + menu = "Motion Profile Buffer status frame" + param = "STATUS_MOTION_BUFF" + device = "fx" + [rio.talonFx.frame.feedback1] + type = "talon.param" + order = 120 + menu = "Feedback1 status frame" + param = "STATUS_FEEDBACK1" + device = "fx" + [rio.talonFx.frame.pidf1] + type = "talon.param" + order = 130 + menu = "PIDF1 status frame" + param = "STATUS_PIDF1" + device = "fx" + [rio.talonFx.frame.firmware] + type = "talon.param" + order = 140 + menu = "Firmware and API status frame" + param = "STATUS_FIRMWARE_API" + device = "fx" + [rio.talonFx.frame.gadgeteer] + type = "talon.param" + order = 150 + menu = "UART Gadgeteer status frame" + param = "STATUS_UART_GADGETEER" + device = "fx" + [rio.talonFx.limit] + type = "menu" order = 70 - menu = "motion status frame" - param = "STATUS_MOTION" - device = "fx" - [talonFx.frame.pidf_0] + menu = "configure soft and hard limits" + [rio.talonFx.limit.forward_hard_source] + type = "talon.hard.source" + order = 10 + forward = true + menu = "forward hard limit source" + device = "fx" + help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." + [rio.talonFx.limit.forward_hard_normal] + type = "talon.hard.normal" + order = 20 + forward = true + menu = "forward hard limit normal" + device = "fx" + help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." + [rio.talonFx.limit.reverse_hard_source] + type = "talon.hard.source" + order = 30 + forward = false + menu = "reverse hard limit source" + device = "fx" + help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." + [rio.talonFx.limit.reverse_hard_normal] + type = "talon.hard.normal" + order = 40 + forward = false + menu = "reverse hard limit normal" + device = "fx" + help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." + [rio.talonFx.limit.forward_soft_enabled] + type = "talon.param" + order = 50 + menu = "forward soft limit enabled" + param = "SOFT_LIMIT_ENABLE_FORWARD" + device = "fx" + [rio.talonFx.limit.forward_soft_threshold] + type = "talon.param" + order = 60 + menu = "forward soft limit threshold" + param = "SOFT_LIMIT_THRESHOLD_FORWARD" + device = "fx" + [rio.talonFx.limit.reverse_soft_enabled] + type = "talon.param" + order = 70 + menu = "reverse soft limit enabled" + param = "SOFT_LIMIT_ENABLE_REVERSE" + device = "fx" + [rio.talonFx.limit.reverse_soft_threshold] + type = "talon.param" + order = 80 + menu = "reverse soft limit threshold" + param = "SOFT_LIMIT_THRESHOLD_REVERSE" + device = "fx" + [rio.talonFx.factory_default] type = "talon.param" order = 80 - menu = "PIDF 0 status frame" - param = "STATUS_PIDF0" - device = "fx" - [talonFx.frame.misc] - type = "talon.param" - order = 90 - menu = "Misc. status frame" - param = "STATUS_MISC" - device = "fx" - [talonFx.frame.comm] - type = "talon.param" - order = 100 - menu = "Communication status frame" - param = "STATUS_COMM" - device = "fx" - [talonFx.frame.motionBuff] - type = "talon.param" - order = 110 - menu = "Motion Profile Buffer status frame" - param = "STATUS_MOTION_BUFF" - device = "fx" - [talonFx.frame.feedback1] - type = "talon.param" - order = 120 - menu = "Feedback1 status frame" - param = "STATUS_FEEDBACK1" - device = "fx" - [talonFx.frame.pidf1] - type = "talon.param" - order = 130 - menu = "PIDF1 status frame" - param = "STATUS_PIDF1" - device = "fx" - [talonFx.frame.firmware] - type = "talon.param" - order = 140 - menu = "Firmware and API status frame" - param = "STATUS_FIRMWARE_API" - device = "fx" - [talonFx.frame.gadgeteer] - type = "talon.param" - order = 150 - menu = "UART Gadgeteer status frame" - param = "STATUS_UART_GADGETEER" + menu = "configure factory defaults" + param = "FACTORY_DEFAULTS" device = "fx" - [talonFx.limit] + + [rio.FxP6] type = "menu" - order = 70 - menu = "configure soft and hard limits" - [talonFx.limit.forward_hard_source] - type = "talon.hard.source" + order = 17 + menu = "work with talonFX's" + [rio.FxP6.run] + type = "p6.run" order = 10 - forward = true - menu = "forward hard limit source" - device = "fx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [talonFx.limit.forward_hard_normal] - type = "talon.hard.normal" + menu = "run active talon fx's" + bus = "rio" + [rio.FxP6.select] + type = "p6.select" order = 20 - forward = true - menu = "forward hard limit normal" - device = "fx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [talonFx.limit.reverse_hard_source] - type = "talon.hard.source" + menu = "active fx's" + bus = "rio" + [rio.FxP6.status] + type = "p6.status" order = 30 - forward = false - menu = "reverse hard limit source" - device = "fx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [talonFx.limit.reverse_hard_normal] - type = "talon.hard.normal" + menu = "status of active talonFx's" + bus = "rio" + [rio.FxP6.mode] + type = "p6.modeMenu" order = 40 - forward = false - menu = "reverse hard limit normal" - device = "fx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [talonFx.limit.forward_soft_enabled] - type = "talon.param" + menu = "config active mode" + bus = "rio" + [rio.FxP6.mode.status] + type = "p6.modeStatus" + order = 10 + menu = "active mode" + bus = "rio" + [rio.FxP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + bus = "rio" + [rio.FxP6.mode.openLoop] + type = "menu" + order = 20 + menu = "open loop modes" + bus = "rio" + [rio.FxP6.mode.openLoop.deadband] + type = "p6.param" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" + bus = "rio" + [rio.FxP6.mode.position] + type = "menu" + order = 30 + menu = "position modes" + [rio.FxP6.mode.position.velocity] + type = "p6.param" + order = 20 + menu = "velocity target" + param = "VELOCITY" + bus = "rio" + [rio.FxP6.mode.position.feedFwd] + type = "p6.param" + order = 30 + menu = "velocity feed forward" + param = "FEED_FORWARD" + bus = "rio" + [rio.FxP6.mode.velocity] + type = "menu" + order = 40 + menu = "velocity modes" + [rio.FxP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + bus = "rio" + [rio.FxP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + bus = "rio" + [rio.FxP6.mode.motion] + type = "menu" + order = 50 + menu = "motion magic modes" + [rio.FxP6.mode.motion.motionType] + type = "p6.mmType" + order = 10 + menu = "select motion magic type" + bus = "rio" + [rio.FxP6.mode.motion.feedFwd] + type = "p6.param" + order = 20 + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" + bus = "rio" + [rio.FxP6.mode.motion.velocity] + type = "p6.param" + order = 30 + menu = "select velocity target (rot/s)" + param = "VELOCITY" + bus = "rio" + [rio.FxP6.mode.motion.acceleration] + type = "p6.param" + order = 40 + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" + bus = "rio" + [rio.FxP6.mode.motion.jerk] + type = "p6.param" + order = 50 + menu = "select jerk target (rot/s^3)" + param = "JERK" + bus = "rio" + [rio.FxP6.mode.diff] + type = "menu" + order = 60 + menu = "differential modes" + [rio.FxP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" + bus = "rio" + [rio.FxP6.mode.diff.slot] + type = "p6.param" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" + bus = "rio" + [rio.FxP6.mode.diff.target] + type = "p6.param" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" + bus = "rio" + [rio.FxP6.mode.follow] + type = "menu" + order = 70 + menu = "follower modes" + [rio.FxP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" + bus = "rio" + [rio.FxP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + bus = "rio" + [rio.FxP6.mode.neutral] + type = "menu" + order = 80 + memnu = "neutral modes" + [rio.FxP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + bus = "rio" + [rio.FxP6.mode.foc] + type = "p6.param" + order = 100 + menu = "(pro) enable FOC" + param = "FOC" + bus = "rio" + [rio.FxP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + bus = "rio" + [rio.FxP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + bus = "rio" + [rio.FxP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" + bus = "rio" + [rio.FxP6.units] + type = "p6.units" order = 50 - menu = "forward soft limit enabled" - param = "SOFT_LIMIT_ENABLE_FORWARD" - device = "fx" - [talonFx.limit.forward_soft_threshold] - type = "talon.param" + menu = "select active units" + bus = "rio" + [rio.FxP6.feedback] + type = "menu" order = 60 - menu = "forward soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_FORWARD" - device = "fx" - [talonFx.limit.reverse_soft_enabled] - type = "talon.param" + menu = "setup feedback configs" + bus = "rio" + [rio.FxP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" + bus = "rio" + [rio.FxP6.feedback.offset] + type = "p6.param" + order = 10 + menu = "set rotor offset" + param = "ROTOR_OFFSET" + bus = "rio" + [rio.FxP6.feedback.sense2mech] + type = "p6.param" + order = 20 + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + bus = "rio" + [rio.FxP6.feedback.rotor2sense] + type = "p6.param" + order = 30 + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + bus = "rio" + [rio.FxP6.feedback.source] + type = "p6.feedback" + order = 35 + menu = "set feedback source" + bus = "rio" + [rio.FxP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + bus = "rio" + [rio.FxP6.slot] + type = "menu" order = 70 - menu = "reverse soft limit enabled" - param = "SOFT_LIMIT_ENABLE_REVERSE" - device = "fx" - [talonFx.limit.reverse_soft_threshold] - type = "talon.param" + menu = "setup slot configs" + [rio.FxP6.slot.active] + type = "p6.slot" + order = 10 + menu = "active slot" + bus = "rio" + [rio.FxP6.slot.kP] + type = "p6.param" + order = 20 + menu = "kP" + param = "SLOT_KP" + bus = "rio" + [rio.FxP6.slot.kI] + type = "p6.param" + order = 30 + menu = "kI" + param = "SLOT_KI" + bus = "rio" + [rio.FxP6.slot.kD] + type = "p6.param" + order = 40 + menu = "kD" + param = "SLOT_KD" + bus = "rio" + [rio.FxP6.slot.kS] + type = "p6.param" + order = 50 + menu = "kS" + param = "SLOT_KS" + bus = "rio" + [rio.FxP6.slot.kV] + type = "p6.param" + order = 60 + menu = "kV" + param = "SLOT_KV" + bus = "rio" + [rio.FxP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + bus = "rio" + [rio.FxP6.slot.kG] + type = "p6.param" + order = 70 + menu = "kG" + param = "SLOT_KG" + bus = "rio" + [rio.FxP6.slot.gravity] + type = "p6.gravity" + order = 80 + menu = "gravity type" + bus = "rio" + [rio.FxP6.motion] + type = "menu" order = 80 - menu = "reverse soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_REVERSE" - device = "fx" - [talonFx.factory_default] - type = "talon.param" - order = 80 - menu = "configure factory defaults" - param = "FACTORY_DEFAULTS" - device = "fx" - -[FxP6] - type = "menu" - order = 17 - menu = "work with talonFX's" - [FxP6.run] - type = "p6.run" - order = 10 - menu = "run active talon fx's" - [FxP6.select] - type = "p6.select" - order = 20 - menu = "active fx's" - [FxP6.status] - type = "p6.status" - order = 30 - menu = "status of active talonFx's" - [FxP6.mode] - type = "p6.modeMenu" - order = 40 - menu = "config active mode" - [FxP6.mode.status] - type = "p6.modeStatus" - order = 10 - menu = "active mode" - [FxP6.mode.select] - type = "p6.mode" - order = 15 - menu = "select mode type" - [FxP6.mode.openLoop] + menu = "setup motion magic configs" + [rio.FxP6.motion.accel] + type = "p6.param" + order = 10 + menu = "acceleration" + param = "MM_ACCEL" + bus = "rio" + [rio.FxP6.motion.cruise] + type = "p6.param" + order = 20 + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + bus = "rio" + [rio.FxP6.motion.jerk] + type = "p6.param" + order = 30 + menu = "jerk" + param = "MM_JERK" + bus = "rio" + [rio.FxP6.motion.kA] + type = "p6.param" + order = 40 + menu = "Expo kA" + param = "MM_EXPO_KA" + bus = "rio" + [rio.FxP6.motion.kV] + type = "p6.param" + order = 50 + menu = "Expo kV" + param = "MM_EXPO_KV" + bus = "rio" + [rio.FxP6.diff] type = "menu" - order = 20 - menu = "open loop modes" - [FxP6.mode.openLoop.deadband] + order = 90 + menu = "setup differential configs" + [rio.FxP6.diff.duty] + type = "p6.param" + order = 10 + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + bus = "rio" + [rio.FxP6.diff.torque] type = "p6.param" order = 20 - menu = "set torque current deadband (A)" - param = "TORQUE_CURRENT_DEADBAND" - [FxP6.mode.position] + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + bus = "rio" + [rio.FxP6.diff.volt] + type = "p6.param" + order = 30 + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + bus = "rio" + [rio.FxP6.diff.remote] + type = "p6.param" + order = 40 + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + bus = "rio" + [rio.FxP6.diff.fx] + type = "p6.param" + order = 50 + menu = "differential fx id" + param = "DIFF_FX_ID" + bus = "rio" + [rio.FxP6.current] type = "menu" - order = 30 - menu = "position modes" - [FxP6.mode.position.velocity] + order = 100 + menu = "setup current limit configs" + [rio.FxP6.current.statorEn] + type = "p6.param" + order = 10 + menu = "stator limit enable" + param = "STATOR_LIM_EN" + bus = "rio" + [rio.FxP6.current.statorLim] + type = "p6.param" + order = 20 + menu = "stator current limit" + param = "STATOR_LIM" + bus = "rio" + [rio.FxP6.current.suppEn] + type = "p6.param" + order = 30 + menu = "supply limit enable" + param = "SUPP_LIM_EN" + bus = "rio" + [rio.FxP6.current.suppLim] + type = "p6.param" + order = 40 + menu = "supply current limit" + param = "SUPP_LIM" + bus = "rio" + [rio.FxP6.current.tripI] + type = "p6.param" + order = 50 + menu = "supply current trip threshold (A)" + param = "SUPP_TRIP_THRES" + bus = "rio" + [rio.FxP6.current.tripT] + type = "p6.param" + order = 60 + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" + bus = "rio" + [rio.FxP6.motor] + type = "menu" + order = 110 + menu = "setup motor output and ramp configs" + [rio.FxP6.motor.invert] + type = "p6.invert" + order = 10 + menu = "motor inversion" + bus = "rio" + [rio.FxP6.motor.neutral] + type = "p6.neutral" + order = 20 + menu = "neutral mode" + bus = "rio" + [rio.FxP6.motor.duty] + type = "menu" + order = 30 + menu = "duty cycle configs" + bus = "rio" + [rio.FxP6.motor.duty.peakFwd] + type= "p6.param" + order = 20 + menu = "peak forward %" + param = "PEAK_FWD_DC" + bus = "rio" + [rio.FxP6.motor.duty.peakRev] + type = "p6.param" + order = 30 + menu = "peak reverse %" + param = "PEAK_REV_DC" + bus = "rio" + [rio.FxP6.motor.duty.dead] + type = "p6.param" + order = 40 + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" + bus = "rio" + [rio.FxP6.motor.duty.openRamp] + type = "p6.param" + order = 50 + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" + bus = "rio" + [rio.FxP6.motor.duty.closedRamp] + type = "p6.param" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" + bus = "rio" + [rio.FxP6.motor.volt] + type = "menu" + order = 40 + menu = "voltage configs" + [rio.FxP6.motor.volt.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (V)" + param = "PEAK_FWD_V" + bus = "rio" + [rio.FxP6.motor.volt.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (V)" + param = "PEAK_REV_V" + bus = "rio" + [rio.FxP6.motor.volt.supplyConst] + type = "p6.param" + order = 30 + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" + bus = "rio" + [rio.FxP6.motor.volt.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" + bus = "rio" + [rio.FxP6.motor.volt.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" + bus = "rio" + [rio.FxP6.motor.torque] + type = "menu" + order = 50 + menu = "torque current configs" + [rio.FxP6.motor.torque.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (A)" + param = "PEAK_FWD_I" + bus = "rio" + [rio.FxP6.motor.torque.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (A)" + param = "PEAK_REV_I" + bus = "rio" + [rio.FxP6.motor.torque.dead] + type = "p6.param" + order = 30 + menu = "neutral deadband (A)" + param = "TORQUE_NEUTRAL_DEADBAND" + bus = "rio" + [rio.FxP6.motor.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" + bus = "rio" + [rio.FxP6.motor.closedRamp] + type = "p6.param" + order = 50 + menuu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + bus = "rio" + [rio.FxP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [rio.FxP6.limits.hard] + type = "menu" + order = 10 + menu = "hard limits" + [rio.FxP6.limits.hard.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_HARD_EN" + bus = "rio" + [rio.FxP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 10 + menu = "FWD limit normal" + bus = "rio" + [rio.FxP6.limits.hard.fwdSource] + type = "p6.fwdSource" + order = 20 + menu = "FWD limit source" + bus = "rio" + [rio.FxP6.limits.hard.fwdRemote] + type = "p6.param" + order = 30 + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" + bus = "rio" + [rio.FxP6.limits.hard.fwdAutoSet] + type = "p6.param" + order = 40 + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + bus = "rio" + [rio.FxP6.limits.hard.fwdAutoSetValue] + type = "p6.param" + order = 50 + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + bus = "rio" + [rio.FxP6.limits.hard.revEn] + type = "p6.param" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" + bus = "rio" + [rio.FxP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + bus = "rio" + [rio.FxP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + bus = "rio" + [rio.FxP6.limits.hard.revRemote] + type = "p6.param" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" + bus = "rio" + [rio.FxP6.limits.hard.revAutoSet] + type = "p6.param" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" + bus = "rio" + [rio.FxP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + bus = "rio" + [rio.FxP6.limits.soft] + type = "menu" + order = 20 + menu = "soft limits" + [rio.FxP6.limits.soft.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_SOFT_EN" + bus = "rio" + [rio.FxP6.limits.soft.fwdThres] + type = "p6.param" + order = 20 + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" + bus = "rio" + [rio.FxP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + bus = "rio" + [rio.FxP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + bus = "rio" + [rio.FxP6.audio] + type = "menu" + order = 130 + menu = "setup audio configs" + [rio.FxP6.audio.disable] + type = "p6.param" + order = 10 + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" + bus = "rio" + [rio.FxP6.audio.beepBoot] type = "p6.param" order = 20 - menu = "velocity target" - param = "VELOCITY" - [FxP6.mode.position.feedFwd] + menu = "beep on boot" + param = "BEEP_ON_BOOT" + bus = "rio" + [rio.FxP6.audio.beepConfig] type = "p6.param" order = 30 - menu = "velocity feed forward" - param = "FEED_FORWARD" - [FxP6.mode.velocity] + menu = "beep on config" + param = "BEEP_ON_CONFIG" + bus = "rio" + [rio.FxP6.factory] + type = "p6.factory" + order = 140 + menu = "config factory default" + bus = "rio" + [rio.FxP6.grapher] type = "menu" + order = 150 + menu = "set status frames" + [rio.FxP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + bus = "rio" + [rio.FxP6.grapher.rate] + type = "p6.param" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" + bus = "rio" + + [rio.servo] + type = "menu" + order = 20 + menu = "work with servos" + [rio.servo.run] + type = "servo.run" + order = 10 + menu = "set servo positions" + [rio.servo.select] + type = "servo.select" + order = 20 + menu = "active servos" + + [rio.solenoid] + type = "menu" + order = 25 + menu = "work with solenoids" + [rio.solenoid.run] + type = "solenoid.run" + order = 10 + menu = "set solenoid positions" + [rio.solenoid.select] + type = "solenoid.select" + order = 20 + menu = "active solenoids" + + [rio.digital_output] + type = "menu" + order = 30 + menu = "work with digital outputs" + [rio.digital_output.set] + type = "digital_output.run" + order = 10 + menu = "set digital output" + [rio.digital_output.select] + type = "digital_output.select" + order = 20 + menu = "active digital outputs" + + [rio.canifier] + type = "menu" + order = 40 + menu = "work with a canifier" + [rio.canifier.select] + type = "canifier.select" + order = 10 + menu = "active canifier" + [rio.canifier.status] + type = "canifier.status" + order = 20 + menu = "status of active canifier" + [rio.canifier.general_inputs] + type = "canifier.inputs" order = 40 - menu = "velocity modes" - [FxP6.mode.velocity.acceleration] - type = "p6.param" - order = 20 - menu = "select acceleration target" - param = "ACCELERATION" - [FxP6.mode.velocity.feedFwd] - type = "p6.param" - order = 30 - menu = "feed forward" - param = "FEED_FORWARD" - [FxP6.mode.motion] - type = "menu" + menu = "state of general inputs" + [rio.canifier.pwm_inputs] + type = "canifier.pwm_in" order = 50 - menu = "motion magic modes" - [FxP6.mode.motion.motionType] - type = "p6.mmType" + menu = "state of PWM inputs" + [rio.canifier.quad] + type = "canifier.param" + order = 60 + menu = "quadrature encoder position" + param = "QUAD_POSITION" + [rio.canifier.pwm_output] + type = "canifier.pwm_out" + order = 70 + menu = "set PWM output" + [rio.canifier.general_output] + type = "menu" + order = 80 + menu = "set general output pins" + [rio.canifier.general_output.limf] + type = "default" order = 10 - menu = "select motion magic type" - [FxP6.mode.motion.feedFwd] - type = "p6.param" + menu = "LIMF" + [rio.canifier.general_output.limr] + type = "default" order = 20 - menu = "select feed forward (sel unit)" - param = "FEED_FORWARD" - [FxP6.mode.motion.velocity] - type = "p6.param" + menu = "LIMR" + [rio.canifier.general_output.quad_a] + type = "default" order = 30 - menu = "select velocity target (rot/s)" - param = "VELOCITY" - [FxP6.mode.motion.acceleration] - type = "p6.param" + menu = "QUAD A" + [rio.canifier.general_output.quad_b] + type = "default" order = 40 - menu = "select acceleration target (rot/s^2)" - param = "ACCELERATION" - [FxP6.mode.motion.jerk] - type = "p6.param" + menu = "QUAD B" + [rio.canifier.general_output.quad_idx] + type = "default" order = 50 - menu = "select jerk target (rot/s^3)" - param = "JERK" - [FxP6.mode.diff] + menu = "QUAD IDX" + [rio.canifier.general_output.scl] + type = "default" + order = 60 + menu = "SCL" + [rio.canifier.general_output.sda] + type = "default" + order = 70 + menu = "SDA" + [rio.canifier.general_output.spi_clk_pwm0] + type = "default" + order = 80 + menu = "SPI CLK PWM 0" + [rio.canifier.general_output.spi_cs] + type = "default" + order = 90 + menu = "SPI CS" + [rio.canifier.general_output.spi_miso_pwm2] + type = "default" + order = 100 + menu = "SPI MISO PWM 2" + [rio.canifier.general_output.spi_mosi_pwm1] + type = "default" + order = 110 + menu = "SPI MOSI PWM 1" + + [rio.canifier.frame] type = "menu" - order = 60 - menu = "differential modes" - [FxP6.mode.diff.diffType] - type = "p6.diffType" + order = 90 + menu = "configure CAN bus frames" + [rio.canifier.frame.grapher] + type = "default" order = 10 - menu = "select differential type" - [FxP6.mode.diff.slot] - type = "p6.param" + menu = "set grapher defaults" + [rio.canifier.frame.general_1] + type = "canifier.param" order = 20 - menu = "select differential slot" - param = "DIFFERENTIAL_SLOT" - [FxP6.mode.diff.target] - type = "p6.param" + menu = "general 1 status frame" + param = "STATUS_GENERAL1" + [rio.canifier.frame.general_2] + type = "canifier.param" order = 30 - menu = "select differential target (rots)" - param = "DIFFERENTIAL_TARGET" - [FxP6.mode.follow] + menu = "general 2 status frame" + param = "STATUS_GENERAL2" + [rio.canifier.frame.pwm_0] + type = "canifier.param" + order = 40 + menu = "PWM input 0 status frame" + param = "STATUS_PWM_INPUTS0" + [rio.canifier.frame.pwm_1] + type = "canifier.param" + order = 50 + menu = "PWM input 1 status frame" + param = "STATUS_PWM_INPUTS1" + [rio.canifier.frame.pwm_2] + type = "canifier.param" + order = 60 + menu = "PWM input 2 status frame" + param = "STATUS_PWM_INPUTS2" + [rio.canifier.frame.pwm_3] + type = "canifier.param" + order = 70 + menu = "PWM input 3 status frame" + param = "STATUS_PWM_INPUTS3" + [rio.canifier.factory_defaults] + type = "canifier.param" + order = 100 + menu = "configure factory defaults" + param = "FACTORY_DEFAULTS" + + + [rio.swerve] + type = "menu" + order = 50 + menu = "work with swerve" + [rio.swerve.lock] + type = "swerve.azimuth" + order = 10 + menu = "lock azimuth position" + [rio.swerve.azimuth] type = "menu" - order = 70 - menu = "follower modes" - [FxP6.mode.follow.followType] - type = "p6.followType" + order = 20 + menu = "update azimuth zero positions" + [rio.swerve.azimuth.save] + type = "swerve.azimuth.save" order = 10 - menu = "select follower type" - [FxP6.mode.follow.oppose] - type = "p6.param" + menu = "save azimuth zero positions" + [rio.swerve.azimuth.select] + type = "swerve.azimuth.select" order = 20 - menu = "oppose leader direction" - param = "OPPOSE_MAIN" - [FxP6.mode.neutral] - type = "menu" - order = 80 - memnu = "neutral modes" - [FxP6.mode.neutral.select] - type = "p6.neutralOut" - order = 10 - menu = "select neutral output" - [FxP6.mode.foc] - type = "p6.param" - order = 100 - menu = "(pro) enable FOC" - param = "FOC" - [FxP6.mode.overrideNeut] - type = "p6.param" - order = 110 - menu = "override neutral mode" - param = "OVERRIDE_NEUTRAL" - [FxP6.mode.limFwd] - type = "p6.param" - order = 120 - menu = "limit forward motion" - param = "LIM_FWD_MOT" - [FxP6.mode.limRev] - type = "p6.param" - order = 130 - menu = "limit reverse motion" - param = "LIM_REV_MOT" - [FxP6.units] - type = "p6.units" - order = 50 - menu = "select active units" - [FxP6.feedback] + menu = "active azimuth" + [rio.swerve.azimuth.adjust] + type = "swerve.azimuth.adjust" + order = 30 + menu = "active azimuth adjustment" + + [rio.gyro] type = "menu" order = 60 - menu = "setup feedback configs" - [FxP6.feedback.position] - type = "p6.param" - order = 5 - menu = "set position" - param = "POSITION" - [FxP6.feedback.offset] - type = "p6.param" + menu = "work with gyro" + [rio.gyro.pigeon] + type = "menu" order = 10 - menu = "set rotor offset" - param = "ROTOR_OFFSET" - [FxP6.feedback.sense2mech] - type = "p6.param" - order = 20 - menu = "set sensor to mechanism ratio" - param = "SENSOR_TO_MECH_RATIO" - [FxP6.feedback.rotor2sense] - type = "p6.param" - order = 30 - menu = "set rotor to sensor ratio" - param = "ROTOR_TO_SENSOR_RATIO" - [FxP6.feedback.remoteId] - type = "p6.param" - order = 40 - menu = "set remote sensor id" - param = "REMOTE_SENSOR_ID" - [FxP6.slot] + menu = "work with pigeon" + [rio.gyro.pigeon.select] + type = "pigeon.select" + order = 5 + menu = "select active pigeon" + [rio.gyro.pigeon.data] + type = "menu" + order = 10 + menu = "set axis info" + [rio.gyro.pigeon.data.yaw] + type = "pigeon.param" + order = 10 + menu = "set yaw" + param = "YAW" + [rio.gyro.pigeon.data.accumZ] + type = "pigeon.param" + order = 20 + menu = "set accumulator Z-angle" + param = "ACCUM_Z_ANGLE" + [rio.gyro.pigeon.data.fuseHead] + type = "pigeon.param" + order = 30 + menu = "set fused heading" + param = "FUSED_HEADING" + [rio.gyro.pigeon.data.tempComp] + type = "pigeon.param" + order = 40 + menu = "disable temperature compensation" + param = "TEMP_COMP_DISABLE" + [rio.gyro.pigeon.status] + type = "menu" + order = 20 + menu = "set status frame periods" + [rio.gyro.pigeon.status.general] + type = "pigeon.param" + order = 10 + menu = "set general status frame" + param = "GENERAL_STATUS" + [rio.gyro.pigeon.status.sixDeg] + type = "pigeon.param" + order = 20 + menu = "set roll, pitch, yaw status frame" + param = "SIX_DEG_STATUS" + [rio.gyro.pigeon.status.fused] + type = "pigeon.param" + order = 30 + menu = "set 9-axis fused status frame" + param = "FUSED_STATUS" + [rio.gyro.pigeon.status.accumGyro] + type = "pigeon.param" + order = 40 + menu = "set accumulated gyro status frame" + param = "GYRO_ACCUM_STATUS" + [rio.gyro.pigeon.status.genCompass] + type = "pigeon.param" + order = 50 + menu = "set general compass status frame" + param = "GEN_COMPASS_STATUS" + [rio.gyro.pigeon.status.genAccel] + type = "pigeon.param" + order = 60 + menu = "set general accelerometer status frame" + param = "GEN_ACCEL_STATUS" + [rio.gyro.pigeon.status.quaternion] + type = "pigeon.param" + order = 70 + menu = "set quaternion status frame" + param = "SIX_QUAT_STATUS" + [rio.gyro.pigeon.status.mag] + type = "pigeon.param" + order = 80 + menu = "raw magnetometer status frame" + param = "MAG_STATUS" + [rio.gyro.pigeon.status.biasGyro] + type = "pigeon.param" + order = 90 + menu = "biased gyro status frame" + param = "BIAS_GYRO_STATUS" + [rio.gyro.pigeon.status.biasAccel] + type = "pigeon.param" + order = 100 + menu = "biased accelerometer status frame" + param = "BIAS_ACCEL_STATUS" + [rio.gyro.pigeon.defaults] + type = "pigeon.param" + order = 30 + menu = "config factory defaults" + param = "FACTORY_DEFAULT" + + [rio.cancoder] type = "menu" order = 70 - menu = "setup slot configs" - [FxP6.slot.active] - type = "p6.slot" + menu = "work with CANcoders" + [rio.cancoder.select] + type = "cancoder.select" order = 10 - menu = "active slot" - [FxP6.slot.kP] - type = "p6.param" + menu = "select active CANcoders" + bus = "rio" + [rio.cancoder.status] + type = "cancoder.status" order = 20 - menu = "kP" - param = "SLOT_KP" - [FxP6.slot.kI] - type = "p6.param" + menu = "status of active CANcoders" + bus = "rio" + [rio.cancoder.position] + type = "cancoder.param" + param = "POSITION" order = 30 - menu = "kI" - param = "SLOT_KI" - [FxP6.slot.kD] - type = "p6.param" + menu = "set CANcoder position" + bus = "rio" + [rio.cancoder.magOffset] + type = "cancoder.param" + param = "MAG_OFFSET" order = 40 - menu = "kD" - param = "SLOT_KD" - [FxP6.slot.kS] - type = "p6.param" + bus = "rio" + [rio.cancoder.absRange] + type = "cancoder.absRange" order = 50 - menu = "kS" - param = "SLOT_KS" - [FxP6.slot.kV] - type = "p6.param" + bus = "rio" + [rio.cancoder.sensorDirection] + type = "cancoder.sensorDirection" order = 60 - menu = "kV" - param = "SLOT_KV" - [FxP6.slot.kA] - type = "p6.param" - order = 65 - menu = "kA" - param = "SLOT_KA" - [FxP6.slot.kG] - type = "p6.param" - order = 70 - menu = "kG" - param = "SLOT_KG" - [FxP6.slot.gravity] - type = "p6.gravity" - order = 80 - menu = "gravity type" - [FxP6.motion] - type = "menu" - order = 80 - menu = "setup motion magic configs" - [FxP6.motion.accel] - type = "p6.param" - order = 10 - menu = "acceleration" - param = "MM_ACCEL" - [FxP6.motion.cruise] - type = "p6.param" - order = 20 - menu = "cruise velocity" - param = "MM_CRUISE_VEL" - [FxP6.motion.jerk] - type = "p6.param" - order = 30 - menu = "jerk" - param = "MM_JERK" - [FxP6.motion.kA] - type = "p6.param" - order = 40 - menu = "Expo kA" - param = "MM_EXPO_KA" - [FxP6.motion.kV] - type = "p6.param" - order = 50 - menu = "Expo kV" - param = "MM_EXPO_KV" - [FxP6.diff] - type = "menu" - order = 90 - menu = "setup differential configs" - [FxP6.diff.duty] - type = "p6.param" - order = 10 - menu = "peak differential duty cycle" - param = "PEAK_DIFF_DC" - [FxP6.diff.torque] - type = "p6.param" - order = 20 - menu = "peak differential torque current" - param = "PEAK_DIFF_TORQUE" - [FxP6.diff.volt] - type = "p6.param" - order = 30 - menu = "peak differential voltage" - param = "PEAK_DIFF_VOLT" - [FxP6.diff.remote] - type = "p6.param" - order = 40 - menu = "differential remote id" - param = "DIFF_SENSOR_REMOTE_ID" - [FxP6.diff.fx] - type = "p6.param" - order = 50 - menu = "differential fx id" - param = "DIFF_FX_ID" - [FxP6.current] + bus = "rio" + + + +[canivore] + type = "menu" + order = 20 + menu = "work on CANivore" + [canivore.FxP6] type = "menu" - order = 100 - menu = "setup current limit configs" - [FxP6.current.statorEn] - type = "p6.param" + order = 17 + menu = "work with talonFX's" + [canivore.FxP6.run] + type = "p6.run" order = 10 - menu = "stator limit enable" - param = "STATOR_LIM_EN" - [FxP6.current.statorLim] - type = "p6.param" + menu = "run active talon fx's" + bus = "canivore" + [canivore.FxP6.select] + type = "p6.select" order = 20 - menu = "stator current limit" - param = "STATOR_LIM" - [FxP6.current.suppEn] - type = "p6.param" + menu = "active fx's" + bus = "canivore" + [canivore.FxP6.status] + type = "p6.status" order = 30 - menu = "supply limit enable" - param = "SUPP_LIM_EN" - [FxP6.current.suppLim] - type = "p6.param" + menu = "status of active talonFx's" + bus = "canivore" + [canivore.FxP6.mode] + type = "p6.modeMenu" order = 40 - menu = "supply current limit" - param = "SUPP_LIM" - [FxP6.current.tripI] - type = "p6.param" + menu = "config active mode" + bus = "canivore" + [canivore.FxP6.mode.status] + type = "p6.modeStatus" + order = 10 + menu = "active mode" + bus = "canivore" + [canivore.FxP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + bus = "canivore" + [canivore.FxP6.mode.openLoop] + type = "menu" + order = 20 + menu = "open loop modes" + [canivore.FxP6.mode.openLoop.deadband] + type = "p6.param" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" + bus = "canivore" + [canivore.FxP6.mode.position] + type = "menu" + order = 30 + menu = "position modes" + [canivore.FxP6.mode.position.velocity] + type = "p6.param" + order = 20 + menu = "velocity target" + param = "VELOCITY" + bus = "canivore" + [canivore.FxP6.mode.position.feedFwd] + type = "p6.param" + order = 30 + menu = "velocity feed forward" + param = "FEED_FORWARD" + bus = "canivore" + [canivore.FxP6.mode.velocity] + type = "menu" + order = 40 + menu = "velocity modes" + [canivore.FxP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + bus = "canivore" + [canivore.FxP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + bus = "canivore" + [canivore.FxP6.mode.motion] + type = "menu" + order = 50 + menu = "motion magic modes" + bus = "canivore" + [canivore.FxP6.mode.motion.motionType] + type = "p6.mmType" + order = 10 + menu = "select motion magic type" + bus = "canivore" + [canivore.FxP6.mode.motion.feedFwd] + type = "p6.param" + order = 20 + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" + bus = "canivore" + [canivore.FxP6.mode.motion.velocity] + type = "p6.param" + order = 30 + menu = "select velocity target (rot/s)" + param = "VELOCITY" + bus = "canivore" + [canivore.FxP6.mode.motion.acceleration] + type = "p6.param" + order = 40 + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" + bus = "canivore" + [canivore.FxP6.mode.motion.jerk] + type = "p6.param" + order = 50 + menu = "select jerk target (rot/s^3)" + param = "JERK" + bus = "canivore" + [canivore.FxP6.mode.diff] + type = "menu" + order = 60 + menu = "differential modes" + [canivore.FxP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" + bus = "canivore" + [canivore.FxP6.mode.diff.slot] + type = "p6.param" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" + bus = "canivore" + [canivore.FxP6.mode.diff.target] + type = "p6.param" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" + bus = "canivore" + [canivore.FxP6.mode.follow] + type = "menu" + order = 70 + menu = "follower modes" + [canivore.FxP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" + bus = "canivore" + [canivore.FxP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + bus = "canivore" + [canivore.FxP6.mode.neutral] + type = "menu" + order = 80 + menu = "neutral modes" + [canivore.FxP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + bus = "canivore" + [canivore.FxP6.mode.foc] + type = "p6.param" + order = 100 + menu = "(pro) enable FOC" + param = "FOC" + bus = "canivore" + [canivore.FxP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + bus = "canivore" + [canivore.FxP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + bus = "canivore" + [canivore.FxP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" + bus = "canivore" + [canivore.FxP6.units] + type = "p6.units" order = 50 - menu = "supply current trip threshold (A)" - param = "SUPP_TRIP_THRES" - [FxP6.current.tripT] - type = "p6.param" + menu = "select active units" + bus = "canivore" + [canivore.FxP6.feedback] + type = "menu" order = 60 - menu = "supply current trip time (s)" - param = "SUPP_TRIP_TIME" - [FxP6.motor] - type = "menu" - order = 110 - menu = "setup motor output and ramp configs" - [FxP6.motor.invert] - type = "p6.invert" - order = 10 - menu = "motor inversion" - [FxP6.motor.neutral] - type = "p6.neutral" - order = 20 - menu = "neutral mode" - [FxP6.motor.duty] + menu = "setup feedback configs" + [canivore.FxP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" + bus = "canivore" + [canivore.FxP6.feedback.offset] + type = "p6.param" + order = 10 + menu = "set rotor offset" + param = "ROTOR_OFFSET" + bus = "canivore" + [canivore.FxP6.feedback.sense2mech] + type = "p6.param" + order = 20 + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + bus = "canivore" + [canivore.FxP6.feedback.rotor2sense] + type = "p6.param" + order = 30 + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + bus = "canivore" + [canivore.FxP6.feedback.source] + type = "p6.feedback" + order = 35 + menu = "set feedback source" + bus = "canivore" + [canivore.FxP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + bus = "canivore" + [canivore.FxP6.slot] type = "menu" - order = 30 - menu = "duty cycle configs" - [FxP6.motor.duty.peakFwd] - type= "p6.param" + order = 70 + menu = "setup slot configs" + [canivore.FxP6.slot.active] + type = "p6.slot" + order = 10 + menu = "active slot" + bus = "canivore" + [canivore.FxP6.slot.kP] + type = "p6.param" order = 20 - menu = "peak forward %" - param = "PEAK_FWD_DC" - [FxP6.motor.duty.peakRev] + menu = "kP" + param = "SLOT_KP" + bus = "canivore" + [canivore.FxP6.slot.kI] type = "p6.param" order = 30 - menu = "peak reverse %" - param = "PEAK_REV_DC" - [FxP6.motor.duty.dead] + menu = "kI" + param = "SLOT_KI" + bus = "canivore" + [canivore.FxP6.slot.kD] type = "p6.param" order = 40 - menu = "neutral deadband (%)" - param = "NEUTRAL_DEADBAND_DC" - [FxP6.motor.duty.openRamp] + menu = "kD" + param = "SLOT_KD" + bus = "canivore" + [canivore.FxP6.slot.kS] type = "p6.param" order = 50 - menu = "open loop ramp (%)" - param = "OPEN_LOOP_RAMP_DC" - [FxP6.motor.duty.closedRamp] + menu = "kS" + param = "SLOT_KS" + bus = "canivore" + [canivore.FxP6.slot.kV] type = "p6.param" order = 60 - menu = "closed loop ramp (%)" - param = "CLOSED_LOOP_RAMP_DC" - [FxP6.motor.volt] + menu = "kV" + param = "SLOT_KV" + bus = "canivore" + [canivore.FxP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + bus = "canivore" + [canivore.FxP6.slot.kG] + type = "p6.param" + order = 70 + menu = "kG" + param = "SLOT_KG" + bus = "canivore" + [canivore.FxP6.slot.gravity] + type = "p6.gravity" + order = 80 + menu = "gravity type" + bus = "canivore" + [canivore.FxP6.motion] type = "menu" - order = 40 - menu = "voltage configs" - [FxP6.motor.volt.peakFwd] + order = 80 + menu = "setup motion magic configs" + [canivore.FxP6.motion.accel] type = "p6.param" order = 10 - menu = "peak forward (V)" - param = "PEAK_FWD_V" - [FxP6.motor.volt.peakRev] + menu = "acceleration" + param = "MM_ACCEL" + bus = "canivore" + [canivore.FxP6.motion.cruise] type = "p6.param" order = 20 - menu = "peak reverse (V)" - param = "PEAK_REV_V" - [FxP6.motor.volt.supplyConst] + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + bus = "canivore" + [canivore.FxP6.motion.jerk] type = "p6.param" order = 30 - menu = "supply voltage time const" - param = "SUPPLY_V_TIME_CONST" - [FxP6.motor.volt.openRamp] + menu = "jerk" + param = "MM_JERK" + bus = "canivore" + [canivore.FxP6.motion.kA] type = "p6.param" order = 40 - menu = "open loop ramp (V)" - param = "OPEN_LOOP_RAMP_V" - [FxP6.motor.volt.closedRamp] + menu = "Expo kA" + param = "MM_EXPO_KA" + bus = "canivore" + [canivore.FxP6.motion.kV] type = "p6.param" order = 50 - menu = "closed loop ramp (V)" - param = "CLOSED_LOOP_RAMP_V" - [FxP6.motor.torque] + menu = "Expo kV" + param = "MM_EXPO_KV" + bus = "canivore" + [canivore.FxP6.diff] type = "menu" - order = 50 - menu = "torque current configs" - [FxP6.motor.torque.peakFwd] + order = 90 + menu = "setup differential configs" + [canivore.FxP6.diff.duty] type = "p6.param" order = 10 - menu = "peak forward (A)" - param = "PEAK_FWD_I" - [FxP6.motor.torque.peakRev] + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + bus = "canivore" + [canivore.FxP6.diff.torque] type = "p6.param" order = 20 - menu = "peak reverse (A)" - param = "PEAK_REV_I" - [FxP6.motor.torque.dead] + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + bus = "canivore" + [canivore.FxP6.diff.volt] type = "p6.param" order = 30 - menu = "neutral deadband (A)" - param = "TORQUE_NEUTRAL_DEADBAND" - [FxP6.motor.openRamp] + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + bus = "canivore" + [canivore.FxP6.diff.remote] type = "p6.param" order = 40 - menu = "open loop ramp (A)" - param = "OPEN_LOOP_RAMP_I" - [FxP6.motor.closedRamp] + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + bus = "canivore" + [canivore.FxP6.diff.fx] type = "p6.param" order = 50 - menuu = "closed loop ramp (A)" - param = "CLOSED_LOOP_RAMP_I" - [FxP6.limits] - type = "menu" - order = 120 - menu = "setup hard/soft limits" - [FxP6.limits.hard] + menu = "differential fx id" + param = "DIFF_FX_ID" + bus = "canivore" + [canivore.FxP6.current] type = "menu" - order = 10 - menu = "hard limits" - [FxP6.limits.hard.fwdEn] + order = 100 + menu = "setup current limit configs" + [canivore.FxP6.current.statorEn] type = "p6.param" order = 10 - menu = "FWD limit enable" - param = "FWD_HARD_EN" - [FxP6.limits.hard.fwdNormal] - type = "p6.fwdNorm" - order = 10 - menu = "FWD limit normal" - [FxP6.limits.hard.fwdSource] - type = "p6.fwdSource" + menu = "stator limit enable" + param = "STATOR_LIM_EN" + bus = "canivore" + [canivore.FxP6.current.statorLim] + type = "p6.param" order = 20 - menu = "FWD limit source" - [FxP6.limits.hard.fwdRemote] + menu = "stator current limit" + param = "STATOR_LIM" + bus = "canivore" + [canivore.FxP6.current.suppEn] type = "p6.param" order = 30 - menu = "FWD limit remote id" - param = "FWD_REMOTE_ID" - [FxP6.limits.hard.fwdAutoSet] + menu = "supply limit enable" + param = "SUPP_LIM_EN" + bus = "canivore" + [canivore.FxP6.current.suppLim] type = "p6.param" order = 40 - menu = "Autoset Position on FWD limit" - param = "FWD_AUTOSET_POS" - [FxP6.limits.hard.fwdAutoSetValue] + menu = "supply current limit" + param = "SUPP_LIM" + bus = "canivore" + [canivore.FxP6.current.tripI] type = "p6.param" order = 50 - menu = "FWD autoset position value" - param = "FWD_AUTOSET_POS_VALUE" - [FxP6.limits.hard.revEn] + menu = "supply current trip threshold (A)" + param = "SUPP_TRIP_THRES" + bus = "canivore" + [canivore.FxP6.current.tripT] type = "p6.param" order = 60 - menu = "REV limit enable" - param = "REV_HARD_EN" - [FxP6.limits.hard.revNormal] - type = "p6.revNorm" - order = 70 - menu = "REV limit normal" - [FxP6.limits.hard.revSource] - type = "p6.revSource" - order = 80 - menu = "REV limit source" - [FxP6.limits.hard.revRemote] - type = "p6.param" - order = 90 - menu = "REV limit remote id" - param = "REV_REMOTE_ID" - [FxP6.limits.hard.revAutoSet] - type = "p6.param" - order = 100 - menu = "Autoset position on REV limit" - param = "REV_AUTOSET_POS" - [FxP6.limits.hard.revAutoSetValue] - type = "p6.param" - order = 110 - menu = "REV autoset position value" - param = "REV_AUTOSET_POS_VALUE" - [FxP6.limits.soft] + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" + bus = "canivore" + [canivore.FxP6.motor] type = "menu" - order = 20 - menu = "soft limits" - [FxP6.limits.soft.fwdEn] + order = 110 + menu = "setup motor output and ramp configs" + [canivore.FxP6.motor.invert] + type = "p6.invert" + order = 10 + menu = "motor inversion" + bus = "canivore" + [canivore.FxP6.motor.neutral] + type = "p6.neutral" + order = 20 + menu = "neutral mode" + bus = "canivore" + [canivore.FxP6.motor.duty] + type = "menu" + order = 30 + menu = "duty cycle configs" + [canivore.FxP6.motor.duty.peakFwd] + type= "p6.param" + order = 20 + menu = "peak forward %" + param = "PEAK_FWD_DC" + bus = "canivore" + [canivore.FxP6.motor.duty.peakRev] + type = "p6.param" + order = 30 + menu = "peak reverse %" + param = "PEAK_REV_DC" + bus = "canivore" + [canivore.FxP6.motor.duty.dead] + type = "p6.param" + order = 40 + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" + bus = "canivore" + [canivore.FxP6.motor.duty.openRamp] + type = "p6.param" + order = 50 + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" + bus = "canivore" + [canivore.FxP6.motor.duty.closedRamp] + type = "p6.param" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" + bus = "canivore" + [canivore.FxP6.motor.volt] + type = "menu" + order = 40 + menu = "voltage configs" + [canivore.FxP6.motor.volt.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (V)" + param = "PEAK_FWD_V" + bus = "canivore" + [canivore.FxP6.motor.volt.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (V)" + param = "PEAK_REV_V" + bus = "canivore" + [canivore.FxP6.motor.volt.supplyConst] + type = "p6.param" + order = 30 + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" + bus = "canivore" + [canivore.FxP6.motor.volt.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" + bus = "canivore" + [canivore.FxP6.motor.volt.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" + bus = "canivore" + [canivore.FxP6.motor.torque] + type = "menu" + order = 50 + menu = "torque current configs" + [canivore.FxP6.motor.torque.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (A)" + param = "PEAK_FWD_I" + bus = "canivore" + [canivore.FxP6.motor.torque.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (A)" + param = "PEAK_REV_I" + bus = "canivore" + [canivore.FxP6.motor.torque.dead] + type = "p6.param" + order = 30 + menu = "neutral deadband (A)" + param = "TORQUE_NEUTRAL_DEADBAND" + bus = "canivore" + [canivore.FxP6.motor.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" + bus = "canivore" + [canivore.FxP6.motor.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + bus = "canivore" + [canivore.FxP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [canivore.FxP6.limits.hard] + type = "menu" + order = 10 + menu = "hard limits" + [canivore.FxP6.limits.hard.fwdEn] + type = "p6.param" + order = 5 + menu = "FWD limit enable" + param = "FWD_HARD_EN" + bus = "canivore" + [canivore.FxP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 10 + menu = "FWD limit normal" + bus = "canivore" + [canivore.FxP6.limits.hard.fwdSource] + type = "p6.fwdSource" + order = 20 + menu = "FWD limit source" + bus = "canivore" + [canivore.FxP6.limits.hard.fwdRemote] + type = "p6.param" + order = 30 + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" + bus = "canivore" + [canivore.FxP6.limits.hard.fwdAutoSet] + type = "p6.param" + order = 40 + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + bus = "canivore" + [canivore.FxP6.limits.hard.fwdAutoSetValue] + type = "p6.param" + order = 50 + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + bus = "canivore" + [canivore.FxP6.limits.hard.revEn] + type = "p6.param" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" + bus = "canivore" + [canivore.FxP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + bus = "canivore" + [canivore.FxP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + bus = "canivore" + [canivore.FxP6.limits.hard.revRemote] + type = "p6.param" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" + bus = "canivore" + [canivore.FxP6.limits.hard.revAutoSet] + type = "p6.param" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" + bus = "canivore" + [canivore.FxP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + bus = "canivore" + [canivore.FxP6.limits.soft] + type = "menu" + order = 20 + menu = "soft limits" + [canivore.FxP6.limits.soft.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_SOFT_EN" + bus = "canivore" + [canivore.FxP6.limits.soft.fwdThres] + type = "p6.param" + order = 20 + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" + bus = "canivore" + [canivore.FxP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + bus = "canivore" + [canivore.FxP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + bus = "canivore" + [canivore.FxP6.audio] + type = "menu" + order = 130 + menu = "setup audio configs" + [canivore.FxP6.audio.disable] type = "p6.param" order = 10 - menu = "FWD limit enable" - param = "FWD_SOFT_EN" - [FxP6.limits.soft.fwdThres] + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" + bus = "canivore" + [canivore.FxP6.audio.beepBoot] type = "p6.param" order = 20 - menu = "FWD soft limit" - param = "FWD_SOFT_THRES" - [FxP6.limits.soft.revEn] + menu = "beep on boot" + param = "BEEP_ON_BOOT" + bus = "canivore" + [canivore.FxP6.audio.beepConfig] type = "p6.param" order = 30 - menu = "REV limit enable" - param = "REV_SOFT_EN" - [FxP6.limits.soft.revThres] + menu = "beep on config" + param = "BEEP_ON_CONFIG" + bus = "canivore" + [canivore.FxP6.factory] + type = "p6.factory" + order = 140 + menu = "config factory default" + bus = "canivore" + [canivore.FxP6.grapher] + type = "menu" + order = 150 + menu = "set status frames" + [canivore.FxP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + bus = "canivore" + [canivore.FxP6.grapher.rate] type = "p6.param" - order = 40 - menu = "REV soft limit" - param = "REV_SOFT_THRES" - [FxP6.audio] - type = "menu" - order = 130 - menu = "setup audio configs" - [FxP6.audio.disable] - type = "p6.param" - order = 10 - menu = "allow music on disable" - param = "ALLOW_MUSIC_DIS" - [FxP6.audio.beepBoot] - type = "p6.param" - order = 20 - menu = "beep on boot" - param = "BEEP_ON_BOOT" - [FxP6.audio.beepConfig] - type = "p6.param" - order = 30 - menu = "beep on config" - param = "BEEP_ON_CONFIG" - [FxP6.factory] - type = "p6.factory" - order = 140 - menu = "config factory default" - [FxP6.grapher] - type = "menu" - order = 150 - menu = "set status frames" - [FxP6.grapher.default] - type = "p6.graph" - order = 10 - menu = "set to default update rate" - [FxP6.grapher.rate] - type = "p6.param" - order = 20 - menu = "Grapher Frame Rate (Hz)" - param = "GRAPHER_FRAME" - -[servo] - type = "menu" - order = 20 - menu = "work with servos" - [servo.run] - type = "servo.run" - order = 10 - menu = "set servo positions" - [servo.select] - type = "servo.select" - order = 20 - menu = "active servos" - -[solenoid] - type = "menu" - order = 25 - menu = "work with solenoids" - [solenoid.run] - type = "solenoid.run" - order = 10 - menu = "set solenoid positions" - [solenoid.select] - type = "solenoid.select" - order = 20 - menu = "active solenoids" - -[digital_output] - type = "menu" - order = 30 - menu = "work with digital outputs" - [digital_output.set] - type = "digital_output.run" - order = 10 - menu = "set digital output" - [digital_output.select] - type = "digital_output.select" - order = 20 - menu = "active digital outputs" - -[canifier] - type = "menu" - order = 40 - menu = "work with a canifier" - [canifier.select] - type = "canifier.select" - order = 10 - menu = "active canifier" - [canifier.status] - type = "canifier.status" - order = 20 - menu = "status of active canifier" - [canifier.general_inputs] - type = "canifier.inputs" - order = 40 - menu = "state of general inputs" - [canifier.pwm_inputs] - type = "canifier.pwm_in" - order = 50 - menu = "state of PWM inputs" - [canifier.quad] - type = "canifier.param" - order = 60 - menu = "quadrature encoder position" - param = "QUAD_POSITION" - [canifier.pwm_output] - type = "canifier.pwm_out" - order = 70 - menu = "set PWM output" - [canifier.general_output] - type = "menu" - order = 80 - menu = "set general output pins" - [canifier.general_output.limf] - type = "default" - order = 10 - menu = "LIMF" - [canifier.general_output.limr] - type = "default" - order = 20 - menu = "LIMR" - [canifier.general_output.quad_a] - type = "default" - order = 30 - menu = "QUAD A" - [canifier.general_output.quad_b] - type = "default" - order = 40 - menu = "QUAD B" - [canifier.general_output.quad_idx] - type = "default" - order = 50 - menu = "QUAD IDX" - [canifier.general_output.scl] - type = "default" - order = 60 - menu = "SCL" - [canifier.general_output.sda] - type = "default" - order = 70 - menu = "SDA" - [canifier.general_output.spi_clk_pwm0] - type = "default" - order = 80 - menu = "SPI CLK PWM 0" - [canifier.general_output.spi_cs] - type = "default" - order = 90 - menu = "SPI CS" - [canifier.general_output.spi_miso_pwm2] - type = "default" - order = 100 - menu = "SPI MISO PWM 2" - [canifier.general_output.spi_mosi_pwm1] - type = "default" - order = 110 - menu = "SPI MOSI PWM 1" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" + bus = "canivore" - [canifier.frame] + [canivore.cancoder] type = "menu" - order = 90 - menu = "configure CAN bus frames" - [canifier.frame.grapher] - type = "default" + order = 20 + menu = "work with CANcoders" + [canivore.cancoder.select] + type = "cancoder.select" order = 10 - menu = "set grapher defaults" - [canifier.frame.general_1] - type = "canifier.param" + menu = "select active CANcoders" + bus = "canivore" + [canivore.cancoder.status] + type = "cancoder.status" order = 20 - menu = "general 1 status frame" - param = "STATUS_GENERAL1" - [canifier.frame.general_2] - type = "canifier.param" + menu = "status of active CANcoders" + bus = "canivore" + [canivore.cancoder.position] + type = "cancoder.param" + param = "POSITION" order = 30 - menu = "general 2 status frame" - param = "STATUS_GENERAL2" - [canifier.frame.pwm_0] - type = "canifier.param" + menu = "set CANcoder position" + bus = "canivore" + [canivore.cancoder.magOffset] + type = "cancoder.param" + param = "MAG_OFFSET" order = 40 - menu = "PWM input 0 status frame" - param = "STATUS_PWM_INPUTS0" - [canifier.frame.pwm_1] - type = "canifier.param" + bus = "canivore" + [canivore.cancoder.absRange] + type = "cancoder.absRange" order = 50 - menu = "PWM input 1 status frame" - param = "STATUS_PWM_INPUTS1" - [canifier.frame.pwm_2] - type = "canifier.param" + bus = "canivore" + [canivore.cancoder.sensorDirection] + type = "cancoder.sensorDirection" order = 60 - menu = "PWM input 2 status frame" - param = "STATUS_PWM_INPUTS2" - [canifier.frame.pwm_3] - type = "canifier.param" - order = 70 - menu = "PWM input 3 status frame" - param = "STATUS_PWM_INPUTS3" - [canifier.factory_defaults] - type = "canifier.param" - order = 100 - menu = "configure factory defaults" - param = "FACTORY_DEFAULTS" - - -[swerve] - type = "menu" - order = 50 - menu = "work with swerve" - [swerve.lock] - type = "swerve.azimuth" - order = 10 - menu = "lock azimuth position" - [swerve.azimuth] - type = "menu" - order = 20 - menu = "update azimuth zero positions" - [swerve.azimuth.save] - type = "swerve.azimuth.save" - order = 10 - menu = "save azimuth zero positions" - [swerve.azimuth.select] - type = "swerve.azimuth.select" - order = 20 - menu = "active azimuth" - [swerve.azimuth.adjust] - type = "swerve.azimuth.adjust" - order = 30 - menu = "active azimuth adjustment" - -[gyro] - type = "menu" - order = 60 - menu = "work with gyro" - [gyro.pigeon] - type = "menu" - order = 10 - menu = "work with pigeon" - [gyro.pigeon.select] - type = "pigeon.select" - order = 5 - menu = "select active pigeon" - [gyro.pigeon.data] - type = "menu" - order = 10 - menu = "set axis info" - [gyro.pigeon.data.yaw] - type = "pigeon.param" - order = 10 - menu = "set yaw" - param = "YAW" - [gyro.pigeon.data.accumZ] - type = "pigeon.param" - order = 20 - menu = "set accumulator Z-angle" - param = "ACCUM_Z_ANGLE" - [gyro.pigeon.data.fuseHead] - type = "pigeon.param" - order = 30 - menu = "set fused heading" - param = "FUSED_HEADING" - [gyro.pigeon.data.tempComp] - type = "pigeon.param" - order = 40 - menu = "disable temperature compensation" - param = "TEMP_COMP_DISABLE" - [gyro.pigeon.status] - type = "menu" - order = 20 - menu = "set status frame periods" - [gyro.pigeon.status.general] - type = "pigeon.param" - order = 10 - menu = "set general status frame" - param = "GENERAL_STATUS" - [gyro.pigeon.status.sixDeg] - type = "pigeon.param" - order = 20 - menu = "set roll, pitch, yaw status frame" - param = "SIX_DEG_STATUS" - [gyro.pigeon.status.fused] - type = "pigeon.param" - order = 30 - menu = "set 9-axis fused status frame" - param = "FUSED_STATUS" - [gyro.pigeon.status.accumGyro] - type = "pigeon.param" - order = 40 - menu = "set accumulated gyro status frame" - param = "GYRO_ACCUM_STATUS" - [gyro.pigeon.status.genCompass] - type = "pigeon.param" - order = 50 - menu = "set general compass status frame" - param = "GEN_COMPASS_STATUS" - [gyro.pigeon.status.genAccel] - type = "pigeon.param" - order = 60 - menu = "set general accelerometer status frame" - param = "GEN_ACCEL_STATUS" - [gyro.pigeon.status.quaternion] - type = "pigeon.param" - order = 70 - menu = "set quaternion status frame" - param = "SIX_QUAT_STATUS" - [gyro.pigeon.status.mag] - type = "pigeon.param" - order = 80 - menu = "raw magnetometer status frame" - param = "MAG_STATUS" - [gyro.pigeon.status.biasGyro] - type = "pigeon.param" - order = 90 - menu = "biased gyro status frame" - param = "BIAS_GYRO_STATUS" - [gyro.pigeon.status.biasAccel] - type = "pigeon.param" - order = 100 - menu = "biased accelerometer status frame" - param = "BIAS_ACCEL_STATUS" - [gyro.pigeon.defaults] - type = "pigeon.param" - order = 30 - menu = "config factory defaults" - param = "FACTORY_DEFAULT" - -[cancoder] - type = "menu" - order = 70 - menu = "work with CANcoders" - [cancoder.select] - type = "cancoder.select" - order = 10 - menu = "select active CANcoders" - [cancoder.status] - type = "cancoder.status" - order = 20 - menu = "status of active CANcoders" - [cancoder.position] - type = "cancoder.param" - param = "POSITION" - order = 30 - menu = "set CANcoder position" - [cancoder.magOffset] - type = "cancoder.param" - param = "MAG_OFFSET" - order = 40 - [cancoder.absRange] - type = "cancoder.absRange" - order = 50 - [cancoder.sensorDirection] - type = "cancoder.sensorDirection" - order = 60 + bus = "canivore" diff --git a/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt b/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt index 562a512..c2d0593 100644 --- a/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt +++ b/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt @@ -9,7 +9,7 @@ internal class CheckTomlTest { @Test fun `check commands`() { val toml = parseResource("/commands.toml") - val table = toml["talon.output.open-loop_ramp"] as TomlTable + val table = toml["rio.talon.output.open-loop_ramp"] as TomlTable assertThat(table["type"]).isEqualTo("talon.param") } diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a4079..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, From e96dca9d8dc0e15761032eabbf53ecbe7d859f57 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Thu, 11 Apr 2024 20:17:56 -0400 Subject: [PATCH 7/7] fix rio fx's --- .../kotlin/org/strykeforce/thirdcoast/Koin.kt | 2 +- .../cancoder/SelectAbsRangeValueCommand.kt | 2 +- .../SelectCancoderSensorDirectionCommand.kt | 8 +++--- .../thirdcoast/device/DeviceService.kt | 4 +-- .../thirdcoast/device/TalonFxService.kt | 4 +-- .../talon/phoenix6/P6SelectTalonsCommand.kt | 25 ++++++++++++++----- 6 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index c14f775..ce9d2d4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -46,7 +46,7 @@ val tctModule = module { single { LegacyTalonFxService(get()) { id -> TalonFX(id) } } - single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id, "rio")} } + single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id)} } single {TalonFxFDService(get()) {id -> com.ctre.phoenix6.hardware.TalonFX(id, "*")} } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt index 833d238..ecee81a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt @@ -42,7 +42,7 @@ class SelectAbsRangeValueCommand( if(bus == "rio") { cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} - } else if(bus == "caniover") { + } else if(bus == "canivore") { cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range cancoderFDService.active.forEach { it.configurator.apply(cancoderFDService.activeConfiguration.MagnetSensor) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt index 3cc3acb..2ec8d0b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectCancoderSensorDirectionCommand.kt @@ -10,13 +10,13 @@ import org.strykeforce.thirdcoast.device.CancoderFDService import org.strykeforce.thirdcoast.device.CancoderService private val DIRECTION = listOf( - Clockwise_Positive, - CounterClockwise_Positive + CounterClockwise_Positive, + Clockwise_Positive ) private val LABELS = listOf( - "Clockwise Positive", - "Counter-clockwise Positive" + "Counter-clockwise Positive", + "Clockwise Positive" ) class SelectCancoderSensorDirectionCommand( diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt index a650404..0a8c73b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/DeviceService.kt @@ -32,13 +32,13 @@ open class AbstractDeviceService(private val factory: (id: Int) -> T) : override fun get(id: Int): T { val device = _all[id] ?: factory(id) - if (_all.put(id, device) == null) logger.debug("_all add: {}", device) + if (_all.put(id, device) == null) logger.debug("_all add: {}, id: {}", device, id) return device } fun get(id: Int, bus: String): T { val device = _all[id] ?: factory(id) - if(_all.put(id, device) == null) logger.debug("_all add: {}", device) + if(_all.put(id, device) == null) logger.debug("_all add: {}, id: {}", device, id) return device } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt index 754a9f8..07d4f3e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt @@ -189,14 +189,14 @@ open class TalonFxService( return field } - override fun activate(ids: Collection, bus: String): Set { + override fun activate(ids: Collection): Set { dirty = true logger.info { "Number Active: ${active.size}" } active.forEach{ logger.info { "Active TalonFX: ${it.deviceID}" } } - val new = super.activate(ids, bus) + val new = super.activate(ids) logger.info { "Number New: ${new.size}" } telemetryService.stop() active.filter { new.contains(it.deviceID) }.forEach{ diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt index f7b29c1..9ccb58e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt @@ -1,6 +1,7 @@ package org.strykeforce.thirdcoast.talon.phoenix6 import com.ctre.phoenix6.hardware.TalonFX +import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.standalone.inject import org.strykeforce.thirdcoast.command.AbstractCommand @@ -18,6 +19,8 @@ class P6SelectTalonsCommand( toml: TomlTable ): AbstractCommand(parent, key, toml) { + private val logger = KotlinLogging.logger {} + private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() @@ -37,15 +40,25 @@ class P6SelectTalonsCommand( var ids: List var new: Set - ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) + if(bus == "rio") { + ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxService.activate(ids) + } else if(bus == "canivore") { + ids = reader.readIntList(this.prompt("ids"), talonFxFDService.active.map(TalonFX::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxFDService.activate(ids) + } else throw IllegalArgumentException() - new = - if(bus == "rio") talonFxService.activate(ids) - else if(bus == "canivore") talonFxFDService.activate(ids) - else throw IllegalArgumentException() +// ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) +// +// new = +// if(bus == "rio") talonFxService.activate(ids) +// else if(bus == "canivore") talonFxFDService.activate(ids) +// else throw IllegalArgumentException() if(new.isNotEmpty()) { - terminal.info("Activating talons: ${new.joinToString()}") + logger.info("Activating talons: ${new.joinToString()}") } return super.execute() } catch(e: IllegalArgumentException) {