From 264fef72b4fa9a4234267eaef193fb75071241c8 Mon Sep 17 00:00:00 2001 From: NamelessSuperCoder Date: Sun, 4 Mar 2018 21:47:43 -0500 Subject: [PATCH] reformated code Added auton util and made the auton options extend a AutonOption class. Changed PID constance. Updated Splines jar --- PowerUp/build.xml | 38 +- PowerUp/lib/2974Splines.jar | Bin 26839 -> 27028 bytes .../usfirst/frc/team2974/robot/Config.java | 59 ++- .../usfirst/frc/team2974/robot/Gamepad.java | 233 ++++++----- .../org/usfirst/frc/team2974/robot/OI.java | 39 +- .../org/usfirst/frc/team2974/robot/Robot.java | 377 +++++++++--------- .../usfirst/frc/team2974/robot/RobotMap.java | 42 +- .../robot/command/auton/AutonOption.java | 33 ++ .../command/auton/CrossBaseLineNoEncoder.java | 5 +- .../robot/command/auton/CrossBaseline.java | 113 +++--- .../robot/command/auton/DoNothingCommand.java | 7 +- .../auton/DriveStraightByDistance.java | 119 +++--- .../command/auton/DriveStraightByTime.java | 121 +++--- .../robot/command/auton/DriveSwitchScale.java | 106 ++--- .../robot/command/auton/DriveToScale.java | 59 ++- .../robot/command/auton/DriveToSwitch.java | 70 ++-- .../robot/command/auton/DropCube.java | 53 ++- .../robot/command/auton/ElevatorTarget.java | 49 +-- .../robot/command/auton/FindCube.java | 8 +- .../robot/command/auton/GamePosition.java | 240 +++++------ .../robot/command/auton/SimpleSpline.java | 69 ++-- .../robot/command/teleop/DriveCommand.java | 159 ++++---- .../robot/command/teleop/ElevatorCommand.java | 27 +- .../robot/command/teleop/IntakeCommand.java | 148 +++---- .../DebugSmartDashboardProperty.java | 24 +- .../smartdashboard/SmartDashboardManager.java | 18 +- .../SmartDashboardProperty.java | 303 +++++++------- .../team2974/robot/subsystems/Drivetrain.java | 20 +- .../team2974/robot/subsystems/Elevator.java | 30 +- .../robot/subsystems/IntakeOutput.java | 120 +++--- .../frc/team2974/robot/util/AutonLoader.java | 153 +++---- .../frc/team2974/robot/util/AutonUtil.java | 27 ++ .../team2974/robot/util/ButtonMultiple.java | 53 +-- .../frc/team2974/robot/util/ButtonOnce.java | 34 +- .../team2974/robot/util/ElevatorLogger.java | 211 +++++----- .../frc/team2974/robot/util/POVButton.java | 20 +- 36 files changed, 1652 insertions(+), 1535 deletions(-) create mode 100644 PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/AutonOption.java create mode 100644 PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java diff --git a/PowerUp/build.xml b/PowerUp/build.xml index 6dc0a4d..f362128 100644 --- a/PowerUp/build.xml +++ b/PowerUp/build.xml @@ -1,30 +1,30 @@ - + - + - - + + - - + + - - + + - + - - - + + + - + diff --git a/PowerUp/lib/2974Splines.jar b/PowerUp/lib/2974Splines.jar index 550904e989eb0fe111f48e3b2490939089a2f45b..dfaff5ffd2688890947a5e6b093f7102e8ba4cb7 100644 GIT binary patch delta 7808 zcmZ8`1yGz#*Cp=m?hL`*2|n21E(z}L1bu>qAi)L+gF~=jAv3rI2oPR^O9&8{;O+zo zvdO#O?tcG1{ZyZOZ=b$h-BqW$s_(Zc)T|j)0(}TN1_cTZ4h{;=%M1b=v_HKU%ilgt z5Doif#wU5ecwd)XiQ<9EQfuFoF9~g>P0KBthvx-2BZh5IY}R(xX6bMt_4|=Tr%__s z&+uy%a=}E9{7YgdIZ;L}e#j?J|M3E!@f+uHr(aja%)KbfimcHb-h*~)!2}^>JA=>N zmZ7!34f!-r6_XMS`5>oHUS-pqVUe|L{?a5pPRU&ptHPFkxecQFPWI=;9H1Gvx&}Mg zPtZLDkOm7P{S$P@Ua1FX`9kc$$&gnZ!cTr`v0hINor3Q>@RkP@ZbG;%r)t|i(!gKU>vCXRb1-F269q~8cw zd2^b%=fqw`+XkWMO@>gN(Kt4*%PqKf2;_okZ5twtIrGD2jDgpVEL~~|Uhfm#iM18R z44TAoaG4HT#6@%;%Q^1r!m4qMQ7sAoVasGvj+^IFLRXrXuz31T&!uPJi(_vY*ExB& z;7C#g_yTTEV>m^+3qB)QqXBuw@g)M)uF46h&6!m+7jJBhi!+QQNX8EN4_Wr;LWkha-vKc(Gdn(}*!A>mTZZE-h+47J|UV+ZPEdu*TH9&0@4=b0p%Yf1T* zTzBOPL>vuMoWC~X=c(1PCM^b!Qid69zt5f0c%PeS`%h7Gh03LKMvQKUIMr=_(Ubk) z;KU1GnQDB|Y2)`!7$U~)NW^p4bTr+w2r#-EP~~R#uvvk_bXx_gE=}qi-JUjH2G#qx zMjE-VS?gB`JYecWuyK-Pg6!qdQBZ=gQBeMh4i5zd=gG7i zseWV%F;Ydk2x>ekB;!@6TeTf^wXqD3xZ=2axNiT&_(|%Frv>TDo5;=+CS1ps8ka_qfrj zi@|57)M0&a{^9Y(BDXfbffl)80g89zy=rtzS`P-=(~CxxPGLr94jEz8#7-En%wSzS zPu={S0&e*+Ef=K;Dvg=K7Q*iIDMskFlM^`?PYG57Rp3?P%i!-H-$D6y8hCfV1CE6j z7iJL#;&xT887aQ*jDk58d|wwEMzA)*s+0Dm=g0OcMQe~TFYEHt!;F|uxe2G( zrLW*sD4%QF2wNOE@z*goS^25WiDA3sGz~tuQ}y((0)ISYU>q(Q-XDKmr79ThC@QD7{zteN zqFf379aareiEik+pppy><1-9zG}EG6&$Jfr*yqi7;HsMXymO1o`Jzg=;1OUyvVBvD zhId+&lP32V03~xRy(!4E0fB3IyKh_!w_JH8z=qsWqrf-c0*|*o+Jw<2i{smyVdF#6 zs+B**41kd!gH->lr8jA1;iGqir5fnX7I+BqOHH@X>7NX4c5}a0^+$ypHmwId@`4$b zMl=TCq2Vj0=f_-gDEXIm9snR+qkd(wD{OB$vB|R<8qMD9x8<{=kM(<(Z5Be!KJ|U1 zinLTnIYLUwwhFMrneQGb`oGVa8N};MN1_mdaK_Q$3}T+%FmXfDi3)<0wJdhh~${ zmGw9Wd?rCF_PH+-a8H?XKV!>LFz6G73JKH)TLLpvZ(*NN*@O21)EM|>JHo@4V@okp^l z5iWn!S@TDh`VhSTb0_|hBLjgJU_3Xcus!CuGP>bJru-5&A5{rnlIWU^%Vvdwj?0!< zmqY}$6LA(HcL8Bsz>^{Mkmt2jR~6astE<0~e17lMd<3Ztwn5bY@bYN3tj#_Kj>6#_ zDWC`id0@Qzv9mDzsC~O2d>nrw^6ZO)YG8^)^V_41JVOTlL1Dv)@i%LLxzezv@TktJ zZSlB{H;;4i#n+s*7m{OZ(lW(JP}*@e_{?-|rq{*4R5+qQ5O2^tQ zWs54W(|AjOe3qwLXVPDZ3)hl$$5DwFvl)MnBrvJOB*qlqlHIkwBq(Mc|Eda4W^9c9 zI*?XM<@T=sWXPV#x2g|NlJr?izX;6seLFGRZUs_%w<9dIw?%V>SMGwFv|o^(@vdN< zkDCe-Qhebh@H1+K&9#=o1>%?5_sE_+A%1->a*98F1GZa(7#46jY$w|a_|ANfC@m3$ ztb)C7$pCOtaz)!`@wB?fXK9ZEXd;h>+yc1<0$^bJu+wciw=erew!B&EvC1g0p zipL32&9UfpR4l+O^rEVdSZi1XOTqI*Al3-0ig3m2C?jE*&FV{-U-JR)$+dU{?`f_IH0%7UQLctREH;A9aB3g36j`QKS1iA zz!x5TOzt~Z>X}WhaxJ7YXo;_G=*ek_IP&4-E~0KfYTp8F7N0~wE?0MoioTr`_3zo~ z?0NPsxjH|rBj!c#^X~~GM||J0k|bl@uASP7egD8hh98(vUb=;}h*Tos_a;s#dvLUo z`+^9>fTRk3765UrH)(Lt_?Soka4AOVZ&p_@3S4ey(YOCr%X1~3o^%lBh4UFlW)y*D?f2pf z(YX}c!mnA)ZFTa1Bbbjg$0)^i!d&_DvF7c1sjWz>i-nI|n>52d1c6n~8kEsIpl`_? z$$Mv)(QnScS`*W#*BRn4fT~u(t2UYHcFsL^{yiQ4%*y|;x-Zpg|!K1Jtf;p_+A=pO& zz}`=aPB#!!mj5XYW9pg|E5nlo+u^)^K_|LxmTB-6t5)JCXJ@(@UPix?@m7;4s~5V@}>kx60r+-v|YR44! z32HM+J=$e1BNqylO)z>KhvcaSNJfenWL{$Nc!nviW^6-=HKElU9}`T)<237X63UgEiQ2 zyJ}7rdnD({%IxA`Ec0n?$&S*D9kfmtBsiFvv2}k1g6r+JR zfS?RPF*J3kc!su;$baoMsNBox<)W$slPB;<2OG+T7%a1`k1uN~FW{@jJhVA6*-jX+ z!>p)Ml9m_IeN`j(Py-U+MVdPO$tDcCbzi$|CsWLQRxbYOS<&^B0ltunuA^)aN+8C? z4eJK2cGIx=5@{4$q@ESF%u|;8l**6;%ZHV|h=9@{)E2A2CBYn-M%mv7rFNk8KPU=*!AqM&ch?l1Gj@2UJ@Mg!L7r$YthKH4+{rxOF2?0 zVny5}3Ey6jb$d=5T|9^OsO!NlWWz?^neDhid%z+27wi%zMAQU#_1pIP*FMv+7wMX- zmBZRmuZrUj8oXe~*NdU;_@()53w^w0z zvQeWOtLMyt59K4kngTH(r7yfIexlSk(|z&{5~GRxSXN} z)r-Gf^iv~d_k%(i&?a*8^dVPsmHLcHKs1z(9@IOLKHnQZMtq|H0yuUYc7HdP=)GG= zP-cIlqXX@a4uu)LT@nuq*w`puxo2*9DZX%vxrTw1Q z(z!*wpPZGmq$Nw<^0nW=>M-?oFORFo^Fq{^xH|-T)z!QK$aA_4josEs*EX4YB(lqA zQy0dB-q7E=3+&*l%WQhCzw!rCZ+V1sC7IH(1so-jyd(ar@E)mQ4MaR`+! zYOR?Jvm&V={)yuADKLYlQf@`%J6k^-8RuuFfmvhKng+MhYDu*_4_qk4c zPE_TivsL1AGv43o^GFlTF`KJXv{~oO=;-)&l1qoSAN2dxVh%RHbO&7<4f66hMP&B_xD%Dr z7@yjofQ9r*w|7q2b%L)Yld#sD?=n7<5PmTd?;b7w(0j?bAs|BA0Cz5YqR}MtQOY$C zm)bNw4xv#orrj4?7;<>bS}&-dXJ#2#j@%xofHTOZ_kSPW zqIRyhebX>~y=!OtCb;wsLiLhe(D0H82w!1;KhF_jAbv(~!k%O%E`n{s9&LDO6iiQA z_rAEvBW~pTebMkSX81~S@Ec?8xyFTaQswuzCgHu`Jmsy;F3l&EoukjVRJ*dYM&ef( zgE~j5mmq^4Ay8Q`XHY6do;bR(rP-!7sl(^kjE@xA-)On%Oo}U$D%mbltvhphfN!Rh z6>cfR@cbcX%k_htka~4}%BXcCvC;;4a2TVhp7s<+4Oi6;NlmSh7=t{@Eh@>M)1<5btP4|g+EA%Y>C3I;0j-mbEH`YN-)yY>G3(j;2xG7LceJz9P^h}gMjQ>LGs>whzIvtl6;Y*4;4WWB3e?ut zuIi{)d|^El8(SVSlUG$YV}MppSUeq;Xy1!Vdm6|vVC-B`-82qmE2hv%!g>CL*VzG_ z&8su4pj191rjEp=BDetpcea+B)5mD;hC+k*@^dS1qJJP*z~_4hZ>AAK;IW`-&lQzq z$JVL+PZmj3#CwpbOoneXPyJ;1$%riDwo^!Fx0e(nhcFa}GAb)*@6`ihNc?!%z8#0? z26gZ&hv!YU++f6;27RpH_KzVykrD)ZJ?1*#T(!d;2Rj&Hu%7^7?C4J--Shp#@C>-C%Y7}BbWW@YhrZP z9&DFWb_iPe`r_2s2@7SH0gplzu9oQp^;-6C5$m$#ldxwg zS4grKcM!Xt+`f&*65?GgfwJ1Rl!h(8KtjED9ycV5Lxh3o%n->l+(7cynXw4EAN@Z9 zd4UY?b>3Z~o}8l%tDXD^t{;O}(?9Nn4w0xGY0MRccXUs`03`|6jA7;hGXh>@6$rY; zdPWGkrRd=lU^;Dm7#LHTAe3uUEFvq2&CAGgA|jH;vJPRN(>D+g_sfN-=8LYd}w}@A}MXKXWvFaRT<0s$2%It#Fm% zs04CcW%9!lL@o2;1$bw`WHKhd6!U7P^YyP4^{z|Qi~ZG-3)XJ+N7UQb>Qh{Dg}?mD zZ(St5ay1RnFO&ve6NPfLbCtU(=2T{%(VFwhbfHSzmhK3}FJLB3+WCG+lP6=uVm_W3 zE&~RTTVA(~J3`l?@$v>TcR2P#k&2thT^UvKD>hJMT8jv}Nlc@W1v=g68?gk&0}jDr zRcJd-b&{k$?XU36AzntwFQ>xUGR~&c26b>dY(ltx0LXPg#YKb=m(Khl{&}KIjoTPP zFN}iPr6$5Q?KyvGKjDQ*=@6tyff%zRA@VU*3r`fOjAfm{K zD?>*w#^cH&3h)#c4+X!P@Ls>zU>!BtG5p5XR6R95n1rJ;0CPhG2J+*aJ2!`69=E4- zZ*1dIY-u9$xMsj7?{fXbyY^?hpf+B`&)y|!oU&JsKe>7BgI_vGRk5J5KEojc0BaJ{ zg-av53mHxWoB?g1G|x?zkCuaSSQes?mOhXb$HyNT-3-aPDollC2ZLG8f?INw)BSxl zG^wds$-CLf8MBJ2IhAnJ z^4)JQSF(6dtd zMx{NB-?F3<9Rr+~k@q0jR<Ca=9_i7jArE=wjoQT&)|NUd;Z4W=GBJ z*QJ-$VmH5&BZI2gGPws10Nw-FHWQAiSGD+Kp9B#U)%exC8@y-)zb`Bs8!*f`E~&aO z2b=Gy5FNKuCTvDcJPc>qVG!P32Dxk|)a^0{SrfcFG=Xl_OU#Ih55EQLFR8+(n)1gX z89C&UP**A+j#zXOfGd^eT}@Y_ds{N>THAdcr#?FTS3njUL%IABP!S-V%B%4tV-(d- z&Z-Nm;FFY;9_ewYFR`9-*Sc#mSqVC5svN^yRor(EhGzrYp=dzkQCn+!IgFmoQY}m5 zy=gs3qGXm>!ftBbM}7)`K0HYn)Qeed;^=;MfOU!J4wNVRh0lYW?%{io)}c0u80_DS z5p4O8R|(+}Xg;Y1#Gad;n&3V{$;*v5WXN)1Ux<$r)RJqhP2}U z8sh^O;yW%o3z!KEhaD|Qe`fu)Ek~*cZ}_;3R6&kjT~*!!>4PsU&%3zwnyB?QRSpN? ze!`iO`#j+Rl-AxA^Bv94O5N-$Sj3(}sz-F^o@Z83Mp7}F<)rdLwuR)oKDo)q7nWAl z{9Hs!E@ZcQHhhUmC9Br*0*l$ewn^gSpt`?G^q^eKps)R#_i{-`hN5D(m0#e?FS%DV zmo$I>`#6i2bD$GuLWzPBDg>Q+!T`r-9i#YDqJ%GsvvdB%N6Q4PKL~wo$MqMQ>{S24 zm7UOE;I>yF#D3|h{eSB(&>(wlW}H6}l>hpe{>w%|fk2V=5VQmgs2v{*RL6nwZ&}9E zfdP#Q``-wWBPq1gft=-U+W&qyR`?%D*m3?t=Kq)Y|Mq}_BKALsSNQ)Sl0cIkh0q#U z|Knmo*B!;s>Nx(mtWZ7@Ss=PEc$ zqx}~B*MvD!v;6;?rTZVtCrd!5oYl|<{sIG3UV`*bHXbxywSV1I7YQ_H!#|lQp-V1I zf6w!`3j^8*tA8T`u7YS=|J$^*qJk#568t5+b!A|ewnstH0UImvYwD^A>O(NG{;o*% Pr!oDhe%byRL_zsqWs8(= delta 7611 zcmYjWWl$Vjx5eE(xCVFk4DK4--3cCiGB|<326uOdK(Ih?hrt~}kb&S5BoJP5zwftJm(T(?3?7s=ZU^;g_NCSX!!xNJMaGXlQUozR6e?2!B>L+P`ZeHzb=CFL{~= znEFG55I{8Yg2-9si;u}h`p&eBsO%$}IiPeW(wba_cDLfx&vnH0{n#1NuFXAl=&YOo zbP)BTDBhtQGqo8>4I=72oqL@lnsHjtD-A@<9R7hM#zPnlS|y`qoc$QB5>V=wxOgSU zskz8kksk0#BF>5~>JSTk!ngErP3Vpcc(!NSkh?1Ab|jW>;#1c6G+88TpT<$&;&16c zY5DdO=VAK9M;YlPWi^w#>BE~!5UTBV{MYI+8FW#1_wKc zotksQR(B zHl5TtAeI-gfk$ppY(8mD;97^XrHhmAJn~TWQ;Ym1Vo#B=V4hC77W*TRCSuB~J zo9%_3-S>Wr8qox;p`*Qo#}61Jw?*YI*kZv)?!vIB{QU55julyB%Q^XYze6z@!f&?c z0YEj*w@B4+r=4%{mHilGez|L1E3ebmMegn2;5Ob2QX9|p&OWBMj#3v}2!2aRNMPYM z4#HF9UVT*3C81JZGJkUpT;Plk4T0gFjXF}rl=l|xPrx^;Q#3foYrn6wp~o^9t=|(g zUA{s=fl0&yu#jh~;MO!|VeeZ*+;^viJl4YxnRwN51D__L_bXP_=y`LL)|t3=@wtPJ z@9a77ZA4Rf0|^+mCP(pY1S2ldg?UMMNJe!ZhKX%<#R;a85F+bzgm2hGXB$Lx$e7g# zjvy*Q^&PDyBL%XY*?pXx?`rgubFqvw(D(A*Y+>)cr9Ed=u5}8Tcm`^zqN26(jJfh6 z!og{y!omFw8U`F38r+|twHP3cA^Zth3q2|({2zpHqiF*_=^uH`>l6L1CoGohNWzv; zC6(dbSz+=cNf$^>UpqmWV-&%5v3sz_B&uQ!+=yHSd}K!Nxgt!EKk~T*&X;Z3no69T?fj0$gluLeJj%>e+O3RnA z+38U2O+wi1P&2j=^1XqNw^5?jB$U1ooMmz}Pu7w;{#l-;S4OzWnl(>n{4c|Z!U|-O~dnqpv zuRD(kE{AxRb=~!SlMrc!5~#|bORn20d;AVEFDlmi>0WathjKdgxsQLcaK{Ljm~Nto zP--E$s82}`xyHm^3UVBLY{EI^q1xUl`BJ9_l=#`;HRV-rkYm#wa3%dZ6qWD5i~p2@ z?ZW;&&M(e@L?1`CLMVuIv4=MJJ2BFuQ?IR_ODWlfUgJ}q4skY!s!FCP?=s-bszY2- zPG7#=Sno8K>X4|ceqJuTZml_YheNotU8e;QTD*m5MS0;cjgTeLg>l5Kpo_By zn7e20{=O%pCI2vx$qt2!W~N|$2siFHOg(7L@LlBd`Xm{z+gr9e-1t7|v$TQ!j>3Lcs5=L{)QMSjK&iDC&cQQ{|WK`o|X_qEES-x-jX!4GGzHiFj9wB z9@?xd>}VvXTE)TXSVXJCk*Y3BF5{ZLz-Z?=WQOm%A@6D7L7E!DmhdA!VLE<-LyUjA zJmH7yaeN65+qLvv4+keS{e0wbX5&}yddFdY(3z2Wz{5IoK7H{RiDZWRPLocmiuP;y z4nq0JLKVRGQZgX-6dSYKzB8((hYq!Y>Wm!gYf&c*O}~p|4RT8#&E#DcW|}EMoc!Y* zW9G#x0cPxPVt^45|4vAhvJKBnQU|}?83*7tj2vl-L_l%2DJ+3-fyJ@DFd!@&X^OP< zO{RqTtPFEOUz}~B(IW|_H}ioPCO6c-3SttFb;B|9ofz0X_q9eqYHWkYC@qYLb*&7|d4n!amOR?dy22-)O%h+!V?o&}0sT1Ti?4?9 z$d9rW+98EsSbV=*q@QEqZGSuIF;z8r; z0=fAv(hD0%>J;1r;r=!?_xbH!vwRFkqo(vrYkOkIXP6JO#c+nIXC&SuD7>L{W|XSw zz2|^Pdx%04dOmLuWpJ8@$>B~-UMr|Z@QcE+OW9|Q?>_{;0Cx1j?@}LMgWH{gTZY>m zgKK;?$}dmJw~H?G$hXTbH_5k4F8#pgi}zCC^W}RWJeX$To(@d2bdPV;B>OQBCv@R! zqNqP9WQz>rf&LbtDR3OeCwdrd1M+ZeVqQy7Fkx(*CXMq)pet`u{}$8h+`~+U8BPCf zG4lYdKV?ca)F`}^lx;X+>-5fQ7X|)0)Af|U$NcM*jzCpHQUPnlJfTS(m7;LouF+xD z@keA}nBJDh=sI!iy-ZWJ(bvXyNxUoJdyUg8kfXbIAI;TZC%sg8$Fm_HCq-8#Wn4r! zz@pTd&aXidCE_&iixTYyN&*EP7{~Xiym9+Hjt4xdU>iD2RQtX>mJfS!$JMh?R^3Cc zQ>N$xWmX37e%8kLZ?F4MkFmXv!xCgE`r*vL11q-E5wR>*7f(sE{GEOe0ovd@n;DO{ zNM|1d=tC_ydK>@_gY8^+XCH+gWI}y??LxGS8AC87;gkFEDCr2o$HL7g7>lBC5TWJ4BKJ!Hg|(ico(#ISwaSIgban z808zu7E~KW893J9wtxFdB!DdQ1=)%*^_S>$4rpaw(1mDhrPqN7=qTf zQYN)X=cRX@jO|Xs`(i%z#-&Kw1qJ8?cwg*`F=m|o2q{ozJ@??c( zaxac*3a4}BV=v9SW!iLv7!2>e#+}r2pKX7gUP_F9zx4 z<|`%O0>ZIX*L7HpzdcLfH_+i@j}k@qOf7Vn-YnMefBjI89CTZ-%3IFz zvUPL>ByQ)v-2q3GK!^OFPRHz2#;ZXwhVEP&lyQ~cNz&3^iIETHvX!(0^`So>i=j}$ z>Y6O$Ut&K(QFv-T+9iU8-d4QNVd<(9+1bQ7c`c--^iD)sDA#9a#6=mBX)2j~^z@>b zkK4!gIzX(sQw#zSNT#90rF2m;b#aK_ca=_=dsUR!RivSYROG|! zdbNehkvtpKJJDXA*R+S`rBdP9<|t}?7Ht7K-^d3!acLn+_O*vcb-9P zQxjL@v&rGe19~~b<9jo;lE-iSK3fBtVQ#;RrZq1iY7D>hMYDdl0yVm5u|pN~w^iDY zo@xu%ktLa7IDi?Gi~uZXR{nh|l8(sRbwQ zkkg%lqbKiBWc*&IOI1cH4s43VlXODDSyN% z*J~48)fq(K17J_DfxSi7@A{}-^jW~ChvCakiH?Kk_ndk8r8HvqcVaK_r7$OGjzg(G z$WI1J-cM&S_6V0?Wt$AeXVRVU`OWZx9?CqHJoo^wspVOAUGU(RM@@Qle$gfNBA*$Q zxV`*<2%u5fRt%F$-)+!&0>9K>OPAhPO#X`avh<+)TkQ}C9E0_i-J5=Vi5VuDPIr9M z1}>Kjo&xoJh8}Sv6w_J2ZWtb$rY-njxXx&DLGN~UUXHsBRvxo^p${fohmA?@Rvs=* zN>-m%@LbFDE`ojS?4H}`)MSH#E{x1TmN~g@j_=Mi4J=+A`=}-iaYZ8HYuYJidr;NN z+r?%T)i{v@i&Mr4zL$JB9z||9Owm|xvbVHTo=NT2fT{zz0pbKHZB|Pp&|C!uP*pIBlk_J52pK4=Jqgs)P6ekMGa>%Kz_^X|zSBLtno_S(k z3i|O0VdE*2i3|rmX;0O%2S+En2<4Zk<>vmiN9z*6&|B6OoL4aTY8IK?V%Mzbi1sfl z#qk>p`fG&*-JwG+c;bv(zry9^k3fSMI9fcr7K2;ZXd$W;t-W6Iy{<-RGjo5?H8g}aMzgMUu746 zVlXKHzQU^ZVOURH+gI;NE7K>)#v*p!iWzJ}oWprPq+?-;@U1$sk&xFAv!DVI3u}B1 z9PX#W9uq`k+6hdjm6`Z)DL+jQNN%zE%VzEQpA&QHk*np)dTp`_ z-h59VNhhuwBX%{-L)@|K>mp}cifV=2I_JU5Y6a_(WA_%YqwZqRBsrbi$KSg5l5W#M z!?nruRgVGQQpM7Gy@fx$Z8)t%Qn~Rj<;I!C1KZQiUpqWdkGpKD2j*T))OmWP0sV9V z0Zp75@yYOHugEI;y*M?(*$CbW2%`3co7~MNE$!waVf%LYLBvtmamCJ~RVfZTr7o0w7f?86*cH$}o)bxXr4#sS-&85tk#7}-$5zNt z6;r+=p*U|PkTzbKKCDZnkXZB}0L)vORdcQ|A}R|M%Oi(LWUFV)QFdl0l;yxg9;=E| z85}AKpfigWlcX14w=3BS!!#|f#O1ZG09JOX9UIJX z!!lk^8D%oGlhp>?ajbZFU9fHW&*Ek36%r zxu#wv&a183T<~2GRv1Y>?!GNJATGgPcWU$CF%eO(_Lnb@GGHX8w5>(YhAg-XL}TLT z&^L?zI$DeT*(9^RM@W>Q%bY{VoMSCaI)j<35vTk8WuIsG)m6#~Ftu1}W5>rm?BEk( zf|77oRn=zPLFL=!C%=YvsGDko@vQe^*s}pAk3K6gS$F{xV5EI{1vDPN)c=zl+p!iZ zR@5_WblesY=cv7Mdb0$RZ~ffo#(rCa)3sa|&{b8IlfLkoXV=enUgsUC`pJEh5-N|p z(Yd~#(I3ZNHE??lOft`4UkzJZOZ5U`VFHLgs1$@b5J#=~&C!ab;Y5argjqLIn)wjb zg}WJQVmcz`FDgumA?|0jMoyPgI1$$V6m9%;_|-2zk=Ya8wJ-99qBG*Ra*h+X*|=zH zW;Qx1q)FlpxtgSBv}xtT_%pb8_Lc`} zd0S>4ds9)QJq?F2vSH?mw(UHKQ!rxLR&|!o>Ff{ zBoo8{XrtF)Plc`cz##w=m6My9CmSUAj;B8PAx8}7+>(y#NDem4=C3O4!w^wpj{Cj^ zJ@7qK>yABaV_>$Oxlm6NP_PxT)QueSAl$mI8W@c_7v#@$?kyP7hkZjwqe*kg)~yHK z5g%;wP}I5mfVDYs2svUX3#$1H=PiHt9($a8;>`)uZP@p|C}FAr6^ z;oB#fL%Ws#SwoY{l%xS-^c<;xr;?F-d^GD18*+9*-)a9Z;-89 ztyBgCn?uzfmSW{+Z@G`y=b_Z#Y)HaSPqEml_L66!grEGrI-8kJ`928G!t(o=-Y0CA zz*^xsyIoNYp>7?%MuH5h6^V?iP%vwv-D9#^Z`!d+0Jq7z;w%&4WxaC|Uh5U@+8XCveU)0SeJJ_%oiN`*-&G~nb6`00HOwl(Z z)fv(6swM%IMJc{WFYl|&uC?wd$h?nX1`XDKv{)d zOQ7zavyI@IPFA#+TF{`5QSYe0~!$l1HlHDqBLury$CK}-f2p^h@ zFZxw9V(nB76LM@Z%$^chFI`rbRS!I`R-0(XjT+3q@!Z}2QnX*makUI&yFS+M!y&T# zNo>!t%9xUwSYx6cTL9#mRVhsSolV)NCz!z3E{o2kc#Lx&w$XfxBG4HI8;4XGe!^&K z7VnJ_`^IL5Ko48dkVmljd?T@Vs0V-BssFy7f~w0YB$vYps!2R016Ois8Uv(GSdn80 zkV@-k)YvCmEhteZ=M0Wb4=fzqcdYPBYaiI=gs$DwsAdtsb#N3%?f;REa zwblxe9ETt5>pmFq5aUZ%bJHQ28B!C;Wl#V3^E32_^^JrA?6`fcb0 zZM}*KJ++(ZJ{%B?+NXQ=PTf0*Gsy;9~0S$LEF?MrrRHIy&(49+&bXi8+7E>_*`HC&*CqJ2~h|b>!OBMgvGduUZ1ac}6 zeNkj2_egbeBQ@pWpYlw){@8x+@wwoc)Z|yYKT_}#-|sliXA=nwwofnCXCu~zbG+3O zVeUHo9HR?LF)7jRg>JSFG=H0z4oognKq5G}9qtrP%9c^ODWbpQhYTSGroY&25{>eA z3?XmL{1@D<<^DpeHP2r-u$IOq_pw#`zhMT1&qnR9@xg`?K?(_y%u5UTV#9_I_#cX4 zOZC@ewWUNrNBzgNK*NXl+Y-_m`P!=eyO}Bt_wS1Y?Z3rzkP}<1e~SNq({OMC|E`6@ z!}@PA9z@BG2jPd2i z?SEvZBlX`;KRZ$)TpRzV){z^5)#M*aYD%K6r3#OL0r$W2A6vD5{|)-T1zfPzpH{Q{ dTL7oBh61;iDl*Dnm*me){bz_&^2daO`ybWyaIgRX diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java index 8c0a7f3..3855dca 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team2974.robot; +import org.usfirst.frc.team2974.robot.Gamepad.Button; +import org.usfirst.frc.team2974.robot.Gamepad.POV; import org.waltonrobotics.controller.Pose; /** @@ -12,7 +14,7 @@ private Config() { } public enum Robot { - PRACTICE(.7800, .000409, false), COMPETITION(.7239, .0002045, true); + PRACTICE(0.7800, 0.000409, false), COMPETITION(0.7239, 0.0002045, true); private final double robotWidth; @@ -41,9 +43,6 @@ public boolean getSensorPhase() { public static final class Hardware { -// public static final double ROBOT_WIDTH = .78; // new robot: 0.7493 between wheels , 28.5 inches between wheels -// public static final double DISTANCE_PER_PULSE = .000409; // practice bot (ARGGGGGGG): .000409 //compbot = .0002045 - public static final int LEFT_MOTOR_CHANNEL = 0; // pwm public static final int LEFT_ENCODER_CHANNEL1 = 2; // for first digital input public static final int LEFT_ENCODER_CHANNEL2 = 3; // for second digital input @@ -64,6 +63,9 @@ public static final class Hardware { public static final int ELEVATOR_LIMIT_LOWER_CHANNEL = 4; // digital public static final int ROBOT_IDENTIFIER_CHANNEL = 9; + + private Hardware() { + } } public static final class Input { // TODO: get from drive team @@ -79,33 +81,37 @@ public static final class Input { // TODO: get from drive team public static final int SHIFT_DOWN_BUTTON = 2; public static final int SHIFT_DOWN_BUTTON_ALT = 7; - public static final int SHIFT_TOOGLE_BUTTON = 1; + public static final int SHIFT_TOGGLE_BUTTON = 1; // right joystick // // gamepad // - public static final int INTAKE_BUTTON = Gamepad.Button.RIGHT_TRIGGER.index(); - public static final int OUTPUT_BUTTON = Gamepad.Button.LEFT_TRIGGER.index(); + public static final int INTAKE_BUTTON = Button.RIGHT_TRIGGER.index(); + public static final int OUTPUT_BUTTON = Button.LEFT_TRIGGER.index(); + + public static final int ELEVATOR_NUDGE_UP = POV.N.angle(); // pov + public static final int ELEVATOR_NUDGE_DOWN = POV.S.angle(); // pov + public static final int ELEVATOR_ZERO = Button._9.index(); + public static final int ELEVATOR_TOGGLE_CONTROL = Button._10.index(); - public static final int ELEVATOR_NUDGE_UP = Gamepad.POV.N.angle(); // pov - public static final int ELEVATOR_NUDGE_DOWN = Gamepad.POV.S.angle(); // pov - public static final int ELEVATOR_ZERO = Gamepad.Button._9.index(); - public static final int ELEVATOR_TOGGLE_CONTROL = Gamepad.Button._10.index(); + public static final int ELEVATOR_HIGH = Button._4.index(); + public static final int ELEVATOR_MEDIUM = Button._3.index(); + public static final int ELEVATOR_LOW = Button._2.index(); - public static final int ELEVATOR_HIGH = Gamepad.Button._4.index(); - public static final int ELEVATOR_MEDIUM = Gamepad.Button._3.index(); - public static final int ELEVATOR_LOW = Gamepad.Button._2.index(); + private Input() { + } } public static final class Elevator { + public static final double TOLERANCE = 0.1; public static final double INCHES_TO_NU = 775; // TODO: FIXME public static final double HIGH_HEIGHT = 72; // inches, this gets the scale public static final double MEDIUM_HEIGHT = 24.00; // inches, this gets the switch, exchange top, and portal public static final double LOW_HEIGHT = 0; // inches, the floor - public static final int MINUMUM_HEIGHT = 1000; // in nu (native units) + public static final int MINIMUM_HEIGHT = 1000; // in nu (native units) public static final int MAXIMUM_HEIGHT = 20000; // in nu public static final double NUDGE_DISTANCE = 1; // in inches @@ -120,6 +126,9 @@ public static final class Elevator { public static final double KD = 0; public static final int TIMEOUT = 100; // in ms + + private Elevator() { + } } public static final class IntakeOutput { @@ -127,6 +136,9 @@ public static final class IntakeOutput { public static final double MAX_POWER = 0.75; public static final double LOW_POWER = 0.25; // Test this value public static final double HOLD_POWER = 0.1; // TEST + + private IntakeOutput() { + } } public static final class Path { @@ -168,6 +180,9 @@ public static final class Path { public static final Pose L9 = negateX(R9); public static final Pose L10 = negateX(R10); + private Path() { + } + /** * Used for R -> L points. Easy and fast. * @@ -177,7 +192,7 @@ public static final class Path { private static Pose negateX(Pose p) { double angle = Math.toRadians(p.getAngle()); // the new angle is the original angle but x is negated - double newAngle = Math.atan2(Math.sin(angle), -Math.cos(angle)); + double newAngle = StrictMath.atan2(StrictMath.sin(angle), -StrictMath.cos(angle)); if (newAngle < 0) { newAngle += 2 * Math.PI; } @@ -187,11 +202,17 @@ private static Pose negateX(Pose p) { public static final class MotionConstants { - public static final double KV = .267; + public static final double KV = 0.267; public static final double KAcc = 0; public static final double KK = 0; public static final double KS = 1; - public static final double KAng = 0.5; - public static final double KL = 1; + public static final double KAng = 1; + public static final double KL = 2; + + public static final double IL = 0.01; + public static final double IAng = 0.01; + + private MotionConstants() { + } } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Gamepad.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Gamepad.java index 94776d6..e158598 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/Gamepad.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Gamepad.java @@ -7,122 +7,119 @@ */ public class Gamepad extends Joystick { - /** - * @param port the port of the controller - */ - public Gamepad(int port) { - super(port); - } - - /** - * forward is -1 and backward is 1 //TODO check this - * - * @return the leftJoystick thumb stick y value between -1 and 1 - */ - public double getLeftY() { - return getRawAxis(1); - } - - /** - * motorLeft is 1 motorRight is -1 //TODO check this - * - * @return the motorLeft thumb stick x value between -1 and 1 - */ - public double getLeftX() { - return getRawAxis(0); - } - - /** - * leftJoystick is 1 motorRight is -1 //TODO check this - * - * @return the motorRight thumb stick x value between -1 and 1 - */ - public double getRightX() { - return getRawAxis(2); - } - - /** - * forward is -1 and backward is 1 //TODO check this - * - * @return the motorRight thumb stick y value between -1 and 1 - */ - public double getRightY() { - return getRawAxis(3); - } - - /** - * @param index the index of the button - * @return true if button pressed false if not pressed - */ - public boolean getButton(int index) { - return getRawButton(index); - } - - /** - * @param b the button to get - * @return - */ - public boolean getButton(Button b) { - return b.getPressed(this); - } - - public boolean getPOVButton(int angle) { - return getPOV() == angle; - } - - /** - * @param p the POV to get based on compass directions - *

- * N,S,E,W,NE,NW,SE,SW, or CENTER - * @return true if the POV button is pressed false if not - */ - public boolean getPOVButton(POV p) { - return p.getPressed(this); - } - - /** - * d-pad buttons enum - */ - public enum POV { - N(0), S(180), E(90), W(270), NE(45), SE(135), NW(315), SW(225), CENTER(0); - - private int angle; - - POV(int angle) { - this.angle = angle; - } - - public int angle() { - return angle; - } - - public boolean getPressed(Gamepad g) { - return g.getPOV() == angle; - } - } - - /** - * non d-pad buttons enum - */ - public enum Button { - _1(1), _2(2), _3(3), _4(4), - LEFT_BUMPER(5), RIGHT_BUMPER(6), - LEFT_TRIGGER(7), RIGHT_TRIGGER(8), - _9(9), _10(10), - LEFT_STICK_BUTTON(11), RIGHT_STICK_BUTTON(12); - - private int index; - - Button(int index) { - this.index = index; - } - - public int index() { - return index; - } - - public boolean getPressed(Gamepad g) { - return g.getRawButton(index); - } - } + /** + * @param port the port of the controller + */ + public Gamepad(int port) { + super(port); + } + + /** + * forward is -1 and backward is 1 //TODO check this + * + * @return the leftJoystick thumb stick y value between -1 and 1 + */ + public double getLeftY() { + return getRawAxis(1); + } + + /** + * motorLeft is 1 motorRight is -1 //TODO check this + * + * @return the motorLeft thumb stick x value between -1 and 1 + */ + public double getLeftX() { + return getRawAxis(0); + } + + /** + * leftJoystick is 1 motorRight is -1 //TODO check this + * + * @return the motorRight thumb stick x value between -1 and 1 + */ + public double getRightX() { + return getRawAxis(2); + } + + /** + * forward is -1 and backward is 1 //TODO check this + * + * @return the motorRight thumb stick y value between -1 and 1 + */ + public double getRightY() { + return getRawAxis(3); + } + + /** + * @param index the index of the button + * @return true if button pressed false if not pressed + */ + public boolean getButton(int index) { + return getRawButton(index); + } + + /** + * @param b the button to get + */ + public boolean getButton(Button b) { + return b.getPressed(this); + } + + public boolean getPOVButton(int angle) { + return getPOV() == angle; + } + + /** + * @param p the POV to get based on compass directions

N,S,E,W,NE,NW,SE,SW, or CENTER + * @return true if the POV button is pressed false if not + */ + public boolean getPOVButton(POV p) { + return p.getPressed(this); + } + + /** + * d-pad buttons enum + */ + public enum POV { + N(0), S(180), E(90), W(270), NE(45), SE(135), NW(315), SW(225), CENTER(0); + + private final int angle; + + POV(int angle) { + this.angle = angle; + } + + public int angle() { + return angle; + } + + public boolean getPressed(Gamepad g) { + return g.getPOV() == angle; + } + } + + /** + * non d-pad buttons enum + */ + public enum Button { + _1(1), _2(2), _3(3), _4(4), + LEFT_BUMPER(5), RIGHT_BUMPER(6), + LEFT_TRIGGER(7), RIGHT_TRIGGER(8), + _9(9), _10(10), + LEFT_STICK_BUTTON(11), RIGHT_STICK_BUTTON(12); + + private final int index; + + Button(int index) { + this.index = index; + } + + public int index() { + return index; + } + + public boolean getPressed(Gamepad g) { + return g.getRawButton(index); + } + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/OI.java b/PowerUp/src/org/usfirst/frc/team2974/robot/OI.java index 2f2328d..aea6402 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/OI.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/OI.java @@ -1,5 +1,23 @@ package org.usfirst.frc.team2974.robot; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_HIGH; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_LOW; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_MEDIUM; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_NUDGE_DOWN; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_NUDGE_UP; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_TOGGLE_CONTROL; +import static org.usfirst.frc.team2974.robot.Config.Input.ELEVATOR_ZERO; +import static org.usfirst.frc.team2974.robot.Config.Input.GAMEPAD_PORT; +import static org.usfirst.frc.team2974.robot.Config.Input.INTAKE_BUTTON; +import static org.usfirst.frc.team2974.robot.Config.Input.LEFT_JOYSTICK_PORT; +import static org.usfirst.frc.team2974.robot.Config.Input.OUTPUT_BUTTON; +import static org.usfirst.frc.team2974.robot.Config.Input.RIGHT_JOYSTICK_PORT; +import static org.usfirst.frc.team2974.robot.Config.Input.SHIFT_DOWN_BUTTON; +import static org.usfirst.frc.team2974.robot.Config.Input.SHIFT_DOWN_BUTTON_ALT; +import static org.usfirst.frc.team2974.robot.Config.Input.SHIFT_TOGGLE_BUTTON; +import static org.usfirst.frc.team2974.robot.Config.Input.SHIFT_UP_BUTTON; +import static org.usfirst.frc.team2974.robot.Config.Input.SHIFT_UP_BUTTON_ALT; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -7,13 +25,12 @@ import org.usfirst.frc.team2974.robot.util.ButtonOnce; import org.usfirst.frc.team2974.robot.util.POVButton; -import static org.usfirst.frc.team2974.robot.Config.Input.*; - /** - * This class is the glue that binds the controls on the physical operator - * interface to the commands and command groups that allow control of the robot. + * This class is the glue that binds the controls on the physical operator interface to the commands + * and command groups that allow control of the robot. */ -public class OI { +public final class OI { + public static final Joystick leftJoystick; public static final Joystick rightJoystick; public static final Gamepad gamepad; @@ -24,7 +41,7 @@ public class OI { public static final Button shiftTrigger; // public static final Button shiftUpAlt; // ButtonMultiple ^^^ // public static final Button shiftDownAlt; - + //used with Elevator subsystem public static final Button elevatorZero; public static final Button elevatorNudgeUp; @@ -43,12 +60,13 @@ public class OI { leftJoystick = new Joystick(LEFT_JOYSTICK_PORT); gamepad = new Gamepad(GAMEPAD_PORT); - shiftUp = new ButtonMultiple(leftJoystick, SHIFT_UP_BUTTON, SHIFT_UP_BUTTON_ALT); // Shifting buttons approved by Mr.B for Noah + shiftUp = new ButtonMultiple(leftJoystick, SHIFT_UP_BUTTON, + SHIFT_UP_BUTTON_ALT); // Shifting buttons approved by Mr.B for Noah shiftDown = new ButtonMultiple(leftJoystick, SHIFT_DOWN_BUTTON, SHIFT_DOWN_BUTTON_ALT); - shiftTrigger = new JoystickButton(leftJoystick, SHIFT_TOOGLE_BUTTON); + shiftTrigger = new JoystickButton(leftJoystick, SHIFT_TOGGLE_BUTTON); // shiftUpAlt = new JoystickButton(leftJoystick, SHIFT_UP_BUTTON_ALT); // moved to ButtonMultiple :) // shiftDownAlt = new JoystickButton(leftJoystick, SHIFT_DOWN_BUTTON_ALT); - + elevatorNudgeUp = new POVButton(gamepad, ELEVATOR_NUDGE_UP); elevatorNudgeDown = new POVButton(gamepad, ELEVATOR_NUDGE_DOWN); elevatorZero = new ButtonOnce(gamepad, ELEVATOR_ZERO); @@ -60,4 +78,7 @@ public class OI { intake = new JoystickButton(gamepad, INTAKE_BUTTON); output = new JoystickButton(gamepad, OUTPUT_BUTTON); } + + private OI() { + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java index 59604ec..7a50dc2 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java @@ -1,12 +1,14 @@ package org.usfirst.frc.team2974.robot; +import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor; +import static org.usfirst.frc.team2974.robot.RobotMap.pneumaticsShifter; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import org.usfirst.frc.team2974.robot.command.auton.GamePosition; import org.usfirst.frc.team2974.robot.command.auton.SimpleSpline; import org.usfirst.frc.team2974.robot.subsystems.Drivetrain; @@ -16,205 +18,200 @@ import org.waltonrobotics.MotionLogger; import org.waltonrobotics.controller.Pose; -import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor; - /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the IterativeRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the manifest file in the resource - * directory. + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the IterativeRobot documentation. If you change the name of this class + * or the package after creating this project, you must also update the manifest file in the + * resource directory. */ public class Robot extends IterativeRobot { - public static Drivetrain drivetrain; - public static IntakeOutput intakeOutput; - public static Elevator elevator; - - private CommandGroup autonCommands; - - public static ElevatorLogger elevatorLogger; - public static MotionLogger motionLogger; - - private SendableChooser doNothingChooser; - private SendableChooser startLocation; -// private static SendableChooser robotChooser = new SendableChooser<>(); - - private static Config.Robot currentRobot; - private static String gameData; // for ease of access - - /** - * This function is run when the robot is first started up and should be used - * for any initialization code. - */ - @Override - public void robotInit() { - if (RobotMap.robotIdentifier.get()) - currentRobot = Config.Robot.COMPETITION; - else - currentRobot = Config.Robot.PRACTICE; - - motionLogger = new MotionLogger("/home/lvuser/"); - elevatorLogger = new ElevatorLogger("/home/lvuser/"); - drivetrain = new Drivetrain(motionLogger); - intakeOutput = new IntakeOutput(); - elevator = new Elevator(elevatorLogger); - - doNothingChooser = new SendableChooser<>(); - doNothingChooser.addObject("Do Nothing!", true); - doNothingChooser.addDefault("Please move!", false); - - startLocation = new SendableChooser<>(); - startLocation.addObject("Left", 'l'); - startLocation.addObject("Right", 'r'); - startLocation.addDefault("Center", 'c'); + public static Drivetrain drivetrain; + public static IntakeOutput intakeOutput; + public static Elevator elevator; + public static ElevatorLogger elevatorLogger; + public static MotionLogger motionLogger; + private static Config.Robot currentRobot; + private static String gameData; // for ease of access + private CommandGroup autonCommands; + // private static SendableChooser robotChooser = new SendableChooser<>(); + private SendableChooser doNothingChooser; + private SendableChooser startLocation; + + public static Config.Robot getChoosenRobot() { + return currentRobot; + } + + /** + * @return Either 'L' for left position or 'R' for right position. + */ + public static char getSwitchPosition() { + return gameData.charAt(0); // 0 = switch position in string + } + + /** + * @return Either 'L' for left position or 'R' for right position. + */ + public static char getScalePosition() { + return gameData.charAt(1); // 1 = scale position in string + } + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + currentRobot = + RobotMap.robotIdentifier.get() ? Config.Robot.COMPETITION : Config.Robot.PRACTICE; + + motionLogger = new MotionLogger("/home/lvuser/"); + elevatorLogger = new ElevatorLogger("/home/lvuser/"); + drivetrain = new Drivetrain(motionLogger); + intakeOutput = new IntakeOutput(); + elevator = new Elevator(elevatorLogger); + + doNothingChooser = new SendableChooser<>(); + doNothingChooser.addObject("Do Nothing!", true); + doNothingChooser.addDefault("Please move!", false); + + startLocation = new SendableChooser<>(); + startLocation.addObject("Left", 'l'); + startLocation.addObject("Right", 'r'); + startLocation.addDefault("Center", 'c'); // robotChooser.addObject("Practice", Config.Robot.PRACTICE); // robotChooser.addDefault("Competition", Config.Robot.COMPETITION); - // SmartDashboard.putData("TEST AUTON", SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 1, 90), new Pose(1, 2, 0), new Pose(2, 2, 0))); - SmartDashboard.putData("6m drive straight", SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90))); - - updateSmartDashboard(); - } - - public static Config.Robot getChoosenRobot() { - return currentRobot; - } - - @Override - public void disabledInit() { - drivetrain.cancelControllerMotion(); - gameData = null; - drivetrain.reset(); - motionLogger.writeMotionDataCSV(); - elevatorLogger.writeMotionDataCSV(); - } - - @Override - public void disabledPeriodic() { - Scheduler.getInstance().run(); - updateSmartDashboard(); - } - - @Override - public void autonomousInit() { - drivetrain.startControllerMotion(); - elevator.startZero(); - motionLogger.initialize(); - elevatorLogger.initialize(); + SmartDashboard.putData("6m drive straight", + SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90))); + + updateSmartDashboard(); + } + + @Override + public void disabledInit() { + drivetrain.cancelControllerMotion(); + gameData = null; + drivetrain.reset(); + motionLogger.writeMotionDataCSV(); + elevatorLogger.writeMotionDataCSV(); + } + + @Override + public void disabledPeriodic() { + Scheduler.getInstance().run(); + updateSmartDashboard(); + } + + @Override + public void autonomousInit() { + drivetrain.startControllerMotion(); + elevator.startZero(); + motionLogger.initialize(); + elevatorLogger.initialize(); // drivetrain.shiftDown(); - drivetrain.shiftUp(); - - if (doNothingChooser.getSelected()) { // if should do nothing - System.out.println(">:( Nothing has been chosen. Scrubs."); - autonCommands = GamePosition.DO_NOTHING.getCommand(); - return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS! - } - - while(gameData == null || gameData.isEmpty()) { - gameData = DriverStation.getInstance().getGameSpecificMessage(); // "LRL" or something - } - - char startPosition = startLocation.getSelected(); - - System.out.println(startPosition); - System.out.println(gameData); - System.out.println(GamePosition.getGamePosition(startPosition, gameData)); - - autonCommands = GamePosition.getGamePosition(startPosition, gameData).getCommand(); - - if (autonCommands != null) - autonCommands.start(); - } - - /** - * This function is called periodically during autonomous - */ - @Override - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - updateSmartDashboard(); - } - - @Override - public void teleopInit() { + drivetrain.shiftUp(); + + if (doNothingChooser.getSelected()) { // if should do nothing + System.out.println(">:( Nothing has been chosen. Scrubs."); + autonCommands = GamePosition.DO_NOTHING.getCommand(); + return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS! + } + + while ((gameData == null) || gameData.isEmpty()) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); // "LRL" or something + } + + char startPosition = startLocation.getSelected(); + + System.out.println(startPosition); + System.out.println(gameData); + System.out.println(GamePosition.getGamePosition(startPosition, gameData)); + + autonCommands = GamePosition.getGamePosition(startPosition, gameData).getCommand(); + + if (autonCommands != null) { + autonCommands.start(); + } + } + + /** + * This function is called periodically during autonomous + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + updateSmartDashboard(); + } + + @Override + public void teleopInit() { // elevator.startZero(); TODO: check with tim - if (autonCommands != null) - autonCommands.cancel(); - - drivetrain.cancelControllerMotion(); - elevator.enableControl(); - drivetrain.shiftDown(); // start in high gear - drivetrain.reset(); - } - - /** - * This function is called periodically during operator control - */ - @Override - public void teleopPeriodic() { - Scheduler.getInstance().run(); - updateSmartDashboard(); - } - - @Override - public void testInit() { - } - - /** - * This function is called periodically during test mode - */ - @Override - public void testPeriodic() { - } - - /** - * Put things in here you want to update for SmartDashboard. - */ - private void updateSmartDashboard() { - SmartDashboard.putNumber("Left", RobotMap.encoderLeft.getDistance()); - SmartDashboard.putNumber("Right", RobotMap.encoderRight.getDistance()); - - // Selectors - SmartDashboard.putData("Start Location", startLocation); - SmartDashboard.putData("Do Nothing", doNothingChooser); - - // Elevator - SmartDashboard.putNumber("Elevator Position (inches)", elevator.getCurrentPosition()); - SmartDashboard.putBoolean("Elevator Limit Switch", RobotMap.elevatorLimitLower.get()); - SmartDashboard.putNumber("Elevator Error", elevator.getError()); - SmartDashboard.putBoolean("Elevator isZeroing", elevator.isZeroing()); - SmartDashboard.putBoolean("Elevator isZeroed", elevator.isZeroed()); - SmartDashboard.putString("Elevator power mode", elevatorMotor.getControlMode().name()); - SmartDashboard.putBoolean("Elevator elevator mode", elevator.isMotionControlled()); - } - - /** - * Used for testing purposes, insert into autonomousInit() when needed - */ - private void randomizeGameData() { - // randomize gameData - gameData = (Math.random() > .5 ? "L" : "R") + // switch - (Math.random() > .5 ? "L" : "R") + // scale - (Math.random() > .5 ? "L" : "R"); // doesn't matter - - SmartDashboard.putString("Randomized GameData", gameData); - } - - /** - * @return Either 'L' for left position or 'R' for right position. - */ - public static char getSwitchPosition() { - return gameData.charAt(0); // 0 = switch position in string - } - - /** - * @return Either 'L' for left position or 'R' for right position. - */ - public static char getScalePosition() { - return gameData.charAt(1); // 1 = scale position in string - } + if (autonCommands != null) { + autonCommands.cancel(); + } + + drivetrain.cancelControllerMotion(); + elevator.enableControl(); + drivetrain.shiftUp(); // start in high gear + drivetrain.reset(); + } + + /** + * This function is called periodically during operator control + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); + updateSmartDashboard(); + } + + @Override + public void testInit() { + } + + /** + * This function is called periodically during test mode + */ + @Override + public void testPeriodic() { + } + + /** + * Put things in here you want to update for SmartDashboard. + */ + private void updateSmartDashboard() { + SmartDashboard.putNumber("Left", RobotMap.encoderLeft.getDistance()); + SmartDashboard.putNumber("Right", RobotMap.encoderRight.getDistance()); + + // Selectors + SmartDashboard.putData("Start Location", startLocation); + SmartDashboard.putData("Do Nothing", doNothingChooser); + + // Elevator + SmartDashboard.putNumber("Elevator Position (inches)", elevator.getCurrentPosition()); + SmartDashboard.putBoolean("Elevator Limit Switch", RobotMap.elevatorLimitLower.get()); + SmartDashboard.putNumber("Elevator Error", elevator.getError()); + SmartDashboard.putBoolean("Elevator isZeroing", elevator.isZeroing()); + SmartDashboard.putBoolean("Elevator isZeroed", elevator.isZeroed()); + SmartDashboard.putString("Elevator power mode", elevatorMotor.getControlMode().name()); + SmartDashboard.putBoolean("Elevator elevator mode", elevator.isMotionControlled()); + SmartDashboard.putString("Gear", pneumaticsShifter.get() ? "Low" : "High"); + + } + + /** + * Used for testing purposes, insert into autonomousInit() when needed + */ + private void randomizeGameData() { + // randomize gameData + gameData = (Math.random() > 0.5 ? "L" : "R") + // switch + (Math.random() > 0.5 ? "L" : "R") + // scale + (Math.random() > 0.5 ? "L" : "R"); // doesn't matter + + SmartDashboard.putString("Randomized GameData", gameData); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/RobotMap.java b/PowerUp/src/org/usfirst/frc/team2974/robot/RobotMap.java index 95a3cd1..535e6f5 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/RobotMap.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/RobotMap.java @@ -1,20 +1,33 @@ package org.usfirst.frc.team2974.robot; -import com.ctre.CANTalon; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import static org.usfirst.frc.team2974.robot.Config.Hardware.ELEVATOR_LIMIT_LOWER_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.ELEVATOR_MOTOR_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.INTAKE_LEFT_MOTOR_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.INTAKE_RIGHT_MOTOR_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.LEFT_ENCODER_CHANNEL1; +import static org.usfirst.frc.team2974.robot.Config.Hardware.LEFT_ENCODER_CHANNEL2; +import static org.usfirst.frc.team2974.robot.Config.Hardware.LEFT_MOTOR_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.RIGHT_ENCODER_CHANNEL1; +import static org.usfirst.frc.team2974.robot.Config.Hardware.RIGHT_ENCODER_CHANNEL2; +import static org.usfirst.frc.team2974.robot.Config.Hardware.RIGHT_MOTOR_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.ROBOT_IDENTIFIER_CHANNEL; +import static org.usfirst.frc.team2974.robot.Config.Hardware.SHIFTER_CHANNEL; + import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.*; - -import static org.usfirst.frc.team2974.robot.Config.Hardware.*; +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.Talon; /** - * The RobotMap is a mapping from the ports sensors and actuators are wired into - * to a variable name. This provides flexibility changing wiring, makes checking - * the wiring easier and significantly reduces the number of magic numbers - * floating around. + * The RobotMap is a mapping from the ports sensors and actuators are wired into to a variable name. + * This provides flexibility changing wiring, makes checking the wiring easier and significantly + * reduces the number of magic numbers floating around. */ -public class RobotMap { +public final class RobotMap { + public static final Talon motorLeft; public static final Talon motorRight; public static final DigitalInput robotIdentifier; //true for compbot false for practice @@ -38,8 +51,10 @@ public class RobotMap { motorLeft = new Talon(LEFT_MOTOR_CHANNEL); motorRight = new Talon(RIGHT_MOTOR_CHANNEL); - encoderRight = new Encoder(new DigitalInput(RIGHT_ENCODER_CHANNEL1), new DigitalInput(RIGHT_ENCODER_CHANNEL2)); - encoderLeft = new Encoder(new DigitalInput(LEFT_ENCODER_CHANNEL1), new DigitalInput(LEFT_ENCODER_CHANNEL2)); + encoderRight = new Encoder(new DigitalInput(RIGHT_ENCODER_CHANNEL1), + new DigitalInput(RIGHT_ENCODER_CHANNEL2)); + encoderLeft = new Encoder(new DigitalInput(LEFT_ENCODER_CHANNEL1), + new DigitalInput(LEFT_ENCODER_CHANNEL2)); compressor = new Compressor(); pneumaticsShifter = new Solenoid(SHIFTER_CHANNEL); @@ -52,4 +67,7 @@ public class RobotMap { robotIdentifier = new DigitalInput(ROBOT_IDENTIFIER_CHANNEL); } + + private RobotMap() { + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/AutonOption.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/AutonOption.java new file mode 100644 index 0000000..24e3515 --- /dev/null +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/AutonOption.java @@ -0,0 +1,33 @@ +package org.usfirst.frc.team2974.robot.command.auton; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public abstract class AutonOption extends CommandGroup { + + private boolean isOptionSelected; + + public AutonOption() { + + isOptionSelected = false; + } + + public abstract AutonOption right(); + + public abstract AutonOption left(); + + public abstract AutonOption center(); + + @Override + protected final void initialize() { + super.initialize(); + + if (!isOptionSelected) { + throw new RuntimeException( + "Left or Right was not called when the command was initialized! This is a programmer error."); + } + } + + public void setOptionSelected(boolean optionSelected) { + isOptionSelected = optionSelected; + } +} diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseLineNoEncoder.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseLineNoEncoder.java index d646a7f..2cb6dcd 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseLineNoEncoder.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseLineNoEncoder.java @@ -1,10 +1,11 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.CommandGroup; +import static org.usfirst.frc.team2974.robot.Config.Path.CROSS_BASELINE_Y; -import static org.usfirst.frc.team2974.robot.Config.Path.*; +import edu.wpi.first.wpilibj.command.CommandGroup; public class CrossBaseLineNoEncoder extends CommandGroup { + public double finalTime = 2; // TODO fix this public CrossBaseLineNoEncoder left() { diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java index 35c7de7..ed3e3af 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java @@ -1,74 +1,81 @@ package org.usfirst.frc.team2974.robot.command.auton; +import static org.usfirst.frc.team2974.robot.Config.Path.ACCELERATION_MAX; +import static org.usfirst.frc.team2974.robot.Config.Path.C0; +import static org.usfirst.frc.team2974.robot.Config.Path.C1; +import static org.usfirst.frc.team2974.robot.Config.Path.C2; +import static org.usfirst.frc.team2974.robot.Config.Path.VELOCITY_MAX; + import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc.team2974.robot.Robot; -import static org.usfirst.frc.team2974.robot.Config.Path.*; - /** - * Command for crossing the baseline. - * Usage is: new CrossBaseline().left(); + * Command for crossing the baseline. Usage is: new CrossBaseline().left(); */ public class CrossBaseline extends CommandGroup { - private boolean isOptionSelected; + private boolean isOptionSelected; - /** - * Follow up your constructor with a call to either - * left, right, or center to make this command work correctly. - */ - public CrossBaseline() { - super(); + /** + * Follow up your constructor with a call to either left, right, or center to make this command + * work correctly. + */ + public CrossBaseline() { - isOptionSelected = false; + isOptionSelected = false; - // add sequential later. - } + // add sequential later. + } - /** - * Called when on the left side of the field. - * @return this - */ - public CrossBaseline left(double yMovement) { - return right(yMovement); - } + /** + * Called when on the left side of the field. + * + * @return this + */ + public CrossBaseline left(double yMovement) { + return right(yMovement); + } - /** - * Called when on the right side of the field. - * @return this - */ - public CrossBaseline right(double yMovement) { - isOptionSelected = true; + /** + * Called when on the right side of the field. + * + * @return this + */ + public CrossBaseline right(double yMovement) { + isOptionSelected = true; - // drive forward x meters - addSequential(new DriveStraightByDistance(VELOCITY_MAX, ACCELERATION_MAX, yMovement)); - return this; - } + // drive forward x meters + addSequential(new DriveStraightByDistance(VELOCITY_MAX, ACCELERATION_MAX, yMovement)); + return this; + } - /** - * Called when on the center of the field. - * @return this - */ - public CrossBaseline center() { - isOptionSelected = true; + /** + * Called when on the center of the field. + * + * @return this + */ + public CrossBaseline center() { + isOptionSelected = true; - // either go left or right, depends on switch position - if(Robot.getSwitchPosition() == 'R') { - // go right - addSequential(SimpleSpline.pathFromPosesWithAngle(false, C0, C1)); - } else { - // go left - addSequential(SimpleSpline.pathFromPosesWithAngle(false, C0, C2)); - } + // either go left or right, depends on switch position + if (Robot.getSwitchPosition() == 'R') { + // go right + addSequential(SimpleSpline.pathFromPosesWithAngle(false, C0, C1)); + } else { + // go left + addSequential(SimpleSpline.pathFromPosesWithAngle(false, C0, C2)); + } - return this; // ease of use :) <--- smiley face :) - } + return this; // ease of use :) <--- smiley face :) + } - @Override - protected void initialize() { - super.initialize(); + @Override + protected void initialize() { + super.initialize(); - if(!isOptionSelected) - throw new RuntimeException("Left or Right was not called when the command was initialized! This is a programmer error."); - } + if (!isOptionSelected) { + throw new RuntimeException( + "Left or Right was not called when the command was initialized! This is a programmer error."); + } + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DoNothingCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DoNothingCommand.java index db4b42e..a2a0449 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DoNothingCommand.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DoNothingCommand.java @@ -7,8 +7,7 @@ */ public class DoNothingCommand extends CommandGroup { - public DoNothingCommand() { - super(); - System.out.println("We used the DoNothingCommand command. (╯°□°）╯︵ ┻━┻"); - } + public DoNothingCommand() { + System.out.println("We used the DoNothingCommand command. (╯°□°）╯︵ ┻━┻"); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByDistance.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByDistance.java index 0974008..4c96cd9 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByDistance.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByDistance.java @@ -2,23 +2,69 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; - import org.usfirst.frc.team2974.robot.Robot; public class DriveStraightByDistance extends Command { - public static double pmax; - public double amax; - public double time; - public double t1; - public double t0; - public double dtaccel; + + private static double pMax; + private final double aMax; + private final double time; + private final double t1; + private double t0; + private double dtAccel; + private State state; + + public DriveStraightByDistance(double vMax, double aMax, double distance) { + // Use requires() here to declare subsystem dependencies + requires(Robot.drivetrain); + this.aMax = aMax; + time = distance / vMax; + pMax = Math.min(vMax / 2, + 1); // TODO - 2 is found by the encoder rate when the robot is moving at power 1, so + // change this as necessary + t1 = vMax / aMax; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + state = State.ACC; + t0 = Timer.getFPGATimestamp(); + dtAccel = t1 - t0; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + state.run(this); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return time < (Timer.getFPGATimestamp() - t0);// || state == State.END + // ; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.drivetrain.setSpeeds(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } private enum State { ACC { @Override // For accelerating portion of movement - moves to next // state when done public void run(DriveStraightByDistance d) { - if (d.time / 2 < Timer.getFPGATimestamp() - d.t0) { + if ((d.time / 2) < (Timer.getFPGATimestamp() - d.t0)) { d.state = State.DEC; return; } else if (Timer.getFPGATimestamp() >= d.t1) { @@ -26,30 +72,30 @@ public void run(DriveStraightByDistance d) { return; } - double power = (Timer.getFPGATimestamp() - d.t0) / d.dtaccel; + double power = (Timer.getFPGATimestamp() - d.t0) / d.dtAccel; Robot.drivetrain.setSpeeds(power, power); } }, CONST { @Override // Constant velocity portion of motion public void run(DriveStraightByDistance d) { - if (d.time - Timer.getFPGATimestamp() <= d.dtaccel) { + if ((d.time - Timer.getFPGATimestamp()) <= d.dtAccel) { d.state = State.DEC; return; } - Robot.drivetrain.setSpeeds(pmax, pmax); + Robot.drivetrain.setSpeeds(pMax, pMax); } }, DEC { @Override // Decelerating portion of motion public void run(DriveStraightByDistance d) { - if (d.time < Timer.getFPGATimestamp() - d.t0) { + if (d.time < (Timer.getFPGATimestamp() - d.t0)) { d.state = END; return; } - double power = (d.time - Timer.getFPGATimestamp()) / d.dtaccel; + double power = (d.time - Timer.getFPGATimestamp()) / d.dtAccel; Robot.drivetrain.setSpeeds(power, power); } }, @@ -60,54 +106,9 @@ public void run(DriveStraightByDistance d) { d.end(); } }; + public void run(DriveStraightByDistance d) { } } - private State state; - - public DriveStraightByDistance(double vmax, double amax, double distance) { - // Use requires() here to declare subsystem dependencies - requires(Robot.drivetrain); - this.amax = amax; - time = distance / vmax; - pmax = Math.min(vmax / 2, 1); // TODO - 2 is found by the encoder rate when the robot is moving at power 1, so - // change this as necessary - t1 = vmax / amax; - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - state = State.ACC; - t0 = Timer.getFPGATimestamp(); - dtaccel = t1 - t0; - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - state.run(this); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return time < Timer.getFPGATimestamp() - t0;// || state == State.END - // ; - } - - // Called once after isFinished returns true - @Override - protected void end() { - Robot.drivetrain.setSpeeds(0, 0); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - end(); - } - } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByTime.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByTime.java index f8f9e49..1d951c3 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByTime.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveStraightByTime.java @@ -2,23 +2,69 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; - import org.usfirst.frc.team2974.robot.Robot; -public class DriveStraightByTime extends Command{ - public static double pmax; - public double amax; - public double time; - public double t1; - public double t0; - public double dtaccel; +public class DriveStraightByTime extends Command { + + private static double pMax; + private final double aMax; + private final double time; + private final double t1; + private double t0; + private double dtAccel; + private State state; + + public DriveStraightByTime(double vMax, double aMax, double time) { + // Use requires() here to declare subsystem dependencies + requires(Robot.drivetrain); + this.aMax = aMax; + this.time = time; + pMax = Math.min(vMax / 2, + 1); // TODO - 2 is found by the encoder rate when the robot is moving at power 1, so + // change this as necessary + t1 = vMax / aMax; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + state = State.ACC; + t0 = Timer.getFPGATimestamp(); + dtAccel = t1 - t0; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + state.run(this); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return time < (Timer.getFPGATimestamp() - t0);// || state == State.END + // ; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.drivetrain.setSpeeds(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } private enum State { ACC { @Override // For accelerating portion of movement - moves to next // state when done public void run(DriveStraightByTime d) { - if (d.time / 2 < Timer.getFPGATimestamp() - d.t0) { + if ((d.time / 2) < (Timer.getFPGATimestamp() - d.t0)) { d.state = State.DEC; return; } else if (Timer.getFPGATimestamp() >= d.t1) { @@ -26,30 +72,30 @@ public void run(DriveStraightByTime d) { return; } - double power = (Timer.getFPGATimestamp() - d.t0) / d.dtaccel; + double power = (Timer.getFPGATimestamp() - d.t0) / d.dtAccel; Robot.drivetrain.setSpeeds(power, power); } }, CONST { @Override // Constant velocity portion of motion public void run(DriveStraightByTime d) { - if (d.time - Timer.getFPGATimestamp() <= d.dtaccel) { + if ((d.time - Timer.getFPGATimestamp()) <= d.dtAccel) { d.state = State.DEC; return; } - Robot.drivetrain.setSpeeds(pmax, pmax); + Robot.drivetrain.setSpeeds(pMax, pMax); } }, DEC { @Override // Decelerating portion of motion public void run(DriveStraightByTime d) { - if (d.time < Timer.getFPGATimestamp() - d.t0) { + if (d.time < (Timer.getFPGATimestamp() - d.t0)) { d.state = END; return; } - double power = (d.time - Timer.getFPGATimestamp()) / d.dtaccel; + double power = (d.time - Timer.getFPGATimestamp()) / d.dtAccel; Robot.drivetrain.setSpeeds(power, power); } }, @@ -60,53 +106,8 @@ public void run(DriveStraightByTime d) { d.end(); } }; + public void run(DriveStraightByTime d) { } } - - private State state; - - public DriveStraightByTime(double vmax, double amax, double time) { - // Use requires() here to declare subsystem dependencies - requires(Robot.drivetrain); - this.amax = amax; - this.time = time; - pmax = Math.min(vmax / 2, 1); // TODO - 2 is found by the encoder rate when the robot is moving at power 1, so - // change this as necessary - t1 = vmax / amax; - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - state = State.ACC; - t0 = Timer.getFPGATimestamp(); - dtaccel = t1 - t0; - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - state.run(this); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return time < Timer.getFPGATimestamp() - t0;// || state == State.END - // ; - } - - // Called once after isFinished returns true - @Override - protected void end() { - Robot.drivetrain.setSpeeds(0, 0); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - end(); - } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java index e0f8066..cac8bc0 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java @@ -1,65 +1,67 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.CommandGroup; +import static org.usfirst.frc.team2974.robot.Config.Elevator.HIGH_HEIGHT; +import static org.usfirst.frc.team2974.robot.Config.Elevator.LOW_HEIGHT; +import static org.usfirst.frc.team2974.robot.Config.Path.L10; +import static org.usfirst.frc.team2974.robot.Config.Path.L2; +import static org.usfirst.frc.team2974.robot.Config.Path.L3; +import static org.usfirst.frc.team2974.robot.Config.Path.L5; +import static org.usfirst.frc.team2974.robot.Config.Path.L6; +import static org.usfirst.frc.team2974.robot.Config.Path.L7; +import static org.usfirst.frc.team2974.robot.Config.Path.L8; +import static org.usfirst.frc.team2974.robot.Config.Path.L9; +import static org.usfirst.frc.team2974.robot.Config.Path.R10; +import static org.usfirst.frc.team2974.robot.Config.Path.R2; +import static org.usfirst.frc.team2974.robot.Config.Path.R3; +import static org.usfirst.frc.team2974.robot.Config.Path.R5; +import static org.usfirst.frc.team2974.robot.Config.Path.R6; +import static org.usfirst.frc.team2974.robot.Config.Path.R7; +import static org.usfirst.frc.team2974.robot.Config.Path.R8; +import static org.usfirst.frc.team2974.robot.Config.Path.R9; -import static org.usfirst.frc.team2974.robot.Config.Elevator.*; -import static org.usfirst.frc.team2974.robot.Config.Path.*; +import edu.wpi.first.wpilibj.command.WaitCommand; /** - * Drives to the switch, - * puts a cube in, - * gets another cube, - * drives to the scale, - * puts a cube in - * + * Drives to the switch, puts a cube in, gets another cube, drives to the scale, puts a cube in */ -public class DriveSwitchScale extends CommandGroup { +public class DriveSwitchScale extends AutonOption { - private boolean isOptionSelected; + public DriveSwitchScale left() { + addSequential(new DriveToSwitch().left()); // should also drop cube in + addSequential(SimpleSpline.pathFromPosesWithAngle(true, L5, L7, L8)); + addParallel(new ElevatorTarget(LOW_HEIGHT)); + addSequential(SimpleSpline.pathFromPosesWithAngle(false, L8, L9)); + addSequential(new FindCube()); + // TODO: gather cube too + addSequential(SimpleSpline.pathFromPosesWithAngle(true, L9, L10)); + addParallel(new ElevatorTarget(HIGH_HEIGHT)); + addSequential(SimpleSpline.pathFromPosesWithAngle(false, L10, L6, L2, L3)); + addSequential(new WaitCommand(0.5)); + addSequential(new DropCube()); - public DriveSwitchScale() { - super(); - isOptionSelected = false; - } + setOptionSelected(true); - public DriveSwitchScale left() { - addSequential(new DriveToSwitch().left()); // should also drop cube in - addSequential(SimpleSpline.pathFromPosesWithAngle(true, L5, L7, L8)); - addParallel(new ElevatorTarget(LOW_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, L8, L9)); - addSequential(new FindCube()); - // TODO: gather cube too - addSequential(SimpleSpline.pathFromPosesWithAngle(true, L9, L10)); - addParallel(new ElevatorTarget(HIGH_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, L10, L6, L2, L3)); - addSequential(new DropCube()); + return this; + } - isOptionSelected = true; + @Override + public AutonOption center() { + return this; + } - return this; - } + public DriveSwitchScale right() { + addSequential(new DriveToSwitch().right()); // should also drop cube in + addSequential(SimpleSpline.pathFromPosesWithAngle(true, R5, R7, R8)); + addParallel(new ElevatorTarget(LOW_HEIGHT)); + addSequential(SimpleSpline.pathFromPosesWithAngle(false, R8, R9)); + addSequential(new FindCube()); + addSequential(SimpleSpline.pathFromPosesWithAngle(true, R9, R10)); + addParallel(new ElevatorTarget(HIGH_HEIGHT)); + addSequential(SimpleSpline.pathFromPosesWithAngle(false, R10, R6, R2, R3)); + addSequential(new WaitCommand(0.5)); + addSequential(new DropCube()); + setOptionSelected(true); - public DriveSwitchScale right() { - addSequential(new DriveToSwitch().right()); // should also drop cube in - addSequential(SimpleSpline.pathFromPosesWithAngle(true, R5, R7, R8)); - addParallel(new ElevatorTarget(LOW_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, R8, R9)); - addSequential(new FindCube()); - addSequential(SimpleSpline.pathFromPosesWithAngle(true, R9, R10)); - addParallel(new ElevatorTarget(HIGH_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, R10, R6, R2, R3)); - addSequential(new DropCube()); - - isOptionSelected = true; - - return this; - } - - @Override - protected void initialize() { - super.initialize(); - - if(!isOptionSelected) - throw new RuntimeException("Left or Right was not called when the command was initialized! This is a programmer error."); - } + return this; + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToScale.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToScale.java index df9d127..409ed7e 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToScale.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToScale.java @@ -1,48 +1,35 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.CommandGroup; - import static org.usfirst.frc.team2974.robot.Config.Elevator.HIGH_HEIGHT; -import static org.usfirst.frc.team2974.robot.Config.Path.*; +import static org.usfirst.frc.team2974.robot.Config.Path.L0; +import static org.usfirst.frc.team2974.robot.Config.Path.L1; +import static org.usfirst.frc.team2974.robot.Config.Path.L2; +import static org.usfirst.frc.team2974.robot.Config.Path.L3; +import static org.usfirst.frc.team2974.robot.Config.Path.R0; +import static org.usfirst.frc.team2974.robot.Config.Path.R1; +import static org.usfirst.frc.team2974.robot.Config.Path.R2; +import static org.usfirst.frc.team2974.robot.Config.Path.R3; + +import org.usfirst.frc.team2974.robot.util.AutonUtil; /** * Drives to scale from the left or the right starting position. */ -public class DriveToScale extends CommandGroup { - - private boolean isOptionSelected; // all drive commands use this - - public DriveToScale() { - super(); - - isOptionSelected = false; - } - - public DriveToScale left() { - addParallel(new ElevatorTarget(HIGH_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, L0, L1, L2, L3)); - addSequential(new DropCube()); - - isOptionSelected = true; - - return this; - } - - public DriveToScale right() { - addParallel(new ElevatorTarget(HIGH_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, R0, R1, R2, R3)); - addSequential(new DropCube()); +public class DriveToScale extends AutonOption { - isOptionSelected = true; - return this; - } + public DriveToScale left() { + return AutonUtil.driveToSinglePoint(this, + HIGH_HEIGHT, L0, L1, L2, L3); + } - @Override - protected void initialize() { - super.initialize(); + @Override + public AutonOption center() { + return this; + } - if(!isOptionSelected) - throw new RuntimeException("Left or Right was not called when the command was initialized! This is a programmer error."); - } + public DriveToScale right() { + return AutonUtil.driveToSinglePoint(this, + HIGH_HEIGHT, R0, R1, R2, R3); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToSwitch.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToSwitch.java index 2fab1ea..14599d7 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToSwitch.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveToSwitch.java @@ -1,59 +1,37 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.CommandGroup; - import static org.usfirst.frc.team2974.robot.Config.Elevator.MEDIUM_HEIGHT; -import static org.usfirst.frc.team2974.robot.Config.Path.*; +import static org.usfirst.frc.team2974.robot.Config.Path.L0; +import static org.usfirst.frc.team2974.robot.Config.Path.L4; +import static org.usfirst.frc.team2974.robot.Config.Path.L5; +import static org.usfirst.frc.team2974.robot.Config.Path.R0; +import static org.usfirst.frc.team2974.robot.Config.Path.R4; +import static org.usfirst.frc.team2974.robot.Config.Path.R5; + +import edu.wpi.first.wpilibj.command.WaitCommand; +import org.usfirst.frc.team2974.robot.util.AutonUtil; /** * Drives to the switch from the chosen (left(), right(), center()) starting position. */ -public class DriveToSwitch extends CommandGroup { - - private boolean isOptionSelected; - - public DriveToSwitch() { - super(); - - isOptionSelected = false; - } - - public DriveToSwitch left() { - addParallel(new ElevatorTarget(MEDIUM_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, L0, L4, L5)); - addSequential(new DropCube()); - - isOptionSelected = true; - - return this; - } - - public DriveToSwitch right() { - addParallel(new ElevatorTarget(MEDIUM_HEIGHT)); - addSequential(SimpleSpline.pathFromPosesWithAngle(false, R0, R4, R5)); - addSequential(new DropCube()); - - isOptionSelected = true; - - return this; - } - - public DriveToSwitch center() { - addParallel(new ElevatorTarget(MEDIUM_HEIGHT)); - addSequential(new CrossBaseline().center()); // :) - addSequential(new DropCube()); +public class DriveToSwitch extends AutonOption { - isOptionSelected = true; + public DriveToSwitch left() { + return AutonUtil.driveToSinglePoint(this, MEDIUM_HEIGHT, L0, L4, L5); + } - return this; - } + public DriveToSwitch right() { + return AutonUtil.driveToSinglePoint(this, MEDIUM_HEIGHT, R0, R4, R5); + } + public DriveToSwitch center() { + addParallel(new ElevatorTarget(MEDIUM_HEIGHT)); + addSequential(new CrossBaseline().center()); // :) + addSequential(new WaitCommand(0.5)); + addSequential(new DropCube()); - @Override - protected void initialize() { - super.initialize(); + setOptionSelected(true); - if(!isOptionSelected) - throw new RuntimeException("Left or Right was not called when the command was initialized! This is a programmer error."); - } + return this; + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DropCube.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DropCube.java index a3d48b0..95b902a 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DropCube.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DropCube.java @@ -1,37 +1,36 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.Command; - import static org.usfirst.frc.team2974.robot.Robot.intakeOutput; +import edu.wpi.first.wpilibj.command.Command; + /** * */ public class DropCube extends Command { - public DropCube() { - super(); - requires(intakeOutput); - } - - @Override - protected void initialize() { - intakeOutput.resetTime(); - intakeOutput.highOutput(); - } - - @Override - protected void end() { - intakeOutput.off(); - } - - @Override - protected void interrupted() { - end(); - } - - @Override - protected boolean isFinished() { - return intakeOutput.timeElapsed(); - } + public DropCube() { + requires(intakeOutput); + } + + @Override + protected void initialize() { + intakeOutput.resetTime(); + intakeOutput.highOutput(); + } + + @Override + protected void end() { + intakeOutput.off(); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { + return intakeOutput.timeElapsed(); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/ElevatorTarget.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/ElevatorTarget.java index 2ee1c13..bfed761 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/ElevatorTarget.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/ElevatorTarget.java @@ -1,42 +1,43 @@ package org.usfirst.frc.team2974.robot.command.auton; -import edu.wpi.first.wpilibj.command.Command; - import static org.usfirst.frc.team2974.robot.Robot.elevator; +import edu.wpi.first.wpilibj.command.Command; + /** * */ public class ElevatorTarget extends Command { - private double position; + private final double position; - public ElevatorTarget(double position) { - this.position = position; + public ElevatorTarget(double position) { + this.position = position; - if(!elevator.isMotionControlled()) - elevator.enableControl(); + if (!elevator.isMotionControlled()) { + elevator.enableControl(); + } - requires(elevator); - } + requires(elevator); + } - @Override - protected void initialize() { - elevator.setTarget(position); - } + @Override + protected void initialize() { + elevator.setTarget(position); + } - @Override - protected boolean isFinished() { - return Math.abs(elevator.getCurrentPosition() - position) <= 1; - } + @Override + protected boolean isFinished() { + return Math.abs(elevator.getCurrentPosition() - position) <= 1; + } - @Override - protected void end() { + @Override + protected void end() { // elevator.disableControl(); - } + } - @Override - protected void interrupted() { - end(); - } + @Override + protected void interrupted() { + end(); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java index e7921da..a6ea8c3 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java @@ -9,9 +9,9 @@ */ public class FindCube extends Command { - @Override - protected boolean isFinished() { - return true; - } + @Override + protected boolean isFinished() { + return true; + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/GamePosition.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/GamePosition.java index c754331..f6f9734 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/GamePosition.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/GamePosition.java @@ -6,126 +6,132 @@ /** * This enum is meant to represent all possible autonomous. * - * DriveCommand Station position - * l = left drive station - * c = center drive station - * r = right drive station - *

- * Field position - * L = left - * R = right - * . = any - * X = remove character + * DriveCommand Station position l = left drive station c = center drive station r = right drive + * station

Field position L = left R = right . = any X = remove character */ public enum GamePosition { - // TODO: make these names smaller and better - - DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_RIGHT("lRR."), DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_LEFT("lRL."), - DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_RIGHT("lLR."), DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_LEFT("lLL."), - - DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_RIGHT("cRR."), DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_LEFT("cRL."), - DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_RIGHT("cLR."), DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_LEFT("cLL."), - - DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_RIGHT("rRR."), DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_LEFT("rRL."), - DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_RIGHT("rLR."), DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_LEFT("rLL."), - - DRIVE_STATION_LEFT_SWITCH_RIGHT("lR.."), DRIVE_STATION_LEFT_SWITCH_LEFT("lL.."), - DRIVE_STATION_CENTER_SWITCH_RIGHT("cR.."), DRIVE_STATION_CENTER_SWITCH_LEFT("cL.."), - DRIVE_STATION_RIGHT_SWITCH_RIGHT("rR.."), DRIVE_STATION_RIGHT_SWITCH_LEFT("rL.."), - - DRIVE_STATION_LEFT_SCALE_RIGHT("l.R."), DRIVE_STATION_LEFT_SCALE_LEFT("l.L."), - DRIVE_STATION_CENTER_SCALE_RIGHT("c.R."), DRIVE_STATION_CENTER_SCALE_LEFT("c.L."), - DRIVE_STATION_RIGHT_SCALE_RIGHT("r.R."), DRIVE_STATION_RIGHT_SCALE_LEFT("r.L."), - - CROSS_BASELINE_LEFT("l..."), CROSS_BASELINE_CENTER("c..."), CROSS_BASELINE_RIGHT("r..."), - - DO_NOTHING("...."); - - private final String position; - - GamePosition(String position) { - this.position = position; - } - - private static GamePosition getGamePosition(String string) { - for (GamePosition gamePosition : values()) { - if (string.matches(gamePosition.getPosition())) - return gamePosition; - } - - return DO_NOTHING; - } - - /** - * @param startPos the starting position of the robot, 'l', 'r' or 'c'. - * @param gameData the match game data - * @return the correct game position - */ - public static GamePosition getGamePosition(char startPos, String gameData) { - char[] editData = gameData.toCharArray(); - if(startPos == 'c') { - editData[1] = '.'; // no scale - } else { - if(Character.toUpperCase(startPos) != editData[0]) - editData[0] = '.'; - - if(Character.toUpperCase(startPos) != editData[1]) - editData[1] = '.'; - } + // TODO: make these names smaller and better + + DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_RIGHT("lRR."), DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_LEFT( + "lRL."), + DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_RIGHT("lLR."), DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_LEFT( + "lLL."), + + DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_RIGHT( + "cRR."), DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_LEFT("cRL."), + DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_RIGHT( + "cLR."), DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_LEFT("cLL."), + + DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_RIGHT( + "rRR."), DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_LEFT("rRL."), + DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_RIGHT("rLR."), DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_LEFT( + "rLL."), + + DRIVE_STATION_LEFT_SWITCH_RIGHT("lR.."), DRIVE_STATION_LEFT_SWITCH_LEFT("lL.."), + DRIVE_STATION_CENTER_SWITCH_RIGHT("cR.."), DRIVE_STATION_CENTER_SWITCH_LEFT("cL.."), + DRIVE_STATION_RIGHT_SWITCH_RIGHT("rR.."), DRIVE_STATION_RIGHT_SWITCH_LEFT("rL.."), + + DRIVE_STATION_LEFT_SCALE_RIGHT("l.R."), DRIVE_STATION_LEFT_SCALE_LEFT("l.L."), + DRIVE_STATION_CENTER_SCALE_RIGHT("c.R."), DRIVE_STATION_CENTER_SCALE_LEFT("c.L."), + DRIVE_STATION_RIGHT_SCALE_RIGHT("r.R."), DRIVE_STATION_RIGHT_SCALE_LEFT("r.L."), + + CROSS_BASELINE_LEFT("l..."), CROSS_BASELINE_CENTER("c..."), CROSS_BASELINE_RIGHT("r..."), + + DO_NOTHING("...."); + + private final String position; + + GamePosition(String position) { + this.position = position; + } + + private static GamePosition getGamePosition(String string) { + for (GamePosition gamePosition : values()) { + if (string.matches(gamePosition.getPosition())) { + return gamePosition; + } + } + + return DO_NOTHING; + } + + /** + * @param startPos the starting position of the robot, 'l', 'r' or 'c'. + * @param gameData the match game data + * @return the correct game position + */ + public static GamePosition getGamePosition(char startPos, String gameData) { + char[] editData = gameData.toCharArray(); + if (startPos == 'c') { + editData[1] = '.'; // no scale + } else { + if (Character.toUpperCase(startPos) != editData[0]) { + editData[0] = '.'; + } + + if (Character.toUpperCase(startPos) != editData[1]) { + editData[1] = '.'; + } + } // editData[2] = '.'; // not needed but why not - gameData = new String(editData); - - return getGamePosition(Character.toLowerCase(startPos) + gameData); - } - - public String getPosition() { - return position; - } - - public CommandGroup getCommand() { - return AutonLoader.getAutonCommands(this); - } - - /** - * Unit tests for GamePositions. - * -> is good. - * @param args not used - */ - public static void main(String[] args) { - for(int startingPos = 0; startingPos < 3; startingPos++) { - for(int switchPos = 0; switchPos < 2; switchPos++) { - for(int scalePos = 0; scalePos < 2; scalePos++) { - char start = 'c'; - if(startingPos == 0) - start = 'l'; - else if(startingPos == 1) - start = 'r'; - - char switch_ = 'L'; - if(switchPos == 1) - switch_ = 'R'; - - char scale = 'L'; - if(scalePos == 1) - scale = 'R'; - - if(start == 'c') { - scale = '.'; - } else { - if(Character.toUpperCase(start) != switch_) - switch_ = '.'; - - if(Character.toUpperCase(start) != scale) - scale = '.'; - } - - String string = "" + start + switch_ + scale + "."; - - System.out.println(string + " -> " + getGamePosition(string)); - } - } - } - } + gameData = new String(editData); + + return getGamePosition(Character.toLowerCase(startPos) + gameData); + } + + /** + * Unit tests for GamePositions. -> is good. + * + * @param args not used + */ + public static void main(String[] args) { + for (int startingPos = 0; startingPos < 3; startingPos++) { + for (int switchPos = 0; switchPos < 2; switchPos++) { + for (int scalePos = 0; scalePos < 2; scalePos++) { + char start = 'c'; + if (startingPos == 0) { + start = 'l'; + } else if (startingPos == 1) { + start = 'r'; + } + + char switch_ = 'L'; + if (switchPos == 1) { + switch_ = 'R'; + } + + char scale = 'L'; + if (scalePos == 1) { + scale = 'R'; + } + + if (start == 'c') { + scale = '.'; + } else { + if (Character.toUpperCase(start) != switch_) { + switch_ = '.'; + } + + if (Character.toUpperCase(start) != scale) { + scale = '.'; + } + } + + String string = String.valueOf(start) + switch_ + scale + '.'; + + System.out.println(string + " -> " + getGamePosition(string)); + } + } + } + } + + public String getPosition() { + return position; + } + + public CommandGroup getCommand() { + return AutonLoader.getAutonCommands(this); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java index bfd3035..43cdfe5 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/SimpleSpline.java @@ -1,35 +1,36 @@ package org.usfirst.frc.team2974.robot.command.auton; -import org.waltonrobotics.controller.Pose; -import org.waltonrobotics.motion.Spline; - -import edu.wpi.first.wpilibj.command.Command; - import static org.usfirst.frc.team2974.robot.Config.Path.ACCELERATION_MAX; import static org.usfirst.frc.team2974.robot.Config.Path.VELOCITY_MAX; import static org.usfirst.frc.team2974.robot.Robot.drivetrain; +import edu.wpi.first.wpilibj.command.Command; import java.util.ArrayList; import java.util.Collections; import java.util.List; +import org.waltonrobotics.controller.Pose; +import org.waltonrobotics.motion.Spline; /** * */ public class SimpleSpline extends Command { - private double startAngle; - private double endAngle; - private boolean isBackwards; - private List knots; - private double vMax; - private double aMax; - public SimpleSpline(double maxVelocity, double maxAcceleration, double startAngle, double endAngle, Pose... knots) { - this(maxVelocity,maxAcceleration, startAngle, endAngle, false, knots); + private final double startAngle; + private final double endAngle; + private final boolean isBackwards; + private final List knots; + private final double vMax; + private final double aMax; + + public SimpleSpline(double maxVelocity, double maxAcceleration, double startAngle, + double endAngle, Pose... knots) { + this(maxVelocity, maxAcceleration, startAngle, endAngle, false, knots); } - public SimpleSpline(double maxVelocity, double maxAcceleration, double startAngle, double endAngle, boolean isBackwards, Pose... knots) { - this.knots = new ArrayList<>(); + public SimpleSpline(double maxVelocity, double maxAcceleration, double startAngle, + double endAngle, boolean isBackwards, Pose... knots) { + this.knots = new ArrayList<>(knots.length); Collections.addAll(this.knots, knots); this.startAngle = startAngle; this.endAngle = endAngle; @@ -40,10 +41,28 @@ public SimpleSpline(double maxVelocity, double maxAcceleration, double startAngl requires(drivetrain); } + public static SimpleSpline pathFromPosesWithAngle(boolean isBackwards, Pose... knots) { + return pathFromPosesWithAngle(VELOCITY_MAX, ACCELERATION_MAX, isBackwards, knots); + } + + /** + * Creates a SimpleSpline where the first angle is the angle of the first point and the final + * angle is the angle of the last point. + * + * @param isBackwards will the robot move forwards or backwards + * @param knots the points (with angle) to move through + * @return a new SimpleSpline command + */ + public static SimpleSpline pathFromPosesWithAngle(double maxVelocity, double maxAcceleration, + boolean isBackwards, Pose... knots) { + return new SimpleSpline(maxVelocity, maxAcceleration, knots[0].getAngle(), + knots[knots.length - 1].getAngle(), isBackwards, knots); + } + protected void initialize() { drivetrain.reset(); drivetrain.addControllerMotions( - new Spline(vMax, aMax, 0,0, startAngle, endAngle, isBackwards, knots)); + new Spline(vMax, aMax, 0, 0, startAngle, endAngle, isBackwards, knots)); // drivetrain.startControllerMotion(); } @@ -70,22 +89,4 @@ protected void end() { protected void interrupted() { end(); } - - public static SimpleSpline pathFromPosesWithAngle(boolean isBackwards, Pose... knots) { - return pathFromPosesWithAngle(VELOCITY_MAX, ACCELERATION_MAX, isBackwards, knots); - } - - /** - * Creates a SimpleSpline where the first angle is the angle of - * the first point and the final angle is the angle of the last point. - * - * @param isBackwards - * will the robot move forwards or backwards - * @param knots - * the points (with angle) to move through - * @return a new SimpleSpline command - */ - public static SimpleSpline pathFromPosesWithAngle(double maxVelocity, double maxAcceleration, boolean isBackwards, Pose... knots) { - return new SimpleSpline(maxVelocity, maxAcceleration, knots[0].getAngle(), knots[knots.length - 1].getAngle(), isBackwards, knots); - } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/DriveCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/DriveCommand.java index 1acc235..dcf7556 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/DriveCommand.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/DriveCommand.java @@ -1,91 +1,98 @@ package org.usfirst.frc.team2974.robot.command.teleop; -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team2974.robot.RobotMap; - +import static org.usfirst.frc.team2974.robot.OI.leftJoystick; +import static org.usfirst.frc.team2974.robot.OI.rightJoystick; +import static org.usfirst.frc.team2974.robot.OI.shiftDown; +import static org.usfirst.frc.team2974.robot.OI.shiftUp; import static org.usfirst.frc.team2974.robot.Robot.drivetrain; -import static org.usfirst.frc.team2974.robot.OI.*; + +import edu.wpi.first.wpilibj.command.Command; /** * */ public class DriveCommand extends Command { - public DriveCommand() { - requires(drivetrain); - } - - public double getLeftThrottle() { - if (Math.abs(leftJoystick.getY()) < 0.3) { - return 0; - } - return leftJoystick.getY(); - } - - public double getRightThrottle() { - if (Math.abs(rightJoystick.getY()) < 0.3) { - return 0; - } - return rightJoystick.getY(); - - } - - public double getTurn() { - if (Math.abs(rightJoystick.getX()) < 0.1) { - return 0; - } - return rightJoystick.getX(); - - } - - private void cheesyDrive() { - double throttle = (-getLeftThrottle() + 1) / 2; - double forward = -getRightThrottle(); - double turn = -getTurn(); - - drivetrain.setSpeeds(throttle * (forward - turn), throttle * (forward + turn)); - } - - private void tankDrive() { - drivetrain.setSpeeds(-getLeftThrottle(), -getRightThrottle()); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - if (drivetrain.isTankDrive()) { - tankDrive(); - } else { - cheesyDrive(); - } - - if (shiftUp.get()) //|| shiftUpAlt.get()) - drivetrain.shiftUp(); - else if (shiftDown.get()) // || shiftDownAlt.get()) - drivetrain.shiftDown(); + public DriveCommand() { + requires(drivetrain); + } + + public double getLeftThrottle() { + if (Math.abs(leftJoystick.getY()) < 0.3) { + return 0; + } + return leftJoystick.getY(); + } + + public double getRightThrottle() { + if (Math.abs(rightJoystick.getY()) < 0.3) { + return 0; + } + return rightJoystick.getY(); + + } + + public double getTurn() { + if (Math.abs(rightJoystick.getX()) < 0.1) { + return 0; + } + return rightJoystick.getX(); + + } + + private void cheesyDrive() { + double throttle = (-getLeftThrottle() + 1) / 2; + double forward = -getRightThrottle(); + double turn = -getTurn(); + + drivetrain.setSpeeds(throttle * (forward - turn), throttle * (forward + turn)); + } + + private void tankDrive() { + drivetrain.setSpeeds(-getLeftThrottle(), -getRightThrottle()); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (drivetrain.isTankDrive()) { + tankDrive(); + } else { + cheesyDrive(); + } + + if (shiftUp.get()) //|| shiftUpAlt.get()) + { + drivetrain.shiftUp(); + } else if (shiftDown.get()) // || shiftDownAlt.get()) + { + drivetrain.shiftDown(); + } // else if (RobotMap.pneumaticsShifter.get() && shiftTrigger.get()) { // drivetrain.shiftDown(); // } else if (!RobotMap.pneumaticsShifter.get()) { // drivetrain.shiftUp(); // } - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - drivetrain.setSpeeds(0, 0); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + drivetrain.setSpeeds(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java index 77dbc00..d4a2ec2 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/ElevatorCommand.java @@ -4,6 +4,7 @@ import static org.usfirst.frc.team2974.robot.Config.Elevator.LOW_HEIGHT; import static org.usfirst.frc.team2974.robot.Config.Elevator.MEDIUM_HEIGHT; import static org.usfirst.frc.team2974.robot.Config.Elevator.NUDGE_DISTANCE; +import static org.usfirst.frc.team2974.robot.Config.Elevator.TOLERANCE; import static org.usfirst.frc.team2974.robot.OI.elevatorHigh; import static org.usfirst.frc.team2974.robot.OI.elevatorLow; import static org.usfirst.frc.team2974.robot.OI.elevatorMedium; @@ -18,7 +19,6 @@ public class ElevatorCommand extends Command { - private final double TOLERANCE = .1; public ElevatorCommand() { requires(elevator); @@ -32,18 +32,7 @@ protected void initialize() { @Override protected void execute() { // System.out.println(elevator.isMotionControlled()); - if (!elevator.isMotionControlled()) { -// System.out.println("Joystick input receiving"); -// System.out.println("Abs get Y " + Math.abs(gamepad.getLeftY())); - if (Math.abs(gamepad.getLeftY()) /*TODO look at why getLeftY return 0*/ > TOLERANCE) { - elevator.setPower(-gamepad.getLeftY()); - } else if (Math.abs(gamepad.getRightY()) > TOLERANCE) { - elevator.setPower(-gamepad.getRightY()); - } else { - elevator.setPower(0); - } - - } else { + if (elevator.isMotionControlled()) { // System.out.println("Getting button inputs"); if (elevatorNudgeUp.get() && !elevator.atTopPosition()) { @@ -61,10 +50,20 @@ protected void execute() { } else if (elevatorLow.get()) { elevator.setTarget(LOW_HEIGHT); } + } else { +// System.out.println("Joystick input receiving"); +// System.out.println("Abs get Y " + Math.abs(gamepad.getLeftY())); + if (Math.abs(gamepad.getLeftY()) /*TODO look at why getLeftY return 0*/ > TOLERANCE) { + elevator.setPower(-gamepad.getLeftY()); + } else if (Math.abs(gamepad.getRightY()) > TOLERANCE) { + elevator.setPower(-gamepad.getRightY()); + } else { + elevator.setPower(0); + } + } if (elevatorToggleControl.get()) { -// System.out.println("screeeee toggle pressed!"); if (elevator.isMotionControlled()) { elevator.disableControl(); } else { diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/IntakeCommand.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/IntakeCommand.java index 877d7b5..84b475d 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/IntakeCommand.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/teleop/IntakeCommand.java @@ -1,96 +1,44 @@ package org.usfirst.frc.team2974.robot.command.teleop; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import static org.usfirst.frc.team2974.robot.OI.intake; import static org.usfirst.frc.team2974.robot.OI.output; import static org.usfirst.frc.team2974.robot.Robot.intakeOutput; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + /** * This command does the intake/output function of the State subsystem. */ public class IntakeCommand extends Command { - - public enum State { - OFF { - public void init(){ - updateState(); - intakeOutput.off(); - } - - public State periodic(){ - if(intake.get()){ - return IN; - } - else if(output.get()){ - return OUT; - } - return this; - } - }, - IN { - public void init(){ - updateState(); - intakeOutput.highIntake(); - } - - public State periodic(){ - if(!intake.get()) { - return OFF; - } - return this; - } - }, - OUT { - public void init(){ - updateState(); - intakeOutput.highOutput(); - } - - public State periodic(){ - if(!output.get()) { - return OFF; - } - return this; - } - }; - - public void updateState() { - SmartDashboard.putString("Intake State", this.name()); // this gets the name easily - } - - public abstract void init(); - public abstract State periodic(); - } private State state = State.OFF; public IntakeCommand() { requires(intakeOutput); } - - @Override - protected void initialize(){ - state = State.OFF; - state.init(); - } - - @Override - protected void execute() { - State newState = state.periodic(); - - if(state != newState) { - state = newState; - state.init(); + + @Override + protected void initialize() { + state = State.OFF; + state.init(); + } + + @Override + protected void execute() { + State newState = state.periodic(); + + if (state != newState) { + state = newState; + state.init(); } - } + } - @Override - protected boolean isFinished() { - return false; - } + @Override + protected boolean isFinished() { + return false; + } @Override protected void end() { @@ -101,4 +49,56 @@ protected void end() { protected void interrupted() { end(); } + + public enum State { + OFF { + public void init() { + updateState(); + intakeOutput.off(); + } + + public State periodic() { + if (intake.get()) { + return IN; + } else if (output.get()) { + return OUT; + } + return this; + } + }, + IN { + public void init() { + updateState(); + intakeOutput.highIntake(); + } + + public State periodic() { + if (!intake.get()) { + return OFF; + } + return this; + } + }, + OUT { + public void init() { + updateState(); + intakeOutput.highOutput(); + } + + public State periodic() { + if (!output.get()) { + return OFF; + } + return this; + } + }; + + public void updateState() { + SmartDashboard.putString("Intake State", name()); // this gets the name easily + } + + public abstract void init(); + + public abstract State periodic(); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/DebugSmartDashboardProperty.java b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/DebugSmartDashboardProperty.java index 8ab09a8..c8c869e 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/DebugSmartDashboardProperty.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/DebugSmartDashboardProperty.java @@ -1,25 +1,23 @@ package org.usfirst.frc.team2974.robot.smartdashboard; -//import static org.usfirst.frc.team2974.robot.Config.IS_DEBUG; - import java.util.function.Supplier; public class DebugSmartDashboardProperty extends SmartDashboardProperty { - /** - * This creates a SmartDashboard value to show only when SmartDashboardManager.isDebug is true. - */ - public DebugSmartDashboardProperty(String key, T defaultValue, Supplier valueSupplier) { - super(key, defaultValue, valueSupplier); - } + /** + * This creates a SmartDashboard value to show only when SmartDashboardManager.isDebug is true. + */ + public DebugSmartDashboardProperty(String key, T defaultValue, Supplier valueSupplier) { + super(key, defaultValue, valueSupplier); + } - @Override - protected final void updateSmartDashboard() { + @Override + protected final void updateSmartDashboard() { // if (IS_DEBUG) { - super.updateSmartDashboard(); + super.updateSmartDashboard(); // } else if (SmartDashboardManager.containsBind(getKey())) { -// why dost thou do this to me smartdashboard +// why dost thou do this to me SmartDashboard // SmartDashboardManager.removeBind(getKey()); // TODO: fix so it actually deletes // } - } + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardManager.java b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardManager.java index 7c91adf..7f0bdc6 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardManager.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardManager.java @@ -37,12 +37,12 @@ public static SmartDashboardProperty addBind(String key, T value) { /** *

Creates a SmartDashboard Property that will update automatically when the update method of - * SmartDashboardManager is called.

+ * SmartDashboardManager is called.

* Example: 
{@code
 	 *   addBind("Left Motor Power", 0, () -> SubsystemManager.getSubsystem(Drivetrain.class).getLeftMotorPower());
 	 *   }
- *

If the supplier is null the property is effectively static, unless another supplier - * is added later. + *

If the supplier is null the property is effectively static, unless another supplier is + * added later.

* * @param key SmartDashboard key * @param defaultValue Default value that SmartDashboard will returns if it cannot find the @@ -124,7 +124,7 @@ public static boolean containsBind(String key) { * @return returns the PROPERTIES list */ public static List getProperties() { - return Collections.unmodifiableList(SmartDashboardManager.PROPERTIES); + return Collections.unmodifiableList(PROPERTIES); } /** @@ -134,18 +134,16 @@ public static List getProperties() { * @param key the key of the property * @return the SmartDashboard property with the specified key if it is found */ - @SuppressWarnings("unchecked") public static SmartDashboardProperty getProperty(String key) { - Optional smartDashboardProperty = SmartDashboardManager.PROPERTIES + Optional smartDashboardProperty = PROPERTIES .stream() .filter(p -> p.getKey() .equals(key)) // gts the properties with the same key as the one searching for .findFirst(); /// gets the first SmartDashboard property - if (smartDashboardProperty.isPresent()) { // if there is a SmartDashboard property - return smartDashboardProperty.get(); // returns the SmartDashboard property - } - return null; + // if there is a SmartDashboard property + // returns the SmartDashboard property + return smartDashboardProperty.orElse(null); // throw new RobotRuntimeException("Property " + key // + " does not exist. Did you forget to add it?"); // if it did not find the SmartDashboard property throw error diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardProperty.java b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardProperty.java index 481a46a..0e50244 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardProperty.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/smartdashboard/SmartDashboardProperty.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import java.util.function.Supplier; /** @@ -10,155 +9,155 @@ */ public class SmartDashboardProperty { - private final String key; // SmartDashboard key - private final T defaultValue; // Default value - private T value; // Current value - private Supplier valueSupplier; // Returns the value to be on SmartDashboard - private Runnable onValueChange; // Runs when value changes - - /** - * Used when you want a static value to be on SmartDashboard. - * - * @param key the key that SmartDashboard will take in to find or update the value - * @param defaultValue the default value that SmartDashboard will take in to retrieve a value - */ - public SmartDashboardProperty(String key, T defaultValue) { - this(key, defaultValue, null); - } - - /** - * Initializes variables and sets up the onValueChange Runnable object. - * - * @param key the key that SmartDashboard will take in to find or update the value - * @param defaultValue the default value that SmartDashboard will take in to retrieve a value - * @param valueSupplier the value that you wish to place into SmartDashboard, can be null for a - * static value - */ - public SmartDashboardProperty(String key, T defaultValue, Supplier valueSupplier) { - this.key = key; - this.defaultValue = defaultValue; - this.valueSupplier = valueSupplier; - - setValue(defaultValue); - updateSmartDashboard(); - } - - /** - * Returns the key to retrieve the SmartDashboard value - * - * @return the key meant to be used to retrieved the SmartDashboard value - */ - public final String getKey() { - return key; - } - - /** - * Gets the current value of the valueSupplier - * - * @return returns the current value - */ - public final T getValue() { - return value; - } - - /** - * Sets the value and updates SmartDashboard by running the onValueChange Runnable object run - * method - * - * @param value the value you want value to be - */ - public final void setValue(T value) { - this.value = value; - - updateSmartDashboard(); - if (onValueChange != null) { - onValueChange.run(); - } - } - - /** - * Returns the default value. - * - * @return returns the default value - */ - public final T getDefaultValue() { - return defaultValue; - } - - /** - * Returns the value supplier. This is used to determine the value that is meant to be on - * SmartDashboard - * - * @return returns valueSupplier - */ - public final Supplier getValueSupplier() { - return valueSupplier; - } - - /** - * Sets valueSupplier - * - * @param valueSupplier the new valueSupplier that should return what value should be placed in - * SmartDashboard - */ - public void setValueSupplier(Supplier valueSupplier) { - this.valueSupplier = valueSupplier; - } - - /** - * Returns onValueChange. This tells the how the program should update the SmartDashboard value - * - * @return returns onValueChange - */ - public final Runnable getOnValueChange() { - return onValueChange; - } - - /** - * Sets onValueChange - * - * @param onValueChange the new onValueChange to say how SmartDashboard will be updated - */ - public final void setOnValueChange(Runnable onValueChange) { - this.onValueChange = onValueChange; - } - - /** - * Note: the value will not be shown if it is null. - */ - protected void updateSmartDashboard() { - if (value == null) { - return; - } - - if (value instanceof Number) { // if the value you are going to put in is a number (double, float, int, byte, etc.) - SmartDashboard.putNumber(key, ((Number) value).doubleValue()); - } else if (value instanceof Boolean) { // if the value is a boolean - SmartDashboard.putBoolean(key, (Boolean) value); - } else if (value instanceof Sendable) { // if the value is a Sendable object (SendableChooser, etc...) - SmartDashboard.putData(key, (Sendable) value); - } else { - // if it is something else it uses its toSting method to display it on SmartDashboard - SmartDashboard.putString(key, value.toString()); - } - } - - /** - * This method retrieves the latest value from the value Supplier and if the value changes it will - * use the onValueChange Runnable object to update SmartDashboard - */ - public void update() { - //FIXME this will not run if you have a static variable since the valueSupplier is always null causing it to never send the value to SmartDashboard - - if (valueSupplier != null) { - setValue(valueSupplier.get()); - } - } - - - @Override - public String toString() { - return String.format("SmartDashboardProperty{key='%s', defaultValue=%s, value=%s}", key, - defaultValue, value); - } + private final String key; // SmartDashboard key + private final T defaultValue; // Default value + private T value; // Current value + private Supplier valueSupplier; // Returns the value to be on SmartDashboard + private Runnable onValueChange; // Runs when value changes + + /** + * Used when you want a static value to be on SmartDashboard. + * + * @param key the key that SmartDashboard will take in to find or update the value + * @param defaultValue the default value that SmartDashboard will take in to retrieve a value + */ + public SmartDashboardProperty(String key, T defaultValue) { + this(key, defaultValue, null); + } + + /** + * Initializes variables and sets up the onValueChange Runnable object. + * + * @param key the key that SmartDashboard will take in to find or update the value + * @param defaultValue the default value that SmartDashboard will take in to retrieve a value + * @param valueSupplier the value that you wish to place into SmartDashboard, can be null for a + * static value + */ + public SmartDashboardProperty(String key, T defaultValue, Supplier valueSupplier) { + this.key = key; + this.defaultValue = defaultValue; + this.valueSupplier = valueSupplier; + + setValue(defaultValue); + updateSmartDashboard(); + } + + /** + * Returns the key to retrieve the SmartDashboard value + * + * @return the key meant to be used to retrieved the SmartDashboard value + */ + public final String getKey() { + return key; + } + + /** + * Gets the current value of the valueSupplier + * + * @return returns the current value + */ + public final T getValue() { + return value; + } + + /** + * Sets the value and updates SmartDashboard by running the onValueChange Runnable object run + * method + * + * @param value the value you want value to be + */ + public final void setValue(T value) { + this.value = value; + + updateSmartDashboard(); + if (onValueChange != null) { + onValueChange.run(); + } + } + + /** + * Returns the default value. + * + * @return returns the default value + */ + public final T getDefaultValue() { + return defaultValue; + } + + /** + * Returns the value supplier. This is used to determine the value that is meant to be on + * SmartDashboard + * + * @return returns valueSupplier + */ + public final Supplier getValueSupplier() { + return valueSupplier; + } + + /** + * Sets valueSupplier + * + * @param valueSupplier the new valueSupplier that should return what value should be placed in + * SmartDashboard + */ + public void setValueSupplier(Supplier valueSupplier) { + this.valueSupplier = valueSupplier; + } + + /** + * Returns onValueChange. This tells the how the program should update the SmartDashboard value + * + * @return returns onValueChange + */ + public final Runnable getOnValueChange() { + return onValueChange; + } + + /** + * Sets onValueChange + * + * @param onValueChange the new onValueChange to say how SmartDashboard will be updated + */ + public final void setOnValueChange(Runnable onValueChange) { + this.onValueChange = onValueChange; + } + + /** + * Note: the value will not be shown if it is null. + */ + protected void updateSmartDashboard() { + if (value == null) { + return; + } + + if (value instanceof Number) { // if the value you are going to put in is a number (double, float, int, byte, etc.) + SmartDashboard.putNumber(key, ((Number) value).doubleValue()); + } else if (value instanceof Boolean) { // if the value is a boolean + SmartDashboard.putBoolean(key, (Boolean) value); + } else if (value instanceof Sendable) { // if the value is a Sendable object (SendableChooser, etc...) + SmartDashboard.putData(key, (Sendable) value); + } else { + // if it is something else it uses its toSting method to display it on SmartDashboard + SmartDashboard.putString(key, value.toString()); + } + } + + /** + * This method retrieves the latest value from the value Supplier and if the value changes it + * will use the onValueChange Runnable object to update SmartDashboard + */ + public void update() { + //FIXME this will not run if you have a static variable since the valueSupplier is always null causing it to never send the value to SmartDashboard + + if (valueSupplier != null) { + setValue(valueSupplier.get()); + } + } + + + @Override + public String toString() { + return String.format("SmartDashboardProperty{key='%s', defaultValue=%s, value=%s}", key, + defaultValue, value); + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java index 923f10e..03c1ada 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java @@ -21,7 +21,7 @@ */ public class Drivetrain extends AbstractDrivetrain { - private SendableChooser driveMode; + private final SendableChooser driveMode; public Drivetrain(MotionLogger motionLogger) { super(motionLogger); @@ -75,15 +75,11 @@ public void setSpeeds(double leftPower, double rightPower) { } public void shiftDown() { - if (!pneumaticsShifter.get()) { - pneumaticsShifter.set(true); - } + pneumaticsShifter.set(true); } public void shiftUp() { - if (pneumaticsShifter.get()) { - pneumaticsShifter.set(false); - } + pneumaticsShifter.set(false); } @Override @@ -116,6 +112,16 @@ public double getKL() { return MotionConstants.KL; } + @Override + public double getILag() { + return MotionConstants.IL; + } + + @Override + public double getIAng() { + return MotionConstants.IAng; + } + public boolean isTankDrive() { return driveMode.getSelected(); } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java index c4544ad..004ecff 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Elevator.java @@ -8,7 +8,7 @@ import static org.usfirst.frc.team2974.robot.Config.Elevator.KI; import static org.usfirst.frc.team2974.robot.Config.Elevator.KP; import static org.usfirst.frc.team2974.robot.Config.Elevator.MAXIMUM_HEIGHT; -import static org.usfirst.frc.team2974.robot.Config.Elevator.MINUMUM_HEIGHT; +import static org.usfirst.frc.team2974.robot.Config.Elevator.MINIMUM_HEIGHT; import static org.usfirst.frc.team2974.robot.Config.Elevator.TIMEOUT; import static org.usfirst.frc.team2974.robot.RobotMap.elevatorLimitLower; import static org.usfirst.frc.team2974.robot.RobotMap.elevatorMotor; @@ -23,20 +23,19 @@ import org.usfirst.frc.team2974.robot.Robot; import org.usfirst.frc.team2974.robot.command.teleop.ElevatorCommand; import org.usfirst.frc.team2974.robot.util.ElevatorLogger; +import org.usfirst.frc.team2974.robot.util.ElevatorLogger.ElevatorData; /** * The elevator subsystem, which raises and lowers the intake/outtake

TODO: finish me */ public class Elevator extends Subsystem { - private ElevatorLogger logger; + private final ElevatorLogger logger; + private final Timer timer = new Timer(); private boolean isMotionControlled; - private double power; - private boolean zeroing; private boolean zeroed; - private Timer timer = new Timer(); public Elevator(ElevatorLogger logger) { zeroing = true; @@ -45,11 +44,7 @@ public Elevator(ElevatorLogger logger) { } public double getError() { - if (isMotionControlled) { - return elevatorMotor.getClosedLoopError(0); - } else { - return 0; - } + return isMotionControlled ? elevatorMotor.getClosedLoopError(0) : 0; } @Override @@ -60,7 +55,7 @@ protected void initDefaultCommand() { @Override public void periodic() { logger.addMotionData( - new ElevatorLogger.ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(), + new ElevatorData(Timer.getFPGATimestamp(), getCurrentPosition(), getCurrentPositionNU(), power)); if (zeroing) { @@ -80,7 +75,8 @@ public void initConstants() { elevatorMotor.setNeutralMode(NeutralMode.Brake); elevatorMotor .configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, TIMEOUT); - elevatorMotor.setSensorPhase(Robot.getChoosenRobot().getSensorPhase()); // true for competition bot // false for practice + elevatorMotor.setSensorPhase(Robot.getChoosenRobot() + .getSensorPhase()); // true for competition bot // false for practice elevatorMotor.setInverted(false); elevatorMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, TIMEOUT); @@ -104,7 +100,7 @@ public void initConstants() { zeroed = false; elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); - elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); + elevatorMotor.configReverseSoftLimitThreshold(MINIMUM_HEIGHT, 10); elevatorMotor.configForwardSoftLimitEnable(true, 10); elevatorMotor.configReverseSoftLimitEnable(false, 10); @@ -121,7 +117,7 @@ public void zeroEncoder() { /* +14 rotations forward when using CTRE Mag encoder */ elevatorMotor.configForwardSoftLimitThreshold(MAXIMUM_HEIGHT, 10); // TODO: FIX /* -15 rotations reverse when using CTRE Mag encoder */ - elevatorMotor.configReverseSoftLimitThreshold(MINUMUM_HEIGHT, 10); // TODO: FIX + elevatorMotor.configReverseSoftLimitThreshold(MINIMUM_HEIGHT, 10); // TODO: FIX elevatorMotor.configForwardSoftLimitEnable(true, 10); elevatorMotor.configReverseSoftLimitEnable(true, 10); @@ -188,13 +184,13 @@ public void setTarget(double inches) { elevatorMotor.set( ControlMode.MotionMagic, Math.min(MAXIMUM_HEIGHT, - Math.max(MINUMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */ + Math.max(MINIMUM_HEIGHT, inches * INCHES_TO_NU)) /* This is a hard cap */ ); } } public void setPower(double percent) { - percent = Math.min(.75, Math.max(-.75, percent)); // throttle power in + percent = Math.min(0.75, Math.max(-0.75, percent)); // throttle power in power = percent; SmartDashboard.putNumber("Elevator Power", percent); @@ -208,7 +204,7 @@ public boolean atTopPosition() { } public boolean atLowerPosition() { - return getCurrentPositionNU() <= MINUMUM_HEIGHT; + return getCurrentPositionNU() <= MINIMUM_HEIGHT; } public void startZero() { diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/IntakeOutput.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/IntakeOutput.java index 0393830..5425598 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/IntakeOutput.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/IntakeOutput.java @@ -1,72 +1,72 @@ package org.usfirst.frc.team2974.robot.subsystems; +import static org.usfirst.frc.team2974.robot.Config.IntakeOutput.HOLD_POWER; +import static org.usfirst.frc.team2974.robot.Config.IntakeOutput.LOW_POWER; +import static org.usfirst.frc.team2974.robot.Config.IntakeOutput.MAX_POWER; +import static org.usfirst.frc.team2974.robot.RobotMap.intakeMotorLeft; +import static org.usfirst.frc.team2974.robot.RobotMap.intakeMotorRight; + import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Subsystem; import org.usfirst.frc.team2974.robot.command.teleop.IntakeCommand; -import static org.usfirst.frc.team2974.robot.RobotMap.intakeMotorLeft; -import static org.usfirst.frc.team2974.robot.RobotMap.intakeMotorRight; -import static org.usfirst.frc.team2974.robot.Config.IntakeOutput.*; - /** * The Intake/Outtake for the power cubes. */ public class IntakeOutput extends Subsystem { - private Timer timer; - - public IntakeOutput() { - intakeMotorRight.setInverted(true); - timer = new Timer(); - timer.start(); - } - - @Override - protected void initDefaultCommand() { - setDefaultCommand(new IntakeCommand()); - } - - /** - * Sets the motor powers. - * @param left - * @param right - */ - private void setMotorPowers(double left, double right) { - intakeMotorLeft.set(ControlMode.PercentOutput, left); - intakeMotorRight.set(ControlMode.PercentOutput, right); - } - - public void highIntake() { - setMotorPowers(MAX_POWER, MAX_POWER); - } - - public void highOutput() { - setMotorPowers(-MAX_POWER, -MAX_POWER); - } - - public void lowIntake(){ - setMotorPowers(LOW_POWER, LOW_POWER); - } - - public void hold(){ - setMotorPowers(HOLD_POWER, HOLD_POWER); - } - - public void off(){ - setMotorPowers(0, 0); - } - - public boolean timeElapsed(double time){ - return timer.hasPeriodPassed(time); - } - - public boolean timeElapsed(){ - return timer.hasPeriodPassed(2); - } - - public void resetTime(){ - timer.reset(); - } - + private final Timer timer; + + public IntakeOutput() { + intakeMotorRight.setInverted(true); + timer = new Timer(); + timer.start(); + } + + @Override + protected void initDefaultCommand() { + setDefaultCommand(new IntakeCommand()); + } + + /** + * Sets the motor powers. + */ + private void setMotorPowers(double left, double right) { + intakeMotorLeft.set(ControlMode.PercentOutput, left); + intakeMotorRight.set(ControlMode.PercentOutput, right); + } + + public void highIntake() { + setMotorPowers(MAX_POWER, MAX_POWER); + } + + public void highOutput() { + setMotorPowers(-MAX_POWER, -MAX_POWER); + } + + public void lowIntake() { + setMotorPowers(LOW_POWER, LOW_POWER); + } + + public void hold() { + setMotorPowers(HOLD_POWER, HOLD_POWER); + } + + public void off() { + setMotorPowers(0, 0); + } + + public boolean timeElapsed(double time) { + return timer.hasPeriodPassed(time); + } + + public boolean timeElapsed() { + return timer.hasPeriodPassed(2); + } + + public void resetTime() { + timer.reset(); + } + } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonLoader.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonLoader.java index d3b7d7a..c172ffb 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonLoader.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonLoader.java @@ -1,82 +1,97 @@ package org.usfirst.frc.team2974.robot.util; -import edu.wpi.first.wpilibj.command.CommandGroup; - -import org.usfirst.frc.team2974.robot.command.auton.*; - import static org.usfirst.frc.team2974.robot.Config.Path.CROSS_BASELINE_Y; -import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.*; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.CROSS_BASELINE_CENTER; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.CROSS_BASELINE_LEFT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.CROSS_BASELINE_RIGHT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_CENTER_SWITCH_LEFT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_CENTER_SWITCH_RIGHT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_LEFT_SCALE_LEFT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_LEFT_SWITCH_LEFT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_RIGHT_SCALE_RIGHT; +import static org.usfirst.frc.team2974.robot.command.auton.GamePosition.DRIVE_STATION_RIGHT_SWITCH_RIGHT; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc.team2974.robot.command.auton.CrossBaseline; +import org.usfirst.frc.team2974.robot.command.auton.DoNothingCommand; +import org.usfirst.frc.team2974.robot.command.auton.DriveSwitchScale; +import org.usfirst.frc.team2974.robot.command.auton.DriveToScale; +import org.usfirst.frc.team2974.robot.command.auton.DriveToSwitch; +import org.usfirst.frc.team2974.robot.command.auton.GamePosition; /** * Used solely to get the autonomous commands from GamePosition. */ public class AutonLoader { - public static CommandGroup getAutonCommands(GamePosition gamePosition) { - switch (gamePosition) { - case DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_RIGHT: // dont do this - return getAutonCommands(CROSS_BASELINE_LEFT); - case DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_LEFT: // scale only - return getAutonCommands(DRIVE_STATION_LEFT_SCALE_LEFT); - case DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_RIGHT: // switch only - return getAutonCommands(DRIVE_STATION_LEFT_SWITCH_LEFT); - case DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_LEFT: - return new DriveSwitchScale().left(); - case DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_RIGHT: // never do scale from center - return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_RIGHT); - case DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_LEFT: // never do scale from center - return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_RIGHT); - case DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_RIGHT: // never do scale from center - return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_LEFT); - case DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_LEFT: // never do scale from center - return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_LEFT); - case DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_RIGHT: - return new DriveSwitchScale().right(); - case DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_LEFT: // switch only - return getAutonCommands(DRIVE_STATION_RIGHT_SWITCH_RIGHT); - case DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_RIGHT: // scale only - return getAutonCommands(DRIVE_STATION_RIGHT_SCALE_RIGHT); - case DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_LEFT: // dont do this - return getAutonCommands(CROSS_BASELINE_RIGHT); + private AutonLoader() { + } + + public static CommandGroup getAutonCommands(GamePosition gamePosition) { + switch (gamePosition) { + case DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_RIGHT: // don't do this + return getAutonCommands(CROSS_BASELINE_LEFT); + case DRIVE_STATION_LEFT_SWITCH_RIGHT_SCALE_LEFT: // scale only + return getAutonCommands(DRIVE_STATION_LEFT_SCALE_LEFT); + case DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_RIGHT: // switch only + return getAutonCommands(DRIVE_STATION_LEFT_SWITCH_LEFT); + case DRIVE_STATION_LEFT_SWITCH_LEFT_SCALE_LEFT: + return new DriveSwitchScale().left(); + case DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_RIGHT: // never do scale from center + return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_RIGHT); + case DRIVE_STATION_CENTER_SWITCH_RIGHT_SCALE_LEFT: // never do scale from center + return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_RIGHT); + case DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_RIGHT: // never do scale from center + return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_LEFT); + case DRIVE_STATION_CENTER_SWITCH_LEFT_SCALE_LEFT: // never do scale from center + return getAutonCommands(DRIVE_STATION_CENTER_SWITCH_LEFT); + case DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_RIGHT: + return new DriveSwitchScale().right(); + case DRIVE_STATION_RIGHT_SWITCH_RIGHT_SCALE_LEFT: // switch only + return getAutonCommands(DRIVE_STATION_RIGHT_SWITCH_RIGHT); + case DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_RIGHT: // scale only + return getAutonCommands(DRIVE_STATION_RIGHT_SCALE_RIGHT); + case DRIVE_STATION_RIGHT_SWITCH_LEFT_SCALE_LEFT: // don't do this + return getAutonCommands(CROSS_BASELINE_RIGHT); - // SWITCH ONLY - case DRIVE_STATION_LEFT_SWITCH_RIGHT: // dont do this - return getAutonCommands(CROSS_BASELINE_LEFT); - case DRIVE_STATION_LEFT_SWITCH_LEFT: - return new DriveToSwitch().left(); - case DRIVE_STATION_CENTER_SWITCH_RIGHT: - return new DriveToSwitch().center(); - case DRIVE_STATION_CENTER_SWITCH_LEFT: - return new DriveToSwitch().center(); - case DRIVE_STATION_RIGHT_SWITCH_RIGHT: - return new DriveToSwitch().right(); - case DRIVE_STATION_RIGHT_SWITCH_LEFT: // dont do this - return getAutonCommands(CROSS_BASELINE_RIGHT); + // SWITCH ONLY + case DRIVE_STATION_LEFT_SWITCH_RIGHT: // don't do this + return getAutonCommands(CROSS_BASELINE_LEFT); + case DRIVE_STATION_LEFT_SWITCH_LEFT: + return new DriveToSwitch().left(); + case DRIVE_STATION_CENTER_SWITCH_RIGHT: + return new DriveToSwitch().center(); + case DRIVE_STATION_CENTER_SWITCH_LEFT: + return new DriveToSwitch().center(); + case DRIVE_STATION_RIGHT_SWITCH_RIGHT: + return new DriveToSwitch().right(); + case DRIVE_STATION_RIGHT_SWITCH_LEFT: // don't do this + return getAutonCommands(CROSS_BASELINE_RIGHT); - // SCALE ONLY - case DRIVE_STATION_LEFT_SCALE_RIGHT: // dont do this - return getAutonCommands(CROSS_BASELINE_LEFT); - case DRIVE_STATION_LEFT_SCALE_LEFT: - return new DriveToScale().left(); - case DRIVE_STATION_CENTER_SCALE_RIGHT: // never directly do - return getAutonCommands(CROSS_BASELINE_CENTER); - case DRIVE_STATION_CENTER_SCALE_LEFT: // never directly do - return getAutonCommands(CROSS_BASELINE_CENTER); - case DRIVE_STATION_RIGHT_SCALE_RIGHT: - return new DriveToScale().right(); - case DRIVE_STATION_RIGHT_SCALE_LEFT: // dont do this - return getAutonCommands(CROSS_BASELINE_RIGHT); + // SCALE ONLY + case DRIVE_STATION_LEFT_SCALE_RIGHT: // don't do this + return getAutonCommands(CROSS_BASELINE_LEFT); + case DRIVE_STATION_LEFT_SCALE_LEFT: + return new DriveToScale().left(); + case DRIVE_STATION_CENTER_SCALE_RIGHT: // never directly do + return getAutonCommands(CROSS_BASELINE_CENTER); + case DRIVE_STATION_CENTER_SCALE_LEFT: // never directly do + return getAutonCommands(CROSS_BASELINE_CENTER); + case DRIVE_STATION_RIGHT_SCALE_RIGHT: + return new DriveToScale().right(); + case DRIVE_STATION_RIGHT_SCALE_LEFT: // don't do this + return getAutonCommands(CROSS_BASELINE_RIGHT); - // CROSSING BASELINE - case CROSS_BASELINE_RIGHT: - return new CrossBaseline().right(CROSS_BASELINE_Y); - case CROSS_BASELINE_CENTER: - return new CrossBaseline().center(); - case CROSS_BASELINE_LEFT: - return new CrossBaseline().left(CROSS_BASELINE_Y); - default: - case DO_NOTHING: - return new DoNothingCommand(); - } - } + // CROSSING BASELINE + case CROSS_BASELINE_RIGHT: + return new CrossBaseline().right(CROSS_BASELINE_Y); + case CROSS_BASELINE_CENTER: + return new CrossBaseline().center(); + case CROSS_BASELINE_LEFT: + return new CrossBaseline().left(CROSS_BASELINE_Y); + default: + case DO_NOTHING: + return new DoNothingCommand(); + } + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java new file mode 100644 index 0000000..0efcd1d --- /dev/null +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java @@ -0,0 +1,27 @@ +package org.usfirst.frc.team2974.robot.util; + +import edu.wpi.first.wpilibj.command.WaitCommand; +import org.usfirst.frc.team2974.robot.command.auton.AutonOption; +import org.usfirst.frc.team2974.robot.command.auton.DropCube; +import org.usfirst.frc.team2974.robot.command.auton.ElevatorTarget; +import org.usfirst.frc.team2974.robot.command.auton.SimpleSpline; +import org.waltonrobotics.controller.Pose; + +public final class AutonUtil { + + private AutonUtil() { + } + + public static T driveToSinglePoint(T auton, double elevatorHeight, + Pose... splinePoints) { + auton.addParallel(new ElevatorTarget(elevatorHeight)); + auton.addSequential(SimpleSpline.pathFromPosesWithAngle(false, splinePoints)); + auton.addSequential(new WaitCommand(0.5)); + auton.addSequential(new DropCube()); + + auton.setOptionSelected(true); + + return auton; + } + +} diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonMultiple.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonMultiple.java index bbdcf25..5267001 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonMultiple.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonMultiple.java @@ -4,34 +4,37 @@ import edu.wpi.first.wpilibj.buttons.Button; /** - * This button is used when you have multiple alternate buttons for the same purpose. - * TODO: ask soham/tim if only one other alternate is needed (I think this is good for subsequent years) + * This button is used when you have multiple alternate buttons for the same purpose. TODO: ask + * soham/tim if only one other alternate is needed (I think this is good for subsequent years) */ public class ButtonMultiple extends Button { - private final GenericHID joystick; - private final int[] buttonNumbers; + private final GenericHID joystick; + private final int[] buttonNumbers; - /** - * Creates a multi-button for triggering commands. - * @param joystick the GenericHID object that has the button - * @param buttonNumbers the button numbers - */ - public ButtonMultiple(GenericHID joystick, int... buttonNumbers) { - this.joystick = joystick; - this.buttonNumbers = buttonNumbers; - } + /** + * Creates a multi-button for triggering commands. + * + * @param joystick the GenericHID object that has the button + * @param buttonNumbers the button numbers + */ + public ButtonMultiple(GenericHID joystick, int... buttonNumbers) { + this.joystick = joystick; + this.buttonNumbers = buttonNumbers; + } - /** - * Gets the value of the joystick buttons. - * - * @return true if any of the buttons are pressed, false otherwise. - */ - @Override - public boolean get() { - for(int i = 0; i < buttonNumbers.length; i++) - if(joystick.getRawButton(buttonNumbers[i])) - return true; - return false; - } + /** + * Gets the value of the joystick buttons. + * + * @return true if any of the buttons are pressed, false otherwise. + */ + @Override + public boolean get() { + for (int buttonNumber : buttonNumbers) { + if (joystick.getRawButton(buttonNumber)) { + return true; + } + } + return false; + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonOnce.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonOnce.java index 4b18281..974d317 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonOnce.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ButtonOnce.java @@ -8,24 +8,24 @@ */ public class ButtonOnce extends JoystickButton { - private boolean down; + private boolean down; - public ButtonOnce(GenericHID hid, int channel) { - super(hid, channel); - down = false; - } + public ButtonOnce(GenericHID hid, int channel) { + super(hid, channel); + down = false; + } - @Override - public boolean get() { - if(super.get()) { - if(!down) { - down = true; - return true; - } - } else { - down = false; - } + @Override + public boolean get() { + if (super.get()) { + if (!down) { + down = true; + return true; + } + } else { + down = false; + } - return false; - } + return false; + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ElevatorLogger.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ElevatorLogger.java index 6505463..d029738 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/ElevatorLogger.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/ElevatorLogger.java @@ -12,110 +12,109 @@ */ public class ElevatorLogger { - public static class ElevatorData { - private double time; - private double positionInches; - private double positionNU; - private double power; - - public ElevatorData(double time, double positionInches, double positionNU, double power) { - this.time = time; - this.positionInches = positionInches; - this.positionNU = positionNU; - this.power = power; - } - - public double getTime() { - return time; - } - - public double getPositionInches() { - return positionInches; - } - - public double getPositionNU() { - return positionNU; - } - - public double getPower() { - return power; - } - } - - private final LinkedList motionDataList; - private final String filePath; - - /** - * Call this in robotInit() before making the drivetrain - * - * @param filePath - * - Where do you want to save the logs? To save to the roboRIO, use - * base directory "/home/lvuser/". To save to a thumb drive, use - * winSCP or similar program to find the right filepath - */ - public ElevatorLogger(String filePath) { - motionDataList = new LinkedList<>(); - this.filePath = filePath; - } - - /** - * This is called in the MotionController to add MotionData to the - * motionDataList that MotionLogger has - * - * @param dataAdd - */ - public void addMotionData(ElevatorData dataAdd) { - motionDataList.add(dataAdd); - } - - /** - * Call this in autonomousInit() to clear the motionDataList - */ - public void initialize() { - motionDataList.clear(); - } - - /** - * Call this in disabledInit() to send the motionDataList to a .csv file. - */ - public void writeMotionDataCSV() { - if (motionDataList.isEmpty()) { - return; - } - - String fileName = "Elevator " + new SimpleDateFormat("yyyy-MM-dd hh-mm-ss").format(new Date()); - File file = new File(filePath + fileName + ".csv"); - - StringBuilder sb = new StringBuilder(); - sb.append("Time"); - sb.append(", "); - sb.append("Position inch"); - sb.append(", "); - sb.append("Position NU"); - sb.append(", "); - sb.append("Power"); - sb.append('\n'); - - for (ElevatorData data : motionDataList) { - sb.append(data.getTime()); - sb.append(", "); - sb.append(data.getPositionInches()); - sb.append(", "); - sb.append(data.getPositionNU()); - sb.append(", "); - sb.append(data.getPower()); - sb.append('\n'); - } - - try(PrintWriter pw = new PrintWriter(file)) { - System.out.println("File " + fileName + " has been made!"); - pw.write(sb.toString()); - pw.flush(); - } catch (FileNotFoundException e) { - System.out.println("There is no file at " + file); - e.printStackTrace(); - } - - motionDataList.clear(); - } + private final LinkedList motionDataList; + private final String filePath; + + /** + * Call this in robotInit() before making the drivetrain + * + * @param filePath - Where do you want to save the logs? To save to the roboRIO, use base + * directory "/home/lvuser/". To save to a thumb drive, use winSCP or similar program to find + * the right filepath + */ + public ElevatorLogger(String filePath) { + motionDataList = new LinkedList<>(); + this.filePath = filePath; + } + + /** + * This is called in the MotionController to add MotionData to the motionDataList that + * MotionLogger has + */ + public void addMotionData(ElevatorData dataAdd) { + motionDataList.add(dataAdd); + } + + /** + * Call this in autonomousInit() to clear the motionDataList + */ + public void initialize() { + motionDataList.clear(); + } + + /** + * Call this in disabledInit() to send the motionDataList to a .csv file. + */ + public void writeMotionDataCSV() { + if (motionDataList.isEmpty()) { + return; + } + + String fileName = + "Elevator " + new SimpleDateFormat("yyyy-MM-dd hh-mm-ss").format(new Date()); + File file = new File(filePath + fileName + ".csv"); + + StringBuilder sb = new StringBuilder(); + sb.append("Time"); + sb.append(", "); + sb.append("Position inch"); + sb.append(", "); + sb.append("Position NU"); + sb.append(", "); + sb.append("Power"); + sb.append('\n'); + + for (ElevatorData data : motionDataList) { + sb.append(data.getTime()); + sb.append(", "); + sb.append(data.getPositionInches()); + sb.append(", "); + sb.append(data.getPositionNU()); + sb.append(", "); + sb.append(data.getPower()); + sb.append('\n'); + } + + try (PrintWriter pw = new PrintWriter(file)) { + System.out.println("File " + fileName + " has been made!"); + pw.write(sb.toString()); + pw.flush(); + } catch (FileNotFoundException e) { + System.out.println("There is no file at " + file); + e.printStackTrace(); + } + + motionDataList.clear(); + } + + public static class ElevatorData { + + private final double time; + private final double positionInches; + private final double positionNU; + private final double power; + + public ElevatorData(double time, double positionInches, double positionNU, double power) { + this.time = time; + this.positionInches = positionInches; + this.positionNU = positionNU; + this.power = power; + } + + public double getTime() { + return time; + } + + public double getPositionInches() { + return positionInches; + } + + public double getPositionNU() { + return positionNU; + } + + public double getPower() { + return power; + } + } } diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/POVButton.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/POVButton.java index 2322cce..8881f50 100644 --- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/POVButton.java +++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/POVButton.java @@ -8,16 +8,16 @@ */ public class POVButton extends Button { - private final Gamepad gamepad; - private final int angle; + private final Gamepad gamepad; + private final int angle; - public POVButton(Gamepad gamepad, int angle) { - this.gamepad = gamepad; - this.angle = angle; - } + public POVButton(Gamepad gamepad, int angle) { + this.gamepad = gamepad; + this.angle = angle; + } - @Override - public boolean get() { - return gamepad.getPOV() == angle; - } + @Override + public boolean get() { + return gamepad.getPOV() == angle; + } }