From 283392bd8703a9535d481f58023daa598a0a7bba Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Thu, 10 Jan 2019 19:08:09 -0500 Subject: [PATCH 01/26] Fix valid contour recognition --- src/main/java/frc/robot/subsystems/Limelight.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 9e58d10..a17fe81 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -72,7 +72,7 @@ public static boolean setCamMode(CamMode camMode){ public static Contour getBestContour() { //Check if a valid contour is found - if(limelightTable.getEntry("tv").getNumber(0).equals(1)) { + if(limelightTable.getEntry("tv").getNumber(0).equals(1.0)) { return new Contour(limelightTable.getEntry("tx").getDouble(0), limelightTable.getEntry("ty").getDouble(0), limelightTable.getEntry("ta").getDouble(0), From 29956d5f98e8de5adf2080dcebede11efec710b4 Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Thu, 10 Jan 2019 19:16:02 -0500 Subject: [PATCH 02/26] Fixed inversion on drivetrain --- src/main/java/frc/robot/subsystems/Drivetrain.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index df966cc..b966288 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -19,6 +19,9 @@ public class Drivetrain extends Subsystem { public Drivetrain(){ leftFollower.follow(leftLeader); rightFollower.follow(rightLeader); + + rightLeader.setInverted(true); + rightFollower.setInverted(false); } @Override From 8235b8a0c367308c26b03073577140a0ad2498c9 Mon Sep 17 00:00:00 2001 From: Cole French <16979554+ColeFrench@users.noreply.github.com> Date: Fri, 11 Jan 2019 20:31:44 -0500 Subject: [PATCH 03/26] Replace calls to the old Limelight API with those to the new --- src/main/java/frc/robot/Robot.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d5de1dc..2c5c439 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,9 @@ package frc.robot; +import com._2train395.limelight.api.Limelight; +import com._2train395.limelight.api.Target; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; @@ -15,7 +18,6 @@ import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.ExampleSubsystem; -import frc.robot.subsystems.Limelight; /** * The VM is configured to automatically run this class, and to call the @@ -123,9 +125,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { // Scheduler.getInstance().run(); - Limelight.Contour bestContour = Limelight.getBestContour(); - if(bestContour != null) { - double xOffset = bestContour.tx; + if (Limelight.hasTarget()) { + final Target target = Limelight.getTarget(); + double xOffset = target.getXOffset(); double p = 0.25/27; drivetrain.tankDrive(-p*xOffset, p*xOffset); } else { From 3fb2864f1db1df10c2fcda7c4d83bd90e45c4861 Mon Sep 17 00:00:00 2001 From: Cole French <16979554+ColeFrench@users.noreply.github.com> Date: Sun, 13 Jan 2019 20:44:54 -0500 Subject: [PATCH 04/26] Add ApproachTarget command Adds an untested method to face the target from a specified distance. --- .../frc/robot/commands/ApproachTarget.java | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 src/main/java/frc/robot/commands/ApproachTarget.java diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java new file mode 100644 index 0000000..c538c51 --- /dev/null +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -0,0 +1,59 @@ +package frc.robot.commands; + +import com._2train395.limelight.api.Limelight; +import com._2train395.limelight.api.Target; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.utils.Targets; +import frc.robot.utils.Targets.TargetType; + +public class ApproachTarget extends CommandGroup { + /** + * The distance, in inches, to arrive at from the target. + */ + private static final double DESIRED_DISTANCE_FROM_TARGET = 24.0; + /** + * The height in inches of the center of the camera from the ground. + */ + private static final double CAMERA_HEIGHT = 0.0; // TODO: Measure this value + /** + * The angle at which the camera is mounted, in degrees, from the horizontal. + */ + private static final double CAMERA_ANGLE = 0.0; // TODO: Measure this value + + public ApproachTarget(final TargetType targetType, final ApproachingDirection approachingDirection) { + final Target target = Limelight.getTarget(); + final double distance = Targets.getDistance(target, targetType, CAMERA_HEIGHT, CAMERA_ANGLE); + final double xOffset = Math.toRadians(target.getXOffset()); + + // The skew from Limelight is the same sign no matter which way the target + // appears tilted. Therefore, we need to adjust the sign manually depending on + // from where the robot is approaching. + double skew = 0.0; + switch (approachingDirection) { + case LEFT: + skew = Math.toRadians(-target.getSkew()); + case RIGHT: + skew = Math.toRadians(target.getSkew()); + } + + final double distanceToPos = Math + .sqrt((distance * distance) + (DESIRED_DISTANCE_FROM_TARGET * DESIRED_DISTANCE_FROM_TARGET) + - (2.0 * distance * DESIRED_DISTANCE_FROM_TARGET * Math.cos(skew))); // Law of Cosines + + final double angleBetweenPosAndTarget = DESIRED_DISTANCE_FROM_TARGET * skew / distanceToPos; // Law of Sines + final double angleToPos = xOffset - angleBetweenPosAndTarget; + final double angleFromPosToTarget = angleBetweenPosAndTarget + skew; + + addSequential(new TurnDegrees(Math.toDegrees(angleToPos))); + addSequential(new DriveFeet(distanceToPos / 12.0)); + addSequential(new TurnDegrees(Math.toDegrees(angleFromPosToTarget))); + } + + /** + * Represents where, relative to the target, the robot is approaching from. + */ + public enum ApproachingDirection { + LEFT, RIGHT + } +} From 6b403e735fc5f1517e0bf3586c6fad751f7a758f Mon Sep 17 00:00:00 2001 From: HPrickettMorgan Date: Thu, 31 Jan 2019 23:53:52 -0500 Subject: [PATCH 05/26] Add camera parameters --- libs/limelight-api-0.1.0.jar | Bin 0 -> 13090 bytes .../java/frc/robot/commands/ApproachTarget.java | 4 ++-- 2 files changed, 2 insertions(+), 2 deletions(-) create mode 100644 libs/limelight-api-0.1.0.jar diff --git a/libs/limelight-api-0.1.0.jar b/libs/limelight-api-0.1.0.jar new file mode 100644 index 0000000000000000000000000000000000000000..d5488634e2d7b788c221e43ce0953400e0b7a2c5 GIT binary patch literal 13090 zcmb7rb9|iLx_0a|XxP{`+Ss<)*tV_4wrw@G8{2Gb+sT)Hd!OC+%|2)MoHf6hdHz`U zwbnc{*Sd9$v=}f56aWAO1c0kHl_bF10Q&XvHXz@g5(4r(R3egsG{6AT|C+cNNFiC} z33P(-HUa(ZiS*Zr5vb56-Yug*!K<~ib z?EkeY$lJB9rTM=t_}4jb|D2;qZ)c-zY{9_7^dG1H+T$ZHZ-#Po8kLA zfc2jYwXKZ*&A5sLU>X1z06-NS0D$^W#=MsH7J9}OhP;+e)VgNcwzhHdrgDffXda4< z^-g#niE3-)aqWl13>F0de=KVWq8S`OS8qU>HY`zsm%YeOi7QWi^Wf$x-sgZuSpWVV zF7>h{Vb^)g^=A9&?*5p~OJ5HNV%-B^?yN&<2wzAHlghTHYl1Iak;Mh6;}f>)_mgfx z0I4%ZFGHu3?tC~kPQn&pQM7@)>c@L|YR%zeVmSe(xP^g&5Z59lMBan%4#i{&Ba{>@ z1{em;UwLV@lbG_8N;4G>oBS%1M~)!&5fFd6r;TiVo(buU;x*xFPl5O6tqs;^XzjJ> z%_=`INKzmawwvxO){k#~;)^wPs}#>yU|Y$OTQLyaw=6L1riHzc=e1;1j9e2{{x;3s zZj$Jd?_ra<8E61Et2AS`87tq0e z160(V{`{aoEF~5RSLVK&{oFzo&~DOt;%o7t#>4$+0{p#{LZ4ZJZ5VrHLUXM7TZ9MZ zd&B+q6lh{Oacu>=Lux|5ww&om1q3G`E{8^dNm=!Jv)uuV;Tq4RFknmcMknSb5b#HY!6B&k!5d-sA9#!3SbbZOa%kwr#&yl@ z#wJ8g2JXV+1Rdp^UF=I+k>W~5*I6c(=cU2ZVtoOXO^r_nbGbh;=@@a80@i4I{B-7N zBPu@m$@|+ZXTu&hJ(f-&RD?`Wv)vt8aKD7iG9KLSxLX}mc^wp3J3f5$7{U>o&}v^@ zWgp}R$3T!J{A!)37cqW(-l#%ly*0!r$Sh%TN_wZD{#b55joP|E-UarZa8E(Y&(8V2 zM;0-skOL*V^ICKKuN^zL4ThxzlfNcfdr`d#-&Rz0062# zafwgcT;E1p!ctHFHx!K_ak7w}_@r1!{E82xVMnhxR zKnL=xR6Y(ZzwEuNj2iZ}dbdq4F$v=md2*eC@-5?LT;7k+?EO#Bc?!0{Sw!g)JBSP0 z^UJ*WY<;-HB=Km|(*fs;<7VAtMoqh_lHPo}LsS1}=z+&{mN;bAd z1(0cY582@dVHGS2N!qlSMI=(8QTj$=D&a}xNJ`?jLEm+z!nZye%%Yw6y&h+!`^w1W zwP^3c%xw%axlG_e45Zg#4y9;+aTkx$qmL%Yt+$ZTlxR}0f~nZJ*s8>VEKSnzNYlnu zkH9=v$P*XgO_T#ExO9-{oW=u$ERap*q}zr0KIl@ku`}|BVp)ES`(;=J`Ra`2Sm6u+ z4!t}rQke%Q8tUO(xKkxn&I%iqaF{|^+nbuy5{X5WSCt1W1QDB}pI7M&M9O)V2{dg_ z7M>TC?{H#g@9e^10Iw{~&I$bc@L)jFZDB@G_bozon2qy61KF2BQRl49lc)^4SI-p5 z=kq;LsC*?1#N_SRM%9U*@H!(lL+Lw-KU1O~CpXWl;>%q)lTE%!1(%_Sd`W){$8$Jx z%7`5#$y(&ICmR0}_jrs(W1G z6gD#J0uXR<>_Q}`Rgm%A7_Ud*;-UW{qXo7uBmf7Vc|f;ei@;eN*A0hy?rgJXz)@cz z^aSq=cq3nPm-Yp!OcdF_B-g*sc*kfP1`ZO>hTkr8m*G=RYX+|Qv2Bekk`Xbr|13%d z1wY*=+RfvpWeWiy0vAOvE*P!y6zX*&pgmkC2V>C%b+VdcmK)l0bA^LT0Im8^3K6H` zNC~k37q3fuNW3BVqMoJ@39E1)PlSnt68s|P5c`%Y5Y}~fmZ)vI6`OMmeR%>Mwfl_>+ynR`wB<%B#!6H1DjXM9Wf+iz5H^o zPpB9@ibCkR9CdudE<;>TRWMsa32uQ7XnZg`=k7+jkL~i;wGJ92ajVv1t?0vSAF}e( zAyT?js@-~zGJIonx9mJCC*BaT&~U#25UEtd@O?I(m74B&)M+vg(m9yL ztdD-keWwjD)-?0h?Ydf^V9rf81r5dvFgTni_31qfqf-X`Xi06pxtPWQ>)EIcd6K$I z=H_3PIMe!l%36l3mhK#{lRWOPb*gvnkPEi07X#IxQo)LPvQeGZ5xeKf(4qXr{Ug`Y zg|LXsE3m(47yR&rT@N~K(l(Fiqk0yvH(r%)$#Nb<1bnq zku2HS;rJPk;#_Xc3X6KbqcJJn`>ETGEA|tNThThyER%Z2+Qn z>~%ow!9o)!zrIgxU32UQ#R0ZxbF`g52vOzG429E zVBUJEQ#nupl4uO`_})sW2w&G`*q_$4@GJ|7IOu-u2|7+M2S_m@c`8AxNKaum&FOq) zbkFLZ*Sn`iN)vIJ1(ZvI&w;*ARzBd0ed@E$;ezc2l~(bAmcA1;auiYcf&;$?Mg>*m zGGh^Rsjow=MN;S|1MH6MEmj-xf22sOVa{LA=(A%bY-D z)G7iVQAm$Qs#Or*^#!$`298B`3p5~*!k-5Qx}5R;%O=wVj^_r}53S(Bh|D!sgPeyd zAB=IhOibLOl1DW0W4=e@n+*5;t9G8}tBv@ei0cg(o`np&*M^8JaG~0w>~O}>al&wy z>6|8{VXUfWuZF^nb<4hp$5LH}z>e7w?{r+{H)R00Qu z1K#?Qb85k3lkAY1XQ-oXZ5rb!k?f`5si%+f38Bsh`nXML5$8eUm!9wVtrA8bS@0H) z*L?s0p!_4h|0|&WhyTq}Jd|A$MB@PRHN&IE6A=+vj702E7x0QDBqW?BCJe(Pb1+WJ z&+CGqtkhSE&f2GHUDe2BaQA-M7oXztGd6eta4(c>|2iZXpCp`q&{ytYKk68MGv!g@ z`SN^d3BYit(xsA1#NOkIkL`cX+5F7i#mcD{My?HvK0Cfb%=q?#Qm4Y_sIpfoBXA_V z$E(^??q{*ZzeSYlgO`QwPaBNL=|k4(gC!`^lkT{^XE*p>8x#EEq#z$|6$ znD2;9S!J;o7|BVppKU&Uq|lpQPBu9(I}#5#Dz$(NB&PRosp8a6DYg*TY%SfoYKfn! zudd5Ru_uy2k7_-uGho~LzGua|L`n7tvM?zzqzE(elP0m5+0}xKVjDr|qZ~f*dO~A6 zskLUQiXeUZ_6F>uioKB;S&zCu6PkQU8hh__i$SseMq$PX0j;iD?GYVLH8uSug}(Z- zpxsjcHE^qN1Z*2W=Q30*9o1*so<7UC{^fuWg`d(|9CdA1d8Q_uYY*7oY1j7E&$Dv^ zJBOx?4Pko@^m75KHC3R^!`Qdi$3E2%@t?c=G8`QNb~Nft?s8!Fh3?9kit^0vooRrB zh~(;vPm}^pIpa&lr#JDPKR_5bGp&dZni8;XUyZ8|Qz|Pe&PV`8Jf$g40FEf06VB_q z+yTXcfszra%oLYW#+`htfe}#~IifBRC}_LzDOya9avsLBP8LNGH4tnrtKP+$X=3jj zMLm?G$B2FXD_s_(6it8F!8@sP8t~<=zRFz|XGzTpCED;>p*eR2l)f z2X{txpHxH~+uAi(^BK5;h`_De3=Z0;>XzS)Vw25{W&<3ZYlDP2DaQ>9tNQk{Lk$di zGK`DW%Jw(wUJ)VnC0Q|+jWI^68wFnJ$=#f5ArHQ9dp~f&lf2@oMn?7a-Du(|4+4PF ziGaX>6l~f9Pw<~N^kz{6^1egyDOeY;r%C1-jf2v>o=|?O6Tu+AX}9U~l?Lj=z>lSH zY>_5^Efd)6wtm=1*{?^QzS+Rk@b*$Np7w(e8*A?6U_7x6bG7zSEkG3NOM7m*GH?ddjya@rvk^qCbww@1ZC?jqF$T;<(fB&XcH{vl!vcTbV)d) zM8M=bq<&yOJS!%mQRKm^(K(}=yVxSjM~GY&)fB)&ZIAW&AWyGiniOx>`Vy_kp(2)7 zHik*IzOp0lCwHkvaW@5%ME@6*h^T`r(UIusxKBk(${VtdR&$W!yKVyOyL+Ns-H;W_ zm|3emt;OmL8DZ=~j>(+MF;XSb`galu>L8GmmmZ+Ehrp>ifRe7jJtjcx9cnF_0+Wzd zVLyYua(r_tYTgAfWc_evvrE&?3-cTxeR-O0B_@21YRT#z!p!iqH~FKXS7>EP+K+wv zji+^I=bOdTv%u3A@V^H%0Qw2{^lt}VFa!Vq#h(s5X=AIm3WCMI@(pu23P+V14qNB9THFB6moq#yN89> zHBD>wT?1ntbP@yN-G$wB+jQHLM#tNTc8`0k4%j0hyv_0eOil&b3fziy^4a-i-7&*Y zN%pWPP~^;vNY(t-iC2&zmt41 zM4iU3uB(!Y<`vOze&|r4P|~vagvovBGQ^>FTRQmAoMlxwvMQHdu_bLuSPgMlXytR; z7_S5*B*Rbl_|#%cnd@qn?0ZG?DPyFg5%MxpUW+h;2CbLoSEo5poko|1a+KqMp(gK! zqXj0P{_MlXmuSC`g4lk8O@ZJmNA*RyX&Z|48X2OXovuh#LmCBmJ&r#Cd+= zDAK4yGnp?Vy4zD4C3W+88;HgximfxGZ0HG}XI;0F^?A~MLc1XE*voV4_cbHVSVNbG z6CE8ScH3i}uDgRu*~!b~j|5K$P!_;Q?i zPfPq_%iKsH2weEZ!zv`d=(Ax-oSP`Zl10uw2}hr&swMmR!m9WVn>fFmdG*q~WK_-W z=eD>=*6SNb3L^f(o8ec{%tuMs(rLA{gdgfbQ;wvI?2T7e_n+K%$|w8k9q$owSh&9+ ztm%0fc%1`$1e65*g0&{@RUL}Pk9f-4#pH709e}pV-+{IPEn$klf3GUr1KuMDd0$YA zhn2aD%sUo1p+o<%LRJ5KDNGRwLaPd?th~w;inL87~(12S5*54OAq~rP2!v?ip*Z+VNM z>i}^LKauF*#o!J;?wRM$AzZGbbYY1hV}IeD%|FDuws~t(PSfb4izS zPwYn;0h2L^_Qa2jc3$cq54>3kUmk%1w08AWDigJKyR|zXK%3!%AeeFI1q2Yxq$|aW z$>$@rEYT5iKC)5X1!ke8?G5>~n->a_yacbEqR9}D-xX#(3sPh;!;}!#BUFp7Ge4Bc z=7s^5a^_MWdEhSy=EX`V zS|}w;t4QP7Ez+T0@(Hxkyxnu-a_oCE_57$>U;}C_p2YTYiP^6VXI$t|lJyo$IcAPfS2q;2uY0F#Z)ou2!nJWl?;Ayzhyix+m9pm$9J zCWvOQd(pV477~n-u&Y=)VD!f#ORb^>Rv@xTumcED^kTky-g>~$3C;lcV{H(GEGdE- z9`ziz&6l(YpAtJD&6oxBv`NZ38+}sL81Xhf33q`^Z!6%ab+8(#T^6KOf&<+HiBl8< z?$L`pVc!H?q9$Cr<&F^)L=v)7Uc?<+p|d;xwQ=XOX%<+?pT}!jhC2i0o%Ky%6ltAu zUwsSQE0#R}QojFHsq0K>VuXAv1Q@(k>KOhnKVErftAF`VQ@Q_NugjN!0HnL{xZ|F# zgIF)Z57Z(Hmbap$&&ElW)`5)47f;52f_R070~HebHugf=w=%D7QZ~wn<8a#Q>M+L9 zEaLI>bb;0JC4G*s%kM4ar^r$2L;^2BX4m2(*vtwB8?H*yy$qD#C3WO?KXG(=?ixFRMV)4ZeA6#GQCnRCErzVFYLsa^pp}675 zm07taev~?c5H-Dda~&7KxQ4%pHC>&cu;&nQnD)|2%?(ITHn;oYXj8uCRf9Am_khZf zCvr^<(WjDGE3i^<0DacYSYfeBE3LJ#;Y^2 z#gcVM;T|ms!cPxp^4$`Ykkk4%jCR=}N{^qKoM$x1?qwz%n5iP<1Q~PBLPx3skYUaj zs9?>F=El7pyz{xP<*FGZf7X(_HdcK%*LOo?li@7&(?x-jo6(5nDv{MttHrwn!QUAT z-?osID{ixX9T!A)HF|i3r0{sToj8JUpD(+bf*5nCvhs2Rcb>0A3wwYYge*e}p)& zMpweMigR$^x|fN-m6Uc27zaECDF+n}_lH>hc1ZN}WoA=OAz>o}fz#Ix-YRzUaloWq zra`6&SR#N@>5dyB$c>(J+%{NPJ%NiS=d+*)f3P_lYL=nX+Cc`GMaOr!dOXClVE;jMFPlEh~Q`? zHe})YP`{9DZe{9fewFH`prj@Zw%NXc?9$+*gb;hiY!%rE<9vm>OWD^P+;Jlm6YhnH zva6G3lNE_|;Zm3THW&2D6{2PWaFV0CMPZ%G({l-bf&`3A@yhtLG*!hQOcD|TlAP3j z)RCGt6RYa1qWj$$8bbs4LV-B3*vIcq368iE9&4`^ZZ2SO2khEVC7{(cEi zv+xOLqMfYFge{6^U@1_DffT|Q=P1_R6TPZA16DjHibML-VPzZ~$KuS_T>efoAB}bp zSUe9vM&wtii;05g>4tsscI--GKi^V$Xh_dtjfLk231Vb$mA&rY&0XnwQxDp!x^~fr zsRuB2hwV5*X%46m2T^@{oiS-aR`auw(g^0p5C!eI5ZHBx=yS^OEpXL$BvVo>**SBF z^2NBi-pj2BkD#PN%=(}~uymtnrupM$NilyFpqzK@Qx*YZCm-{tXD|dTn(pqxA{ZJq zO>~bsS@aPzVEukg(nVh$R&cxp7mXTR0MKIy*>d=q>0pROT7Ic!EPM3=wfVpjBs(z- z%_8k66L|G?#~S|jE?b~&kj`;P-VBq*CkOt5vZ5~&$P#evbi3rW={eJz#f&!IvP&$Y zz~!PjsHC+SIgDG`gtP%x0wP31V{Vj}jp6KW0Y@cK1hw??!R67d*GYZq12Rfg?dj|% zqS%f^pXXu}lrH=zRLV)l8lp*#3yZk^&KT%u0?N2IU)2A`7~21ZFZu^#{_2R-Y^N0A zUy#J=D3M4CBQY9d;S0+E$RlGX8;sv#%O7D&{kdq1vTucH8I{p_dculh9G`JQ7;*IU zd+2AHw39*NoCcF`d)n>QhPO+$H`$jbr%ILp;_C^7{KI64ea9fwM3tFqm?$cg1Ih-Q zbMz;)J9=*2PD+C5=Sv`pNLQI18*C`6)yi>nrI8DI8QGySy{`1VXmf~nJYF@aYD^(O z)Vo8q#pioXn)R5cy=BJT_QiFF@^xa*hsG1dN@nd1&E{hefhl6p{&4~usHaRSKq~=vKcsN7!y$`Di|>pXv>3tlTTCSjG4k5+h8?9$ zhx)Dy>rb`1Hzk;=KLoI!#K=!_2)SR+&d7<+x&6Jr0X02gA>(YaS2!qy)+-`C@tV#^ z({+}-j2EqUqBGAOIF^UT31jtt1;d&bC#) z%u_$czS=eXfG(dg8s;CFJumDz4EY3mBD}o#v#u5PK_CCy6&GS|U^gSft8V+@Gn1mq0LjZj<7cJZ9oXmAt&J@@yt^rx%A<>0Goc| z--I$Aunah9g+#p)QrABhkYb;bCl|K+U7>7LuZ32=B1fAHK`SjeX4XY*L3l=e6TWNa zWN`~@C?~qXJ5;s72`*1t{8ntb@+jOYD76pY`rylji?eps4_E{2#$S%-i^xoh=lhAd z5RZ@COqheKsYy}^w22HfsXX0J@Bu(lCP=PfAcDnZeT~tB0J>mKh+PzL&S&!SfYm}; zM~HLS4{Kn-bb$@jQ%jM4l8~SEF}tbSmUp7q=&2m_ zVRQ+DWzcmSNOUnjx;5emfWsBA(0&w1T`Khx2{0lqkXjxyWPoYCk5-$A2Lm3fOg(fA zU|cFrtW(f{X4GAowChv8omDejwx8YZe&x)$NN5&w5(GVKA>bx8~O}w+$b|j zIq?|%FUj?owsop1%&nlh3;9@Nxs;{!%VJMULRDG#CO@gr9VX5wN59hGvI_c(Uj}X~ zccOI%0S|834B+GWGKIK=>?8n2@Z#x+KUdl zYo*aup$0S+2*&JRcmR)f3 z_pS@uY9whnK35uIbBs1Fw{h+x!@QO|9@Ig&mfd#u-B+h+#@z zkq3{Sr)M>(zKRK@12p$X{LO;9x2Evs@&=*gc(+yX&lFs~?TdX{Br(XRfCG`8gg<;qK188hWHyR>$oJV zgRA^uQ}(#S{0=;%g8DO*wTwBk9~I_vVBNSW>W^_bg4KRx=J9-*U<~Q&2rV23#@SIG zLX0fekA*g6OC=2`i=C@~_rHW-%n}xFVq0?Y-E;Pz}ORMz}JP5 zoDtgZhHgr(j$@66x8+yoQ_Z&kBV&!vL++sd@%FvD8#0Nvx>4VmT%A2xiQtg}~gI2`}HtTaH2Lml#SE#2j zbJTA;S=e&WACr_D3wZ0+qb!z;!gR(DibbGYY!`O`aH-~X;ZIG2=CTo~h7IaI7MZ)$ zt?K0-euiX{HhR?7dz2ZuVGZZ*x>ict1et260Kw6bk>Z&Qa}J`T%8jHECwSvS&wS@$7X;sR0T zmZNGl_)aQ;;)sc(Pk`;z_C|n%PeP)6-yHk*0=s1%fwd>VAp6uGKH-$btdeWF;Zt{l zo!Q|;%Xzc@*4aL;tdx`#4HY>7;H(}3X*?`g#-aOYyZS)7K)$ZfxnRsKby6-uh2Cu6 zf5i1k5@aXmJFbd!R(D$&w2RCSi8fq$I1a3E-u~#Kcy+O(k{~&Ng&MK_p>mu!Myfjk z+tq)Swx_~&$FbpLq}Z&HDq(2&A$f@-cT1=f+PkZbz(cw?(QmYE9@R@Q83bu5?c_0% z%wT^h2I|&=Beh*Tqfq7nfOwbWjuWtIlY^}@zD4&A3aD@Y9-a}6owDPME}c~n|3?fT z`f7AvyyqfV5Ag?gud8TwZw71PKEgq+5Zl(LlAeL)WW3$F$UA`hx9-x((`C)U3Z5<0K$Ejp!SotDq&bA3|kQGU>L2l(>p zJ_}O`A&r2#&<7~80d+y$zA59csSlQ4h4x;tsBf(#!i_Gin33ZTvwByZ%@rbyDWX++4w-Qugx(Xg)Nf7E-65hi#PJ#uLHMIbC z7n3m4>a+$p)s`&9@k8*`Q7lcccI}{P6*Ao+!n*lsKs|WNeR!oVRA)sIrw^FV0$o zO$&{&SedssocnF+?Fq{*)#vBu-98!s+j&GQXlS6t?|XULL0JVbR17s;;|u|c=q{t1 z{s4f`p?)BK2j_waNj$T-Lar(Rn_l z+j+6Zg{mG&d}{o2!TF_0DK=TTk;In~y&o3?{mE%-QI@LkJfnxVgM$aWLlD`@=HRmdY=*P!m1vAz<%vr(tRRT19-?X(bU#%Uk5G?@fnE!f72dncMROI)-r*-W%v5EY z6gLibBBQMmb_cczxn2^H{-h#35_VCfHX95Wbkd(RKPeITkfVeuJ|w6? zR}`NpH+=Nsa1A3*rN~ew+r83A%Spv?$y^6 zNchtlt*zJJ#)w|ZsRi}BH&R4RAl$rOOo3VC0=MOq^+eFX3NVpa)pv@Kj|zhB!V2Ux zBPUV|6e4}GQF39~WX8eHr5N9c)o4TynX9YzUx4ODYD0P$r!v0^0lj*Do})2Ml0F{5 zfGiv`0+wmj`)r5Hik(+rRKNw@5u&l&i)FQkHw%Ju-fX`t)lwhsh|sLm*G%S{>{ppl z8;A@}VO@y0zd#0cmD7vAO^OFh&D&vjhN&JIkWa5p*)(Ki@I4PET|f%`=b98ml5I^q zo=0tQ_{0~0bU~cuJ+HvgLCK~<-p_=rOMI%x$DbgymM3MTln!X*59-~8QdS)U!fzPt zJj#>ml|wk}iOrc}APqmh5)r$H6UU4ae@#e)VM!KeNwL$^q2JKzn)s?9K{#k&I?`!r z#S{!|zp<0-v?#N@OqyBzeJjDi!?UrrZ7CyvylLHo}J_LbI|1i zRay)X2nqOq7E`~m>TSRS9P|8o|ChA-uV(+9wEEZNUnAH17cc*1_7|b`dxT%J|4YUF zOHcie@oQm+x65Br=>KH#OI`hs@yjCl?ed*?`ac=}(pvvx{4ySRyL@N-TY3FgnEy_B z{Tl`d@IS!((qX@cdauL&=a$}XA=jUv{!Nwr9{as6^*46N+r9oDKkct=zE`Qfhkmc9 z`wdO`){_2@(0|j{y~lm8{{!;h8|d$W-#5g513MxA z0r=nBWADM=H!OaGGok$-z<*bb-Y@XJZu@(Ih_^pY{KqfyT^08|^85PMZ{$ww|1I+W zyzlp^^>6fC{6C=od!qe*>+e&8-{3sNe*%A(D7=S$AH4jAet0`G|Kt7sYbYZv2KrW0 T002OIdvv~aV#R)CFaZA#(l@eV literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index c538c51..5439251 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -15,11 +15,11 @@ public class ApproachTarget extends CommandGroup { /** * The height in inches of the center of the camera from the ground. */ - private static final double CAMERA_HEIGHT = 0.0; // TODO: Measure this value + private static final double CAMERA_HEIGHT = 16.125; /** * The angle at which the camera is mounted, in degrees, from the horizontal. */ - private static final double CAMERA_ANGLE = 0.0; // TODO: Measure this value + private static final double CAMERA_ANGLE = 60.0; public ApproachTarget(final TargetType targetType, final ApproachingDirection approachingDirection) { final Target target = Limelight.getTarget(); From 2e43104414b0dfa24ed81c180175a13ee5c54fa3 Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Sat, 2 Feb 2019 19:52:47 -0500 Subject: [PATCH 06/26] Add Back Limelight Class --- .../frc/robot/utils/limelight/Contour.java | 22 +++++++ .../frc/robot/utils/limelight/Corners.java | 41 ++++++++++++ .../frc/robot/utils/limelight/Limelight.java | 63 +++++++++++++++++++ 3 files changed, 126 insertions(+) create mode 100644 src/main/java/frc/robot/utils/limelight/Contour.java create mode 100644 src/main/java/frc/robot/utils/limelight/Corners.java create mode 100644 src/main/java/frc/robot/utils/limelight/Limelight.java diff --git a/src/main/java/frc/robot/utils/limelight/Contour.java b/src/main/java/frc/robot/utils/limelight/Contour.java new file mode 100644 index 0000000..ba4845d --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/Contour.java @@ -0,0 +1,22 @@ +package frc.robot.utils.limelight; + +public class Contour { + //Horizontal offset from -27 to 27 degrees + public final double tx; + //Vertical offset from -20.5 to 20.5 degrees + public final double ty; + //Area as a percent of total image + public final double ta; + //Skew of target from -90 to 0 degrees + public final double ts; + //Latency of the pipeline in ms + public final double tl; + + public Contour(double tx, double ty, double ta, double ts, double tl){ + this.tx = tx; + this.ty = ty; + this.ta = ta; + this.ts = ts; + this.tl = tl; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java new file mode 100644 index 0000000..2d7a347 --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -0,0 +1,41 @@ +package frc.robot.utils.limelight; + +import java.util.ArrayList; +import java.util.Collections; + +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; + +/** + * Represents the corners of a target contour + */ +public class Corners { + + MatOfPoint2f corners; + + public Corners(double[] xCorners, double[] yCorners) { + if(xCorners == null || yCorners == null) { + corners = null; + return; + } + + ArrayList temp = new ArrayList<>(); + + for(int i = 0; i < 4; i++) { + temp.add(i, new Point(xCorners[i], yCorners[i])); + } + + corners = new MatOfPoint2f(Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)), + Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)), + Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)), + Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y))); + } + + /** + * @return an array containing corners counterclockwise from the top left + */ + public MatOfPoint2f getCorners() { + return corners; + } + +} diff --git a/src/main/java/frc/robot/utils/limelight/Limelight.java b/src/main/java/frc/robot/utils/limelight/Limelight.java new file mode 100644 index 0000000..8220405 --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/Limelight.java @@ -0,0 +1,63 @@ +package frc.robot.utils.limelight; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * Add your docs here. + */ +public class Limelight { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + public static enum Pipeline{ + leftTarget(0), + rightTarget(1); + + public final int id; + + private Pipeline(int id){ + this.id = id; + } + + } + + public static enum CamMode{ + vision(0), + driver(1); + + public final int id; + + private CamMode(int id){ + this.id = id; + } + } + + private static NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + + public static boolean switchPipeline(Pipeline pipeline) { + return limelightTable.getEntry("pipeline").setNumber(pipeline.id); + } + + public static boolean setCamMode(CamMode camMode){ + return limelightTable.getEntry("camMode").setNumber(camMode.id); + } + + public static Contour getBestContour() { + //Check if a valid contour is found + if(limelightTable.getEntry("tv").getNumber(0).equals(1.0)) { + return new Contour(limelightTable.getEntry("tx").getDouble(0), + limelightTable.getEntry("ty").getDouble(0), + limelightTable.getEntry("ta").getDouble(0), + limelightTable.getEntry("ts").getDouble(0), + limelightTable.getEntry("tl").getDouble(0)); + } else { + return null; + } + } + + public static Corners getContourCorners() { + return new Corners(limelightTable.getEntry("tcornx").getDoubleArray((double[]) null), + limelightTable.getEntry("tcorny").getDoubleArray((double[]) null)); + } +} From 31bf46af467395c3189617b445c8a343aa90a75c Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Sat, 2 Feb 2019 19:53:03 -0500 Subject: [PATCH 07/26] Add VisionProcessor From 987 --- .../utils/limelight/VisionProcessor.java | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 src/main/java/frc/robot/utils/limelight/VisionProcessor.java diff --git a/src/main/java/frc/robot/utils/limelight/VisionProcessor.java b/src/main/java/frc/robot/utils/limelight/VisionProcessor.java new file mode 100644 index 0000000..238c9ca --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/VisionProcessor.java @@ -0,0 +1,58 @@ +package frc.robot.utils.limelight; + +import org.opencv.calib3d.Calib3d; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class VisionProcessor { + + private MatOfPoint3f mObjectPoints; + private Mat mCameraMatrix; + private MatOfDouble mDistortionCoefficients; + + private NetworkTable mLimelightTable; + + public VisionProcessor() { + // Define bottom right corner of left vision target as origin + mObjectPoints = new MatOfPoint3f( + new Point3(0.0, 0.0, 0.0), // top left + new Point3(-1.377, -5.325, 0.0), // bottom left + new Point3(13.25, -5.325, 0.0), // bottom right + new Point3(11.873, 0, 0.0) // top right + ); + + mCameraMatrix = Mat.eye(3, 3, CvType.CV_64F); + mCameraMatrix.put(0, 0, 2.5751292067328632e+02); + mCameraMatrix.put(0, 2, 1.5971077914723165e+02); + mCameraMatrix.put(1, 1, 2.5635071715912881e+02); + mCameraMatrix.put(1, 2, 1.1971433393615548e+02); + + mDistortionCoefficients = new MatOfDouble(2.9684613693070039e-01, -1.4380252254747885e+00, -2.2098421479494509e-03, -3.3894563533907176e-03, 2.5344430354806740e+00); + + mLimelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + mLimelightTable.getEntry("pipeline").setNumber(0); + mLimelightTable.getEntry("camMode").setNumber(0); + mLimelightTable.getEntry("ledMode").setNumber(3); + } + + public void update() { + MatOfPoint2f imagePoints = Limelight.getContourCorners().getCorners(); + SmartDashboard.putBoolean("Valid Corners", imagePoints != null); + + if(imagePoints != null) { + Mat rotationVector = new Mat(); + Mat translationVector = new Mat(); + Calib3d.solvePnP(mObjectPoints, imagePoints, mCameraMatrix, mDistortionCoefficients, rotationVector, translationVector); + + SmartDashboard.putString("translationVector", translationVector.dump()); + } + } +} \ No newline at end of file From 51c32e49b4a81362dfefa8b44ef336b03729400f Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Sat, 2 Feb 2019 19:53:17 -0500 Subject: [PATCH 08/26] Add Testing Facilities for VisionProcessor --- src/main/java/frc/robot/OI.java | 4 + src/main/java/frc/robot/Robot.java | 177 +++++++++++++++-------------- 2 files changed, 95 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index f5b92ce..8e15e52 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -49,4 +49,8 @@ public boolean getShiftHigh() { public boolean getShiftLow() { return rightJoystick.getTrigger(); } + + public boolean getVisionSnapshot() { + return leftJoystick.getRawButton(3); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4bb1b90..436ac2a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,96 +1,101 @@ package frc.robot; -import com._2train395.limelight.api.Limelight; -import com._2train395.limelight.api.Target; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.commands.*; -import frc.robot.subsystems.*; -import frc.robot.utils.*; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.DrivetrainEncoders; +import frc.robot.subsystems.DrivetrainGyro; +import frc.robot.utils.SpeedControllerMap; +import frc.robot.utils.limelight.Limelight; +import frc.robot.utils.limelight.VisionProcessor; +import org.opencv.core.*; /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.gradle file in the - * project. - */ +* The VM is configured to automatically run this class, and to call the +* functions corresponding to each mode, as described in the TimedRobot +* documentation. If you change the name of this class or the package after +* creating this project, you must also update the build.gradle file in the +* project. +*/ public class Robot extends TimedRobot { - public static OI oi; - public static SpeedControllerMap controllerMap = new SpeedControllerMap(); - public static Drivetrain drivetrain = new Drivetrain(); - public static DrivetrainEncoders encoders = new DrivetrainEncoders(); - public static DrivetrainGyro gyro = new DrivetrainGyro(); - public static Compressor compressor = new Compressor(); - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override - public void robotInit() { - oi = new OI(); - compressor.setClosedLoopControl(true); - } - - /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. - * - * This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - } - - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ - @Override - public void disabledInit() { - } - - @Override - public void disabledPeriodic() { - Scheduler.getInstance().run(); - } - - @Override - public void autonomousInit() { - - } - - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - } - - @Override - public void teleopInit() { - - } - - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() { - Scheduler.getInstance().run(); - } + public static OI oi; + public static SpeedControllerMap controllerMap = new SpeedControllerMap(); + public static Drivetrain drivetrain = new Drivetrain(); + public static DrivetrainEncoders encoders = new DrivetrainEncoders(); + public static DrivetrainGyro gyro = new DrivetrainGyro(); + public static Compressor compressor = new Compressor(); + public static VisionProcessor visionProcessor = new VisionProcessor(); + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + oi = new OI(); + compressor.setClosedLoopControl(true); + } + + /** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + * + * This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + } + + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + Scheduler.getInstance().run(); + } + + @Override + public void autonomousInit() { + + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + } + + @Override + public void teleopInit() { + + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } + if(oi.getVisionSnapshot()) { + visionProcessor.update(); + } + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } } From 15d2027b26113cdd45958bcd0d68ac1a82b09f90 Mon Sep 17 00:00:00 2001 From: HPrickettMorgan Date: Sun, 3 Feb 2019 20:36:06 -0500 Subject: [PATCH 09/26] Add ApproachTarget command based on angle offset --- src/main/java/frc/robot/OI.java | 2 + src/main/java/frc/robot/Robot.java | 4 - .../frc/robot/commands/ApproachTarget.java | 152 ++++++++++++------ .../frc/robot/utils/limelight/Corners.java | 11 +- .../utils/limelight/VisionProcessor.java | 102 ++++++------ 5 files changed, 163 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 8e15e52..19c5467 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -15,6 +15,7 @@ public class OI { public final Joystick rightJoystick = new Joystick(RobotMap.RIGHT_JOYSTICK); public final XboxController xboxController = new XboxController(2); + Button approachTarget = new JoystickButton(leftJoystick, 2); // Button backwardsThreeFeet = new JoystickButton(xboxController, 1); // Button right90 = new JoystickButton(xboxController, 2); // Button left90 = new JoystickButton(xboxController, 3); @@ -26,6 +27,7 @@ public OI() { // right90.whenPressed(new TurnDegrees(-90)); // left90.whenPressed(new TurnDegrees(90)); // backwardsThreeFeet.whenPressed(new DriveFeet(-3,true)); + approachTarget.whileHeld(new ApproachTarget()); } public double getLeftY() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 436ac2a..8604ad4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,10 +86,6 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); - - if(oi.getVisionSnapshot()) { - visionProcessor.update(); - } } /** diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index 5439251..f2ec175 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -1,59 +1,117 @@ package frc.robot.commands; -import com._2train395.limelight.api.Limelight; -import com._2train395.limelight.api.Target; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.utils.Targets; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Drivetrain.Gear; import frc.robot.utils.Targets.TargetType; +import frc.robot.utils.limelight.Contour; +import frc.robot.utils.limelight.Limelight; +import org.opencv.core.Point; +public class ApproachTarget extends Command { + + private enum Side { + kLeft, + kRight + }; -public class ApproachTarget extends CommandGroup { - /** - * The distance, in inches, to arrive at from the target. - */ - private static final double DESIRED_DISTANCE_FROM_TARGET = 24.0; - /** - * The height in inches of the center of the camera from the ground. - */ private static final double CAMERA_HEIGHT = 16.125; - /** - * The angle at which the camera is mounted, in degrees, from the horizontal. - */ - private static final double CAMERA_ANGLE = 60.0; - - public ApproachTarget(final TargetType targetType, final ApproachingDirection approachingDirection) { - final Target target = Limelight.getTarget(); - final double distance = Targets.getDistance(target, targetType, CAMERA_HEIGHT, CAMERA_ANGLE); - final double xOffset = Math.toRadians(target.getXOffset()); + //Angle from the horizontal in radians + private static final double CAMERA_ANGLE = Math.PI/6; + private static final double MAX_OFFSET = 45; + private static final double MAX_OFFSET_DISTANCE = 10; + private static final double TARGET_CENTER_HEIGHT = 31.25 + 0.5 * 5.826; - // The skew from Limelight is the same sign no matter which way the target - // appears tilted. Therefore, we need to adjust the sign manually depending on - // from where the robot is approaching. - double skew = 0.0; - switch (approachingDirection) { - case LEFT: - skew = Math.toRadians(-target.getSkew()); - case RIGHT: - skew = Math.toRadians(target.getSkew()); - } + Contour contour; + Side side; + public static final double p = 0.5; + public static final double i = 0; + public static final double d = 0; + public static final double proportionalHeading = 0.01; + + private final Drivetrain.LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); + private final PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); + + public ApproachTarget() { + requires(Robot.drivetrain); + setInterruptible(false); + } + // Called just before this Command runs the first time + @Override + protected void initialize() { + pidController.setOutputRange(-0.5, 0.5); + Robot.drivetrain.shift(Gear.kLow); + contour = Limelight.getBestContour(); + Robot.encoders.zeroEncoders(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + //Get the target + contour = Limelight.getBestContour(); - final double distanceToPos = Math - .sqrt((distance * distance) + (DESIRED_DISTANCE_FROM_TARGET * DESIRED_DISTANCE_FROM_TARGET) - - (2.0 * distance * DESIRED_DISTANCE_FROM_TARGET * Math.cos(skew))); // Law of Cosines + if(contour != null) { + double xOffset = contour.tx; + double yOffset = contour.ty; + + // Calculate distance + double distance = (TARGET_CENTER_HEIGHT-CAMERA_HEIGHT) / + Math.tan(CAMERA_ANGLE+ Math.toRadians(yOffset)) / 12; - final double angleBetweenPosAndTarget = DESIRED_DISTANCE_FROM_TARGET * skew / distanceToPos; // Law of Sines - final double angleToPos = xOffset - angleBetweenPosAndTarget; - final double angleFromPosToTarget = angleBetweenPosAndTarget + skew; + //Set the setpoint for distance + pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distance); + + //If the left side is higher than the right side, the target is to our left + //Otherwise the target is to our right + Point[] corners = Limelight.getContourCorners().getCorners(); + if(corners != null){ + side = corners[0].y > corners[3].y ? Side.kLeft : Side.kRight; + } + + if(side != null){ + double headingCorrection = MAX_OFFSET + * distance / MAX_OFFSET_DISTANCE + * Math.max(Math.min(1, contour.ts/15), 0.25); + linearOutput.setHeadingCorrection(-proportionalHeading * + (xOffset + + ( + side == Side.kLeft + ? -headingCorrection + : headingCorrection + ) + ) + ); + } else { + linearOutput.setHeadingCorrection(-proportionalHeading * xOffset); + } - addSequential(new TurnDegrees(Math.toDegrees(angleToPos))); - addSequential(new DriveFeet(distanceToPos / 12.0)); - addSequential(new TurnDegrees(Math.toDegrees(angleFromPosToTarget))); + if(!pidController.isEnabled()){ + pidController.enable(); + } + } } - - /** - * Represents where, relative to the target, the robot is approaching from. - */ - public enum ApproachingDirection { - LEFT, RIGHT + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return contour == null ; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.drivetrain.tankDrive(0, 0); + pidController.disable(); + } + + // 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/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 2d7a347..3312645 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -3,7 +3,6 @@ import java.util.ArrayList; import java.util.Collections; -import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; /** @@ -11,10 +10,10 @@ */ public class Corners { - MatOfPoint2f corners; + Point[] corners; public Corners(double[] xCorners, double[] yCorners) { - if(xCorners == null || yCorners == null) { + if((xCorners == null || xCorners.length != 4) || (yCorners == null || yCorners.length != 4)) { corners = null; return; } @@ -25,16 +24,16 @@ public Corners(double[] xCorners, double[] yCorners) { temp.add(i, new Point(xCorners[i], yCorners[i])); } - corners = new MatOfPoint2f(Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)), + corners = new Point[]{Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)), Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)), Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y))); + Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y))}; } /** * @return an array containing corners counterclockwise from the top left */ - public MatOfPoint2f getCorners() { + public Point[] getCorners() { return corners; } diff --git a/src/main/java/frc/robot/utils/limelight/VisionProcessor.java b/src/main/java/frc/robot/utils/limelight/VisionProcessor.java index 238c9ca..b36f1e2 100644 --- a/src/main/java/frc/robot/utils/limelight/VisionProcessor.java +++ b/src/main/java/frc/robot/utils/limelight/VisionProcessor.java @@ -1,58 +1,58 @@ package frc.robot.utils.limelight; -import org.opencv.calib3d.Calib3d; -import org.opencv.core.CvType; -import org.opencv.core.Mat; -import org.opencv.core.MatOfDouble; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.MatOfPoint3f; -import org.opencv.core.Point3; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import org.opencv.calib3d.Calib3d; +// import org.opencv.core.CvType; +// import org.opencv.core.Mat; +// import org.opencv.core.MatOfDouble; +// import org.opencv.core.MatOfPoint2f; +// import org.opencv.core.MatOfPoint3f; +// import org.opencv.core.Point3; + +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class VisionProcessor { - private MatOfPoint3f mObjectPoints; - private Mat mCameraMatrix; - private MatOfDouble mDistortionCoefficients; - - private NetworkTable mLimelightTable; - - public VisionProcessor() { - // Define bottom right corner of left vision target as origin - mObjectPoints = new MatOfPoint3f( - new Point3(0.0, 0.0, 0.0), // top left - new Point3(-1.377, -5.325, 0.0), // bottom left - new Point3(13.25, -5.325, 0.0), // bottom right - new Point3(11.873, 0, 0.0) // top right - ); - - mCameraMatrix = Mat.eye(3, 3, CvType.CV_64F); - mCameraMatrix.put(0, 0, 2.5751292067328632e+02); - mCameraMatrix.put(0, 2, 1.5971077914723165e+02); - mCameraMatrix.put(1, 1, 2.5635071715912881e+02); - mCameraMatrix.put(1, 2, 1.1971433393615548e+02); - - mDistortionCoefficients = new MatOfDouble(2.9684613693070039e-01, -1.4380252254747885e+00, -2.2098421479494509e-03, -3.3894563533907176e-03, 2.5344430354806740e+00); - - mLimelightTable = NetworkTableInstance.getDefault().getTable("limelight"); - mLimelightTable.getEntry("pipeline").setNumber(0); - mLimelightTable.getEntry("camMode").setNumber(0); - mLimelightTable.getEntry("ledMode").setNumber(3); - } - - public void update() { - MatOfPoint2f imagePoints = Limelight.getContourCorners().getCorners(); - SmartDashboard.putBoolean("Valid Corners", imagePoints != null); +// private MatOfPoint3f mObjectPoints; +// private Mat mCameraMatrix; +// private MatOfDouble mDistortionCoefficients; + +// private NetworkTable mLimelightTable; + +// public VisionProcessor() { +// // Define bottom right corner of left vision target as origin +// mObjectPoints = new MatOfPoint3f( +// new Point3(0.0, 0.0, 0.0), // top left +// new Point3(-1.377, -5.325, 0.0), // bottom left +// new Point3(13.25, -5.325, 0.0), // bottom right +// new Point3(11.873, 0, 0.0) // top right +// ); + +// mCameraMatrix = Mat.eye(3, 3, CvType.CV_64F); +// mCameraMatrix.put(0, 0, 2.5751292067328632e+02); +// mCameraMatrix.put(0, 2, 1.5971077914723165e+02); +// mCameraMatrix.put(1, 1, 2.5635071715912881e+02); +// mCameraMatrix.put(1, 2, 1.1971433393615548e+02); + +// mDistortionCoefficients = new MatOfDouble(2.9684613693070039e-01, -1.4380252254747885e+00, -2.2098421479494509e-03, -3.3894563533907176e-03, 2.5344430354806740e+00); + +// mLimelightTable = NetworkTableInstance.getDefault().getTable("limelight"); +// mLimelightTable.getEntry("pipeline").setNumber(0); +// mLimelightTable.getEntry("camMode").setNumber(0); +// mLimelightTable.getEntry("ledMode").setNumber(3); +// } + +// public void update() { +// MatOfPoint2f imagePoints = Limelight.getContourCorners().getCorners(); +// SmartDashboard.putBoolean("Valid Corners", imagePoints != null); - if(imagePoints != null) { - Mat rotationVector = new Mat(); - Mat translationVector = new Mat(); - Calib3d.solvePnP(mObjectPoints, imagePoints, mCameraMatrix, mDistortionCoefficients, rotationVector, translationVector); - - SmartDashboard.putString("translationVector", translationVector.dump()); - } - } +// if(imagePoints != null) { +// Mat rotationVector = new Mat(); +// Mat translationVector = new Mat(); +// Calib3d.solvePnP(mObjectPoints, imagePoints, mCameraMatrix, mDistortionCoefficients, rotationVector, translationVector); + +// SmartDashboard.putString("translationVector", translationVector.dump()); +// } +// } } \ No newline at end of file From 56385a577b9e5588835939b6183a177038e89e8a Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Tue, 5 Feb 2019 15:42:25 -0500 Subject: [PATCH 10/26] Add namedCorners to Corners.java --- .../frc/robot/commands/ApproachTarget.java | 11 ++++--- .../frc/robot/utils/limelight/Corners.java | 29 ++++++++++--------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index f2ec175..7f1f751 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.Drivetrain.Gear; import frc.robot.utils.Targets.TargetType; import frc.robot.utils.limelight.Contour; +import frc.robot.utils.limelight.Corners; import frc.robot.utils.limelight.Limelight; import org.opencv.core.Point; public class ApproachTarget extends Command { @@ -66,10 +67,9 @@ protected void execute() { //If the left side is higher than the right side, the target is to our left //Otherwise the target is to our right - Point[] corners = Limelight.getContourCorners().getCorners(); - if(corners != null){ - side = corners[0].y > corners[3].y ? Side.kLeft : Side.kRight; - } + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) + side = corners.topLeft.y > corners.topRight.y ? Side.kLeft : Side.kRight; if(side != null){ double headingCorrection = MAX_OFFSET @@ -88,9 +88,8 @@ protected void execute() { linearOutput.setHeadingCorrection(-proportionalHeading * xOffset); } - if(!pidController.isEnabled()){ + if(!pidController.isEnabled()) pidController.enable(); - } } } diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 3312645..3f2f704 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -10,31 +10,32 @@ */ public class Corners { - Point[] corners; + public final boolean validCorners; + public final Point topLeft; + public final Point topRight; + public final Point bottomLeft; + public final Point bottomRight; public Corners(double[] xCorners, double[] yCorners) { if((xCorners == null || xCorners.length != 4) || (yCorners == null || yCorners.length != 4)) { - corners = null; + validCorners = false; + topLeft = null; + topRight = null; + bottomLeft = null; + bottomRight = null; return; } + validCorners = true; ArrayList temp = new ArrayList<>(); for(int i = 0; i < 4; i++) { temp.add(i, new Point(xCorners[i], yCorners[i])); } - corners = new Point[]{Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y))}; + topLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)); + bottomLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)); + topRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)); + bottomRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y)); } - - /** - * @return an array containing corners counterclockwise from the top left - */ - public Point[] getCorners() { - return corners; - } - } From 0605eee94549abf73d94d08ca55945db76eb0892 Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Wed, 6 Feb 2019 17:02:32 -0500 Subject: [PATCH 11/26] Tuning ApproachTarget --- src/main/java/frc/robot/OI.java | 6 ++- src/main/java/frc/robot/Robot.java | 16 ++++-- .../frc/robot/commands/ApproachTarget.java | 53 ++++++++++++------- .../frc/robot/utils/limelight/Corners.java | 4 +- 4 files changed, 53 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 19c5467..f61e200 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -27,7 +27,7 @@ public OI() { // right90.whenPressed(new TurnDegrees(-90)); // left90.whenPressed(new TurnDegrees(90)); // backwardsThreeFeet.whenPressed(new DriveFeet(-3,true)); - approachTarget.whileHeld(new ApproachTarget()); + approachTarget.whenPressed(new ApproachTarget()); } public double getLeftY() { @@ -55,4 +55,8 @@ public boolean getShiftLow() { public boolean getVisionSnapshot() { return leftJoystick.getRawButton(3); } + + public boolean getSquareUp() { + return leftJoystick.getRawButton(2); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8604ad4..a60d277 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,15 +1,18 @@ package frc.robot; +import java.util.concurrent.CountDownLatch; + import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.DrivetrainEncoders; import frc.robot.subsystems.DrivetrainGyro; import frc.robot.utils.SpeedControllerMap; +import frc.robot.utils.limelight.Contour; import frc.robot.utils.limelight.Limelight; import frc.robot.utils.limelight.VisionProcessor; -import org.opencv.core.*; /** * The VM is configured to automatically run this class, and to call the @@ -85,8 +88,15 @@ public void teleopInit() { */ @Override public void teleopPeriodic() { - Scheduler.getInstance().run(); - } + Scheduler.getInstance().run(); + + Contour contour = Limelight.getBestContour(); + if(contour != null) { + + SmartDashboard.putNumber("skew", contour.ts); + SmartDashboard.putNumber("skew_magnitude", Math.min(-contour.ts, 90+contour.ts)); + } + } /** * This function is called periodically during test mode. diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index 7f1f751..985059c 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -1,40 +1,42 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Drivetrain.Gear; -import frc.robot.utils.Targets.TargetType; import frc.robot.utils.limelight.Contour; import frc.robot.utils.limelight.Corners; import frc.robot.utils.limelight.Limelight; -import org.opencv.core.Point; public class ApproachTarget extends Command { - private enum Side { + enum Side { kLeft, kRight }; - private static final double CAMERA_HEIGHT = 16.125; + static final double CAMERA_HEIGHT = 16.125; //Angle from the horizontal in radians - private static final double CAMERA_ANGLE = Math.PI/6; - private static final double MAX_OFFSET = 45; - private static final double MAX_OFFSET_DISTANCE = 10; - private static final double TARGET_CENTER_HEIGHT = 31.25 + 0.5 * 5.826; + static final double CAMERA_ANGLE = Math.PI/6; + static final double MAX_OFFSET = 15; + static final double MAX_OFFSET_DISTANCE = 8; + static final double TARGET_CENTER_HEIGHT = 31.25 + 0.5 * 5.826; + static final double MAX_TIME_WITHOUT_CONTOUR = 0.1; Contour contour; Side side; - public static final double p = 0.5; - public static final double i = 0; - public static final double d = 0; - public static final double proportionalHeading = 0.01; + static final double p = 0.5; + static final double i = 0; + static final double d = 1; + static final double proportionalHeading = 0.013; - private final Drivetrain.LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); - private final PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); + Drivetrain.LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); + PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); + double lastContourSeenTime = 0; + public ApproachTarget() { requires(Robot.drivetrain); setInterruptible(false); @@ -45,6 +47,7 @@ protected void initialize() { pidController.setOutputRange(-0.5, 0.5); Robot.drivetrain.shift(Gear.kLow); contour = Limelight.getBestContour(); + lastContourSeenTime = Timer.getFPGATimestamp(); Robot.encoders.zeroEncoders(); } @@ -53,11 +56,12 @@ protected void initialize() { protected void execute() { //Get the target contour = Limelight.getBestContour(); - if(contour != null) { + lastContourSeenTime = Timer.getFPGATimestamp(); + double xOffset = contour.tx; double yOffset = contour.ty; - + // Calculate distance double distance = (TARGET_CENTER_HEIGHT-CAMERA_HEIGHT) / Math.tan(CAMERA_ANGLE+ Math.toRadians(yOffset)) / 12; @@ -73,30 +77,39 @@ protected void execute() { if(side != null){ double headingCorrection = MAX_OFFSET - * distance / MAX_OFFSET_DISTANCE - * Math.max(Math.min(1, contour.ts/15), 0.25); + * Math.min(1, distance / MAX_OFFSET_DISTANCE); linearOutput.setHeadingCorrection(-proportionalHeading * (xOffset + - ( + ( side == Side.kLeft ? -headingCorrection : headingCorrection ) ) ); + SmartDashboard.putNumber("Heading setpoint", -proportionalHeading * + (xOffset + + (side == Side.kLeft + ? -headingCorrection + : headingCorrection) + )); + } else { linearOutput.setHeadingCorrection(-proportionalHeading * xOffset); + SmartDashboard.putNumber("Heading setpoint", xOffset); } if(!pidController.isEnabled()) pidController.enable(); + } else { + linearOutput.setHeadingCorrection(0); } } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return contour == null ; + return Timer.getFPGATimestamp() - lastContourSeenTime > MAX_TIME_WITHOUT_CONTOUR; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 3f2f704..42ef1f7 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -35,7 +35,7 @@ public Corners(double[] xCorners, double[] yCorners) { topLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)); bottomLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)); - topRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)); - bottomRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y)); + bottomRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)); + topRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y)); } } From 68b3d2bd3944b10fd78df0618d5916cb532a9c86 Mon Sep 17 00:00:00 2001 From: Henry Date: Mon, 25 Feb 2019 18:00:53 -0500 Subject: [PATCH 12/26] Create ApproachTarget CommandGroup based on old ApproachTarget command --- .../robot/commands/{ApproachTarget.java => DriveToTarget.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/frc/robot/commands/{ApproachTarget.java => DriveToTarget.java} (100%) diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java similarity index 100% rename from src/main/java/frc/robot/commands/ApproachTarget.java rename to src/main/java/frc/robot/commands/DriveToTarget.java From aa236a5ce79e4a3326deab4d98a10aed670e16af Mon Sep 17 00:00:00 2001 From: Henry Date: Fri, 1 Mar 2019 19:05:08 -0500 Subject: [PATCH 13/26] Finish implementing ApproachTarget based on old method --- .../java/frc/robot/commands/AimAtTarget.java | 64 ++++++++ .../frc/robot/commands/ApproachTarget.java | 12 ++ .../frc/robot/commands/DriveToTarget.java | 140 +++++++++++------- 3 files changed, 159 insertions(+), 57 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AimAtTarget.java create mode 100644 src/main/java/frc/robot/commands/ApproachTarget.java diff --git a/src/main/java/frc/robot/commands/AimAtTarget.java b/src/main/java/frc/robot/commands/AimAtTarget.java new file mode 100644 index 0000000..722fd81 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAtTarget.java @@ -0,0 +1,64 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +import frc.robot.utils.limelight.Contour; +import frc.robot.utils.limelight.Limelight; + +public class AimAtTarget extends Command { + + //Full power at 15 degrees + double p = 1.0/15.0; + //We want to overshoot/not settle this turn + double d = 0; + double maxOutput = 0.5; + //We only need this to roughly align, this gives time to either stop early or accelerate past + double acceptableErrorDegrees = 5; + + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput()); + + Contour contour; + + public AimAtTarget() { + requires(Robot.drivetrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + pidController.setOutputRange(-maxOutput, maxOutput); + + contour = Limelight.getBestContour(); + double xOffset = contour.tx; + if(contour != null) { + pidController.setSetpoint(xOffset + Robot.gyro.getYaw()); + } + pidController.enable(); + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all + return Math.abs(pidController.getError()) < acceptableErrorDegrees || + contour == null; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java new file mode 100644 index 0000000..043c3a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -0,0 +1,12 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class ApproachTarget extends CommandGroup { + + public ApproachTarget() { + addSequential(new AimAtTarget()); + addSequential(new DriveToTarget()); + } + +} diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index 59bef40..af163b9 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -10,106 +10,132 @@ import frc.robot.utils.limelight.Contour; import frc.robot.utils.limelight.Corners; import frc.robot.utils.limelight.Limelight; -public class ApproachTarget extends Command { + +public class DriveToTarget extends Command { enum Side { kLeft, kRight }; - static final double CAMERA_HEIGHT = 16.125; - //Angle from the horizontal in radians - static final double CAMERA_ANGLE = Math.PI/6; - static final double MAX_OFFSET = 15; - static final double MAX_OFFSET_DISTANCE = 8; - static final double TARGET_CENTER_HEIGHT = 31.25 + 0.5 * 5.826; - static final double MAX_TIME_WITHOUT_CONTOUR = 0.1; + //Camera Parameters + static final double cameraHeight = 16.125; + static final double cameraAngle = 30; - Contour contour; - Side side; + /** + * agressiveSkewThreshold: The threshold in native limelight skew that determines if we take an agressive appraoch. + * maxOffsetAgressive: The angle offset an agressive approach will target. + * maxOffsetNormal: The angle offset a normal approach will target. + * offsetScalingFactor: The maximum distance at which we will use the maximum offset. Below this distance we scale offset down linearly + */ + + static final double aggressiveSkewThreshold = 15; + static final double maxOffsetAggressive = 25; + static final double maxOffsetNormal = 12.5; + static final double maxOffsetDistance = 6.0; + + static final double lowTargetHeight = 33.893; + static final double highTargetHeight = 0; + + //The amount of time the command will continue to run without seeing a contour in seconds. + static final double maxTimeWithoutContour = 0.1; + + //The PID coefficients of the linear drive. static final double p = 0.5; static final double i = 0; static final double d = 1; - static final double proportionalHeading = 0.013; - + static final double maxOutput = 0.5; + + //The proportional constant for the angle adjustment + static final double rotationP = 0.013; + + //The PID controller and PIDOutput. We maintain a reference to linearOutput to set the heading correction. LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); - double lastContourSeenTime = 0; + Contour contour; + Side side; + double initialSkewAbs; + double xOffset; + double distance; + + //A timer which starts when a contour is not seen to determine if we should kill the command. + Timer contourNotSeen = new Timer(); - public ApproachTarget() { + public DriveToTarget() { requires(Robot.drivetrain); setInterruptible(false); } + + + private double calculateHeadingCorrection() { + double additionalOffset; + + if(side == null) { + additionalOffset = 0; + } else { + double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal; + double additionalOffsetSign = side == Side.kRight ? 1 : -1; + double distanceScaling = Math.min(1, distance/maxOffsetDistance); + + additionalOffset = maxOffset * distanceScaling * additionalOffsetSign; + } + return rotationP * (xOffset + additionalOffset); + + } + // Called just before this Command runs the first time @Override protected void initialize() { - pidController.setOutputRange(-0.5, 0.5); + //Drive slow in Robot.drivetrain.shift(Gear.kLow); + pidController.setOutputRange(-maxOutput, maxOutput); + contour = Limelight.getBestContour(); - lastContourSeenTime = Timer.getFPGATimestamp(); - Robot.encoders.zeroEncoders(); + if(contour != null) { + initialSkewAbs = Math.abs(contour.ts); + } } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - //Get the target contour = Limelight.getBestContour(); - if(contour != null) { - lastContourSeenTime = Timer.getFPGATimestamp(); - double xOffset = contour.tx; + if(contour == null) { + contourNotSeen.start(); + linearOutput.setHeadingCorrection(0); + } else { + contourNotSeen.stop(); + contourNotSeen.reset(); + + xOffset = contour.tx; double yOffset = contour.ty; - // Calculate distance - double distance = (TARGET_CENTER_HEIGHT-CAMERA_HEIGHT) / - Math.tan(CAMERA_ANGLE+ Math.toRadians(yOffset)) / 12; + double distanceFeet = (lowTargetHeight - cameraHeight) / + Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; - //Set the setpoint for distance - pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distance); + //Set the setpoint for distance relative to current position + pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distanceFeet); - //If the left side is higher than the right side, the target is to our left - //Otherwise the target is to our right + //Determine the side by picking which upper corner is higher Corners corners = Limelight.getContourCorners(); - if(corners.validCorners) - side = corners.topLeft.y > corners.topRight.y ? Side.kLeft : Side.kRight; - - if(side != null){ - double headingCorrection = MAX_OFFSET - * Math.min(1, distance / MAX_OFFSET_DISTANCE); - linearOutput.setHeadingCorrection(-proportionalHeading * - (xOffset + - ( - side == Side.kLeft - ? -headingCorrection - : headingCorrection - ) - ) - ); - SmartDashboard.putNumber("Heading setpoint", -proportionalHeading * - (xOffset + - (side == Side.kLeft - ? -headingCorrection - : headingCorrection) - )); - - } else { - linearOutput.setHeadingCorrection(-proportionalHeading * xOffset); - SmartDashboard.putNumber("Heading setpoint", xOffset); + if(corners.validCorners) { + side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; } - if(!pidController.isEnabled()) + linearOutput.setHeadingCorrection(calculateHeadingCorrection()); + + if(!pidController.isEnabled()){ pidController.enable(); - } else { - linearOutput.setHeadingCorrection(0); + } } } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return Timer.getFPGATimestamp() - lastContourSeenTime > MAX_TIME_WITHOUT_CONTOUR; + return contourNotSeen.hasPeriodPassed(maxTimeWithoutContour); } // Called once after isFinished returns true From 135c48d411d98f1b7483c5b0e3ae5c186e8bbd4f Mon Sep 17 00:00:00 2001 From: Henry Date: Fri, 1 Mar 2019 19:13:37 -0500 Subject: [PATCH 14/26] Add in support for both target types --- .../java/frc/robot/commands/ApproachTarget.java | 3 ++- src/main/java/frc/robot/commands/DriveToTarget.java | 13 +++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index 043c3a8..a3f133c 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -6,7 +6,8 @@ public class ApproachTarget extends CommandGroup { public ApproachTarget() { addSequential(new AimAtTarget()); - addSequential(new DriveToTarget()); + //TODO: Decide this based on robot state + addSequential(new DriveToTarget(DriveToTarget.TargetType.kLowTarget)); } } diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index af163b9..c09e127 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -13,10 +13,17 @@ public class DriveToTarget extends Command { + public enum TargetType { + kHighTarget, + kLowTarget; + } + enum Side { kLeft, kRight }; + + //Camera Parameters static final double cameraHeight = 16.125; @@ -35,7 +42,7 @@ enum Side { static final double maxOffsetDistance = 6.0; static final double lowTargetHeight = 33.893; - static final double highTargetHeight = 0; + static final double highTargetHeight = 41.787; //The amount of time the command will continue to run without seeing a contour in seconds. static final double maxTimeWithoutContour = 0.1; @@ -55,6 +62,7 @@ enum Side { Contour contour; Side side; + final double targetHeight; double initialSkewAbs; double xOffset; double distance; @@ -62,9 +70,10 @@ enum Side { //A timer which starts when a contour is not seen to determine if we should kill the command. Timer contourNotSeen = new Timer(); - public DriveToTarget() { + public DriveToTarget(TargetType targetType) { requires(Robot.drivetrain); setInterruptible(false); + targetHeight = (targetType == TargetType.kHighTarget) ? highTargetHeight : lowTargetHeight; } From 2fa6fa8891a9ff62a8573201a193f7bd33d71a86 Mon Sep 17 00:00:00 2001 From: Henry Date: Fri, 1 Mar 2019 19:15:44 -0500 Subject: [PATCH 15/26] Add TODOs for tuning --- src/main/java/frc/robot/commands/DriveToTarget.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index c09e127..383c061 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -36,8 +36,8 @@ enum Side { * offsetScalingFactor: The maximum distance at which we will use the maximum offset. Below this distance we scale offset down linearly */ - static final double aggressiveSkewThreshold = 15; - static final double maxOffsetAggressive = 25; + static final double aggressiveSkewThreshold = 15; //TODO: Tune + static final double maxOffsetAggressive = 25; //TODO: Tune static final double maxOffsetNormal = 12.5; static final double maxOffsetDistance = 6.0; From 0b0cfb4890993170c5ed8724a4891a458ceeda35 Mon Sep 17 00:00:00 2001 From: Henry Date: Sat, 2 Mar 2019 13:20:30 -0500 Subject: [PATCH 16/26] Remove unusued jar --- libs/limelight-api-0.1.0.jar | Bin 13090 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 libs/limelight-api-0.1.0.jar diff --git a/libs/limelight-api-0.1.0.jar b/libs/limelight-api-0.1.0.jar deleted file mode 100644 index d5488634e2d7b788c221e43ce0953400e0b7a2c5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13090 zcmb7rb9|iLx_0a|XxP{`+Ss<)*tV_4wrw@G8{2Gb+sT)Hd!OC+%|2)MoHf6hdHz`U zwbnc{*Sd9$v=}f56aWAO1c0kHl_bF10Q&XvHXz@g5(4r(R3egsG{6AT|C+cNNFiC} z33P(-HUa(ZiS*Zr5vb56-Yug*!K<~ib z?EkeY$lJB9rTM=t_}4jb|D2;qZ)c-zY{9_7^dG1H+T$ZHZ-#Po8kLA zfc2jYwXKZ*&A5sLU>X1z06-NS0D$^W#=MsH7J9}OhP;+e)VgNcwzhHdrgDffXda4< z^-g#niE3-)aqWl13>F0de=KVWq8S`OS8qU>HY`zsm%YeOi7QWi^Wf$x-sgZuSpWVV zF7>h{Vb^)g^=A9&?*5p~OJ5HNV%-B^?yN&<2wzAHlghTHYl1Iak;Mh6;}f>)_mgfx z0I4%ZFGHu3?tC~kPQn&pQM7@)>c@L|YR%zeVmSe(xP^g&5Z59lMBan%4#i{&Ba{>@ z1{em;UwLV@lbG_8N;4G>oBS%1M~)!&5fFd6r;TiVo(buU;x*xFPl5O6tqs;^XzjJ> z%_=`INKzmawwvxO){k#~;)^wPs}#>yU|Y$OTQLyaw=6L1riHzc=e1;1j9e2{{x;3s zZj$Jd?_ra<8E61Et2AS`87tq0e z160(V{`{aoEF~5RSLVK&{oFzo&~DOt;%o7t#>4$+0{p#{LZ4ZJZ5VrHLUXM7TZ9MZ zd&B+q6lh{Oacu>=Lux|5ww&om1q3G`E{8^dNm=!Jv)uuV;Tq4RFknmcMknSb5b#HY!6B&k!5d-sA9#!3SbbZOa%kwr#&yl@ z#wJ8g2JXV+1Rdp^UF=I+k>W~5*I6c(=cU2ZVtoOXO^r_nbGbh;=@@a80@i4I{B-7N zBPu@m$@|+ZXTu&hJ(f-&RD?`Wv)vt8aKD7iG9KLSxLX}mc^wp3J3f5$7{U>o&}v^@ zWgp}R$3T!J{A!)37cqW(-l#%ly*0!r$Sh%TN_wZD{#b55joP|E-UarZa8E(Y&(8V2 zM;0-skOL*V^ICKKuN^zL4ThxzlfNcfdr`d#-&Rz0062# zafwgcT;E1p!ctHFHx!K_ak7w}_@r1!{E82xVMnhxR zKnL=xR6Y(ZzwEuNj2iZ}dbdq4F$v=md2*eC@-5?LT;7k+?EO#Bc?!0{Sw!g)JBSP0 z^UJ*WY<;-HB=Km|(*fs;<7VAtMoqh_lHPo}LsS1}=z+&{mN;bAd z1(0cY582@dVHGS2N!qlSMI=(8QTj$=D&a}xNJ`?jLEm+z!nZye%%Yw6y&h+!`^w1W zwP^3c%xw%axlG_e45Zg#4y9;+aTkx$qmL%Yt+$ZTlxR}0f~nZJ*s8>VEKSnzNYlnu zkH9=v$P*XgO_T#ExO9-{oW=u$ERap*q}zr0KIl@ku`}|BVp)ES`(;=J`Ra`2Sm6u+ z4!t}rQke%Q8tUO(xKkxn&I%iqaF{|^+nbuy5{X5WSCt1W1QDB}pI7M&M9O)V2{dg_ z7M>TC?{H#g@9e^10Iw{~&I$bc@L)jFZDB@G_bozon2qy61KF2BQRl49lc)^4SI-p5 z=kq;LsC*?1#N_SRM%9U*@H!(lL+Lw-KU1O~CpXWl;>%q)lTE%!1(%_Sd`W){$8$Jx z%7`5#$y(&ICmR0}_jrs(W1G z6gD#J0uXR<>_Q}`Rgm%A7_Ud*;-UW{qXo7uBmf7Vc|f;ei@;eN*A0hy?rgJXz)@cz z^aSq=cq3nPm-Yp!OcdF_B-g*sc*kfP1`ZO>hTkr8m*G=RYX+|Qv2Bekk`Xbr|13%d z1wY*=+RfvpWeWiy0vAOvE*P!y6zX*&pgmkC2V>C%b+VdcmK)l0bA^LT0Im8^3K6H` zNC~k37q3fuNW3BVqMoJ@39E1)PlSnt68s|P5c`%Y5Y}~fmZ)vI6`OMmeR%>Mwfl_>+ynR`wB<%B#!6H1DjXM9Wf+iz5H^o zPpB9@ibCkR9CdudE<;>TRWMsa32uQ7XnZg`=k7+jkL~i;wGJ92ajVv1t?0vSAF}e( zAyT?js@-~zGJIonx9mJCC*BaT&~U#25UEtd@O?I(m74B&)M+vg(m9yL ztdD-keWwjD)-?0h?Ydf^V9rf81r5dvFgTni_31qfqf-X`Xi06pxtPWQ>)EIcd6K$I z=H_3PIMe!l%36l3mhK#{lRWOPb*gvnkPEi07X#IxQo)LPvQeGZ5xeKf(4qXr{Ug`Y zg|LXsE3m(47yR&rT@N~K(l(Fiqk0yvH(r%)$#Nb<1bnq zku2HS;rJPk;#_Xc3X6KbqcJJn`>ETGEA|tNThThyER%Z2+Qn z>~%ow!9o)!zrIgxU32UQ#R0ZxbF`g52vOzG429E zVBUJEQ#nupl4uO`_})sW2w&G`*q_$4@GJ|7IOu-u2|7+M2S_m@c`8AxNKaum&FOq) zbkFLZ*Sn`iN)vIJ1(ZvI&w;*ARzBd0ed@E$;ezc2l~(bAmcA1;auiYcf&;$?Mg>*m zGGh^Rsjow=MN;S|1MH6MEmj-xf22sOVa{LA=(A%bY-D z)G7iVQAm$Qs#Or*^#!$`298B`3p5~*!k-5Qx}5R;%O=wVj^_r}53S(Bh|D!sgPeyd zAB=IhOibLOl1DW0W4=e@n+*5;t9G8}tBv@ei0cg(o`np&*M^8JaG~0w>~O}>al&wy z>6|8{VXUfWuZF^nb<4hp$5LH}z>e7w?{r+{H)R00Qu z1K#?Qb85k3lkAY1XQ-oXZ5rb!k?f`5si%+f38Bsh`nXML5$8eUm!9wVtrA8bS@0H) z*L?s0p!_4h|0|&WhyTq}Jd|A$MB@PRHN&IE6A=+vj702E7x0QDBqW?BCJe(Pb1+WJ z&+CGqtkhSE&f2GHUDe2BaQA-M7oXztGd6eta4(c>|2iZXpCp`q&{ytYKk68MGv!g@ z`SN^d3BYit(xsA1#NOkIkL`cX+5F7i#mcD{My?HvK0Cfb%=q?#Qm4Y_sIpfoBXA_V z$E(^??q{*ZzeSYlgO`QwPaBNL=|k4(gC!`^lkT{^XE*p>8x#EEq#z$|6$ znD2;9S!J;o7|BVppKU&Uq|lpQPBu9(I}#5#Dz$(NB&PRosp8a6DYg*TY%SfoYKfn! zudd5Ru_uy2k7_-uGho~LzGua|L`n7tvM?zzqzE(elP0m5+0}xKVjDr|qZ~f*dO~A6 zskLUQiXeUZ_6F>uioKB;S&zCu6PkQU8hh__i$SseMq$PX0j;iD?GYVLH8uSug}(Z- zpxsjcHE^qN1Z*2W=Q30*9o1*so<7UC{^fuWg`d(|9CdA1d8Q_uYY*7oY1j7E&$Dv^ zJBOx?4Pko@^m75KHC3R^!`Qdi$3E2%@t?c=G8`QNb~Nft?s8!Fh3?9kit^0vooRrB zh~(;vPm}^pIpa&lr#JDPKR_5bGp&dZni8;XUyZ8|Qz|Pe&PV`8Jf$g40FEf06VB_q z+yTXcfszra%oLYW#+`htfe}#~IifBRC}_LzDOya9avsLBP8LNGH4tnrtKP+$X=3jj zMLm?G$B2FXD_s_(6it8F!8@sP8t~<=zRFz|XGzTpCED;>p*eR2l)f z2X{txpHxH~+uAi(^BK5;h`_De3=Z0;>XzS)Vw25{W&<3ZYlDP2DaQ>9tNQk{Lk$di zGK`DW%Jw(wUJ)VnC0Q|+jWI^68wFnJ$=#f5ArHQ9dp~f&lf2@oMn?7a-Du(|4+4PF ziGaX>6l~f9Pw<~N^kz{6^1egyDOeY;r%C1-jf2v>o=|?O6Tu+AX}9U~l?Lj=z>lSH zY>_5^Efd)6wtm=1*{?^QzS+Rk@b*$Np7w(e8*A?6U_7x6bG7zSEkG3NOM7m*GH?ddjya@rvk^qCbww@1ZC?jqF$T;<(fB&XcH{vl!vcTbV)d) zM8M=bq<&yOJS!%mQRKm^(K(}=yVxSjM~GY&)fB)&ZIAW&AWyGiniOx>`Vy_kp(2)7 zHik*IzOp0lCwHkvaW@5%ME@6*h^T`r(UIusxKBk(${VtdR&$W!yKVyOyL+Ns-H;W_ zm|3emt;OmL8DZ=~j>(+MF;XSb`galu>L8GmmmZ+Ehrp>ifRe7jJtjcx9cnF_0+Wzd zVLyYua(r_tYTgAfWc_evvrE&?3-cTxeR-O0B_@21YRT#z!p!iqH~FKXS7>EP+K+wv zji+^I=bOdTv%u3A@V^H%0Qw2{^lt}VFa!Vq#h(s5X=AIm3WCMI@(pu23P+V14qNB9THFB6moq#yN89> zHBD>wT?1ntbP@yN-G$wB+jQHLM#tNTc8`0k4%j0hyv_0eOil&b3fziy^4a-i-7&*Y zN%pWPP~^;vNY(t-iC2&zmt41 zM4iU3uB(!Y<`vOze&|r4P|~vagvovBGQ^>FTRQmAoMlxwvMQHdu_bLuSPgMlXytR; z7_S5*B*Rbl_|#%cnd@qn?0ZG?DPyFg5%MxpUW+h;2CbLoSEo5poko|1a+KqMp(gK! zqXj0P{_MlXmuSC`g4lk8O@ZJmNA*RyX&Z|48X2OXovuh#LmCBmJ&r#Cd+= zDAK4yGnp?Vy4zD4C3W+88;HgximfxGZ0HG}XI;0F^?A~MLc1XE*voV4_cbHVSVNbG z6CE8ScH3i}uDgRu*~!b~j|5K$P!_;Q?i zPfPq_%iKsH2weEZ!zv`d=(Ax-oSP`Zl10uw2}hr&swMmR!m9WVn>fFmdG*q~WK_-W z=eD>=*6SNb3L^f(o8ec{%tuMs(rLA{gdgfbQ;wvI?2T7e_n+K%$|w8k9q$owSh&9+ ztm%0fc%1`$1e65*g0&{@RUL}Pk9f-4#pH709e}pV-+{IPEn$klf3GUr1KuMDd0$YA zhn2aD%sUo1p+o<%LRJ5KDNGRwLaPd?th~w;inL87~(12S5*54OAq~rP2!v?ip*Z+VNM z>i}^LKauF*#o!J;?wRM$AzZGbbYY1hV}IeD%|FDuws~t(PSfb4izS zPwYn;0h2L^_Qa2jc3$cq54>3kUmk%1w08AWDigJKyR|zXK%3!%AeeFI1q2Yxq$|aW z$>$@rEYT5iKC)5X1!ke8?G5>~n->a_yacbEqR9}D-xX#(3sPh;!;}!#BUFp7Ge4Bc z=7s^5a^_MWdEhSy=EX`V zS|}w;t4QP7Ez+T0@(Hxkyxnu-a_oCE_57$>U;}C_p2YTYiP^6VXI$t|lJyo$IcAPfS2q;2uY0F#Z)ou2!nJWl?;Ayzhyix+m9pm$9J zCWvOQd(pV477~n-u&Y=)VD!f#ORb^>Rv@xTumcED^kTky-g>~$3C;lcV{H(GEGdE- z9`ziz&6l(YpAtJD&6oxBv`NZ38+}sL81Xhf33q`^Z!6%ab+8(#T^6KOf&<+HiBl8< z?$L`pVc!H?q9$Cr<&F^)L=v)7Uc?<+p|d;xwQ=XOX%<+?pT}!jhC2i0o%Ky%6ltAu zUwsSQE0#R}QojFHsq0K>VuXAv1Q@(k>KOhnKVErftAF`VQ@Q_NugjN!0HnL{xZ|F# zgIF)Z57Z(Hmbap$&&ElW)`5)47f;52f_R070~HebHugf=w=%D7QZ~wn<8a#Q>M+L9 zEaLI>bb;0JC4G*s%kM4ar^r$2L;^2BX4m2(*vtwB8?H*yy$qD#C3WO?KXG(=?ixFRMV)4ZeA6#GQCnRCErzVFYLsa^pp}675 zm07taev~?c5H-Dda~&7KxQ4%pHC>&cu;&nQnD)|2%?(ITHn;oYXj8uCRf9Am_khZf zCvr^<(WjDGE3i^<0DacYSYfeBE3LJ#;Y^2 z#gcVM;T|ms!cPxp^4$`Ykkk4%jCR=}N{^qKoM$x1?qwz%n5iP<1Q~PBLPx3skYUaj zs9?>F=El7pyz{xP<*FGZf7X(_HdcK%*LOo?li@7&(?x-jo6(5nDv{MttHrwn!QUAT z-?osID{ixX9T!A)HF|i3r0{sToj8JUpD(+bf*5nCvhs2Rcb>0A3wwYYge*e}p)& zMpweMigR$^x|fN-m6Uc27zaECDF+n}_lH>hc1ZN}WoA=OAz>o}fz#Ix-YRzUaloWq zra`6&SR#N@>5dyB$c>(J+%{NPJ%NiS=d+*)f3P_lYL=nX+Cc`GMaOr!dOXClVE;jMFPlEh~Q`? zHe})YP`{9DZe{9fewFH`prj@Zw%NXc?9$+*gb;hiY!%rE<9vm>OWD^P+;Jlm6YhnH zva6G3lNE_|;Zm3THW&2D6{2PWaFV0CMPZ%G({l-bf&`3A@yhtLG*!hQOcD|TlAP3j z)RCGt6RYa1qWj$$8bbs4LV-B3*vIcq368iE9&4`^ZZ2SO2khEVC7{(cEi zv+xOLqMfYFge{6^U@1_DffT|Q=P1_R6TPZA16DjHibML-VPzZ~$KuS_T>efoAB}bp zSUe9vM&wtii;05g>4tsscI--GKi^V$Xh_dtjfLk231Vb$mA&rY&0XnwQxDp!x^~fr zsRuB2hwV5*X%46m2T^@{oiS-aR`auw(g^0p5C!eI5ZHBx=yS^OEpXL$BvVo>**SBF z^2NBi-pj2BkD#PN%=(}~uymtnrupM$NilyFpqzK@Qx*YZCm-{tXD|dTn(pqxA{ZJq zO>~bsS@aPzVEukg(nVh$R&cxp7mXTR0MKIy*>d=q>0pROT7Ic!EPM3=wfVpjBs(z- z%_8k66L|G?#~S|jE?b~&kj`;P-VBq*CkOt5vZ5~&$P#evbi3rW={eJz#f&!IvP&$Y zz~!PjsHC+SIgDG`gtP%x0wP31V{Vj}jp6KW0Y@cK1hw??!R67d*GYZq12Rfg?dj|% zqS%f^pXXu}lrH=zRLV)l8lp*#3yZk^&KT%u0?N2IU)2A`7~21ZFZu^#{_2R-Y^N0A zUy#J=D3M4CBQY9d;S0+E$RlGX8;sv#%O7D&{kdq1vTucH8I{p_dculh9G`JQ7;*IU zd+2AHw39*NoCcF`d)n>QhPO+$H`$jbr%ILp;_C^7{KI64ea9fwM3tFqm?$cg1Ih-Q zbMz;)J9=*2PD+C5=Sv`pNLQI18*C`6)yi>nrI8DI8QGySy{`1VXmf~nJYF@aYD^(O z)Vo8q#pioXn)R5cy=BJT_QiFF@^xa*hsG1dN@nd1&E{hefhl6p{&4~usHaRSKq~=vKcsN7!y$`Di|>pXv>3tlTTCSjG4k5+h8?9$ zhx)Dy>rb`1Hzk;=KLoI!#K=!_2)SR+&d7<+x&6Jr0X02gA>(YaS2!qy)+-`C@tV#^ z({+}-j2EqUqBGAOIF^UT31jtt1;d&bC#) z%u_$czS=eXfG(dg8s;CFJumDz4EY3mBD}o#v#u5PK_CCy6&GS|U^gSft8V+@Gn1mq0LjZj<7cJZ9oXmAt&J@@yt^rx%A<>0Goc| z--I$Aunah9g+#p)QrABhkYb;bCl|K+U7>7LuZ32=B1fAHK`SjeX4XY*L3l=e6TWNa zWN`~@C?~qXJ5;s72`*1t{8ntb@+jOYD76pY`rylji?eps4_E{2#$S%-i^xoh=lhAd z5RZ@COqheKsYy}^w22HfsXX0J@Bu(lCP=PfAcDnZeT~tB0J>mKh+PzL&S&!SfYm}; zM~HLS4{Kn-bb$@jQ%jM4l8~SEF}tbSmUp7q=&2m_ zVRQ+DWzcmSNOUnjx;5emfWsBA(0&w1T`Khx2{0lqkXjxyWPoYCk5-$A2Lm3fOg(fA zU|cFrtW(f{X4GAowChv8omDejwx8YZe&x)$NN5&w5(GVKA>bx8~O}w+$b|j zIq?|%FUj?owsop1%&nlh3;9@Nxs;{!%VJMULRDG#CO@gr9VX5wN59hGvI_c(Uj}X~ zccOI%0S|834B+GWGKIK=>?8n2@Z#x+KUdl zYo*aup$0S+2*&JRcmR)f3 z_pS@uY9whnK35uIbBs1Fw{h+x!@QO|9@Ig&mfd#u-B+h+#@z zkq3{Sr)M>(zKRK@12p$X{LO;9x2Evs@&=*gc(+yX&lFs~?TdX{Br(XRfCG`8gg<;qK188hWHyR>$oJV zgRA^uQ}(#S{0=;%g8DO*wTwBk9~I_vVBNSW>W^_bg4KRx=J9-*U<~Q&2rV23#@SIG zLX0fekA*g6OC=2`i=C@~_rHW-%n}xFVq0?Y-E;Pz}ORMz}JP5 zoDtgZhHgr(j$@66x8+yoQ_Z&kBV&!vL++sd@%FvD8#0Nvx>4VmT%A2xiQtg}~gI2`}HtTaH2Lml#SE#2j zbJTA;S=e&WACr_D3wZ0+qb!z;!gR(DibbGYY!`O`aH-~X;ZIG2=CTo~h7IaI7MZ)$ zt?K0-euiX{HhR?7dz2ZuVGZZ*x>ict1et260Kw6bk>Z&Qa}J`T%8jHECwSvS&wS@$7X;sR0T zmZNGl_)aQ;;)sc(Pk`;z_C|n%PeP)6-yHk*0=s1%fwd>VAp6uGKH-$btdeWF;Zt{l zo!Q|;%Xzc@*4aL;tdx`#4HY>7;H(}3X*?`g#-aOYyZS)7K)$ZfxnRsKby6-uh2Cu6 zf5i1k5@aXmJFbd!R(D$&w2RCSi8fq$I1a3E-u~#Kcy+O(k{~&Ng&MK_p>mu!Myfjk z+tq)Swx_~&$FbpLq}Z&HDq(2&A$f@-cT1=f+PkZbz(cw?(QmYE9@R@Q83bu5?c_0% z%wT^h2I|&=Beh*Tqfq7nfOwbWjuWtIlY^}@zD4&A3aD@Y9-a}6owDPME}c~n|3?fT z`f7AvyyqfV5Ag?gud8TwZw71PKEgq+5Zl(LlAeL)WW3$F$UA`hx9-x((`C)U3Z5<0K$Ejp!SotDq&bA3|kQGU>L2l(>p zJ_}O`A&r2#&<7~80d+y$zA59csSlQ4h4x;tsBf(#!i_Gin33ZTvwByZ%@rbyDWX++4w-Qugx(Xg)Nf7E-65hi#PJ#uLHMIbC z7n3m4>a+$p)s`&9@k8*`Q7lcccI}{P6*Ao+!n*lsKs|WNeR!oVRA)sIrw^FV0$o zO$&{&SedssocnF+?Fq{*)#vBu-98!s+j&GQXlS6t?|XULL0JVbR17s;;|u|c=q{t1 z{s4f`p?)BK2j_waNj$T-Lar(Rn_l z+j+6Zg{mG&d}{o2!TF_0DK=TTk;In~y&o3?{mE%-QI@LkJfnxVgM$aWLlD`@=HRmdY=*P!m1vAz<%vr(tRRT19-?X(bU#%Uk5G?@fnE!f72dncMROI)-r*-W%v5EY z6gLibBBQMmb_cczxn2^H{-h#35_VCfHX95Wbkd(RKPeITkfVeuJ|w6? zR}`NpH+=Nsa1A3*rN~ew+r83A%Spv?$y^6 zNchtlt*zJJ#)w|ZsRi}BH&R4RAl$rOOo3VC0=MOq^+eFX3NVpa)pv@Kj|zhB!V2Ux zBPUV|6e4}GQF39~WX8eHr5N9c)o4TynX9YzUx4ODYD0P$r!v0^0lj*Do})2Ml0F{5 zfGiv`0+wmj`)r5Hik(+rRKNw@5u&l&i)FQkHw%Ju-fX`t)lwhsh|sLm*G%S{>{ppl z8;A@}VO@y0zd#0cmD7vAO^OFh&D&vjhN&JIkWa5p*)(Ki@I4PET|f%`=b98ml5I^q zo=0tQ_{0~0bU~cuJ+HvgLCK~<-p_=rOMI%x$DbgymM3MTln!X*59-~8QdS)U!fzPt zJj#>ml|wk}iOrc}APqmh5)r$H6UU4ae@#e)VM!KeNwL$^q2JKzn)s?9K{#k&I?`!r z#S{!|zp<0-v?#N@OqyBzeJjDi!?UrrZ7CyvylLHo}J_LbI|1i zRay)X2nqOq7E`~m>TSRS9P|8o|ChA-uV(+9wEEZNUnAH17cc*1_7|b`dxT%J|4YUF zOHcie@oQm+x65Br=>KH#OI`hs@yjCl?ed*?`ac=}(pvvx{4ySRyL@N-TY3FgnEy_B z{Tl`d@IS!((qX@cdauL&=a$}XA=jUv{!Nwr9{as6^*46N+r9oDKkct=zE`Qfhkmc9 z`wdO`){_2@(0|j{y~lm8{{!;h8|d$W-#5g513MxA z0r=nBWADM=H!OaGGok$-z<*bb-Y@XJZu@(Ih_^pY{KqfyT^08|^85PMZ{$ww|1I+W zyzlp^^>6fC{6C=od!qe*>+e&8-{3sNe*%A(D7=S$AH4jAet0`G|Kt7sYbYZv2KrW0 T002OIdvv~aV#R)CFaZA#(l@eV From 0711537dd8a931a3711701e64bcde2979887611a Mon Sep 17 00:00:00 2001 From: Henry Date: Sat, 2 Mar 2019 15:17:26 -0500 Subject: [PATCH 17/26] change lowTargetHeight to targetHeight in distance calculation --- src/main/java/frc/robot/commands/DriveToTarget.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index 383c061..13e04aa 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -121,7 +121,7 @@ protected void execute() { xOffset = contour.tx; double yOffset = contour.ty; - double distanceFeet = (lowTargetHeight - cameraHeight) / + double distanceFeet = (targetHeightgi - cameraHeight) / Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; //Set the setpoint for distance relative to current position From 9d70ab01f953486a181aa2e24cc8f30dae845f4a Mon Sep 17 00:00:00 2001 From: Henry Date: Sat, 2 Mar 2019 15:18:27 -0500 Subject: [PATCH 18/26] fix typo --- src/main/java/frc/robot/commands/DriveToTarget.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index 13e04aa..dfa7047 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -121,7 +121,7 @@ protected void execute() { xOffset = contour.tx; double yOffset = contour.ty; - double distanceFeet = (targetHeightgi - cameraHeight) / + double distanceFeet = (targetHeight - cameraHeight) / Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; //Set the setpoint for distance relative to current position From ac93cd6559bdde750704ed6c40ba80d287be456f Mon Sep 17 00:00:00 2001 From: Henry Date: Wed, 6 Mar 2019 16:22:24 -0500 Subject: [PATCH 19/26] Improve documentation and refactoring --- src/main/java/frc/robot/commands/DriveToTarget.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index dfa7047..91386ae 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.subsystems.Drivetrain.Gear; import frc.robot.utils.LinearOutput; @@ -11,6 +10,10 @@ import frc.robot.utils.limelight.Corners; import frc.robot.utils.limelight.Limelight; +/** + * A command designed to approach a target perpendicularly which the robot is already pointed at. + * All units are in feet and degrees unless otherwise notated. + */ public class DriveToTarget extends Command { public enum TargetType { @@ -26,7 +29,7 @@ enum Side { //Camera Parameters - static final double cameraHeight = 16.125; + static final double cameraHeightInches = 16.125; static final double cameraAngle = 30; /** @@ -62,7 +65,7 @@ enum Side { Contour contour; Side side; - final double targetHeight; + final double targetHeightInches; double initialSkewAbs; double xOffset; double distance; @@ -73,7 +76,7 @@ enum Side { public DriveToTarget(TargetType targetType) { requires(Robot.drivetrain); setInterruptible(false); - targetHeight = (targetType == TargetType.kHighTarget) ? highTargetHeight : lowTargetHeight; + targetHeightInches = (targetType == TargetType.kHighTarget) ? highTargetHeight : lowTargetHeight; } @@ -121,7 +124,7 @@ protected void execute() { xOffset = contour.tx; double yOffset = contour.ty; - double distanceFeet = (targetHeight - cameraHeight) / + double distanceFeet = (targetHeightInches - cameraHeightInches) / Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; //Set the setpoint for distance relative to current position From 80af2037eeaefb1b02ffd808b33c9453c372793e Mon Sep 17 00:00:00 2001 From: Henry Date: Wed, 6 Mar 2019 16:25:18 -0500 Subject: [PATCH 20/26] Improve Corners.java documentation --- src/main/java/frc/robot/utils/limelight/Corners.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 42ef1f7..f4d805b 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -33,6 +33,13 @@ public Corners(double[] xCorners, double[] yCorners) { temp.add(i, new Point(xCorners[i], yCorners[i])); } + /** + * This block maximizes certain functions (x+y, x-y, -x+y, -x-y) over the temporary list of points. + * Each function corresponds to the location in the image the point lies in, i.e. the point with the greatest + * x+y is going to be the bottom-rightest point in the list. Note, the coordinate system of the limelight is + * origined in the top-left, with an inverted y-axis. + */ + topLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)); bottomLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)); bottomRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)); From df54caca7c2c7f198fa8a60ba76eb01d56961919 Mon Sep 17 00:00:00 2001 From: Henry Prickett-Morgan Date: Wed, 6 Mar 2019 20:10:04 -0500 Subject: [PATCH 21/26] Tuning DriveToTarget --- src/main/java/frc/robot/OI.java | 19 ++-- src/main/java/frc/robot/Robot.java | 5 + .../java/frc/robot/commands/AimAtOffset.java | 106 ++++++++++++++++++ .../java/frc/robot/commands/AimAtTarget.java | 21 ++-- .../frc/robot/commands/ApproachTarget.java | 3 +- .../frc/robot/commands/DriveToTarget.java | 87 +++++++------- .../java/frc/robot/subsystems/Drivetrain.java | 8 +- .../frc/robot/subsystems/DrivetrainGyro.java | 4 +- .../frc/robot/utils/limelight/Corners.java | 4 +- 9 files changed, 187 insertions(+), 70 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AimAtOffset.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index d27dab8..3eff03a 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -6,9 +6,8 @@ import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import edu.wpi.first.wpilibj.buttons.Trigger; -import frc.robot.commands.*; -import frc.robot.triggers.*; -import frc.robot.commands.ElevatorPreset.PresetHeight;; +import frc.robot.commands.AimAtTarget; +import frc.robot.commands.ApproachTarget;; public class OI { Joystick leftJoystick = new Joystick(0); @@ -27,13 +26,13 @@ public class OI { static final double xboxDeadzone = 0.25; public void setUpTriggers() { - elevatorTrigger = new ElevatorTrigger(); - elevatorTrigger.whenActive(new ElevatorJoystick()); - high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); - medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); - low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); - stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); - climbMode.whileHeld(new LevelRobot()); + // elevatorTrigger = new ElevatorTrigger(); + // elevatorTrigger.whenActive(new ElevatorJoystick()); + // high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); + // medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); + // low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); + // stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); + climbMode.whenPressed(new ApproachTarget()); } private double getJoyY(Joystick stick) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb05dee..e2fae63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import frc.robot.subsystems.DrivetrainGyro; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.RollerIntake; +import frc.robot.utils.limelight.Corners; +import frc.robot.utils.limelight.Limelight; /** * The VM is configured to automatically run this class, and to call the @@ -87,6 +89,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) + SmartDashboard.putBoolean("Left", corners.topRight.y > corners.topLeft.y); } /** diff --git a/src/main/java/frc/robot/commands/AimAtOffset.java b/src/main/java/frc/robot/commands/AimAtOffset.java new file mode 100644 index 0000000..5946d64 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAtOffset.java @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.commands.DriveToTarget.Side; +import frc.robot.utils.limelight.Contour; +import frc.robot.Robot; +import frc.robot.utils.limelight.*; + +public class AimAtOffset extends Command { + static final double aggressiveSkewThreshold = 100000; //TODO: Tune + static final double maxOffsetAggressive = 20; //TODO: Tune + static final double maxOffsetNormal = 15; + static final double maxOffsetDistance = 6.0; + + Contour contour; + Side side; + final double targetHeight; + double initialSkewAbs; + double xOffset; + double distance; + + static final double cameraHeight = 8.375; + static final double cameraAngle = 30; + + static final double lowTargetHeight = 28.337; + static final double highTargetHeight = 35.962; + //Full power at 15 degrees + double p = 1.0/50; + //We want to overshoot/not settle this turn + double d = p * 10; + double maxOutput = 0.3; + //We only need this to roughly align, this gives time to either stop early or accelerate past + double acceptableErrorDegrees = 5; + + //Initalize a PIDController with a 10 ms period + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01); + + public AimAtOffset() { + requires(Robot.drivetrain); + targetHeight = lowTargetHeight; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + contour = Limelight.getBestContour(); + if(contour != null) { + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) { + side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; + } + + double yOffset = contour.ty; + xOffset = contour.tx; + + distance = (targetHeight - cameraHeight) / + Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; + + double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal; + double additionalOffsetSign = side == Side.kLeft ? -1 : 1; + double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance)); + + double additionalOffset = maxOffset * distanceScaling * additionalOffsetSign; + + pidController.setOutputRange(-maxOutput, maxOutput); + + pidController.setSetpoint(Robot.gyro.getYaw() - (xOffset + additionalOffset)); + pidController.enable(); + } + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all + return Math.abs(pidController.getError()) < acceptableErrorDegrees || + contour == null; + } + + // Called once after isFinished returns true + @Override + protected void end() { + pidController.disable(); + Robot.drivetrain.tankDrive(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/src/main/java/frc/robot/commands/AimAtTarget.java b/src/main/java/frc/robot/commands/AimAtTarget.java index 722fd81..adea9c8 100644 --- a/src/main/java/frc/robot/commands/AimAtTarget.java +++ b/src/main/java/frc/robot/commands/AimAtTarget.java @@ -9,14 +9,15 @@ public class AimAtTarget extends Command { //Full power at 15 degrees - double p = 1.0/15.0; + double p = 1.0/50; //We want to overshoot/not settle this turn - double d = 0; - double maxOutput = 0.5; + double d = p * 10; + double maxOutput = 0.3; //We only need this to roughly align, this gives time to either stop early or accelerate past double acceptableErrorDegrees = 5; - PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput()); + //Initalize a PIDController with a 10 ms period + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01); Contour contour; @@ -28,12 +29,13 @@ public AimAtTarget() { @Override protected void initialize() { pidController.setOutputRange(-maxOutput, maxOutput); - contour = Limelight.getBestContour(); - double xOffset = contour.tx; + double xOffset = 0; if(contour != null) { - pidController.setSetpoint(xOffset + Robot.gyro.getYaw()); + xOffset = contour.tx; } + + pidController.setSetpoint(Robot.gyro.getYaw() - xOffset); pidController.enable(); } @@ -48,17 +50,20 @@ protected void execute() { protected boolean isFinished() { //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all return Math.abs(pidController.getError()) < acceptableErrorDegrees || - contour == null; + contour == null; } // Called once after isFinished returns true @Override protected void end() { + pidController.disable(); + Robot.drivetrain.tankDrive(0,0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + end(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index a3f133c..b5f8a62 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -5,8 +5,7 @@ public class ApproachTarget extends CommandGroup { public ApproachTarget() { - addSequential(new AimAtTarget()); - //TODO: Decide this based on robot state + addSequential(new AimAtOffset()); addSequential(new DriveToTarget(DriveToTarget.TargetType.kLowTarget)); } diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index dfa7047..e599ff2 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -22,11 +22,9 @@ enum Side { kLeft, kRight }; - - //Camera Parameters - static final double cameraHeight = 16.125; + static final double cameraHeight = 8.375; static final double cameraAngle = 30; /** @@ -36,25 +34,25 @@ enum Side { * offsetScalingFactor: The maximum distance at which we will use the maximum offset. Below this distance we scale offset down linearly */ - static final double aggressiveSkewThreshold = 15; //TODO: Tune - static final double maxOffsetAggressive = 25; //TODO: Tune + static final double aggressiveSkewThreshold = 100000; //TODO: Tune + static final double maxOffsetAggressive = 20; //TODO: Tune static final double maxOffsetNormal = 12.5; static final double maxOffsetDistance = 6.0; - static final double lowTargetHeight = 33.893; - static final double highTargetHeight = 41.787; + static final double lowTargetHeight = 28.337; + static final double highTargetHeight = 35.962; //The amount of time the command will continue to run without seeing a contour in seconds. - static final double maxTimeWithoutContour = 0.1; + static final double maxTimeWithoutContour = 0.30; //The PID coefficients of the linear drive. - static final double p = 0.5; + static final double p = 0.075; static final double i = 0; - static final double d = 1; - static final double maxOutput = 0.5; - + static final double d = 0; + static final double maxOutput = 0.3; + static final double minOutput = 0.15; //The proportional constant for the angle adjustment - static final double rotationP = 0.013; + static final double rotationP = 0.005; //The PID controller and PIDOutput. We maintain a reference to linearOutput to set the heading correction. LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); @@ -68,7 +66,7 @@ enum Side { double distance; //A timer which starts when a contour is not seen to determine if we should kill the command. - Timer contourNotSeen = new Timer(); + double contourLastSeenTime; public DriveToTarget(TargetType targetType) { requires(Robot.drivetrain); @@ -84,12 +82,15 @@ private double calculateHeadingCorrection() { additionalOffset = 0; } else { double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal; - double additionalOffsetSign = side == Side.kRight ? 1 : -1; - double distanceScaling = Math.min(1, distance/maxOffsetDistance); + double additionalOffsetSign = side == Side.kLeft ? -1 : 1; + double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance)); additionalOffset = maxOffset * distanceScaling * additionalOffsetSign; } - return rotationP * (xOffset + additionalOffset); + SmartDashboard.putNumber("Addtl Offset", additionalOffset); + SmartDashboard.putNumber("Dist", distance); + + return -rotationP * (xOffset + additionalOffset); } @@ -98,11 +99,19 @@ private double calculateHeadingCorrection() { protected void initialize() { //Drive slow in Robot.drivetrain.shift(Gear.kLow); - pidController.setOutputRange(-maxOutput, maxOutput); - + pidController.setOutputRange(minOutput, maxOutput); contour = Limelight.getBestContour(); if(contour != null) { - initialSkewAbs = Math.abs(contour.ts); + initialSkewAbs = Math.min( + Math.abs(contour.ts + 90), + Math.abs(contour.ts) + ); + //Determine the side by picking which upper corner is higher + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) { + side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; + //side = contour.ts < -45.0 ? Side.kLeft : Side.kRight; + } } } @@ -112,39 +121,31 @@ protected void execute() { contour = Limelight.getBestContour(); if(contour == null) { - contourNotSeen.start(); linearOutput.setHeadingCorrection(0); - } else { - contourNotSeen.stop(); - contourNotSeen.reset(); - - xOffset = contour.tx; - double yOffset = contour.ty; - - double distanceFeet = (targetHeight - cameraHeight) / - Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; + return; + } - //Set the setpoint for distance relative to current position - pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distanceFeet); - - //Determine the side by picking which upper corner is higher - Corners corners = Limelight.getContourCorners(); - if(corners.validCorners) { - side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; - } + contourLastSeenTime = Timer.getFPGATimestamp(); + xOffset = contour.tx; + double yOffset = contour.ty; + + distance = (targetHeight - cameraHeight) / + Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; - linearOutput.setHeadingCorrection(calculateHeadingCorrection()); + //Set the setpoint for distance relative to current position + pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distance); - if(!pidController.isEnabled()){ - pidController.enable(); - } + linearOutput.setHeadingCorrection(calculateHeadingCorrection()); + SmartDashboard.putNumber("HeadingCorrection", calculateHeadingCorrection()); + if(!pidController.isEnabled()){ + pidController.enable(); } } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return contourNotSeen.hasPeriodPassed(maxTimeWithoutContour); + return Timer.getFPGATimestamp() - contourLastSeenTime > maxTimeWithoutContour; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a83f740..faec2b5 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -28,10 +28,10 @@ public enum Gear { , RobotMap.driveShiftHighChannelID); public Drivetrain(){ - leftLeader.setInverted(true); - leftFollower.setInverted(true); - rightLeader.setInverted(false); - rightFollower.setInverted(false); + leftLeader.setInverted(false); + leftFollower.setInverted(false); + rightLeader.setInverted(true); + rightFollower.setInverted(true); leftFollower.follow(leftLeader); rightFollower.follow(rightLeader); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainGyro.java b/src/main/java/frc/robot/subsystems/DrivetrainGyro.java index 197ebec..fa55cea 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainGyro.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainGyro.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; +import frc.robot.Robot; +import frc.robot.RobotMap; public class DrivetrainGyro { @@ -19,7 +21,7 @@ public class DrivetrainGyro { private static final int PITCH_INDEX = 1; private static final int ROLL_INDEX = 2; - private final PigeonIMU pigeon = new PigeonIMU(0); + private final PigeonIMU pigeon = new PigeonIMU(Robot.speedControllerMap.getTalonByID(RobotMap.driveLeftFollowerSparkID)); public double getYaw(){ double[] returnArray = new double[3]; diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 42ef1f7..8bdc415 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -17,7 +17,7 @@ public class Corners { public final Point bottomRight; public Corners(double[] xCorners, double[] yCorners) { - if((xCorners == null || xCorners.length != 4) || (yCorners == null || yCorners.length != 4)) { + if((xCorners == null || xCorners.length != 8) || (yCorners == null || yCorners.length != 8)) { validCorners = false; topLeft = null; topRight = null; @@ -29,7 +29,7 @@ public Corners(double[] xCorners, double[] yCorners) { validCorners = true; ArrayList temp = new ArrayList<>(); - for(int i = 0; i < 4; i++) { + for(int i = 0; i < 8; i++) { temp.add(i, new Point(xCorners[i], yCorners[i])); } From 05786cfb1223fdf378a6770298df806e9d5c02d7 Mon Sep 17 00:00:00 2001 From: Henry Date: Tue, 12 Mar 2019 13:40:47 -0400 Subject: [PATCH 22/26] Add HeadingOffsetCalculator.java to unify DriveToTarget and TurnAtOffset --- .../limelight/HeadingOffsetCalculator.java | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java diff --git a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java new file mode 100644 index 0000000..af4f11b --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.utils.limelight; + +import com._2train395.limelight.api.Target; + +/** + * Add your docs here. + */ +public class HeadingOffsetCalculator { + + //TODO: Make unified target type with heights baked in + + public enum TargetType { + kHighTarget(35.962), + kLowTarget(28.337); + + double heightInches; + + public double getHeightInches() { + return heightInches; + } + + private TargetType(double heightInches) { + this.heightInches = heightInches; + } + } + + static final double cameraHeightInches = 8.375; + static final double cameraAngle = 30; + + static final double maxOffset = 12.5; + static final double maxOffsetDistance = 6.0; + + private static double calculateDistance(Contour contour, TargetType targetType) { + if(contour == null) { + throw new IllegalArgumentException("Contour passed in was null"); + } + + return (targetType.getHeightInches() - cameraHeightInches) / + Math.tan(Math.toRadians(cameraAngle + contour.ty)) / 12; + } + + private static Side getSide(Corners corners) { + corners = Limelight.getContourCorners(); + + if(!corners.validCorners) { + return null; + } + + return corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; + } + + /** + * Calculates the distance to turn past cenete to hit the offset point. + * A positive angle denotes turning in a positive direction. + */ + public static double calculateAdditionalOffset(Contour contour, Corners corners, TargetType targetType) { + if(contour == null) { + throw new IllegalArgumentException("Contour passed in was null"); + } + double distance = calculateDistance(contour, targetType); + Side side = getSide(corners); + + double additionalOffsetSign = side == Side.kLeft ? 1 : -1; + double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance)); + + return maxOffset * distanceScaling * additionalOffsetSign; + } + + /** + * Calculates the distance to turn to hit the offset point. + * A positive angle denotes turning in a positive direction. + */ + public static double calculateTotalOffset(Contour contour, Corners corners, TargetType targetType) { + return -contour.tx + calculateAdditionalOffset(contour, corners, targetType); + } + + private HeadingOffsetCalculator() { + + } + + private enum Side { + kLeft, + kRight + }; +} From f3f9bd54268ea6aa7091dd22c373fbaf3f86deec Mon Sep 17 00:00:00 2001 From: Henry Date: Sat, 2 Mar 2019 13:18:28 -0500 Subject: [PATCH 23/26] Remove unused Limelight lib from build.gradle --- build.gradle | 1 - 1 file changed, 1 deletion(-) diff --git a/build.gradle b/build.gradle index c1e3041..2a7ec8c 100644 --- a/build.gradle +++ b/build.gradle @@ -50,7 +50,6 @@ dependencies { nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) testCompile 'junit:junit:4.12' - compile 'com.2train395.limelight.api:limelight-api:1.0.0' } // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') From 35495928cd4039271bf7f66323cc9007468465d2 Mon Sep 17 00:00:00 2001 From: Henry Date: Fri, 1 Mar 2019 19:24:51 -0500 Subject: [PATCH 24/26] Add more descriptive variable names to Contour.java --- .../frc/robot/utils/limelight/Contour.java | 22 +++++++++---------- .../frc/robot/utils/limelight/Limelight.java | 8 +++---- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/utils/limelight/Contour.java b/src/main/java/frc/robot/utils/limelight/Contour.java index ba4845d..046c6ba 100644 --- a/src/main/java/frc/robot/utils/limelight/Contour.java +++ b/src/main/java/frc/robot/utils/limelight/Contour.java @@ -2,21 +2,21 @@ public class Contour { //Horizontal offset from -27 to 27 degrees - public final double tx; + public final double xOffset; //Vertical offset from -20.5 to 20.5 degrees - public final double ty; + public final double yOffset; //Area as a percent of total image - public final double ta; + public final double percentArea; //Skew of target from -90 to 0 degrees - public final double ts; + public final double skewAngle; //Latency of the pipeline in ms - public final double tl; + public final double pipelineLatency; - public Contour(double tx, double ty, double ta, double ts, double tl){ - this.tx = tx; - this.ty = ty; - this.ta = ta; - this.ts = ts; - this.tl = tl; + public Contour(double xOffset, double yOffset, double percentArea, double skewAngle, double pipelineLatency){ + this.xOffset = xOffset; + this.yOffset = yOffset; + this.percentArea = percentArea; + this.skewAngle = skewAngle; + this.pipelineLatency = pipelineLatency; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/limelight/Limelight.java b/src/main/java/frc/robot/utils/limelight/Limelight.java index 8220405..f97f116 100644 --- a/src/main/java/frc/robot/utils/limelight/Limelight.java +++ b/src/main/java/frc/robot/utils/limelight/Limelight.java @@ -47,10 +47,10 @@ public static Contour getBestContour() { //Check if a valid contour is found if(limelightTable.getEntry("tv").getNumber(0).equals(1.0)) { return new Contour(limelightTable.getEntry("tx").getDouble(0), - limelightTable.getEntry("ty").getDouble(0), - limelightTable.getEntry("ta").getDouble(0), - limelightTable.getEntry("ts").getDouble(0), - limelightTable.getEntry("tl").getDouble(0)); + limelightTable.getEntry("ty").getDouble(0), + limelightTable.getEntry("ta").getDouble(0), + limelightTable.getEntry("ts").getDouble(0), + limelightTable.getEntry("tl").getDouble(0)); } else { return null; } From a7c72af0722df9a2134fe87af9b3fc792bf15dbc Mon Sep 17 00:00:00 2001 From: Henry Date: Tue, 12 Mar 2019 15:45:44 -0400 Subject: [PATCH 25/26] Update HeadingOffsetCalculator to use new limelight interface --- .../frc/robot/utils/limelight/HeadingOffsetCalculator.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java index af4f11b..6b08aea 100644 --- a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java +++ b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java @@ -10,7 +10,7 @@ import com._2train395.limelight.api.Target; /** - * Add your docs here. + * This class is responsible for calculating the heading offset used by AimAtOffset and DriveToTarget */ public class HeadingOffsetCalculator { @@ -43,7 +43,7 @@ private static double calculateDistance(Contour contour, TargetType targetType) } return (targetType.getHeightInches() - cameraHeightInches) / - Math.tan(Math.toRadians(cameraAngle + contour.ty)) / 12; + Math.tan(Math.toRadians(cameraAngle + contour.yOffset)) / 12; } private static Side getSide(Corners corners) { @@ -78,7 +78,7 @@ public static double calculateAdditionalOffset(Contour contour, Corners corners, * A positive angle denotes turning in a positive direction. */ public static double calculateTotalOffset(Contour contour, Corners corners, TargetType targetType) { - return -contour.tx + calculateAdditionalOffset(contour, corners, targetType); + return -contour.xOffset + calculateAdditionalOffset(contour, corners, targetType); } private HeadingOffsetCalculator() { From a7afba8429deb956c03ca2537d8f74b50728a00a Mon Sep 17 00:00:00 2001 From: Henry Date: Tue, 12 Mar 2019 16:06:29 -0400 Subject: [PATCH 26/26] Refactor ApproachTarget --- src/main/java/frc/robot/OI.java | 19 ++- .../{AimAtTarget.java => AimAtOffset.java} | 59 +++++--- .../frc/robot/commands/ApproachTarget.java | 8 +- .../frc/robot/commands/DriveToTarget.java | 128 +++++------------- src/main/java/frc/robot/enums/TargetType.java | 16 +++ src/main/java/frc/robot/utils/Targets.java | 52 ------- .../limelight/HeadingOffsetCalculator.java | 58 +++----- 7 files changed, 122 insertions(+), 218 deletions(-) rename src/main/java/frc/robot/commands/{AimAtTarget.java => AimAtOffset.java} (51%) create mode 100644 src/main/java/frc/robot/enums/TargetType.java delete mode 100644 src/main/java/frc/robot/utils/Targets.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index d27dab8..e98c815 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -6,9 +6,8 @@ import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import edu.wpi.first.wpilibj.buttons.Trigger; -import frc.robot.commands.*; -import frc.robot.triggers.*; -import frc.robot.commands.ElevatorPreset.PresetHeight;; +import frc.robot.commands.ApproachTarget; +import frc.robot.enums.TargetType;; public class OI { Joystick leftJoystick = new Joystick(0); @@ -27,13 +26,13 @@ public class OI { static final double xboxDeadzone = 0.25; public void setUpTriggers() { - elevatorTrigger = new ElevatorTrigger(); - elevatorTrigger.whenActive(new ElevatorJoystick()); - high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); - medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); - low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); - stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); - climbMode.whileHeld(new LevelRobot()); + // elevatorTrigger = new ElevatorTrigger(); + // elevatorTrigger.whenActive(new ElevatorJoystick()); + // high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); + // medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); + // low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); + // stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); + climbMode.whenPressed(new ApproachTarget(TargetType.kLowTarget)); } private double getJoyY(Joystick stick) { diff --git a/src/main/java/frc/robot/commands/AimAtTarget.java b/src/main/java/frc/robot/commands/AimAtOffset.java similarity index 51% rename from src/main/java/frc/robot/commands/AimAtTarget.java rename to src/main/java/frc/robot/commands/AimAtOffset.java index 722fd81..288c826 100644 --- a/src/main/java/frc/robot/commands/AimAtTarget.java +++ b/src/main/java/frc/robot/commands/AimAtOffset.java @@ -1,64 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + package frc.robot.commands; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; import frc.robot.utils.limelight.Contour; -import frc.robot.utils.limelight.Limelight; +import frc.robot.Robot; +import frc.robot.utils.limelight.*; +import frc.robot.enums.TargetType; -public class AimAtTarget extends Command { +public class AimAtOffset extends Command { - //Full power at 15 degrees - double p = 1.0/15.0; + TargetType targetType; + Contour contour; + + //Full power at 15 degrees + double p = 1.0/50; //We want to overshoot/not settle this turn - double d = 0; - double maxOutput = 0.5; + double d = p * 10; + double maxOutput = 0.3; //We only need this to roughly align, this gives time to either stop early or accelerate past double acceptableErrorDegrees = 5; - - PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput()); - Contour contour; - - public AimAtTarget() { + //Initalize a PIDController with a 10 ms period + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01); + + public AimAtOffset(TargetType targetType) { requires(Robot.drivetrain); + setInterruptible(false); + + this.targetType = targetType; + pidController.setOutputRange(-maxOutput, maxOutput); } // Called just before this Command runs the first time @Override protected void initialize() { - pidController.setOutputRange(-maxOutput, maxOutput); - contour = Limelight.getBestContour(); - double xOffset = contour.tx; + if(contour != null) { - pidController.setSetpoint(xOffset + Robot.gyro.getYaw()); + Corners corners = Limelight.getContourCorners(); + + double totalOffset = HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType); + pidController.setSetpoint(Robot.gyro.getYaw() + totalOffset); + pidController.enable(); } - pidController.enable(); - } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all - return Math.abs(pidController.getError()) < acceptableErrorDegrees || - contour == null; + return Math.abs(pidController.getError()) < acceptableErrorDegrees || contour == null; } // Called once after isFinished returns true @Override protected void end() { + pidController.disable(); + Robot.drivetrain.tankDrive(0,0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + end(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java index a3f133c..b3acdcc 100644 --- a/src/main/java/frc/robot/commands/ApproachTarget.java +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -1,13 +1,13 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.enums.TargetType; public class ApproachTarget extends CommandGroup { - public ApproachTarget() { - addSequential(new AimAtTarget()); - //TODO: Decide this based on robot state - addSequential(new DriveToTarget(DriveToTarget.TargetType.kLowTarget)); + public ApproachTarget(TargetType targetType) { + addSequential(new AimAtOffset(targetType)); + addSequential(new DriveToTarget(targetType)); } } diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java index 91386ae..1d7b297 100644 --- a/src/main/java/frc/robot/commands/DriveToTarget.java +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -8,7 +8,9 @@ import frc.robot.utils.LinearOutput; import frc.robot.utils.limelight.Contour; import frc.robot.utils.limelight.Corners; +import frc.robot.utils.limelight.HeadingOffsetCalculator; import frc.robot.utils.limelight.Limelight; +import frc.robot.enums.TargetType; /** * A command designed to approach a target perpendicularly which the robot is already pointed at. @@ -16,97 +18,45 @@ */ public class DriveToTarget extends Command { - public enum TargetType { - kHighTarget, - kLowTarget; - } - - enum Side { - kLeft, - kRight - }; - - - - //Camera Parameters - static final double cameraHeightInches = 16.125; - static final double cameraAngle = 30; - - /** - * agressiveSkewThreshold: The threshold in native limelight skew that determines if we take an agressive appraoch. - * maxOffsetAgressive: The angle offset an agressive approach will target. - * maxOffsetNormal: The angle offset a normal approach will target. - * offsetScalingFactor: The maximum distance at which we will use the maximum offset. Below this distance we scale offset down linearly - */ - - static final double aggressiveSkewThreshold = 15; //TODO: Tune - static final double maxOffsetAggressive = 25; //TODO: Tune - static final double maxOffsetNormal = 12.5; - static final double maxOffsetDistance = 6.0; - - static final double lowTargetHeight = 33.893; - static final double highTargetHeight = 41.787; - //The amount of time the command will continue to run without seeing a contour in seconds. - static final double maxTimeWithoutContour = 0.1; + static final double maxTimeWithoutContour = 0.30; + //A double variable holding the last time a valid contour was seen. + double contourLastSeenTime; //The PID coefficients of the linear drive. static final double p = 0.5; static final double i = 0; - static final double d = 1; - static final double maxOutput = 0.5; - + static final double d = 0; + static final double maxOutput = 0.3; + static final double minOutput = 0.15; + //The proportional constant for the angle adjustment - static final double rotationP = 0.013; + static final double rotationP = -0.005; //The PID controller and PIDOutput. We maintain a reference to linearOutput to set the heading correction. LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); - Contour contour; - Side side; - final double targetHeightInches; - double initialSkewAbs; - double xOffset; - double distance; - - //A timer which starts when a contour is not seen to determine if we should kill the command. - Timer contourNotSeen = new Timer(); + TargetType targetType; + Contour contour; + public DriveToTarget(TargetType targetType) { requires(Robot.drivetrain); setInterruptible(false); - targetHeightInches = (targetType == TargetType.kHighTarget) ? highTargetHeight : lowTargetHeight; - } - - private double calculateHeadingCorrection() { - double additionalOffset; - - if(side == null) { - additionalOffset = 0; - } else { - double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal; - double additionalOffsetSign = side == Side.kRight ? 1 : -1; - double distanceScaling = Math.min(1, distance/maxOffsetDistance); - - additionalOffset = maxOffset * distanceScaling * additionalOffsetSign; - } - return rotationP * (xOffset + additionalOffset); + this.targetType = targetType; + pidController.setOutputRange(minOutput, maxOutput); } + + // Called just before this Command runs the first time @Override protected void initialize() { //Drive slow in Robot.drivetrain.shift(Gear.kLow); - pidController.setOutputRange(-maxOutput, maxOutput); - - contour = Limelight.getBestContour(); - if(contour != null) { - initialSkewAbs = Math.abs(contour.ts); - } } // Called repeatedly when this Command is scheduled to run @@ -114,40 +64,34 @@ protected void initialize() { protected void execute() { contour = Limelight.getBestContour(); - if(contour == null) { - contourNotSeen.start(); - linearOutput.setHeadingCorrection(0); - } else { - contourNotSeen.stop(); - contourNotSeen.reset(); - - xOffset = contour.tx; - double yOffset = contour.ty; - - double distanceFeet = (targetHeightInches - cameraHeightInches) / - Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; + if(contour != null) { + contourLastSeenTime = Timer.getFPGATimestamp(); - //Set the setpoint for distance relative to current position - pidController.setSetpoint(Robot.encoders.getAveragedEncoderFeet() + distanceFeet); - - //Determine the side by picking which upper corner is higher Corners corners = Limelight.getContourCorners(); - if(corners.validCorners) { - side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; - } - - linearOutput.setHeadingCorrection(calculateHeadingCorrection()); - if(!pidController.isEnabled()){ - pidController.enable(); - } + //Set the setpoint for distance relative to current position + pidController.setSetpoint( + Robot.encoders.getAveragedEncoderFeet() + + HeadingOffsetCalculator.calculateDistance(contour, targetType) + ); + + linearOutput.setHeadingCorrection( + rotationP * HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType) + ); + } else { + linearOutput.setHeadingCorrection(0); + } + + if(!pidController.isEnabled()){ + pidController.enable(); } + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return contourNotSeen.hasPeriodPassed(maxTimeWithoutContour); + return Timer.getFPGATimestamp() - contourLastSeenTime > maxTimeWithoutContour; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/enums/TargetType.java b/src/main/java/frc/robot/enums/TargetType.java new file mode 100644 index 0000000..fe04628 --- /dev/null +++ b/src/main/java/frc/robot/enums/TargetType.java @@ -0,0 +1,16 @@ +package frc.robot.enums; + +public enum TargetType { + kHighTarget(35.962), + kLowTarget(28.337); + + double heightInches; + + public double getHeightInches() { + return heightInches; + } + + private TargetType(double heightInches) { + this.heightInches = heightInches; + } +} diff --git a/src/main/java/frc/robot/utils/Targets.java b/src/main/java/frc/robot/utils/Targets.java deleted file mode 100644 index 5935eb0..0000000 --- a/src/main/java/frc/robot/utils/Targets.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.utils; - -import com._2train395.limelight.api.Target; - -public class Targets { - // All constants are in either inches or radians. - private static final double TAPE_LENGTH = 5.5; - private static final double TAPE_WIDTH = 2.0; - private static final double TAPE_ANGLE = Math.toRadians(14.5); - private static final double TAPE_SEPARATION = 8.0; - private static final double TARGET_LENGTH = (2.0 * (TAPE_WIDTH * Math.cos(TAPE_ANGLE)) - + (TAPE_LENGTH * Math.sin(TAPE_ANGLE))) + TAPE_SEPARATION; - private static final double TARGET_WIDTH = (TAPE_LENGTH * Math.cos(TAPE_ANGLE)) - + (TAPE_WIDTH * Math.sin(TAPE_ANGLE)); - - /** - * @param target - * @param targetType - * @param cameraHeight the height in inches of the center of the camera from the - * ground - * @param cameraAngle the angle at which the camera is mounted, in degrees, - * from the horizontal - * @return the distance in inches to the target, parallel to the ground - */ - public static double getDistance(final Target target, final TargetType targetType, final double cameraHeight, - double cameraAngle) { - final double yOffset = Math.toRadians(target.getYOffset()); - final double targetHeight = targetType.getHeight(); - cameraAngle = Math.toRadians(cameraAngle); - return (targetHeight - cameraHeight) / Math.tan(cameraAngle + yOffset); - } - - public enum TargetType { - LOADING_STATION_HATCH(31.5 - (TARGET_WIDTH / 2)), - CARGO_SHIP_HATCH(LOADING_STATION_HATCH.getHeight()), - ROCKET_HATCH(LOADING_STATION_HATCH.getHeight()), - ROCKET_PORT(39.125 - (TARGET_WIDTH / 2)); - - private final double height; - - TargetType(final double height) { - this.height = height; - } - - /** - * @return the height of the center of this target from the ground, in inches - */ - public double getHeight() { - return height; - } - } -} diff --git a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java index 6b08aea..33e86ac 100644 --- a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java +++ b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java @@ -1,52 +1,23 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot.utils.limelight; -import com._2train395.limelight.api.Target; - +import frc.robot.enums.TargetType; /** - * This class is responsible for calculating the heading offset used by AimAtOffset and DriveToTarget + * This class is responsible for calculating the heading offset used by + * AimAtOffset and DriveToTarget */ public class HeadingOffsetCalculator { - - //TODO: Make unified target type with heights baked in - - public enum TargetType { - kHighTarget(35.962), - kLowTarget(28.337); - - double heightInches; - - public double getHeightInches() { - return heightInches; - } - - private TargetType(double heightInches) { - this.heightInches = heightInches; - } - } - static final double cameraHeightInches = 8.375; static final double cameraAngle = 30; + + /** + * maxOffset: The maximum angle offset an approach will target. + * maxOffsetDistance: The distance at which we will use the maximum offset. Below this distance we scale offset down linearly + */ static final double maxOffset = 12.5; static final double maxOffsetDistance = 6.0; - private static double calculateDistance(Contour contour, TargetType targetType) { - if(contour == null) { - throw new IllegalArgumentException("Contour passed in was null"); - } - - return (targetType.getHeightInches() - cameraHeightInches) / - Math.tan(Math.toRadians(cameraAngle + contour.yOffset)) / 12; - } - - private static Side getSide(Corners corners) { + public static Side getSide(Corners corners) { corners = Limelight.getContourCorners(); if(!corners.validCorners) { @@ -56,8 +27,17 @@ private static Side getSide(Corners corners) { return corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; } + public static double calculateDistance(Contour contour, TargetType targetType) { + if(contour == null) { + throw new IllegalArgumentException("Contour passed in was null"); + } + + return (targetType.getHeightInches() - cameraHeightInches) / + Math.tan(Math.toRadians(cameraAngle + contour.yOffset)) / 12; + } + /** - * Calculates the distance to turn past cenete to hit the offset point. + * Calculates the distance to turn past center to hit the offset point. * A positive angle denotes turning in a positive direction. */ public static double calculateAdditionalOffset(Contour contour, Corners corners, TargetType targetType) {