From 1b0c5af86e6681a7ab7746917f75280a507ed81c Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Mon, 18 Jan 2021 10:56:04 -0500 Subject: [PATCH] Make examples runnable, start work on examples (#235) Co-authored-by: Chris Co-authored-by: Matt --- .gitignore | 18 +- gradlew | 0 photonlib-cpp-examples/build.gradle | 1 - photonlib-java-examples/build.gradle | 152 +++++++++++++--- photonlib-java-examples/settings.gradle | 1 - .../src/dev/native/cpp/main.cpp | 8 + .../java/org/photonlib/examples/examples.json | 12 ++ .../photonlib/examples/simposeest/Main.java | 39 ++++ .../simposeest/resources/blue_square.png | Bin 0 -> 1970 bytes .../simposeest/resources/green_square.png | Bin 0 -> 1980 bytes .../simposeest/resources/red_square.png | Bin 0 -> 1979 bytes .../simposeest/robot/AutoController.java | 104 +++++++++++ .../examples/simposeest/robot/Constants.java | 96 ++++++++++ .../examples/simposeest/robot/Drivetrain.java | 132 ++++++++++++++ .../robot/DrivetrainPoseEstimator.java | 106 +++++++++++ .../simposeest/robot/OperatorInterface.java | 51 ++++++ .../simposeest/robot/PoseTelemetry.java | 55 ++++++ .../examples/simposeest/robot/Robot.java | 85 +++++++++ .../simposeest/sim/DrivetrainSim.java | 168 ++++++++++++++++++ 19 files changed, 996 insertions(+), 32 deletions(-) mode change 100644 => 100755 gradlew delete mode 100644 photonlib-java-examples/settings.gradle create mode 100644 photonlib-java-examples/src/dev/native/cpp/main.cpp create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/green_square.png create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/AutoController.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java create mode 100644 photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java diff --git a/.gitignore b/.gitignore index 21cd24014..4f698d642 100644 --- a/.gitignore +++ b/.gitignore @@ -104,15 +104,15 @@ fabric.properties # Android studio 3.1+ serialized cache file .idea/caches/build_file_checksums.ser - -photon-server/.gradle -photon-server/target -photon-server/src/main/java/META-INF -photon-server/.settings -photon-server/.classpath -photon-server/.project -photon-server/settings -photon-server/dependency-reduced-pom.xml +# Temporary build files +**/.gradle +**/target +**/src/main/java/META-INF +**/.settings +**/.classpath +**/.project +**/settings +**/dependency-reduced-pom.xml # photon-server/photon-vision.iml New client/photon-client/* diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 379e268a3..b19336013 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -5,7 +5,6 @@ plugins { id 'google-test-test-suite' id 'edu.wpi.first.NativeUtils' version '2020.10.1' id 'edu.wpi.first.GradleJni' version '0.10.1' - id 'edu.wpi.first.GradleVsCode' version '0.12.0' } repositories { diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 75473cf58..8d8f60228 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,9 +1,42 @@ -apply plugin: 'java' +import edu.wpi.first.toolchain.NativePlatforms -String wpilibVersion = "2021.1.2" -String opencvVersion = "3.4.7-5" -String ejmlVersion = "0.38" -String jacksonVersion = "2.10.0" +plugins { + id 'edu.wpi.first.NativeUtils' version '2021.1.1' +} + +apply plugin: 'java' +apply plugin: 'cpp' +apply plugin: 'edu.wpi.first.NativeUtils' + +// This adds all of the WPILib dependencies. +nativeUtils.addWpiNativeUtils() +nativeUtils { + wpi { + configureDependencies { + wpiVersion = "2021.1.2" + opencvVersion = "3.4.7-5" + wpimathVersion = "2021.1.2" + } + } + dependencyConfigs { + gui { + groupId = "edu.wpi.first.halsim" + artifactId = "halsim_gui" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = "2021.1.2" + sharedPlatforms << "osxx86-64" << "linuxx86-64" << "windowsx86-64" + } + } + combinedDependencyConfigs { + halsim_gui { + libraryName = "halsim_gui" + targetPlatforms << "osxx86-64" << "linuxx86-64" << "windowsx86-64" + dependencies << "gui_shared" + } + } +} repositories { maven { @@ -11,25 +44,102 @@ repositories { } } +ext { + exampleFile = new File("$projectDir/src/main/java/org/photonlib/examples/examples.json") +} + dependencies { implementation project(':photon-lib') implementation project(':photon-targeting') - implementation "edu.wpi.first.wpilibj:wpilibj-java:${wpilibVersion}" - implementation "edu.wpi.first.wpimath:wpimath-java:${wpilibVersion}" - implementation "edu.wpi.first.ntcore:ntcore-java:${wpilibVersion}" - implementation "edu.wpi.first.wpiutil:wpiutil-java:${wpilibVersion}" - implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}" - implementation "edu.wpi.first.cscore:cscore-java:${wpilibVersion}" - implementation "edu.wpi.first.cameraserver:cameraserver-java:${wpilibVersion}" - implementation "edu.wpi.first.hal:hal-java:${wpilibVersion}" - implementation "org.ejml:ejml-simple:${ejmlVersion}" - implementation "com.fasterxml.jackson.core:jackson-annotations:${jacksonVersion}" - implementation "com.fasterxml.jackson.core:jackson-core:${jacksonVersion}" - implementation "com.fasterxml.jackson.core:jackson-databind:${jacksonVersion}" + implementation 'edu.wpi.first.cscore:cscore-java:2021.+' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2021.+' + implementation 'edu.wpi.first.ntcore:ntcore-java:2021.+' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2021.+' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2021.+' + implementation 'edu.wpi.first.wpimath:wpimath-java:2021.+' + implementation 'edu.wpi.first.hal:hal-java:2021.+' + implementation "org.ejml:ejml-simple:0.38" + implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" + implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" + implementation "com.fasterxml.jackson.core:jackson-databind:2.10.0" + implementation 'edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:3.4.7-3' } -ext { - exampleDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/") - exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json") -} \ No newline at end of file +model { + components { + all { + nativeUtils.useAllPlatforms(it) + } + dev(NativeExecutableSpec) { + targetBuildTypes 'debug' + sources { + cpp { + source { + srcDirs 'src/dev/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/dev/native/include' + } + } + } + binaries.all { binary -> + nativeUtils.useRequiredLibrary(binary, 'wpilib_executable_shared') + nativeUtils.useRequiredLibrary(binary, 'opencv_shared') + nativeUtils.useRequiredLibrary(binary, 'halsim_gui') + } + } + } + tasks { + def found = false + $.components.each { + if (it in NativeExecutableSpec && it.name == "dev") { + it.binaries.each { + if (found) + return + + def arch = it.targetPlatform.name + if (arch == NativePlatforms.desktop) { + found = true + def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib' + + def doFirstTask = { task -> + def extensions = '' + it.tasks.install.installDirectory.get().getAsFile().eachFileRecurse { + def name = it.name + if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib'))) { + return + } + def file = it + if (name.startsWith("halsim_gui") || name.startsWith("libhalsim_gui".toString())) { + extensions += file.absolutePath + File.pathSeparator + } + } + if (extensions != '') { + task.environment 'HALSIM_EXTENSIONS', extensions + } + } + + new groovy.json.JsonSlurper().parseText(exampleFile.text).each { entry -> + project.tasks.create("run${entry.foldername}", JavaExec) { run -> + main = "org.photonlib.examples." + entry.foldername + ".Main" + classpath = sourceSets.main.runtimeClasspath + run.dependsOn it.tasks.install + run.systemProperty 'java.library.path', filePath + run.environment 'LD_LIBRARY_PATH', filePath + run.workingDir filePath + doFirst { doFirstTask(run) } + + if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) { + run.jvmArgs = ['-XstartOnFirstThread'] + } + } + } + + } + } + } + } + } +} diff --git a/photonlib-java-examples/settings.gradle b/photonlib-java-examples/settings.gradle deleted file mode 100644 index fe6bf76ca..000000000 --- a/photonlib-java-examples/settings.gradle +++ /dev/null @@ -1 +0,0 @@ -rootproject.name = 'photonlib-java-examples' \ No newline at end of file diff --git a/photonlib-java-examples/src/dev/native/cpp/main.cpp b/photonlib-java-examples/src/dev/native/cpp/main.cpp new file mode 100644 index 000000000..70059c2f2 --- /dev/null +++ b/photonlib-java-examples/src/dev/native/cpp/main.cpp @@ -0,0 +1,8 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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. */ +/*----------------------------------------------------------------------------*/ + +int main() {} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json index 21570e837..fe36863dd 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json @@ -34,5 +34,17 @@ "packagetoreplace": null, "dependencies": [], "foldername": "aimandrange" + }, + { + "name": "SimPoseEstimation", + "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field. Includes simulation support.", + "tags": [], + "gradlebase": "java", + "language": "java", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "simposeest" } ] \ No newline at end of file diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java new file mode 100644 index 000000000..6eb052216 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java @@ -0,0 +1,39 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest; + +import edu.wpi.first.wpilibj.RobotBase; +import org.photonlib.examples.simposeest.robot.Robot; + +/** +* Do NOT add any static variables to this class, or any initialization at all. Unless you know what +* you are doing, do not modify this file except to change the parameter class to the startRobot +* call. +*/ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png new file mode 100644 index 0000000000000000000000000000000000000000..63b52bdd4cc44a0b38b88f4742bc82327052368c GIT binary patch literal 1970 zcmZuyYgiL!79L{M!LFJCWGRX;v7jirfXE6&L*gK!MuLKsmW3c{P`O{Mu@E3lA;B%8 zfPf;Gbl0LHXu&A3G+gA;8WTVv1>_<&2`E7zA_Rl<8`>Yc&$B<~JM+Ego%cP@%y-TR znTI@At}tH#LC{LCeb`|LGC;xo0K5#WG3DYA5Da1ud(xqD*}74%A+l)<8U)?H^oc%r zDcBoE?emX?Afq_^G8i~zc^-n6dwO9s-}8Z*{(in=co5eg)xcS5VYi zo$KEf$mU4{28Wd!Ee?mHWKCv?qYFB;aZj&sD*Ec<9x>XccpK+BUC{}93&ChD)nqv>B86K+o+=wSS7Wmq3+wtzZdaedN|LZ!}Byq}($>Xewj z?9Xlmri3ExzhH}L+Mwf{@I2Ep7I-GexHiNS)^zuy8Veu>sr5VQkehbgEHei-@E4L6gRa3TzIr+$^Q(b$l?}FQyw0M3KJj#eCm>WRkS_4e}j8|n6ql)hZJfvD=?V(=+@xfnoM9Pw6! zw^UlG5s=DQg?Bg7?_J@DGvJ10(n=);JD(_g3Lpn<*m{I#k;i@}B#pIE)&gj|cFY0c zEy-ig2ubVOD7EJ+JzKNuLj!sd}hn zr?gq1caU#Y+kE8y6_T*sH-a`+s^r+vnVF{Qe3%ye#dS6rn>JlIaKq<-R{GYEu_NXY zVi|Y5bpEUK7yRO=2lDL*4?0uGCKFTaR{1nI7@_b@F%Tdz8MyF0a}|(@>cGX8l5ui6 zRshI3LT5P!Z#FqyC?xsXHByYx(tuI$K(T=}PD;(*3DV|AdO;$T>4hhPt)(E*qQtFE z+yIL{Cj>k^@pA{rCU5bSZAS>>uE%kxeE^4izQ!S&KAdI$q#K8N#JIPexEaW@avtDN zS2vI)I9Gm#k%?hvA_N%x>x%+Vf4z#!U6@nLO>p>3!Zv_QUqJB~d@Nk1pZjf^!B37i zZT>;KzBmImxVW`$K7jBXL0RG*+AY@fdb_jg&-e3_e-`Hj*&>9!;VzQHWFn=iRm0xL zPp*AmGyfMMDe9zLmt%wuWz{WmB^iI9Bl`V9lGFGIo(nxz-Y3t5Y2U=iDzaf(NQ@j0 z8}=_Uomf5zu5QaqH_(~Yoho2geE8psv~Twp0$1KI*}HMq9DySq)(D*N(BN?$&lQUn zneH8t)I9)v9`)f<@Zm&PbvdqeNy+MuJ1*hV#)=mtS$GrFRnjsWAZYg`n;d;-)wQlT z!)5QZ>HL9!Act^WMX+!+9ih|d*+)+{v4)0@q4TC=fsT7H1qMM_n1sw6m8n!t& z?~mpWf|HJF&+bNe-{l285s<>_HEb7zw?`(95C2OrMQp5%cK>rgrs-U3JNArM`b1?HVCb;n8;=!32f^XTg4=)QoD4?`hV*;2tR$Gw8aHo%& z_i^%Wk`L)^#8@jwVcS;C&0q>G<9=zXwow1$2bOv+GQ(rG-pyyXtkdPK>r#p$k7_){ z5;t=cx2TFsum|PDYE_J^_7F|pRU6`dG$BDdoR3#kw;V@SAI zQMTM90>=tSOtjQkDCM$hMHdV-l5p9A!fF-+E06{x7}#(4wLNEl%zX2F&-*^_GvAr_ z9sbdfK*Q~3+esvnVQ>%{P9o{i!1EOJ5xAchNs_>#hlK|+NTTQFa{!Q8enlPK|e z;i;Tm^7e?L-d?)2t20sQG--|ut~q~UwZ17pbCwz$#U1v^_5M{ire9_LTl^8V^wH47 z-j1PH=FO{6fO&U)mU+hzZAL0I2phWw4|9*{}#73W7++sLdlas_mTlD z+ep9EJ76H=dUkEv&PwIHE!jk4*~GKc#N-oJ-d=ww!2fx3!FkgBIo7k*`=L$OknJ&d z5xm1quj%q(X4*7w_XS@7aW8r~+$Z<3_(Q1?tmQ}@s7W?G3*I$E&epvPF zT31h4Myji!N-jJajGy`YSRWLYhw?56Sy@!tFGkJYJh+V#mW!ZldB=As#AZZrq5f^O zi5pOVfhIK`A^#N`*^lx}@;BBlYU>X2Bo2(5&K~Hg?=7fEgBR^BG~$l(s)Q^JObhQ2 z{0*=y-%k(WFDo=cb83-+yhF!_cSK89>=>EFnp8SM-W4qc8jH9lH3lKu0ARz|SExzN zL&(NVsoajS4}|$5WP@nwH^#K+0k)(Z>fbJ8srj&QfbHG@^*$nbwWEW^m%y+p|3>M_y<% z>{b$e*ALZ-H8 zYOkhBfuYAtMaYgjoLd9I(4Q4rwmkzr@DEijeuucIo(3Dsy3SDg@q=H&+5yU++ zLh4@!2`Q74fX#_I1=yEXUlBX8;}`)%z92~7a)M2Ow$Ff+{uh%(Si;r&1h9`G!V)BI zM3^bF3)H;DGP;aQlDzwY#DvFhZJHOC?AugGcy8a+_s?Q z#LRD4FkbMZ#9J2&wx4~YQe1)bHpkdoQNPBu+&F}g=ezO^`LOMSRYK`Xbel|SM=in+ zisJnd^3$rgUM@JB);U7Ry3lKBF4Y3q@|#VGAqcZb6_GXESVt5r`uRlrkJv3W~UFMv*%fo@&eDr9|v*~{IT@t4K1cy*sRgRcD)dB3K_IHO$CS?rB1 zdhK}&*Nc;TPIkV)8$s30+%xH@4RHuZn>v^Hri7P(@2=}hyUQVv{5DI+-R#_a7S%%2IYI?x=g)kMHu8Mr$({t=i4Z)&UZwCxq2x?0^J}P| zEEj9|aQTF{o4CLkjuFRv(l{~{0%@-KL6%W$2I`?8DvNh8~0ii^#Q^|F;< zBL2@D^_Z^N&@2RUB6o#)NRp<~N}UxPBx1JA@;oMb(miRCCVHoe#y^?jFMFS30oqe> z?k8EbozkdB~?}S{X zI>*t}I5^0TbTWrNzdV5DnKk_sxA>W~hvM=2f;9cO`tqNaV}Ek^oweT&{A>H2PVTmf ztKYl)WPND8X8_9rlU|$S!}W!nie+d$A{`Be9N(HW@mgrTBg(S(9WL*b$VPeB3hqPc zq-`1AMK5_!#<}cQ;=kG_j7uNL8r_I^+^uZO6IG9QmV)8$blxSsT~aUaR)kH)iZS)8 Zhnpl#gW>q46a@TnNx=ajsECnJ^k0asf)D@z literal 0 HcmV?d00001 diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png new file mode 100644 index 0000000000000000000000000000000000000000..733814f18da08fef826cc287d6639bf02f480aca GIT binary patch literal 1979 zcmZuye>hb68lSPFPIWP3>~5FDoc!3w6Dlje#%RW|Mv)&;#IrN5G77cwbHcEiu2G0K zF*D52z1z0f==zz}%HwV-KZe9GL}+J1(_9WhANo(j*0k+v?|q&;wJxR=@DQFu>gTD7^dA;s3nSjn}ie@$_=@ z)@1gRn|3L1@hhyKb$o8tZ1y2azh6 z)as23jZUTRi@PfXTQ%Wjk4BWld07nPKP)EpWgp|G4bBwT^-va^q%>nOYLvp|{+$&& zx3rX)mMHZ-_BI|QpsBn_ia8$^`r2WN{y+u@R!h@;p*x59nU%<2smRU$CPc3v8cpv# znEhDw#)1%-c%`7m5i10taU3Y+mG2;ry+7703}CfN;)lfr+4U>{Mx9t2tPk}c%98YE zE&!XUO0#Ehe%knoT0ZytQGX_B4U2p`|D5=#!|j=;4ZfGs7KRQwP+H>Vre9A^G%fqv zXXFsiK>lJxVB@WmOKkAuGdro#SE0h2#0EN_=_8mWQ=Zw$X7wpMxA_~xgwOd=88AT` zjbXtA2dJNh2n_PwcoJTZIWkJ71cVsE`xW_6VefUK4nN8WMh+`OToA$Y?z%NZu-IMK zfb?I)_MQOJa}k*;;6f3rw&p;mnaKB~PfK z#ZbGJa^xZcN!SusU;axk@Arx{4F<047KNX*Im=i_=V?^Un8UpL~7cfdDRznyxV+q0&U$$H)bt$_(a_vZ?n3m zhrY2TiRfv;Vy*?l@5QqphmQ!5D7eYA@dj6tL;TWgno6cbc{c)Y!uc82tmr_Gotx#C z{r~F7TPY{IYut{#vLNKmr2{OX4+U80)EU9a{6dJn??I0pDJ%1$hyxS8UD_u@pscGj zAf1UAae~YR@n_u$CFn+Ow7m)U_$lTvuV8u6x=h0 z(?OsVMd@ndj7)wWCQvzEX0ix_yWEnIjS*%bcVY$xY%FfxIz!t3mpp z7JJnRFMj6Fjm;Zjz`Xs_Ik#d2T48s6^qPdqYOX_g&2kIoWy`UL%yXKLs(MJRx(?A9 z9|=r_3be|MQGgt2k{$uQ+M6R|&tXM5%N6TZptqHSSkP;~E)3Qu9Ky1ell_&L`lJw3 zrp-~9j~MtILqcDyeWc&#I>xgBFS5UD-O6};{AFBPk1>n4X~-hA&v(k^gG?^w%7l=Q z86(a^FF=MjEQv@s|AaO5*`jmXVVIzE(oh*hwww}y zj)>Z01r4W5S>`7F1D8GB82wY7iy^8LPkbP&$5f2Ovq?SBGAE8 zJXZ@wEvZR*1{g1w?SSV>cg-poiGT2{_b_U;nxwyB!@>i@!lt0nV03phZ&Z8o*;~-) zbeKREvPOxNl!t0gC`?EO-8z(cn@nkLUsq&9bRfaJX`|6#)c2rOx+K^Yt+FA(_GlH2 z1Y4t3Ixym_JQ0cr0>^lYJ&b=(E)Q6cU~T-_;y;4tN2E|YGjo;*)<+vy9|2@3SON8f zJAcI5c;{VNsRs+kJyT)ENSHo;>Yo|5ht$rk#=UWAI<+of1zM4`D)oO?Eo*r*wrv@C zHHyiBJhh{S=HVpI02^Lx!*!HTx4t(}hfS!0`3^L}0xeVp?D;Tvp6}yJ!mH@pfz>cy z(v_6QySslYO_tu0RAIZ3_I~?!z<2Cbyld-*Xni98L2GMnP2Q6wY71*deE``J{ZIN?ojV)|9}~D$O>0P!7?r2>J^(SS>9%muK>|*mmLCI~UC; zfQyNKgR1HMZ#)FrV-{-$t6?v|tR4kL87+L8eqWH0)cgLPAmc0dRK_@;T5unnugumh zyXo`IuuHnxlb_TjaZTleYd=`^rV58-O9xy`;Dn)*xzyazv+9b(@wq5. + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import java.util.List; + +/** +* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current +* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain. +* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it +* thinks it ought to be. +*/ +public class AutoController { + + private Trajectory trajectory; + + private RamseteController ramsete = new RamseteController(); + + private Timer timer = new Timer(); + + boolean isRunning = false; + + Trajectory.State desiredDtState; + + public AutoController() { + + // Change this trajectory if you need the robot to follow different paths. + trajectory = + TrajectoryGenerator.generateTrajectory( + new Pose2d(2, 2, new Rotation2d()), + List.of(), + new Pose2d(6, 4, new Rotation2d()), + new TrajectoryConfig(2, 2)); + } + + /** @return The starting (initial) pose of the currently-active trajectory */ + public Pose2d getInitialPose() { + return trajectory.getInitialPose(); + } + + /** Starts the controller running. Call this at the start of autonomous */ + public void startPath() { + timer.reset(); + timer.start(); + isRunning = true; + } + + /** Stops the controller from generating commands */ + public void stopPath() { + isRunning = false; + timer.reset(); + } + + /** + * Given the current estimate of the robot's position, calculate drivetrain speed commands which + * will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this + * method. + * + * @param curEstPose Current estimate of drivetrain pose on the field + * @return The commanded drivetrain motion + */ + public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) { + + if (isRunning) { + double elapsed = timer.get(); + desiredDtState = trajectory.sample(elapsed); + } else { + desiredDtState = new Trajectory.State(); + } + + return ramsete.calculate(curEstPose, desiredDtState); + } + + /** + * @return The position which the auto controller is attempting to move the drivetrain to right + * now. + */ + public Pose2d getCurPose2d() { + return desiredDtState.poseMeters; + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java new file mode 100644 index 000000000..243862f63 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -0,0 +1,96 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.util.Units; +import org.photonvision.SimVisionTarget; + +/** +* Holding class for all physical constants that must be used throughout the codebase. These values +* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and +* determine how the physical robot is being built and configured. 2) Read the game manual and look +* at the field drawings 3) Match with how your vision coprocessor is configured. +*/ +public class Constants { + + ////////////////////////////////////////////////////////////////// + // Drivetrain Physical + ////////////////////////////////////////////////////////////////// + public static final double kMaxSpeed = 3.0; // 3 meters per second. + public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second. + + public static final double kTrackWidth = 0.381 * 2; + public static final double kWheelRadius = 0.0508; + public static final int kEncoderResolution = 4096; + + ////////////////////////////////////////////////////////////////// + // Electrical IO + ////////////////////////////////////////////////////////////////// + public static final int kGyroPin = 0; + + public static final int kDtLeftEncoderPinA = 0; + public static final int kDtLeftEncoderPinB = 1; + public static final int kDtRightEncoderPinA = 2; + public static final int kDtRightEncoderPinB = 3; + + public static final int kDtLeftLeaderPin = 1; + public static final int kDtLeftFollowerPin = 2; + public static final int kDtRightLeaderPin = 3; + public static final int kDtRightFollowerPin = 4; + + ////////////////////////////////////////////////////////////////// + // Vision Processing + ////////////////////////////////////////////////////////////////// + // Name configured in the PhotonVision GUI for the camera + public static final String kCamName = "mainCam"; + + // Physical location of the camera on the robot, relative to the center of the + // robot. + public static final Transform2d kCameraToRobot = + new Transform2d( + new Translation2d(-0.25, 0), // in meters + new Rotation2d()); + + // See + // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf + // page 208 + public static final double targetWidth = + Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters + + // See + // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf + // page 197 + public static final double targetHeight = + Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters + public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters + + // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf + // pages 4 and 5 + public static final double kFarTgtXPos = Units.feetToMeters(54); + public static final double kFarTgtYPos = + Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0); + public static final Pose2d kFarTargetPose = + new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0)); + + public static final SimVisionTarget kFarTarget = + new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight); +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java new file mode 100644 index 000000000..cbe1d45bd --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java @@ -0,0 +1,132 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; + +/** +* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor +* controller PWM values which attempt to speed up or slow down the wheels to match the desired +* speed. +*/ +public class Drivetrain { + + // PWM motor controller output definitions + PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin); + PWMVictorSPX leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin); + PWMVictorSPX rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin); + PWMVictorSPX rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin); + + SpeedControllerGroup leftGroup = new SpeedControllerGroup(leftLeader, leftFollower); + SpeedControllerGroup rightGroup = new SpeedControllerGroup(rightLeader, rightFollower); + + // Drivetrain wheel speed sensors + // Used both for speed control and pose estimation. + Encoder leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB); + Encoder rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB); + + // Drivetrain Pose Estimation + DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator(); + + // Kinematics - defines the physical size and shape of the drivetrain, which is + // required to convert from + // chassis speed commands to wheel speed commands. + DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth); + + // Closed-loop PIDF controllers for servoing each side of the drivetrain to a + // specific speed. + // Gains are for example purposes only - must be determined for your own robot! + SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3); + PIDController leftPIDController = new PIDController(8.5, 0, 0); + PIDController rightPIDController = new PIDController(8.5, 0, 0); + + public Drivetrain() { + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + leftEncoder.setDistancePerPulse( + 2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); + rightEncoder.setDistancePerPulse( + 2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); + + leftEncoder.reset(); + rightEncoder.reset(); + + rightGroup.setInverted(true); + } + + /** + * Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign + * new outputs to the motor controllers. + * + * @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward. + * @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise. + */ + public void drive(double xSpeed, double rot) { + // Convert our fwd/rev and rotate commands to wheel speed commands + DifferentialDriveWheelSpeeds speeds = + kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)); + + // Calculate the feedback (PID) portion of our motor command, based on desired + // wheel speed + var leftOutput = leftPIDController.calculate(leftEncoder.getRate(), speeds.leftMetersPerSecond); + var rightOutput = + rightPIDController.calculate(rightEncoder.getRate(), speeds.rightMetersPerSecond); + + // Calculate the feedforward (F) portion of our motor command, based on desired + // wheel speed + var leftFeedforward = feedforward.calculate(speeds.leftMetersPerSecond); + var rightFeedforward = feedforward.calculate(speeds.rightMetersPerSecond); + + // Update the motor controllers with our new motor commands + leftGroup.setVoltage(leftOutput + leftFeedforward); + rightGroup.setVoltage(rightOutput + rightFeedforward); + + // Update the pose estimator with the most recent sensor readings. + poseEst.update( + new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()), + leftEncoder.getDistance(), + rightEncoder.getDistance()); + } + + /** + * Force the pose estimator and all sensors to a particular pose. This is useful for indicating to + * the software when you have manually moved your robot in a particular position on the field (EX: + * when you place it on the field at the start of the match). + * + * @param pose + */ + public void resetOdometry(Pose2d pose) { + leftEncoder.reset(); + rightEncoder.reset(); + poseEst.resetToPose(pose); + } + + /** @return The current best-guess at drivetrain Pose on the field. */ + public Pose2d getCtrlsPoseEstimate() { + return poseEst.getPoseEst(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java new file mode 100644 index 000000000..3b0510ac2 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -0,0 +1,106 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N5; +import org.photonvision.PhotonCamera; + +/** +* Performs estimation of the drivetrain's current position on the field, using a vision system, +* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman +* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at. +*/ +public class DrivetrainPoseEstimator { + + // Sensors used as part of the Pose Estimation + private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin); + private PhotonCamera cam = new PhotonCamera(Constants.kCamName); + // Note - drivetrain encoders are also used. The Drivetrain class must pass us + // the relevant readings. + + // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much + // you trust your + // various sensors. Smaller numbers will cause the filter to "trust" the + // estimate from that particular + // component more than the others. This in turn means the particualr component + // will have a stronger + // influence on the final pose estimate. + Matrix stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05); + Matrix localMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); + Matrix visionMeasurementStdDevs = + VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); + + private final DifferentialDrivePoseEstimator m_poseEstimator = + new DifferentialDrivePoseEstimator( + gyro.getRotation2d(), + new Pose2d(), + stateStdDevs, + localMeasurementStdDevs, + visionMeasurementStdDevs); + + public DrivetrainPoseEstimator() {} + + /** + * Perform all periodic pose estimation tasks. + * + * @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels + * @param leftDist Distance (in m) the left wheel has traveled + * @param rightDist Distance (in m) the right wheel has traveled + */ + public void update( + DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) { + + m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist); + + var res = cam.getLatestResult(); + if (res.hasTargets()) { + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis(); + Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget(); + Pose2d camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse()); + m_poseEstimator.addVisionMeasurement( + camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime); + } + } + + /** + * Force the pose estimator to a particular pose. This is useful for indicating to the software + * when you have manually moved your robot in a particular position on the field (EX: when you + * place it on the field at the start of the match). + * + * @param pose + */ + public void resetToPose(Pose2d pose) { + m_poseEstimator.resetPosition(pose, gyro.getRotation2d()); + } + + /** @return The current best-guess at drivetrain position on the field. */ + public Pose2d getPoseEst() { + return m_poseEstimator.getEstimatedPosition(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java new file mode 100644 index 000000000..c2e151cce --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java @@ -0,0 +1,51 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.XboxController; + +public class OperatorInterface { + private XboxController opCtrl = new XboxController(0); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + private SlewRateLimiter speedLimiter = new SlewRateLimiter(3); + private SlewRateLimiter rotLimiter = new SlewRateLimiter(3); + + public OperatorInterface() {} + + public double getFwdRevSpdCmd() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + return -speedLimiter.calculate(opCtrl.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed; + } + + public double getRotateSpdCmd() { + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + return -rotLimiter.calculate(opCtrl.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed; + } + + public boolean getSimKickCmd() { + return opCtrl.getXButtonPressed(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java new file mode 100644 index 000000000..02f9192b3 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java @@ -0,0 +1,55 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** Reports our expected, desired, and actual poses to dashboards */ +public class PoseTelemetry { + + Field2d field = new Field2d(); + + Pose2d actPose = new Pose2d(); + Pose2d desPose = new Pose2d(); + Pose2d estPose = new Pose2d(); + + public PoseTelemetry() { + SmartDashboard.putData("Field", field); + update(); + } + + public void update() { + field.getObject("DesPose").setPose(desPose); + field.getObject("ActPose").setPose(actPose); + field.getObject("Robot").setPose(estPose); + } + + public void setActualPose(Pose2d in) { + actPose = in; + } + + public void setDesiredPose(Pose2d in) { + desPose = in; + } + + public void setEstimatedPose(Pose2d in) { + estPose = in; + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java new file mode 100644 index 000000000..fa94b5096 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java @@ -0,0 +1,85 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import org.photonlib.examples.simposeest.sim.DrivetrainSim; + +public class Robot extends TimedRobot { + + AutoController autoCtrl = new AutoController(); + Drivetrain dt = new Drivetrain(); + OperatorInterface opInf = new OperatorInterface(); + + DrivetrainSim dtSim = new DrivetrainSim(); + + PoseTelemetry pt = new PoseTelemetry(); + + @Override + public void robotInit() { + // Flush NetworkTables every loop. This ensures that robot pose and other values + // are sent during every iteration. + setNetworkTablesFlushEnabled(true); + } + + @Override + public void autonomousInit() { + resetOdometery(); + autoCtrl.startPath(); + } + + @Override + public void autonomousPeriodic() { + ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(dt.getCtrlsPoseEstimate()); + dt.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + pt.setDesiredPose(autoCtrl.getCurPose2d()); + } + + @Override + public void teleopPeriodic() { + dt.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd()); + } + + @Override + public void robotPeriodic() { + pt.setEstimatedPose(dt.getCtrlsPoseEstimate()); + pt.update(); + } + + @Override + public void disabledPeriodic() { + dt.drive(0, 0); + } + + @Override + public void simulationPeriodic() { + if (opInf.getSimKickCmd()) { + dtSim.applyKick(); + } + dtSim.update(); + pt.setActualPose(dtSim.getCurPose()); + } + + private void resetOdometery() { + Pose2d startPose = autoCtrl.getInitialPose(); + dtSim.resetPose(startPose); + dt.resetOdometry(startPose); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java new file mode 100644 index 000000000..9a0811e1b --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java @@ -0,0 +1,168 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.sim; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.PWMSim; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.numbers.N2; +import org.photonlib.examples.simposeest.robot.Constants; +import org.photonvision.SimVisionSystem; + +/** +* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated +* PhotonVision system and one vision target. +* +*

This class and its methods are only relevant during simulation. While on the real robot, the +* real motors/sensors/physics are used instead. +*/ +public class DrivetrainSim { + + // Simulated Sensors + AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin); + EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA); + EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA); + + // Simulated Motor Controllers + PWMSim leftLeader = new PWMSim(Constants.kDtLeftLeaderPin); + PWMSim leftFollower = new PWMSim(Constants.kDtLeftFollowerPin); + PWMSim rightLeader = new PWMSim(Constants.kDtRightLeaderPin); + PWMSim rightFollower = new PWMSim(Constants.kDtRightFollowerPin); + + // Simulation Physics + // Configure these to match your drivetrain's physical dimensions + // and characterization results. + LinearSystem drivetrainSystem = + LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + DifferentialDrivetrainSim drivetrainSimulator = + new DifferentialDrivetrainSim( + drivetrainSystem, + DCMotor.getCIM(2), + 8, + Constants.kTrackWidth, + Constants.kWheelRadius, + null); + + // Simulated Vision System. + // Configure these to match your PhotonVision Camera, + // pipeline, and LED setup. + double camDiagFOV = 75.0; // degrees + double camPitch = 15.0; // degrees + double camHeightOffGround = 0.85; // meters + double maxLEDRange = 20; // meters + int camResolutionWidth = 640; // pixels + int camResolutionHeight = 480; // pixels + double minTargetArea = 10; // square pixels + + SimVisionSystem simVision = + new SimVisionSystem( + Constants.kCamName, + camDiagFOV, + camPitch, + Constants.kCameraToRobot, + camHeightOffGround, + maxLEDRange, + camResolutionWidth, + camResolutionHeight, + minTargetArea); + + public DrivetrainSim() { + simVision.addSimVisionTarget(Constants.kFarTarget); + } + + /** + * Perform all periodic drivetrain simulation related tasks to advance our simulation of robot + * physics forward by a single 20ms step. + */ + public void update() { + + double leftMotorCmd = 0; + double rightMotorCmd = 0; + + if (DriverStation.getInstance().isEnabled() && !RobotController.isBrownedOut()) { + // If the motor controllers are enabled... + // Roughly model the effect of leader and follower motor pushing on the same + // gearbox. + // Note if the software is incorrect and drives them against each other, speed + // will be + // roughly matching the physical situation, but current draw will _not_ be + // accurate. + leftMotorCmd = (leftLeader.getSpeed() + leftFollower.getSpeed()) / 2.0; + rightMotorCmd = (rightLeader.getSpeed() + rightFollower.getSpeed()) / 2.0; + } + + // Update the physics simulation + drivetrainSimulator.setInputs( + leftMotorCmd * RobotController.getInputVoltage(), + -rightMotorCmd * RobotController.getInputVoltage()); + drivetrainSimulator.update(0.02); + + // Update our sensors based on the simulated motion of the robot + leftEncoderSim.setDistance((drivetrainSimulator.getLeftPositionMeters())); + leftEncoderSim.setRate((drivetrainSimulator.getLeftVelocityMetersPerSecond())); + rightEncoderSim.setDistance((drivetrainSimulator.getRightPositionMeters())); + rightEncoderSim.setRate((drivetrainSimulator.getRightVelocityMetersPerSecond())); + gyroSim.setAngle( + -drivetrainSimulator + .getHeading() + .getDegrees()); // Gyros have an inverted reference frame for + // angles, so multiply by -1.0; + + // Update PhotonVision based on our new robot position. + simVision.processFrame(drivetrainSimulator.getPose()); + } + + /** + * Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the + * robot onto a specific spot in the field (IE, at the start of each match). + * + * @param pose + */ + public void resetPose(Pose2d pose) { + drivetrainSimulator.setPose(pose); + } + + /** @return The simulated robot's position, in meters. */ + public Pose2d getCurPose() { + return drivetrainSimulator.getPose(); + } + + /** + * For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if + * you magically kicked your robot to the side in a way the encoders and gyro didn't measure. + * + *

This distrubance should be corrected for once a vision target is in view. + */ + public void applyKick() { + Pose2d newPose = + drivetrainSimulator + .getPose() + .transformBy(new Transform2d(new Translation2d(0, 0.5), new Rotation2d())); + drivetrainSimulator.setPose(newPose); + } +}