From 3dc348ea29bc0e77637283193c2210a82bc12c20 Mon Sep 17 00:00:00 2001 From: Computer 0 <48131223+TheGamer1002@users.noreply.github.com> Date: Thu, 27 Apr 2023 09:34:57 -0400 Subject: [PATCH] starting encoder stuff --- FRC_TBD_5348e8acf25c64ea.wpilog | Bin 0 -> 68599 bytes src/main/java/frc/robot/RobotContainer.java | 30 ++++++------ .../frc/robot/commands/FineDriveCommand.java | 4 ++ .../java/frc/robot/commands/TrackPiece.java | 15 ++++-- src/main/java/frc/robot/subsystems/Arm.java | 21 +++++++-- src/main/java/frc/robot/subsystems/Claw.java | 6 +-- .../java/frc/robot/subsystems/Vision.java | 43 ++++++++++++------ 7 files changed, 84 insertions(+), 35 deletions(-) create mode 100644 FRC_TBD_5348e8acf25c64ea.wpilog diff --git a/FRC_TBD_5348e8acf25c64ea.wpilog b/FRC_TBD_5348e8acf25c64ea.wpilog new file mode 100644 index 0000000000000000000000000000000000000000..256a1ac74d00c08908556cb5e74fd25ed08b2d9b GIT binary patch literal 68599 zcmb`Q2YejG^~ZP5w(e|$3kD3vcH}mGr zytlhrQCGWg$pTH+@RQU=eVvHY{}U*-n}tGZwOJ^`{|m)@I=kAsXu+nwaV>`b5617} z##xm$iyCUPt+~pI;>Hfsxq1{%Zz}J|=Q_-MF>MygXKgxRLU|$AnQu1BXO%B*EI%O` zix2r;=ig6i%UI35@HO^TvE~J-cC(?wY)-ePn-^KlQseZ}rhTs`wL=-8H!9zy3t;{_ zGh18?idwb3wb`uNg{;~>sB&nRY8R!7&25W2+ndZhJDtuJr%#npOP|Eky_kmSC#fxB z^|O}khF?)H^=3yVwJ`*sEb*ZTID`S>sJyFQNPtFkt5WmhiOsak{!!nUp41kz%KLg$-cU>x)46O2<@8N_i>gs2`$=j` z7@;5jI}E>~_Azd&a@k@&m#Iz_Q%V}@m-zFJu_WqaKS}K{hUky~8u(?1h3R$XigdOm zx4yEX06{tnl?zuS=Vwz*8M8%EQ~mXe8hQhAID_l}z@8k^(AL@7nlYPlseDUiMY~x{ zwLp%_1~X$e7tIzmQ_zlP)$fR^$9PtsH=A?$7EB_Q_4GfcGDShOV;Eups^2>RQIpP? z%$jR*`Sw&%QIHjkG7toN7V1Gyjj5&r8`aj_c;wMCqjun2P7tE;YKr(!1k}8aj!fEYNiOLuc68EQCU0B2C*~i_L%)=buxVcGfH5;3I05&NASe3~YAmhSZt|M8WYG?lx714V* zXy#JMSFkxGNWYd|lU1GhJl23};tqE+o0+(5O5Qg>Ty;(4o#TqAy z#k_owSk~-GJ*Fzwc+Mj4z&u0Hs-nTn7IOJyDA?W{MjJ0?Zb5?(Th>fQRiu5yo?=sD zP*2UDUsu0mQL?JON)a6DPh|`3Y2{(DFNe`+XPzx+STGp&mL|4R zs)AK;7VTl#9L3DoZ?qqrDuf&Cl_F`P1gW}ydAQLxQPdw>e+B7pNwLigInpO_Rx6t= zCW93;&tqc_58aY%$h8)e3)3Nn$7B&XMFLrGVm2(85s*_v~1=cK}OzCOgFFHXbnsmZL)7FVa7(fvHiGW}zEH(dp=~QXd@BMwpWl68-ZrzHPx{7j*3|*jTV*-!1^sLt1>BM z$0H=_8uYJ5mFYZ4*a(_me8!@wC+iv$hmS0C(mo{FGO+x@uf&Ry_gR*>L(KcoT5LI> zej5;ihEDt}cZe+U)5g~7D?ntyfS#rvB5sGw&kBcK8a#)|s5naM=P);vaxjMj>X(Bd zfg|RA4tKaL_d}8OBf#~GxDYpL^5+N#T#9xd3Gf*F`u1?h?9Y)7F9|_uCOQf%znRDq zG*bHKD30an9}4Op4Xj^KpRghGKS%4#RC)Gbr3@%!0%#=%k^5Q&ke{n|D>6(5Xcg0= zlL4a0;4z^3?PZ7>IR$hK??IjdqUd`HWWTV1C2xovP|6vl%qnP!-vn5oRg^+liV>_P z-i@@GZwAS4GjAz0d={vg!zdQm0$88}5kgpsg0+Z(nKFWjAuL6h$zkMr4DljdhKvxJ$k(TB(8uV*G3mo)f2ul&R zhQnypv=*?j`1SR0P6$g8wN^xBWKd2BOA(b3RoE`0LNSD;2y5psieO~{3!L@s(2fvJ zQe=2p9$}nv?!!wWNOtQZ`n8DL!_@q$Y?NZ4M z3t*1Ludf-FT&ikNu)xJ5Z%~vGFG^adC@QlPw7|(saH^`Jb&8^`lMzkys;abgoJMQR z^`Hgra$Rm!6}O(_=ti^+!1>*yMQfpOsi6&Gk8PCcF~PB_3bj$x;c+rLba_@)+;JR7 zV}BEHfu=3GR#n<2PNN0o@t_4RC>`Ib$~s3+Y3D)b}~da?}E_B(=qRfV1`LQj!_dN^2B=qV!fR2ir(so-H%p{H^v zxx3Q7qF;Vm* z56+n~9K}r0nI4=U%WxD^ML%|P=mE5!fD;%350NYSiJL`{ytBXxjO0a27M7c%j5DZ`;dX}5A2 zaDF*qLApo}AYH~Z>!hDDtXnS!o88bX(XEk_P?z(bz50?%?!i4HwBNk~Y`<8KD0_%h z)D_N1X7)oXfGdHNw4YM+OfepLCGTF!@LdI-UnW`*Q4FCeBCp~|N`7Arq~B^oKnig* zMdH<>kk`lyDF)LNk=Jk}`JZcnlw8`lB5I1vYdMqRyuScba=3;{SN%fl#p~pIQHZT6 zig%r;(qGD})D>V;Wd4#f$vmzHQ_?)dD4QbkdXA(u+pmE1+h_^q;e^^0fxi-gzm^9& z@is-^uSKQaAg@$0=%$Fgfg{_QC%F+wNl#+Oql6%sBKby<{2O_)9h3@B{*6fftvuP5 zSqO(I%KTfA{5yHFEpvGC?>L$4^(K&Ir`_-gubbSCo6PoRkR{Ew5LoJmoo`@q}}zY^O)+o1`rR26-n zi2j2-+V*G>(0>ro_sgSgsRd`MD)s#$`T=>gEp-I+0~}56^g*Dd-D$|Q+Jg+J^Vjh_ zBo7RoTziO1NVgR}44~ib55jz}m|}a_y*we>ALVI^iMBs_&>oSeDW=;V@u2-lo~D>| z`;(hS`PWB5lT09lNWDGkCQ@|s&mc-hH=`xs{_FvNOaUG#4fmJ_e5*V>S~6}chx23{ zsePL|JX}g{n;T9^guehTnM8<~nEQ)c@3f729Q43-e!ZE>N6O1RuAjLVn}6vZ{3n1M zi(g;oZXwmva!({CmkVh*lJT#A1+EMU7$qzB*F@s*GLYzp$9SJaA<(xGIdoF)Ne9c4 zaufr33fREq6JfVQPVOnE0W7vn8h;uMkkfdSl-$#9GMVz6X`QS0d1+e6WZd(NhR&qj>o}!2^iz5D?w1{FPPm%b4oJbz$6%ggtF|IhCBJvfE zq@3}qK*|l&P&v3)#U6YOEV&*OVt9(ey(a4Ob!mONB6y0(*Ey2x;te3>>_Uv+DH7k{ zL`uQG31VOho-EOc-YN3l6nSq+^PJe7BJV9xqi;)VRE*py65r-Tx{u=>5aoP_9l;WU zXNur=MDV-PU^{*l9{jEd{+Be^mRJa!DN6h=5&WJs*p@gv_&p9LJAEHu$*DGc;_ZF6 zpC)tt0AM+DE%|BHpy&r&Q1U4sN(&k#?e?Ll+mC>hn*#+;tt#Y4qL3d;3n{v4RpiGU zNo(6rfRx)Ex_q@N^ApabJ7zuwGw_ZXn(D))+&&e1@iXaO6uh;nc%O+X{ajk5E_bcU z{G2n%JpK))+&m!qYgOdGIg%E#UjQk$400T{D)0*t_#bJYqfZw&Wd+Irm&*!7OR?eMGxr#$ zICUKKpg47u^qLXZ_1ng=d6%9QO#tWjtSGzPAjDtEG7|crKBLR5T>#X?{h%S5On@C%R^auMohiJ5Qf=RB^wsqtqf7GjlaX(ED$}X<-c+NXo)|A?@Mzff2h1-H zU!PvxwjG{aH2UlM=)IYU6k6N?RKMMdi>jO?HFnSwpNtc-q$KH%VEWCAA~Qms)Ywr^ zOc2#fF@*tu`|Tw}c-T~_F+d=*TxpKoFFp|DnLYzoBuCGd8Uuwku*@B)eh}IqtNI8D zQ)5sUIHh|A1MU~H6BQ6GXKD-<_$+BkA;X=(_uGDm{D^5&V<({jO#8Gh9)bq=Z7RhE zgv*>7Lj>+12u|7Jox%0X7K_}{19!_q{M|HTXB*G6rzG|+!24|nM0~jXsj-W$GgIe! z-cx&YW6dWRpY78a)I%!neR__M7-%S`IJ!&G=*v;-?o=K&c z*Y0S6-?~F=L5M%HX6$Z5F?}*?q7cn6KxM6|6sA>N4-B)rnZk*Cfan)a6vY$+wTjR^ zY$yf4h65_=u!K;pB6GOSBtb_g2r33^6`><+DEX$5fXXg_T;W

PVYPOXg9a$`0Ky z*;QkdjicU7D)gohvQ-o@X){T&hJs>UL0d&CcEQ#pOk=1VR9RyY!?udhavMskyFCH* z%W8-g>I7~Tk$bw3dnq8D(5)hJFB?g!9j%~RF?g#89c@GDNr^Fl%6b_)lqm#J71?84 z?6C@LJD3@sJ=Vn@r@*%576Pe?a*uPd$1AXHxx=%^+iV(SdxI@K9fwZ88hdwf?PSLL zfGumrCD*PPvNray#idDdf`YiBZ&#&Gu&JcveLA-z|5dM+lXWH743T>d{FGJt{bORUs$YP}1#W1>L$l zyef9GjirG!1z6cBLUi$})G0QVmQYhcm0b`!K3K2jRD$_rnufRC2QtuEmY-hITwJZwEb1U_6UcYX=KobZ&cr~zI!T@f*vTT|j= ze$x1ZISarK-W$;EQW5?Dp0PmJU#n#KKzb(TU_kvIHL#Y;7F0bQc(9(heu9t=q!n{5 zkin}+LPp63*6N9NF&juRRFLuvS|1XR*W#HgSmbEVCsh1@DIgKq{P%XDIbio{he;xY0>CnBgw zJjN!HUQ_aV72|=5%#_WfdrO+Yl=LfhOiT#KDWaQP=w^Ae9T5wUZg!ztJ}H;ltn#NZv(6Uo-_=I0;7f(*+yBcUo^DTLD)qv}2TUeSI+vJ6flJ0A> zp`_t7pprAA;LBA7P1{Tobd9{AqBB>8uCbxC7G4Xeow)B&jwpuUgTmt3vZOl%h`sK!fk)u{LXwpTsl@E_P9#Z96!@ zt*gpibg?_-*&cpfmEGxLuajrnatn@KmA%eplY3ncwzPW;dbG6O$tC)SLs(jmG=GXZ zJzQ4rloI`GqEqJKRL~_e4-u1krDD2hZ z;hCWMMGWUbs*D^Cw{X-6liKkUjSk zUGH?HJ4-^k5dC(Rp14lzw+<%NPr;hyrOAqRvzTg06;qY8`z=&f=hN%V%DTMSYUcB1 zOI5DDBbPO^#WH-mP|T;Zt4HB~o60-#xeha5Oq+%BS({FnP+rJ&=9|s(S>=lx%TF+V zs_%N`#IoviD{8@))2U37DsD;@OrD?Gj3)U_1y)0%r1m%Kx-qjf6>I^|Zz|wC_2INd zAAFlzR;r5MQSdWh{pJFW4LPEIrt5doh@w^J*cHwG<29|3+vgITxV0p zlCaSwwyylAg_4#0ka9SdwNK|_iv1HKJezl%Np7` zTU#?`Q!bTnX-qZochjB+oM(uI;}&ep=ORsj=jnQ%*=)?x8wSn?&hrfeE}XiE=j**Q zGs{?GIp_izP>9Ls0zGkyI9lphpIr#pFt0wVtF8;vVHfHHAMoffy4B$#usm;da3(%R zrQe8i-bH$^v9rri6Ms{~#XyhgNlX&8nZMj21=~}MnqREz7uK?ILVxZ3=Rl40MX8UG zpX-C`AWm+*nXgW>nZK#Cn9gN6@sc27Ra+`MQEh6uL{B^<4i*yaQW?-BHPEGApvz=H zlhr_%d4VpM0ZmZ@UG4?CLIyNd4RnPU=t>#TG&RtbUZAUFK-1MgS9yW1mI3Xj2D;h{ zbd3yXh8pM^FVM9zpqXl*YrBA!vDy6>0I`Kcx6O||&$5|elH%0x3op`jQY6K>;W{tU zFQrI|lfy5)NY_h|6laI)y-2^3A}LM}zw#peT8gANKm6K@bb}O0ae}zPi*%zDNpXg_ z(TnsODU#w8@f$DFZ>30zbHs1GNWYUIO;(&Ee&<^(I7i&zu<|8hg2} ztLiY@p4;_-&v|Tn=$%7%0OxsYD972<2s@BFbp5TmGCiqLKIl#WJ=0HJfa?2@JM|r> z9a2_TTa9>h=~Z|%I`0D0b9B0y9cqi3yY$5KhnHP?3H!NcPEsS(-Eye38tQIB!M^B+ zqwbMIDTZzC>4Ktng5C?1XKc!?z=B!q$fZ@S?%pm$is;@4q5)~UEe>naSzPul-;a=huiijqKk?ekb;5#0Y zEoHO^K-;5dS~10u^+x!-7w3{vI(|Dm|F3mJ5+ zJ6hMJI*Yk%u07XT=yvHScIh8+DLaEIi)NwN?V=>R=n-6$ur9(ixsEPZ84n?$j?0XP z`SFkZ_=t5(Y8CACpHR!rRxN9C8w>1Cz=@TJji7;rzWBT;m&=%`?2$*~|5{S3yLM1q zdlcVc5*Kj2+1`kb>4OqXYWh@mUlCMjN6)JfieH#py(pE1Tg^j(t+{-AstDz>?p#$| z)mbP%A*qdLE%`HQ9gMF-Xs0Eq4JPTRHIqvf?MBe$k9l3*ky}TXQ(LUd4M>RU4OGt# z_!>eI|;y)O4^CfdTFOX+{;i6cv#lMxFxQTiu1rRcMpLlP0_Fg1bxsV7*~C4m|r z+dZzm*fXeuE!LB%?COl7&wEi%Jc_DIp!&a~^b$ZN&+3+FHA|W_r&MT)`t<}Wj(!0jrLSCTD;9TZJ%hgn$LcPTvbzn7 zYp;M*dY;!U5OmetnMoDRmSnLrpG5*Wnd>ZebQY7X`CNN)9qoVAn(&IA7z${q38_Pe z$F*0{gwlr2-Gl}#P+B_i4(&*(xmR_4DLBZO9gV|lz?D*SJ>t~2JFn?|R}GJsT8!=0 zjmfz7x(sb`s@*KG1&)=cKk{gad~Q-(cHW+G?G4nQ#5ZYO-jg)7Ed{=8k*eBwUGH5p zn!3qA{Yyv34XChTbXqLQ^e!=ccJtadZJ{$LgKs9$&y+-7`6zFd~?WKdcQly#22O5jLp`#315HFEs}J0d^!SkRao%)b?*jH?Rjjg`z+X^*e4n`X2|%8=F7=?;HJOMj^aUPTWVc8 zN1G67zl*=~@D!k*1M2BZdVoTqg%xdP7B`u6jRi?b+Vo{bT>CemDx)-=Z80~LVlWaM z16FZvC9L*iz5n@@@p;aOH$KsIe4-hj^5bXvK$O>3rLxUtrZ$T?0^`N_oX+Y7{>&cE z{=k2rvlsB)FC=t!P%6V}+#VMGg|5E{DlFg?9R35i=hJOMhjoRk8~K0q_^XwUk+;4H z@_$2+L)dxxU9i3c%k%XlJxdwV)Z0G+?knIt9~|!)7s1-|{|54ZK<*!~e(KVXQ4K!- zCAeRMJ6nw#&E^e^->;Z0{#PG>qgy89OFF3;U-9Gr^!~Vrn`48SX*IsK&Lp)B1183` zZ_xaC0nM*AoAV}aU0#&hK&gW5VT9l4`as|@1MG~$w`fJsRNZ4M)UEMbJ-+Ki#~O3H zqY2+dX+j9I%qIc;9?+n{)05mG?Q`82aDM=IPS{=#@0K_14Yn53wD}>~Q7v}UL^ME0 z1FAzdAfoAF)K8iS)BR2#ienEwYkW^vX~qxysKxr@YSD)E7^_E1YV9>R#2`@6Y)jky z?P0jFn7)9}Er;OH3uw=K?uCiojaIkTUa|P0g0;3R2Di5|H-w3g;f=AQ1Hd?do=?W~1dM8d9R*MmpaxF&>`5GZz(zDXjM*YHyWX)K za1?vX=tHM8ql_QpvHrM3w6%nFCaE2}8BOmip)eLd6CsiIqkUug7I5(=+|I$FAMioz zxbAp$o9h>gpDWm0+XZ0v55*2)cWW;OZ3obTOre{kA#E>r1%NvO7_@Wi28^(3EnWlY z06-56@I~FAQEjkz9moSgK0uWm(GD?|uV;4HKQ;)*u5uYW&}q%sksk-d`r|Uu76)2q zuy(x%A`F5E^8!S`4s^S(T?fVVdx6K=^?n=%qZL7uS&ywyx6Hw@_ydAv<{m;5c8b!3 z5C)q66QDx?4O+YQBzH(VUH3S+JA)gvcI}B9!CW^z3HB~v2d!OuVn?;wO@BiJc0~h% z)~-D@AfoYN)IYmk%;8`LIkc`aMzzVp_W+LoI4JVe6&%qP zF^Yd+wzzw2AdX!sH-^z!&DetXsBUYcVtsd= zr%cs+&C2oM>?tctbC^6J2E^WzFoctI)C?om8)eD^mh+GHEPa&Jj$eru?S&Qvx!<}> z3TcAQCbPk8&ShH`n(NHW_HZ0~#q?EZLQ*R<;V>F437TwtTcSR&N5|sLlboPsp%u*- z6SWy39Ll=20FMPYXc6Woe@N$YTnE_Wz^)2d_WQ9TECNp|fyNVNjP|z9U`cj1F5Cx#1;yL= z0~%5M(LOQ#9MJHZlJjwx0BF#(Z9~;9cS5Z11umClUIga8U zLZbrLQMJ{P)=`sU`prOL9d#=XlR*qzM=_%Mpq(7+dz)*}Hr)x}6bZfKVYSMN@FRBR z-QY|GC&(a)6K=RR-3P`r1zAFl)s+u|GhJ4ePy-bM;vqIrr^NJ!0W+q?dgDj~bsGO@ zdg&vqqn^S=`=Ld0>nPlowVl^d`^EI9(FClcp21-TS|YcO>Y*j-1A9g+{;ar;dLGS~ z8MPT<)=@73JPTmCb(FXKVb@U~fIS;*xpkB`JIXrhWAOI}Uv3@c&5yi}`V5UY0F97a zM|n3Q;yMb}@Hw;InXx@^wAN9cAY&F?sTs5RasOCvf!sU6i1hdDrw30RX_NOh~76YJZ5vbe2p-VwyPAbLLNEyS5DVt9YF zDhbd$fW}LS7S16x?*+&~D$<1XNUO#HR3$Al*uI>#6PMk zeT3mU9Zjl6lY)k8RVKAQ(r~Sg>5PTpIunQaKn4w03#mR_=g0caau3)2L9CJJV2c=G zxE=`50*MZ`Kp}@~Eg%Q0NE3FrE&-@kTAEP96{Dh#4cD5Oz7#ZLL9929G+YnnAJvvV zN@`1wz(t3kNu@{R#79QhV=-Zt+lRykqTX)LX&l)E*r9-VuJYZ_-h{y(8XE{gS8QEk z-`Ol=sLik*;t@-m6AOiNTU@2BiB-b6ZDNT%#8YrivWIxYA|B#7Q#gxU#!nti?iM6)Z<)unlYsDg3J5o4{)`~^6 zcB*g|trd!B?eY61m~IDHtrbg~6R!#9wzx`L6Z#D2d)vekYwfPiN!D7ih}KRN&Z4zq z5v{Ee&Z4zKF}XRh%MI+=ujE-*4ZAvpINZ zO)PCrtP{>{ah0|vHVfyri6z$BE1Z+8wPF#iyhzzB`ST2*N5FaM@n=Nmi_Xn-#6IsAy%3#mB6#)+L^3WTn?$GZS-B?f7L* zX$voDeID!eaB^#6rf_bXSYnJC=Oj~@ScE7mgtO3}SVV(bgtO3}P(&}yy;5w^Mxj)f ks2j1E2}bR!OWd)Vl}HlfQ9V(7zHZ$UUp`dzbsz2j0Rn+IVE_OC literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4fd1b46..6511734 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -45,9 +46,7 @@ public class RobotContainer { private final Tank m_Tank = new Tank(); private final PID m_PID = new PID(); private final PDP m_PDP = new PDP(); - private final Vision m_Vision = new Vision(m_Tank); - - + private final Vision m_Vision = new Vision(m_Tank, m_Claw); // INSTANTIATES ALL COMMANDS private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw); @@ -63,8 +62,8 @@ public class RobotContainer { private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank, m_Arm, m_Claw); - private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank); - + private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank, m_Claw); + // private final XTracking m_XTracking = new XTracking(m_Tank, null, m_Target); // private final YTracking m_YTracking = new YTr @@ -100,9 +99,6 @@ public RobotContainer() { Logger.configureLoggingAndConfig(this, false); - - - } /** @@ -128,11 +124,8 @@ private void configureButtonBindings() { .whileTrue(m_FineDriveCommand); new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand); - new JoystickButton(m_armController2, XboxController.Button.kX.value).whileTrue(m_TrackPiece); - - // new JoystickButton(m_armController2, XboxController.Button.kB.value) // .whileTrue(m_PlaceConeSecondLevelCommand); } @@ -250,6 +243,19 @@ public static double getControllerRightStickX() { } return axis; } + + public static double getControllerRightStickY() { + double axis = m_armController2.getRawAxis(5); + if (Math.abs(axis) < 0.4) { + axis = 0; + } + return -axis; + } + + public static void rumbleGabeController(double rumble) { + XboxController controller = m_driverController; + controller.setRumble(RumbleType.kBothRumble, rumble); + } /** * gets value from the right trigger axis (yes, it's an axis) for grabbing up @@ -301,6 +307,4 @@ public void robotPeriodic() { timeWidget.setDouble(DriverStation.getMatchTime()); } - - } diff --git a/src/main/java/frc/robot/commands/FineDriveCommand.java b/src/main/java/frc/robot/commands/FineDriveCommand.java index b59800d..f7f2e1b 100644 --- a/src/main/java/frc/robot/commands/FineDriveCommand.java +++ b/src/main/java/frc/robot/commands/FineDriveCommand.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RebindHat; +import frc.robot.Robot; +import frc.robot.RobotContainer; import frc.robot.subsystems.Tank; public class FineDriveCommand extends CommandBase { @@ -30,6 +32,7 @@ public void initialize() { @Override public void execute() { // double check getMaxSpeed(), might be wrong + RobotContainer.rumbleGabeController(1); m_drivebase.arcadeDrive( RebindHat.ControllerToYAxis() * multWidget.getDouble(0), RebindHat.ControllerToXAxis() * multWidget.getDouble(0)); } @@ -37,6 +40,7 @@ public void execute() { @Override public void end(boolean interrupted) { m_drivebase.arcadeDrive(0, 0); + RobotContainer.rumbleGabeController(0); } @Override diff --git a/src/main/java/frc/robot/commands/TrackPiece.java b/src/main/java/frc/robot/commands/TrackPiece.java index 4c90e0a..915d0a1 100644 --- a/src/main/java/frc/robot/commands/TrackPiece.java +++ b/src/main/java/frc/robot/commands/TrackPiece.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Claw; import frc.robot.subsystems.Tank; import frc.robot.subsystems.Vision; @@ -13,11 +15,13 @@ public class TrackPiece extends CommandBase { /** Creates a new TrackPiece. */ private Vision m_Vision; private Tank m_Tank; - public TrackPiece(Vision vision, Tank tank) { + private Claw m_Claw; + public TrackPiece(Vision vision, Tank tank, Claw claw) { // Use addRequirements() here to declare subsystem dependencies. m_Vision = vision; m_Tank = tank; - addRequirements(m_Vision, m_Tank); + m_Claw = claw; + addRequirements(m_Vision, m_Tank, m_Claw); } // Called when the command is initially scheduled. @@ -26,25 +30,30 @@ public void initialize() { //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint())); m_Vision.enable(); SmartDashboard.putString("a", "pid on"); + m_Claw.closeClaw(); } @Override public void execute() { + //m_Tank.setAllMotors(-0.2); + RobotContainer.rumbleGabeController(1); SmartDashboard.putNumber(getName(), m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint())); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + RobotContainer.rumbleGabeController(0); m_Vision.disable(); m_Tank.arcadeDrive(0,0); SmartDashboard.putString("a", "pid off"); + m_Claw.stopClaw(); } // Returns true when the command should end. @Override public boolean isFinished() { - return m_Vision.atSetpoint(); + return false; } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 9782b51..cd21f4d 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -42,6 +42,9 @@ public class Arm extends SubsystemBase { public AbsoluteEncoder encoder; + double lastEncoderValue; + int rotations; + public Arm() { kP = 0.0001; @@ -71,15 +74,17 @@ public Arm() { armPID.setOutputRange(kMinOutput, kMaxOutput); // set the spark max to alternate encoder mode - - // configure the data port on top to be used with the REV Through Bore Encoder using the absolute encoder adapter + // configure the data port on top to be used with the REV Through Bore Encoder + // using the absolute encoder adapter encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle); encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry(); Shuffleboard.getTab("Motors").add("Arm", armMotor); + lastEncoderValue = armMotor.getEncoder().getPosition(); + // configure the PID loop to use the alternate encoder armPID.setFeedbackDevice(encoder); @@ -94,8 +99,18 @@ public Arm() { @Override public void periodic() { // SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get()); + double encoderValue = armMotor.getEncoder().getPosition(); + double change = encoderValue - lastEncoderValue; + if (Math.abs(change) > 0.5) { + if (change < 0) { + rotations++; + } else { + rotations--; + } + } + lastEncoderValue = encoderValue; - encoderWidget.setDouble(armMotor.getEncoder().getPosition()); + encoderWidget.setDouble(encoderValue); } /** diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index d8e2e9b..23604e6 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -25,7 +25,7 @@ public class Claw extends SubsystemBase { public Claw() { clawMotor = new CANSparkMax(8, MotorType.kBrushless); clawMotor.setIdleMode(IdleMode.kBrake); - clawMotor.setSmartCurrentLimit(17); + clawMotor.setSmartCurrentLimit(20); clawMotor.burnFlash(); addChild("Claw Motor", clawMotor); @@ -39,11 +39,11 @@ public void periodic() { } public void closeClaw() { - clawMotor.set(1); + clawMotor.set(0.75); } public void openClaw() { - clawMotor.set(-1); + clawMotor.set(-0.75); } public void stopClaw() { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index a876883..b47fb37 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -9,25 +9,28 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import frc.robot.RobotContainer; public class Vision extends PIDSubsystem { /** Creates a new Vision. */ - private static double kP = 0.02; - private static double kI = 0.05; - private static double kD = 0.0007; + private static double kP = 0.1; + private static double kI = 0; + private static double kD = 0; public PIDController pidController; public Tank m_tank; + public Claw m_claw; - public Vision(Tank tank) { + public Vision(Tank tank, Claw claw) { super( // The PIDController used by the subsystem new PIDController(kP, kI, kD)); addChild(getName(), m_controller); m_tank = tank; + m_claw = claw; m_controller.setSetpoint(0); - m_controller.setTolerance(2); + m_controller.setTolerance(1); m_controller.setIntegratorRange(-0.43, 0.43); Shuffleboard.getTab(getName()).add(m_controller); @@ -36,20 +39,34 @@ public Vision(Tank tank) { @Override public void useOutput(double output, double setpoint) { + + double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0); + // Use the output here + double forward = 1 - Math.abs(output) / 30; + if (forward < .1) { + forward = .1; + } + forward *= 0.6; + if (ty < 0) { + forward = 0.5; + } + double stick = RobotContainer.getControllerRightStickY(); + if (stick > .5) { + forward += (stick / 2) - .25; + } - if (0.40 < output) { - output = 0.40; - } else if (-0.40 > output) { - output = -0.40; - } else if (0 < output && output < 0.40) { + if (0.50 < output) { + output = 0.50; + } else if (-0.50 > output) { + output = -0.50; + } else if (0.1 < output && output < 0.40) { output = 0.40; - } else if (-0.40 < output && output < 0) { + } else if (-0.40 < output && output < -0.1) { output = -0.40; } - - m_tank.arcadeDrive(0, -output * 1); + m_tank.arcadeDrive(-forward, -output * 1); SmartDashboard.putNumber("will this work?", output); SmartDashboard.putNumber("setpoint", setpoint);