From 80b65741ce1fbb1ee7371ce7872afd34d35273a8 Mon Sep 17 00:00:00 2001 From: Chris Date: Sun, 10 Jan 2021 17:53:10 -0600 Subject: [PATCH 1/8] Not pretty and needs debugging. but it's a start. --- java/sim-pose-estimation/.gitignore | 161 +++++++++++++ java/sim-pose-estimation/.vscode/launch.json | 21 ++ .../sim-pose-estimation/.vscode/settings.json | 17 ++ .../.wpilib/wpilib_preferences.json | 6 + java/sim-pose-estimation/build.gradle | 73 ++++++ .../gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 58702 bytes .../gradle/wrapper/gradle-wrapper.properties | 5 + java/sim-pose-estimation/gradlew | 183 +++++++++++++++ java/sim-pose-estimation/gradlew.bat | 100 ++++++++ java/sim-pose-estimation/settings.gradle | 27 +++ .../src/main/deploy/example.txt | 3 + .../src/main/java/frc/robot/Drivetrain.java | 213 ++++++++++++++++++ .../src/main/java/frc/robot/Main.java | 25 ++ .../src/main/java/frc/robot/Robot.java | 90 ++++++++ .../vendordeps/WPILibOldCommands.json | 37 +++ .../vendordeps/photonlib.json | 35 +++ 16 files changed, 996 insertions(+) create mode 100644 java/sim-pose-estimation/.gitignore create mode 100644 java/sim-pose-estimation/.vscode/launch.json create mode 100644 java/sim-pose-estimation/.vscode/settings.json create mode 100644 java/sim-pose-estimation/.wpilib/wpilib_preferences.json create mode 100644 java/sim-pose-estimation/build.gradle create mode 100644 java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar create mode 100644 java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties create mode 100644 java/sim-pose-estimation/gradlew create mode 100644 java/sim-pose-estimation/gradlew.bat create mode 100644 java/sim-pose-estimation/settings.gradle create mode 100644 java/sim-pose-estimation/src/main/deploy/example.txt create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/Main.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/Robot.java create mode 100644 java/sim-pose-estimation/vendordeps/WPILibOldCommands.json create mode 100644 java/sim-pose-estimation/vendordeps/photonlib.json diff --git a/java/sim-pose-estimation/.gitignore b/java/sim-pose-estimation/.gitignore new file mode 100644 index 0000000..3322773 --- /dev/null +++ b/java/sim-pose-estimation/.gitignore @@ -0,0 +1,161 @@ +# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +.classpath +.project +.settings/ +bin/ +imgui.ini + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/java/sim-pose-estimation/.vscode/launch.json b/java/sim-pose-estimation/.vscode/launch.json new file mode 100644 index 0000000..5b804e8 --- /dev/null +++ b/java/sim-pose-estimation/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/java/sim-pose-estimation/.vscode/settings.json b/java/sim-pose-estimation/.vscode/settings.json new file mode 100644 index 0000000..0a91062 --- /dev/null +++ b/java/sim-pose-estimation/.vscode/settings.json @@ -0,0 +1,17 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + } +} diff --git a/java/sim-pose-estimation/.wpilib/wpilib_preferences.json b/java/sim-pose-estimation/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..0168953 --- /dev/null +++ b/java/sim-pose-estimation/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2021", + "teamNumber": 1736 +} \ No newline at end of file diff --git a/java/sim-pose-estimation/build.gradle b/java/sim-pose-estimation/build.gradle new file mode 100644 index 0000000..0d4b22f --- /dev/null +++ b/java/sim-pose-estimation/build.gradle @@ -0,0 +1,73 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2021.1.2" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project EmbeddedTools. +deploy { + targets { + roboRIO("roborio") { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = frc.getTeamNumber() + } + } + artifacts { + frcJavaArtifact('frcJava') { + targets << "roborio" + // Debug can be overridden by command line, for use with VSCode + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' + } + } +} + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.deps.wpilib() + nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + + + implementation wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + + testImplementation 'junit:junit:4.12' + + // Enable simulation gui support. Must check the box in vscode to enable support + // upon debugging + simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) + simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false) + + // Websocket extensions require additional configuration. + // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false) + // simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, false) +} + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) +} diff --git a/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..cc4fdc293d0e50b0ad9b65c16e7ddd1db2f6025b GIT binary patch literal 58702 zcma&OV~}W3vL#%;<*Hk@ZQHhO+qTVHwr$(CZQFL$+?np4n10i5zVAmKMC6WrGGd+F zD|4@NHj-D$z)bJV;MYNJ&!D%)v-fQ%q0JG$_z5GVUJTPg0MHPf1TvicY#6DXYBBQ4M`$iC~gA;06+%@0HFQPLj-JXogAJ1j+fRqw^4M` zcW^RxAfl%+w9SiS>QwBUTAfuFAjPXc2DHf6*sr+V+jLQj^m@DQgHTPmAb@F z8%GyCfcQkhWWlT31%4$PtV4tV*LI?J#C4orYI~WU(cSR{aEs^ycxY`1>j1po>yDMi zh4W$pMaecV*mCsOsPLxQ#Xc!RXhpXy*p3S2Hl8t}H7x#p5V6G5va4jV;5^S^+>+x&#zzv4!R}wB;)TyU zE_N~}nN>DTG+uZns%_eI=DL1E#<--Sccx30gvMT}^eu`2-u|{qQZ58(rA2aBYE*ZD zm|*12zg*@J$n|tbH%Mp|d|O9W%VT~xG})R=Ld5z<(z%DOO6=MF3Xh-aF%9Hf$?1N9%8Pkev{wun$jZ2 z^i*EhRt8Ve<7`Wyz~iMZDye+XVn}O%qbhV`wHL+%P+n)K&-UMuZw^RRfeQ)%K=k*m zq5l7mf`4K_WkV5B73~MxajljrjGiJqpiV#>0FkyyrB)@HY!;Ln(7JJ*W(>d5#^ubU zVAkTMs*CHzzvUa^nRu0*f-(ek+VZw+@P~}a;;(K=|!9Mhv(~y-mlW);J zb&bB=vySHG`u?j&_6dh^*se*l_B3avjlE|!!Cb0pXyEXRbLy*@WEQ4|)M<`p8Q!rfDJ2RI!u1hPzNjy&)(kcY~GaD6?)7#dCbm`NFh?Y_g$#!+Qrie7%<7P}<-+W@{sxi4JYI{iY zk0(>m$DxOI=~-&eXf2bfh^&(U@o)>(iA1_wJ%B(+nFH+ceib%HEck32QL=J(BNFh`f>St1%llF8chX7#cp*;z}& zcTeXkwsXhf+e;##!FS2yi=2cChcYfzm$wQJ z9%4kAq)wLHf5wfcj!A|xDsAiAOHRzf*)Z-|daN9y5jK-*R{Q0?xaSX-3m|WeuZ`BJ z>eTi@uQ{OGSDIJ#Iu@JPtOy!C?q)g*6SHORg)eAJGh8b-I*X_+xNqZ|OXEsQ-RWte ze`zjjeV9PpE3ac2za+Rs=PA;%QZ>T{x(TRzwWLp_X^2yC-DOEMUy5So!npzL&-@}u z#>uK#&`i&c%J$!bsntEJhY@rF(>6eY;6RoI5Qkn!&<80X5+1(x$T|wR-ad?4N1N^a0)nBj#&EkVvQ?I_+8t*%l#VK&I?uo$ERI1HMu4P2rLMeH%m3 zZ|HA^*O^dA$gb$`Cw;z9?G?m3@nH6TNYJ04Fd-M2wp8@(;vAvJ ztFoni)BLwncQ3@cO*^+6u;(&D<;N;RKb)_NQ_Qu&?@h3MWvo>6FHG%%*smTwj3;dG zQJnT7Wb?4!XmV^>N@ZkA7Jv9kAfD-gCHu2i+!A!}y98SO><8g}t;1JOOxj>#l zM!?y|j5fR3WY2(&_HSGjgMa?Zif<M@d8W z)4>Ptm@zj|xX=bbt$=j}@a_s|xdp6-tRlq6D|xb_;`9oJlkYF1AH%?Pzv$eIAogMi zf(_H*5t({Arfs5XAPj46pjiudQw?dulW-=OUqBVa)OW9E;^R+NDr&LES&m_nmP>Ga zPf)7_&Gn(3v1qu_a^qW9w4#XIEfgiHOQ(LDi=E&(-DcUSfuQE0`ULsRvS}fpS@<)3 z|CbQSi49rU{<4|XU;kiV|C7}Gld$}Yh5YXjg^W$~ovobybuZ^&YwBR^=qP3G=wxhT z?C_5Trbu~95mOoIXUmEOY646_j4ZL)ubCM{qFkl1u*%xs%#18a4!(*b<&edy<8t2w z_zUxWS5fypUp9ue+eswoJSyv*J&=*3;2;q9U?j>n^q?)}c8+}4Ns8oToBJgD;Ug=y zOa0>{VFrLJutjR{PJmm(P9lPzoPi{K!I{l)pGwDy59p-uxHB9I&7zl11lkCu(}*A< zh492AmxsgwEondBpB^{`I*L&Ut40fjM^JS8VdAWQMlwc>_RUM5|Mjes!36DGqW`xs z4tU4`CpOk|vew8!(L}fEvv5&-3#GqZ(#1EZF4ekDQ@y*$tMDEeG?nOUiS-KXG=rAZ zHUDlMo@X&yzo1TdE6b6!s#f{*45V-T3`e2)w5Ra3l>JWf46`v?Y6B&7*1$eS4M(3% z9C~G@N@RXm)8~EXL*9IObA+PwD)`%64fON_8}&pqjrg|2LmP{W^<0@W`9s^*i#F}V;E8~`-}(4@R4kz?t(RjA;y-r%s^=)15%C> zbF;NZET~nybEsmUr8sH^Hgq^xc^n$ZP=GcZ!-X-Go7J4nByj8%?aQ`c{88;p15Kf>|0h+5BLkM&@KI-(flp^npO3MC~W@Uyjv* z6Hu!4#(NtZJ0*;_{8^xcLrC4-zK$BVo7S5V=eg?R8P;BOpK3Xwms+Jt-8R6us zf_rUHFYHn~lu!)U$e$#%UBz7d8YS;mq}xx$T1PIi=4={c-_cY6OVc<=){mOVn>~J$ zW*2PB%*40eE^c+d=PP7J@bqIX_h4u6b6#W|ir<;IlR`#s`Q*_Z8Q?*s_&emuu8D;NSiPX9mK?>$CwcbjhCuv zO&u(0)@}8nZe=Fl*0uMri02oYDjs#g$OHCZ6oTXV2Y0TrZ}+o%{%i)OAJBj2xHC|F5o+`Qmq`$`2EaL=uePwq%k<;6S2n=w%_9vj$8NO|{` zTEg*tK8PU#DnQ#dQ2mMJaaL|HV;BCn?eQ%d0vY@S7Pu@7 zsf5u`T=bL7NfyYO?K^PR_|jap@K|qQ zmO8CK+&O3fzgEnp2|_=^K9ln~QhxjgMM>EQqY@k@@#np@FnZq|C{EyEP7^NurUm0q zW5rKmiy%__KE>YItATyMhE({0%ve10la=mUd<^AcB{T_$Y`2_N-x;F#3xTORXvhPZ7psmqhXy?WxxB5w!m*4&Q;?t$4Kt?m_em-htVDxora24&6~5z$MG(RT{trtp(L( zy&VDT{@p9_DGoq+I|abw$E!TyTO7j6dWQ25dqdKV*z3E?n-p|IG42ZUnNok? zY4K{y{27bUT@#|Zcni!tIgjE`j=-0rl(tVlWEn>5x7BJBkt0iw6j^4n1f2i^6ebo; zt^&Yb##}W0$3xhH&Nz*nANYpO$emARR6-FWX;C?(l7+}<97Ay#!y%BI6^st=LaJ>n zu{ORVJ9%`f*oy85MUf@Fek@T_+ML0-0b$lkEE2y8h%#P^X6+cn)IEXa@T7CQ{fV z-{^wJGN*+T!NsAH@VNM3tWG;%y{pVF2m z2*0+i?o40zSKVq_S18#=0RrJIse+;5cv#a`*`wNs+B%Ln8#e0v^I>7a_33h?lHo14 zg)CbDfGMyH2cj%7C`>|Rrg;U?$&y!z(U10>(dHKQsf9*=z)&@9u@w%y+e@*CnUS|E z*O^cQqM*!sD|e!u(yhXPi$Sl<$daf3sq@Iexafxt3F#2R&=cK z!gT-qto{oVdGUIxC0q`tg)B-Zy(pxGx}&svoA}7p=}jb3jEjQ!v6=afKI!2`&M{#tY$~3LR}#G#U2up2L{} zMGSX>Yjg6-^vWgeX0i;Nb0=gQmYa!|r0rRUshm2+z3AlehjfTqRGnRAmGhHY3`R_@ zPh4GAF@=nkRz;xMO3TPh$)9Iq?Fs5B@~)QIntSyeBy^10!ts?9Z@tK&L6xJd9 zNzaaz6zvrtr&MPQ@UD)njFUtFupwB zv+8%r`c@#asm}cKW^*x0%v_k3faHOnRLt7vzVFlqslue32rt(NNXnkS+fMSM&^u)8 zC`p{on>0pf=1id|vzdTnBLB;v%*ta`o_lzj21u+U-cTRXR%sxE%4k<(bU!orfsJ&v z3FLM2UT_*)BJm1^W;Z{0;z^_e=N&QXSO>rdB`*cp>yGnjHJt$ zcJd~52X&k1b<-`2R{bqLm*E(W{=|-)RTB*i$h4TdV12@beTkR&*iJ==ck*QlFiQ52 zBZ|o_LP06C?Sgs3VJ=oZQU0vK6#}f9gHSs)JB7TU2h~}UVe%unJA!URBgJ# zI~26)lGD4yk~ngKRg;(s4f@PccDZaL{Y=%6UKHl&k|M@Zc4vdx-DX4{belQ);URF? zyxW+|Ziv}%Y!sFdY@YO))Z|f34L(WjN*v#EfZHn6m)X@;TzQ@wIjl4B_TieZY}qY`mG}3VL{w?; z&O>sZ8)YnW+eLuW@rhClOOCZe2YP@4YWKN?P{c~zFUj*U?OayavPUo!r{uqA1<8h! zs0=rKKlwJYk~34F9$q6fQ&jnw_|@cTn{_kA8sUZ#2(Lb@R$NL*u>08yYGx{p6OeX~ zr7!lwGqMSury(v5=1_9%#*MORl2apGf(MQIQTMN35yE3l`^OS7r;SKS6&v-5q}Gw* zNWI*4OKBD&2YbCr8c{ifn~-9w-v+mV49W+k)$jjU@WA+Aok01SA#X$Sspj}*r52!- zNqOS<0%uMUZeSp+*i1TEO$KGKn7EwzW=s?(b5X^@3s5k*80ns2I2|bTHU+bWZ$x;j z`k@>)1G#JgT=F!8awgol?DqK^S4R*g?e}2rOYRVMUKKxSudO(hOLnnL zQqpxPNouLiQFYJs3?7!9f6!-#Pi83{q3-GgOA|{btKup4fYDu-JFOK~Q1c3KD@fdJ z?uABYOkHA^Fc~l0gTAy4geF<-1UqdS=b=UM6Xi30mPhy1-f^aQh9H(jwFl5w*X`Mh z=Ee5C?038GEqSVTd!67bn9*zQg-r8RIH3$$ zf8vWEBbOc`_0U{b)t)Toa~~<7c-K_=G%*iTW^?6mj9{#)@|# zku9R^IDzbzzERz~fpxFrU*it;-Iu&m!CAtM&$)6^2rMyV4 z$+e!$(e)!UY(Sc9n6hkr^n&cvqy8}NfZz+AQc8fU9lNczlP>5D3qzWoR55YvH94^* z-S%SVQ96pK3|Yo`75D&85)xij9Dl8AO8{J*{_yhs-KtsLXUYqwieO(nfrkB@%|OyI>yF+1G?m7>X&djb(HBNNw3KX;Ma*oMV)cV0xzxmIy+5>yz>l_LLH)VyRnYYce zw$?q!hJzX0TlE0+o5QJDM~sPrjVCN7#|32#rUkc>?-eN6Q0RqQTAl~`&isrQg)ass z+x5XapaYh{Dj`+V096?w)w2!Cnmh?x1WmFC$jEFY4;V)XAl3*tBS)V)3TbL)g46_g zCw9pl^!3OCTOcaEP!?==guEAw;VZ}fE6K-;@qD-Rx~td+j(N>)Wv$_mqFTH_wVZNEEuDG!0T`HXLsf+_E=X3lw4`_&d5&YMl%H733ckO){vZm znFLS`;5J#^`5~unet`V#*Y5In3yb|Ax z|A6b^F37!_z$_{6h{7l~<{u7{Fx*A*#zw{GD)6e}n6f<|)&7`S-txiz3Jm4S5hV&8 zm|Ncc{j_~`^pQ*I#w21;(jwi8GnH4efO;R|r4$tH~i;Bcmp^sP9) zjhJne@yzU&XvFNoc~i(wQ?nE`o6Hk~!;x(%xh7?zvigH2g`!v8L-vEN0DvV3?m( zSW(TZ%2AWf`rS}GGMqUj!8yCp#|fR--Vxfj=9}YD97Gocdj=S z0zkF-jsO>EcPTB1zRO$++k^bH%O`=UkHdHT^5?{$)ot<-K2XIE7js*4OjF)BsVjCJ z*KN)!FdM*sh=fB$p8*EzZmGJp?B_=a-90$FI{S$LLjBU$(lxUj;9 zIBszmA*129W+YE;Yy{J~3uyOr<2A(`*cu0IJN#tmUfz2jIWQi_h)_-V6o+5CjbX!1$lz6?QYU za&|O#F%~hmGUhil{M+J|*0<3&{a1%ONp-^!Qx*LOTYY}L!r9BbTxCjHMuUR0E(uH` z!b$*ZMdnB{b2vsb<&P6})+%O=%a8@~$fjbtfF@Z>^Q@enTOJ%VT)Rdc!wX|@iq9i}HaFZAeY6g8xGZY7h-r1sy_<#YU6}I?L zwvf0ePE5PKbK>2RiJOFO5xNhMY+kt`Qi?Oxo&@xH$<^Q;Nb(&rjPBAcv;XtmSY90z z;oIFFl%lDq$o&kYQ;aSHZHD@W({Y1hw<-I>7f_X8wc?%hNDlo~Ig;63RlHNhw~#R3 zA*f5D_Qo`4_ajY4Gr{mLs*(Fxh(U%oua_u3r%`H!TI)@R!!iqV8IOhIOzI@=7QJ=G zV$(9mEVL(7DvPn0j%_cOZN|vvNg8*PHma`6+oS;PDz%iOFyo0n0e%$<#A3r~$=I0T zDL*{AREUGx&C2}?I9cVL`UcPyawTqA4j-4%Mr-4`9#8GX1jiJkKGpHVr1~Rj#zFaZ zqmE!<|1JCi!LDG?1^Ys62xz(p;Uu!QZB7!C0#piy1_9=e?^s@-sd1gs!h$;Q`TNtf z3N4Elsgl#={#U`~&}FNvH78MLjjavl1x*4pNVr338>%sfHu>bxo2#eZN2ee9q#*Jg zDk_=OBR;8t6=pBN0aj)&Nj}pzqqUYW(tfk?bXTdKbNQFSUMCyN-!b0#3?Z;ijzx$M z^Eo6Eq*NO!Y8K;84H4MHj_xwBYc|3>+D(PFj7ejhECG@5@Pk&8dG<)HwwO2~j7KV6 z0$s}=*D;ek#8$a*sxVlC_`qFkM0%BQQ@v2H&Aq@G9XCQt^^x<8w*=MbZV)@aPrrn; z`6r*&f`x&1lp)`5>-|-4%l&W4jy~LydfN;iq?Y8Xx>Sh#2Lx@FXo|5{WKp@y-x;)7 zl;;_Y*-Nu3pcH-)p0(tP~3xO_u~>HpCdEfgyq7V-!ZZ{?`6v_b-vx< zuu|gm5mG6c@D{FYMLuzvG+A2T&6&`n>XM%s`+Qtj)5XdpyFOnz3KLSCOxaCEUl()M z3b~FYqA3FT1#SY{p36h%M^gBQpB2QzEdtM9hMBMRMu{|rf}(;S85&|A!|Aj}?fMKaju!y>_AS}#hRe_!&%8V=6+oPPtE zOOJ-Rcrf>hNq@lG{{@$H?6ikt@!A2OePLe{MBIWSPz7{u(I} z$PXzD;leHG?Xl0FnWt+Wrkrk*|e3P~YVF@N$y&L929cc=#-!*k)HZKDo8!#+t|?9p0z1KSDKclB&M6~hN5<9~^DIltXKR$+iK*h9k$|@Qoy9H}PSI;b(v>w`8(k70@sfa4nRweeiwZ-syP3zPSsyK_8Te9*(FQdm+ z84ZDah4PGehH72w=Q8bx;pK5juT67rJKb|ovD#COI^l6z0eBidn$!Y?T2;5sN+vTV z$`%Edb<%-Oq@NPZy<2Z3m;$}!9JzIuVK6;fJi>>m3q!Lr!2xXRq+l0LvZIR_PNYrP57E#sCvD^4UU2GVr*Rx`QcT}yQanF z3i~!-2Vkk4S%4Hd2baDvrM2g(&1jZaA1!vLi!I#5wX6g^&PE`0-TovM(%wuaPXAno z`a&j{ai=TsgKpc1C3|)tY#!4>SPBbMnchi}glCBwaNE(4`gi}JY0;`|m`s{HtaP@& zHxwCt#2&z9A7O+=v>za}LW~}G>_tWo$dsRX)f1L=+tZF5E&RBA#jUC|N9ZPa_&z5= zekCOsIfOh`p(&S8dnkE~9#(;BAh8qzi5JYT0nP7x&Hga3v`XFdRN|$5Ry#mq*AN$J zV)l~LSq}2d{EJ@%{TLnkRVn*sdM{_b|4!x73|Ux9{%S;FPyhfZ{xg;P2ZmMuA*cMG zipYNeI7{u98`22!_phwRk|lyX#49r%Lq1aZAabxs6MP79J3Kxh0z1E>MzLS6Ee5u+ z@od~O#6yMa;R}eI*a|ZB$ar0BT`%X4+kyxqW4s+D3rV176EAsfS**6-swZ9OIPRZ& zlmIH>ppe;l28`Kd0z(alw^r<%RlDpI6hv)6Gs?GIpffKApgx^)2-6jAzjZE0BtPBC z0z8!#C5AP${zTF$-Z^v%^ie8LI*rvR+*xc=>fa;`SRUSLAio?qL;jVFV1Bw4K>D+i zyEQ}vyG2HTx>W?Ul&MhxUXK7n;yfN)QS`foM!4>4-(PGwxW!^^UyKOz(v+1BejI*& zQSkV|m5=JF4T0k*+|h|3dx`ZKBVX7H4{5iakAxnD#J=9igW@LS;HE_8$lZy1l|$wX zn<8-$u=7&li+^MB(1y~Mz7lj7?oYf%1k{wT#?(Mep094qqnPv7*OYkQ#7$pkU5U24 zzPLEwAb<VIp_uUE~+r5)jt(>>Bg48_{)twH$QJDSBrUS!j{lX z)SK$6dfLWt)c9%Cml+sRp*OHXB?e4hbYZQo!@=6 zBPTpi&6&atD*#Cn6f@5<>79Mq7o0^E!NH)bD26g}?@qg%*AYeE6Tec@F?y9Q8i}^s zz`)l`8>;h75!kL!`&*_hsX1%2)(lWr|7!}@gn%MfwY8vN0=pMm3WesCRv5e*5m4z|u(zbYCpuxO9$bY)hkL|}mRj{3dlRgNK)#PJp#vR=ka^TZ(tKVI<>M~ekIfd2 zm3UDUNW*ZvS5L|SF334|YD>LJk(EqgPpVxtzwclUNaH70zWDVt^1+cz|F?RdF4HHn z@4~Gs`lj!0dWi2n#>7C@B$Qf7|t{1!3mtrO1H7 zi{=I#^Oa1jJiFI!j>PualW+ncHJ)TelW$bv2MqUG1xK7R z%TsQfTn)7D3}XYU+{?Hq!I&fqi4>DmryMiO?!aN!T4fnwq2vsuB^s6fPW@u*h-JwG zNniJFR(RI*?5HV=tqO)lv}CRv_eNEBR%z}Vnftv0+DUH^OCODH#&;{+aw^1vR z-c~|Mk+o?j-^Z+rR4s z-gNA5guTuab7N`{Y@eT&)!xF8#AeetvQ6d!W4BlO;0#0TxS_( zMm-A-u+h7-PjmOQHlh{Hxn+J$jh?uEtc8RG8tu->og@ z86A%eUt+P8E3oLXIrq#K(nCF@L12>=DVT3ec6Vn=B^B;>D=O%op+0BT;T)FHZ`I93 z^5|bpJC_kB92`alM40Am>Yz5o1gxkIGRYQ)x^+R|TCK)r;Qyq6+~S9Uy9nr^nkvc- zxw~#_9eBBJcZNK0yFZxUK4h>u$8;4k-KpNTblRgS(y&u~u&J;O!aqAMYJp+(BED*d z^I#F7vPOEADj}Pziprs=a{%qgz#eso$j`At7pN~bDw%&ba-+4pI}T*?w-z^_~DfD~Z3Tg+#M#u{s&uRF^dr5RFZh7<|WNEG;P z-_SzXTbHc^yD$r;WJqqJkA7^(zN`nzQ5V16nG~Zobuy)a)(T@Ik>V!qOfw;e z)?AZXjzDJg%BkIEY&bm&BczLuWY~k}3Zyx#)jxg1A9R`sz!_dCb!|13b*3PiA@(E6 z9HmG2R>-YrW93UMQO}XE4loI(*er9J*wDUd1se!pzdpoB_v6^lQl}+!6e5MS`+bU#_b*a5Pkt;o+lOV4loyn2P z$3;z-cX>$R{6M4q%b}aMBF}6N+0RCE70bB;XwHV~JLO&!EB)Cgo9ta_>>Os1HNfaY z4PNu7BGhw`6}cm>glh6i^)Ja{rpLHix?C?u;(e&GI{?!E7$9hd*5c^iL?;6Kwn z@qbBE|3UMF|F$Ok>7YY?CeMzMes@CZJQ?&|R8v5M@XvW}jjxhjl`gzl;rvy6Nn9$K z;1TKGpUgZs`vR!t-sD~2ar{58-;2k`H(MIWr_cujtSCpjue(R z(a7R{q`G+;8qD8D1e?1zWv+pPFtk=k#>f`yqZo)3KwCBgABgQbq%hu4q}h+Bdyh?* z#Rlr*$38^Ru%m9FUTQL2Xy^j|f%*4H*{zWFRsMbs6@u{JM{48fq;F;QFV%6Dn!6X0 zEAr2G{RmY8;Jlmws#%7Hl_TvQMbLnN0KGK=9)1u=Vb&#V27UwM#U+)$hn#hlXxBxO zM~<3s(W;fe-0%mVWtZ)oN|h-01@5z=u(z!V>)I9-IepH|_q6NR_DA>2hxGKt-QX;H6(^FXwcBndi1s%qn2sH-rsuON7*ARP6Qt$2XIy3d#cn8sLh&7#USTFn3 zQm-o6-Bnofon2V;oq-v1@Ye@NuH$Z~+th}Cs>F7=H#=4PKLp%-!EwR&0`a}XL=br< zF>&?HNr}9ahB-EA7a({^_6`taBwmB~hJG)p>8r^vq0J_+o`sOq<{s2~2t}W&1f5`l zj;E0nmt?YRp{ONhti9{4&rvt5uoS0CO@%+Yv>+}ROQAGP3VLu^S4fe{ZRoGviEXMF zhM=I=Eg2~^5PIwEq{~Wt?inz13!axZU3knx_)Ey9<)z<=!TnCPHvs1l^spF`@INYQ zY|J1RWri-^D9mVY5Z{u+bXg#}3rUwSXX>&@PN+017W@!L5H8CvZf0wZxQ=UrHJ{Um z$Z;~3t6ARGql*O1^YY(h4awy!h_brE6&k9B&5l;ya>jDyW5?o$q~=1iV!t7#8&QOx6P zhQIm55sij*Ef-G_?k^$AjK2j?=QQ?^=r{MDaGZ7`Yo*Kp1uoZ=&5|O)D#xAHL)n9_l6-E!b zVV@8ny;`XU#X2((4cTmv5unmYzUmJ>Hm+Kvht&a+j3nr!sljTHUZn^0w@L|WKw2TO zRO>T!>jutIzNI5U_KL}vd00oi6$aJqPeJwq)lIr(2Gt#52i@sqCFaWC)pS$pYoRCK zd*$)r6FCClYp+n>gCqVF>x)ghAbl+h${~Mc_sQGk@+sR@b(88l zcx?*Usr}v|kV!RPfS%HK>Bn{7tdEV$CB5Z@=uy4>^(o(%@R|_7dq69s1(X_8szPZ! zSS~$LCX>-}F=io=YcY~9!vqo3&dh9_Mosio`zO6i|$&p;-9%+~sdYNrVE?Q8rS+eHx z4O$l|b3FUT#2jb(WU<`oKAjGQUsoCgE1(c>3byBNPhKeJ7f4S-hBRqRyePY)im;>H z)hyFuFTDqx*ZgXo$hn+u>TGs~=Bjqr3bhPmXG)v8){EU;N*58NKU5;EIZl z9%|JomX+b6M#jS2`B%~!+`EStMD{|y^P=`xPbD$o6;|!((h!+y%7Y{DuC!NCKDIN1 zER-J?vZ$2el4y~!-0vWjNRoC|ARB`IX@M&;?ZpULcAIu`zlH9 z&JK#H);Ij~fqoT{59}OI#ViA%!lPYyd@kHg*hyI;iMdCtw2&eLHOd1*N%2Y!BG*H_ zu@E?VbtZlI{7B{C>A^b3njh=KdF!=rQ!)oIjwkP{t^I{2q&emQ-C1&U&fPC_viACTbT;(A3qRJeGINz^!0N26vQ~o|#pmjp-Zq46%+{X9n zLGKqhLh4`-(*oDHqHU~-45_+pe(BICF$*0jD&FW?ED=vn=t?p9X(%AH9+;6NcJ8JF zASkf}LfT7Z3u*#i$ml`gKIS>3jrTla--x##EDM{w{>Iu9qV!x95ECU*W_O`q>hcCa zswU!;H3R{}(A6aQ(B)lImTF$BzF;$V_?It*+8ZeiZa|b8n_DN4jUfI0jIA6Q6*c0f(uq~DxrNm!$~G=Uz=qP*)?qc(}|7MQZT&B=Um zr{Lj_R7QJAlwD=CoYpjQsUyu1)C9p5CE)%3nb)~WtP;@6(qGG`*qDT zS(zM>&R<;Z23V|80%3s!`0QpTt0Ay;*xLJeE|DP5@x?a!1)`g= z-1}G_LxiiO(*?R*{(yH#&yl|Seyx6*+ETayQtv7Htk3WPvI;U!@h-e$)gw9>pyKmB zk8#$3BF-ou%=`9_3)Q`0ttk$cymvULFS`Khmjes=2(-QY@eVjJ)rSD)z)1No&o+dz zrGItPZ$QuD;Nqt~U{J?9VlM0g{kx!4$?!?=o?um>#7tjMzrLfv<@pI&cp*5H>XPPZ zu8Xh&6y7v0pGDiQqd-~tBjK%-SO8$8kG&44|{09|FO5BoNkV6~JX>g{b#NHJW?gmM# zhbcS|M9fDc44(seG%$hK#va#4YL98mddGDi2qr;@CeiWO!!`DrF<%=_^*3JgoZiSj zdEv30G5`7ex`XP4#6cG;AQ}(|>CcCTGiom^pc*j-Mz1_oGp4iP*>N125YeWCw#L4H z*>u2Ih8jVRJ?rOj-7KbU7KXpYs2UZf)Vf}(lsM(oiB>tgqX2tILJitw_x z&7gq;`b}qrL{lEA3DaXDOi~HQ!^?xxjjVW|#Z+Ek&GKA2dYgO@zB2V*eY zx>@D06X)(FUz3xz99V3v*k7x|wxiFxv>=N$1Chfp>CErJq)gnf=P!u-QKrYnulzdQ zP56u!AH2^QVnuxTJjcQtlflq>PSm4C!$^fv4V_XsIO2d=O8|J`4bUDtjBchJ!14~3 z#mgUPYF*Z?k;Y)Igdx3yQg8L)M=c%}p3!P-0KOuXI+{*LXJ&w)$gzxeTyr`)h-Nc! z`$xa<>T2pbuU0VR?#FPEM44XDRw+cM6U1R2aLQpGHX40=4Er=lp&2aN#P1IA3|r+L z?5jaRyCgN)b(KuS+(x9rPLLjY&4^YY{0T2Ai%`f0p}sG*R!}{DSf7GdPJ=C2MT1ND zUJ@#y06`CNc9n?13R2KY1K*SYeV87wG%bjcIbn+AR8*FS<{?wWomTT5@`}~z3bFAJ zLR-wmE$iwwJ-TnVEhl{{?+??DJ?DWk~VaX-L3-RLtprT2%z-GfD{UVBR~T}zymA0 z6VZ;1Qr%5q#+Oz#3)`D(%WVWWS4BW6%ZvAtt!u25FO@e{X`)_LH>p&pFzx(wvNEO- z!2$Z}`iynmY2j&UCmRNB)9Cn3MXRls&PFVHzkzr;)B^BCMY~6lYY>0rsKT zm4}RV`Q7tbn)Aseay%@-I6ZT~PBsO?D|>kG*%(PGo=|gZ#0zsmE})xxtAvaCe&$1? z(7GyH&^jm!cguuMo@CPA&-lrdE&Aq8GIOuUK9jt{K0ldcvJJp7I`ZMx-EYj$)hl~) zFM!U~HxgO+lb$1cIK-nvz<5OPs(@d4tB6DUa3?-bJ98|dv-kIdtMS;9BuLc{a~_wW zO$u`rNymsAeMH9zh(|w=<*V z&&B{&O0Am`<$iBa)>pNZ6cO`d^3B5%=gmsH(HYZw6!U(c@}#)19F}`BT+yOfamJY$ zYOmy2m^k+ADH2klhAJMLq;6>t3)NREUgk*cjJHg{NBkVhDORNK;v5362&NN=y*Ef- z$vxYTG5Ga{SI&C93^Gsu9G-osqbC9PbsC&@xxGlF?o{!rs9|YpEE?P8ix#yS`7JUy z%ez(_Q%I^RwPrW%rFF(+mE}rp#Wtg@^>O7T(@LFA7j{LNrL=XGDyB-|3<*mqLL_UA zUZz?ulF$5O59-WWZ!d@hRxC@4d6?okW%`1$#<5w9eh>4Cyr#xe5%VPG@TBe#HA^O} z1&q{T_TMTr($f<()ah%TXapiGp}`MAC7>0I=Cx*t+bXy+gMyk*#(A~ft=&4YBdQki zQ}I=c;etc@sD4?l`eYaksPtJnx5OUaZ6u;7p64DUuI`omrWjht5$8+cqb6Hw75WNX z@D(fl7tDl2H)H%QYyX3>cL0*DZPv8+ZgaP7+t_W}wr$(CZQHhO+qUig`^@>y%s1~j z6Y)pXii(P=SQS<4iS=aOnR(rqe#b*BR~GN+bMNQSnhcMHxhVf6D7_zYs}@oo$eK9sZig1_lH0|C z&<1W;8dh6lutS+|02t0VqRfh9R+%!~9YsQ>cw-uGi!YMSo?19?Sty(u{GRqmTx8Zv zLz|nph}CNn+4a~dDzMog(j+NForDvDjLwub!b;p@dLHSBO0kjaI0CPZ)8B2(HNL&A zdr8Pw@u(POF1J*groJ~!1|E(GmnR3L6`P*3C;v?R zDw-pBC=u%}<}P_);mn-_cE}am&b1_WlqnWVzFS;*NhwoOb%+#0nI|H*Bw6_0R(=Kj z;7@eEqYkW2OvWkoz|yY1gZAJw8=>KShthS*ANzYdDT61^AK)>0H%LV4q3}hw?bkA$ zF$tz;<5T59v0Zd$)unmJ{vu_7eGDP6+pe(H&n^3E)g^rB?pn?GT9l1gztAUpR*+Kvt=FE~M zq5rZM&9v>ww1mzrK)vx*0;;?tnqA@Q;FBC@$2~=gy#jW$bAJUNIl_YpT)``*9nnkV zF!&XBK8(PeQfnScH*JaYqy{1bN4MwF=&g2)`!Kuo165*d^1Sc_d{I4>6V=>74c%g4 zXE_M`b@syq%jQx9VRp@ba!rY|MRhr!S3bN!1RT}^I(2gXE`KT57Y;maGA&dHM#`4* zy%@6YB0A6Z^?fg!$4Gq0auM47(jE$Y4osH zhydBwQ-S~vMS7)hg;AC=MRf~AHZu|Ue*bk=ff`!Ol1%=|W-a+~l)QH04q^oeMZHj~ z8$8jQn(n1#O!_7sg1hi;{v%?nd&gK7tfN3I{A0j zcg`ISk^Ir4G=(SvV$v}DE(nE+%rgFkT%cu5VR0Qa^H4-xPC*7Y*+E8#xvyepS#xYE+FyIIi0|5$J%mKAB58%MgleT%Zx42e^L`TdA~Ips z=NvgHNpYZju?*J>oNcmd^(nFUc+-bu4*+9)qIwU^g?1_4-&-`uZm&f7F^1?@3IvJc{gnlh?no$E9jFIfJ8i+33;o-!b2hD@}}{o}J4{l{44v z3Cd{3Lj%9^E43SBXmIvwsA2_8sXgRu=4=H{j9R(fYcCzOXriTZ51l+HcXr@)^?rK* zmc89=w8MW+txdobBh`X4rMvY#vuv0GIEO67sgL}mIw$pNW6s8Fd=t z@58{pFs^Oz&g}CPr8EL~QyUjk&}1qyO4;-6m0MRd4J9T2r5_j+YdeKP%Q+jnWNdV| zUJLU&d%m|g&3B83R^8K^WM{0at+=9UdVAzTnL+CqdcT#($38|-fQ|BJbHY4vk=ANj zvX?ek_oYp6t8bQz-T){|-5OGrv`IGd?>X*h(s{MvQ{j>fZbx<^-)&(j8(N+z^sftB z;V$0+Wd0oUR^&)Q+2bHfLt#V~jZT$UPUbkd#vD#zZJ&huG+-;T%sU~ONA?a`Va|T%I0yd%0*Xr3>p#slVg7Y<6o&Bx856S zg;7Q>mCFF?xq_m}VG5`(0fIX(V=yvQ;xjpwNhrLFMui8xdBw2aFOvI3t6-NG3%+d= z>1un%A{1+tFrn2nu2%`-hiqYhXDga3%{ZVkC@ROtTcA;g*E@K4i_G1&^P#Pl_9*m& zwBVKqZhrf4bhw@M)78cm zBMB!;A)H{6h6AjEv&|DGxYRmY|e_ARf_dMIvm*-i4hR#IU_#A_QYP@L|sHs zo@Ky_Bx6e2??_k;7vjibD#pM*T7`h9V&s(moOn_x^N|9{gkOtFY~gDqSo+7meUjBR zK2jiOsA%PwD|1*KC^m(-WZ5j2AWi;81kCi5t)KouHKt|R6m{m!!n|4YN3yyBo0mSZ zN^yj9>I9Y6dI&$!T7&$%3Ccxua0-&DoNJFbCV%1;h^-U&1Q+@47qrKld+QNGOrh{a z27PfD|L06XuL1+ZMc{_7rB7bd&WD%*lbypj>|K|<#2#t+qPXH zTm`5QC)ktLW5+G&4lhvX8DgOK)|mvQ_b^HuJ&=wP%Z6%;E+Bx|#|Q}vOoGR(jK}sD zk9x4A-V%Hs#G>J5XldT-W&|Kv(!mEi;J38jdK>L|Q7~<_no&|~Fdc~yhC~%VqQc2e z2|pva(YaxgaE`xa5=u=WkhtI|f`XRHhA6|>1`)hDgYzt9kByS$l*OQ2O-a#Iq%SLz zV^&-mn{^KrM6&BueyiV}>&)9rr)de2+DkV8##PSmko(<`nqPVr^n_V~UoIi`_yVdB zzcj4`b5QijKNrR%0AYi<`{NDb!y1^#Pv|K2N8<&wlO7-JDa5Yp?eM)pf>PbMq@)Wr zvki0Y1yLr2WfDb`RBPgq^VC(KH;ofR#9^i$TaMi9J6p5TP5F8<&ofnvL|`*(;urRO z?0k?7WiOd&^v);ux~R9Hznc3moOxE+O$lYV0Ku|hENFV~?Lt!QZlMNp1%d#^Rv!pC zfq`*V)n<`Io8N2XGBOjLYB}#{g#>o-?Hmb6$VyvSN@nI?3{y-pdNvcYe%&%CIeh?s zWfdM@$o~R)P|M>ElHW0BAMI=ozdH-Fle#Dvq-bpmPg-!rDY|1*o|1dvDh9{`{gt%n zFemDyrWMrywXJ+rV5r%UR~0T*75`i&rM4=%7}ulJyHu{rZw;C$r+nn@cLyLgh0d-A z(3SS5tW>ZK0in8bOH$vW>HIcipgUXYGUq49#>Ixff27cCfWz$0vR4Dmq}CBw<~4Sh zDe9adM$vVItE_)3FJT5Bgk}V=1g+Qvf5+hpxwh78gHe$<|r1^Nh?B&_~xSq+nVdY+~dc4GJ?e5EpV zXs-H~6poV`Kh5kok2qSUMD?0&WXKs7T0?Z-J8zti^WD-*_fo zhAqM(p+l2*(|b>aZC+?aK~^_VCZkP0>}TxdEC-KcmAx*YS?wTK?cW>PjS+NxM==Wg zg}e_*NcH%2(J=+WVL+;P)kz0c@48^4ZuemowCO=rriJFSD|#7D2oO{}$kCbL0#0%2 zQe&D2wwJ3%d|+L`bE=&9k_~(BOe$ZFap$YMGL$&$D0=mJ9n%He#RRlC3f=|WyrI0L zA_qS=kzzw8f_QiJYg_b?xA6UgBS0tT_Y$!9>(J-Q|m=O+8+wIPlb5i=-aU~kBf=4dD zd6Q8*EoKqRCcMNO5q%nez-osz1XT6PZ+r7r7A_{!vpDIfE$$yCUU66H>HOUO>u7aE zs*>|KS24COy<^3O^xXssCI`2iF%;A&7{j1UDk9dvv< zsUbj2HMoFr%{j!bRrmyt%jM|4UKza#}%Vf*_fEvi$*6J-h}oRdsdinr_W1-)p24zB*p9tfDdUa27+yi5W`#8+~eE_NyvNZgCP48jF8P; zgYS#IP!@sLe^SeCy4jwre}sC*A4Vk3|EzFISR4QEai+j{bL%-B#Nlt4WJN3eh+Uo) zVtaBF&A%PtbaaH`A~$h0I(5#|WARn>4Hbxy+Jn-$LdJWL+&({?oGdxCC?@gw`D44O zZ)fV$Yi@4u-zGU|!cfh6Eq?2C3Nn%TL2ZoA1+5g5O#q6$QGS|1C!;H{)PU?dDlSGU zLGKxOa;zm!C-Zghet4U7l(%LaEQnKF+>ECNt@`F07q-JO?%%X~*k}Yndc#f*iq0`hgW#iOvymYI0Ur}T;8qZ+%f1paM#v7e! zUS~+CMQqEbYZ%Ix+4iKAGa>>DLya7d_5zQo_zm&bP6F_75Qk^L7A%?p74r#_+3V6R z@m)%h$SZlQi)PpLLYyya^FulLkrPuM%+!YnWBCX|f#M*ph-`6S5IH3F;Os;ZZ&cDq z<~WF?be7SQre3OHq63A%t27ee4>e--Q*N)lFkAI_P@Yoq?Bd0s)IIqLY)xtXU`k>x zfQK0;b2n0v{oPhQju4$`uD>)Syw=X_l}YEfVF8)awhULL-sJNdq;z8~(wyAEW&sDx zxqHk8ufaTXHNnIUP~eE&k>D!g#IVt73wHY+ugJwtuy74u* z1qC32jRV4EWbz*0B5d5qGm7FB;V0Z>C63g4n6hW?!BfHU=hqZbuGx&ccdij#|lWok>4#{m^Fy>{`JdOS zjIM(Tuf4sYrJltP%2vW!U)Mt5hd5_vs^{onYW=T{?nF6taSUF>uPLMY@>8Y#vd&fU zJg$MqI>EOkIj}Gpu%?+k{%zvX7zqvMeuMm%YD6eLoHxL?e6eW>J~|~Z&lHB^r_Ag0 z{*SlMeG(r}i;4UY6e1TDhAnY@tyh=*e7>7?vlwq>&py69o*=hIE389P!iE)Fe1v;HN5fVGS&&jBzQk*Q}Rb%{FF5H zt;vL@*J)TU^_AGy%>+&9)+R@9XQHe9%Cr#w>Q$NM0~WAiktZl>9`I-Ypc0UjVU1rn z_FPNg@88w2iz;NHBJ8)vM$%1oe7QzSs;NxSieG5h->Cq6`M#YqU;tx=1hYym@h%fi zzWLOcEgsbZ>jW|mkR)qpxv-Z}J6iTzy?L3sZiv!nbZ3a;A~Hu3j6-^%FcrouBW^*9 zwOO;eD$2J8edza=ZDF&}5X#=B9O(;A4zyM&5yTvxuoqjP+FZY!ZYI`_D=;czTJF-e z1-$=(BE%9~*+c%p5UT&+n27&>tc8D77L`o(F_e)w^~KRuv4^AdNE-D~2I(p(SCPRP zc{V^gm}JdYd(~~{max0nhdPp5j3){eJ z$LuzR9V>9)451K&?27Aps3vsd_bU(1EDOA~g;@vOO2Ty`4MFO9u=`!_wEKPQp>9L& zzuUbCBGHhsuxYBy-^Uw`)=n5pSF5)!a6qfH$^u&=0GA(}B-Ixjj|ce?Bp(~$q^7BqWU|H8 zKU!?5P@+8*_63=^7)|h<=`vW)2%PZF(`Q0Lr0x5QLjWKIQZB9)OOB_ISy!Mx`E{lJ z1=1d&Ic*{{_h#6sNH^Hz)~vB7gCTbuUkVrOm(pCye57-0NUsKiFMeA#@NBB+F5<+s{(H7mQAPQx`OR z8xRz&uf&f&-?8paW&Q%EHCq$Lv~}lCIW%s>Wxj&$Majn9D~*{Yn8jBZ3b9-fuz!82Hn?&ZI2_JZYAy$kb_?7m*?J z7EcrbL2*)gJ(Wl`yg~c)vC1w>dR$LezB90-T0%EZo|KuQOirNpKJAd) zr+w2F#9m@j64vevMEx_$M}ESx!oajKsI7|Q#c-fWRsS7nAgMlxf$l`eoBx6_u1LP` z5wVEEAYNPN*iXKJza7=aP+z_r$z;5})SQGWl0SrU7qL5T>MpzjZPVq~an6pv29s{gIn1Rh z$*Vp>0p=05JN|HRiyOCbpgpZ@;9Xj|o3DNV!%Xn6t3hE>(=2$dFuEx{osGXYv`m73 z@j>86*-gsSS^3mR)HB6Bj1fy+E{@9e{bcRLU_iAqDzdQUqG)+sqNE`h1 z$3w4loJ+!{F4NdK!E7Vu6L}j5d=VnffP!j5b(b5(u}{;?o9PB`YLsrEsOeE8IUM8F zj!}~kYF^$l^i7CS$AnS+a4#EnWySE!?hNnzWe>=ETyc4WCXpNzZ9R&vLWR9n2)aFS zeT`FE>ZzLpjPr*qdk%A3<`U8cpr3K~?abpqM})l-j}Hz+9tJcw;_-BzCtzpYoNVk^ zd4xI@9~_|+Y_6S*Kx+?A$c)OqC718Wiat0Sl%qFMhix0?j{gw1XO9$zQhjjoeDj|S z8hS*$R7Ol=9=Sd-9s*OgZAC1sMC*(iexn}3CMYJdNZu8^S5)5@Bxo7ayS4fG2D@ns z(Y9t_4DB(20CAx~=eL=RM?RRc4|4V{?Qe z=>g3K7H^2nxwHm|*N+zhk9ET-=0ak5wZAxM<)DFY7|^q+@a_=>AXMj@vZG11mH%nQ zn9XfRt7)!V&u0~v+`DaED;5~WX_cQ6~@iQ$)`#bKdk&+uvYtZMGQ??&zRmpw zbc5donS&q;jPQE_7rh5{ONJKBM;cxKH>r!f)K=VDf}bfc1B4Nv3C}__D{B|kU4Q04E((6!W^q+&Xb=m`c#S!$wEEp4py_0 zDJO?v%A16hzF;#-Lt+DUyec?VXUS?%21=wBiJ<}TTQMa&n$+5wnHr4sni_Hb`tFO; z((Kg?Xh0p)JZnUc=-mE(Ls`z5)+Qr8;F0R92sj9yEJx1kK&wQ8S2S`)h+Qk?^jShBw0n z^g^Pht7xCZvs&|5W95{bypf4acXhX`O_>*QyEk183j48^Ws>JcasVrhs5G9;&2dyi z%>jCf;J1W^x5i(=Cvt|^PAWSdNG}XTJ@;UD+R!_#xn5!VD8@`C$I>Ipes@q*x>0`l z)z8=i*VF~+bxTYjaCr)lzaDau^|9V&q!IlGwQu0TKbn4oBljDL$D`d(xUR1D_M2H5 z_D)E{)YMOgPe9j&Ta=X`w!K8L8Fz1tOon!uWan9)huounS4Mh4dF)BRXPW~rZ){=b z8GKrX8h<5U_7;gkNu2?Vha=mHR?g_-tDJ7e(~;kBqw^DncZb0-heR1$Eu84i7(X`&aR*AQIwovW z>fz)N@L0uBeI%!;>fF*(y?aB?LspSl*h;#V3|hH@lSBCC>z%=##r4vBD?~% zIcaMD#Ep&MMR|QloYSVm4m`6&D~o=K)KUR!2dn`e7}AFYi4ni=M| zwlXp`cKoTc{O?pVGTu@effshzIQL;~Uran3$O8b$6lS*o0sT!BoyZd(zz&P7axA%@Nz)_qI zkD$LWxQoOtM=CJA^aux0eMxT|$TTV{XcUf%R6YWWWpb~~Wr+7tk~!$o(-O!M!{#H? z)jCw2taNz0WO)=*Gud3!7Hi9?DqB;9JQ_pLDASj_PC!c^M|om%q>Zz+S3oK5Y^V&l+!?6vHO@6@c? z%)vqVE`pRD|ItbFC1kt4ApdNC)&9im8NW=RUr>

@up^y4&I8N>~wvL%f(S2W%NN zf&x46sN${5Gh+I9cd>g-O|x3@x#@hdvU54zx*WtnC#5%quWk43w{;_G!4&;N;wy-O z?urjbDnKfp2u4gknf&*wBJS`YfdzBa#pf^Lo9ei}Z)MCk6MP}h0OYrd8`jVipqsRTq}lh>h#|o4yiA zbPQLKXatZ+L=I$?XEGfd7x*_lf|=3xKLi)yj}jQ9pD+OPrv;Mqe+~uywe$sD4D}uV z4@_J6*&E>)?K_L=^f9)ZpbIb0tyI>qF^OuZ;8LrA_T9JRowWUXNjyBVFxj7 zcFv)I!ZI!9%3&ro1=#}qZ!W@`!*%Do@xlC)>lS-KJPYY3@3mXj^ZUgyXXo8DiZ)0M z@ORv8NQ5xIiv%yy7WuvM3l7ZnaX8M-u4s`LZ2-*e2V%BIin4U@4b=3ps|#~L^v#DXv3GDk8H#;lK%qAV<%I5Z8dd3-sIMfqq2WY52;$Y7| zC@8Z_G%EJ3tOhCq_Ad3l4=IN9=Ee$7k#R%^@JPd7SnqL~*a3EWdfPj^Ft)B}bgnkr zBT1I)!g2ha@JU#wQW1op@1SkuaGVJcEJVhstebVvoHV+n`EI?;^p~M~tfk#K1CBi- zF<+3FQvDXkoVE)E6Bj9T)Vlo9rjgCj>S}EH&DnJgn49L@7ZaI=v&F?OY*>NLOQ-u43cR-0P{LGZCyKsW{^hNC8iDiqJ{~) zNqU!S?7Gb=jXSc_T>xTosLbq!#)VKVs^hKlReb|!_v(O0B(=A8tA0Fic+K)>Lc!(J zge-eb*cuWjJCE_q)D}kLQ`X73XAD=didg`EDAk|uw*rjJ1Yj*bj<;`v&pOnps=(g<^CaeJRd*q!NQ`O zTAcA*KCphxtD>M<0l)OpWo@|W=Vs)XFpM7C;96VQR+W3~AXoqC9@yN@7J9kuboR-H zHL8|U?V*D#Jg&`hR95a1#ByH}mfw|kcIP#b2%C}r_nxhIoWdo%k*DB;N)%#~P458H zR&1-?mh?}HxGi(-dh@nkK_H45IB{y)%qwup^p85vZeUpqh|G;9wr%q$_*4*|PS(bw z3$<2M;y;*(WAtHSM--PRyA1<)1Xe^(yuRRaZX9nR0oP5%Wg)P(ak|_q$^7Cd)NP#f zFt*;;hP)je2EkvO_Juc*@6Fd}(xbH@+`c?h1(9yjJzcLY^!{hs3;2?q^IfrF`+D{7 zeAjrrb~tUbxms|met4=I%jCVN6O3DEeY8_%NiNb1EvTu>AI1J!n@36jd$2##c}B>0 z4L;|^v$`6=K#^tk;MTA+ji{smQT)gaODj-((|WI%X2JbpJ46#0RZ&FMJeh+Z<&>04 z)cI;7Dm)CZ1Q9H0Ge@zDXKAsB9dZbg4?1joh3}_)K2k;c^(s6)kl-$}hLll_T0$(y z-4SgpruNv#}%R(l@3!%tj5l!d~Np>{BXo}gF5QWAP7*n?JW-N~>|I~-Sokci&_Ho87f;meu+(2@Yz45X{^W92m`3_^%9FadE5^cGO72ffn`$&G} zGOIPIF?FsLh^0eater8)<@~LjNIyP(W7F~ackhd7ase+Gfo@-RBG6$Q+CeDbE-eiO! z66k;0^Ze3P9kEj(yiZ!_vx)K5>+Jrl2af_iKMbiG*Z6y})9{?`w@LyvBpEEC99HEm z94J&4%248p>c%Nb+Y?Mm9%w8P;5(?F8nINf&_*-><^LeQ6{hj_UPeUhLmtxd+Vmgt zX+WF*G|x;d1!gF0D5?$*b6|tDV#m<_?(f{b+Jd?J92?)y8t>gZ+-KQ+Bj*PJW__xR zdf03Su)GBsi{L~F7m?zTiiu`Wk!YO=QO{H#)PP2?loJ6bfRs0oKxO3+aYm9`#}5V$ z`x646$5C08JvW-c>mV&jy+a+V^zH9IQ#Inj?BmB?I0~jhx7qLD!cSQ9{<) zCB(xvh>|7z&?P1A6fTeZ=vH4`HaRJenyQMrBMl$uNuOX#!uWTr0YsU$pvq9H4wY>t zl^X-E=|ppy073iT6Xv?zU&~*SOz)S{s$uTKR(W@_aAsUm!9UD9D`~`uK!3`Buc{%2B4{J%ioRlMx&#kB{e!Avb zJrlj#<)~p=4r6CfO9_3Cn1xhg=x7nk+LY}yn%fvBEBY;q4p`CSxj7WfX^CU5+@tJWJi(W&KcO*jj5x;xDLZ*AxFvIAYA@P8yW`o)9#pos(U zSgS*I-N9vd=^11lccI*yNQxzMgJ!_I?64MNHZL9-U_DIfm>8g{k^fj)WeFHM8I_z& zZ3l@3<|n0jQSo~R0*Qcqvf~?+vNohOl*bzy=)XeN;2a3p1~0V$$gAWoVuI=*iPkyO z;E~luur&+0{@(mshrT+g9pcf!^T48w$vch$Nigsv6ylw&q=E-ICa#nDgi$8vmBC($ z=yLuLM0U-^2^S`{_ZwTz$|kB|ZzUr`AM@J;{X1nZJEj`$4skl+fss?6#-GZt`JdU# zvVUW}%8!tF0rBe>`+r}#|FsnVkBs^MUX+ze>dHSpWnWVCqdl~T@Zci3NHq%q1q0&Z zjiRz*rIA75MSd&j>=Hq=uts|mK)cc}S884FYT9`Ym2Gbq-?zNU&7M-!u<)j1^s21K z7oJaB$L#M;cjw#E-oI~{yJTr2o((;6binRCTJm*%J0nrPf%?1jgigQI5bI~2dsFN451~NyCYYvfVfu5!YwE`!Uv%`& zB-2spw{|p}vcNP<;@k3}sV|3_r|H|Z4JC9~&KtI*)@JhM?U=mg#m3PjRVoE+M zVYM5uWSO==K5bE81EEz2?F$jdRB^ec45FWK&Dz+e}E=Op=h#{z^;qey2Dx+2Q2qzwA-MpAB% z6U&685w0+}tjouEmcVXOF$U)7w=8u*B7piVzASTr-X|xfrQR1uvc@IZr$CD4MUVF| zMre!R*v|cBT}rB>9#r~c4@(}lBCp$9)X`O$7f_9s)8|{>$Da!Go_qr=;4rtnr7TgXUpffMV9akHEvEw*Z&g!2Env6(!b;)$Zkq!j9UGy>Zopi zUQ<$5Ex<;BxM?&1+E#8>B$er2c?TqH!q^=LX)1lV=@=!xtMbm`$gt70@|} z8AM$V_n1o@=*E15EncO@{DFc)hEBSA@Nbk=GkNsF#}_mBtmF20k$-)eOP+G`q*EAP^>>5d@ea zg6^gb37{ol+=uYC3->5=jbqd}&J|19Oh}yYviQ}E@&>94`r85c>mo=XKA{q~2C*8q z1(8IqD#!fuWdW8DT^RfX)ssdyOzHq^sC=mmY``qcE8^g-o852h1`FBL)_0fHqqzW%Y(brO+X5H!1sl*7|2>*^XZQ^Um1qp- zj{+=uY~SxwTj1)2rmt7luK=kSptJDqqF#W3sech+R{=RBs5U1mcd@_EU~~8?dsmUjsf7tKBg%yZYVwFEDFu zWWQwnb~$%v)IaYXT;h~afPZz{4^@br zn($GS68Obz0BZLqKb0MyvEEp-F z%XZOu9nt29ll>hIY!o7Ulpi znv6Q&d-;x1Q#smNV37IAjmqJ`f>4;j)zs}@5Ggb8NHQ&r9}YcFk1=s0qSmfDIT zL}IzQfY+Hb7z3YWw>3^;vPtIw+@lL;+6f0j=R`K1?Rs$3&Ft1)@NM5zV1L&`Vbl&7 zswRx&Edg?U7fqYMBpWQ6jO&vI*KI5odc0(9&B?LUS$lNhs$&T-QLab-p|8suK`a9N zU;>Q)dneC-M2!FT|4RScQqNRUcScY|-Hb2FWK7ixX)w*zIKVgM!)R>CsoYSb9@Lsy zLJk9)H;@1=N~KM;fxCA80PT1w>bSwB_El6JKa7XzdPVs_qfTy_HegHLC>RgUxX-lj zs_$O^k~(_!_WADl_zRBtc0-mj? zs$_XlVRk8UA;TzI%p`NZo^_F0EiGU(u~@&bF!!jgly!a1es#9LBez7Usio}j;#J*M zYwchj{qF*wFL`?T^AP-=5n(>kT+$T_0iGHp4PM3Z+@Rs&k(ghDz;|7e>IBW%Q&>Q* z*|!8m`k0#8(2SfZzjS1JdAS)iL*a3Q>Tt-uHB0^>6;1Ac&)lXvA#A+^~TF&^<-Px{Arzw?$8;b z6(xcC)ary#!{#M(-LV!}WvwJ94Y}p+dl+)^9$xeZPD9+g#b-y4E)=6{dZvMSy(4bs zQqd@m1o^6YxMp0{hxGGmxj9Cv;|d+QcXE|*vQbI!0Pil2SOuAXlwDZl!rN-01kujv z`f06S5M~gsjn6G_ql(Z9v;Hz>hvm)t+G*Reo}Oz2DoZC~IJYFxV3=*1bcDI#V-ehb z`yS4?O;M_uUKUWRm9-0*%jA%+L}L(ouJ)NW*6>k4H0cLNq(fNgHv4Jnoecj0zTR!} zd#20Z0rVivt#5;(=aRdjZc}W37m&` zO8hf+O$5W$AK*8A8`$z*=vRHy=*QmoFlAg=(s#RhNTHVYC1}1K@hC|GVLZ=F6-*0x z{+sO$vPen^=y*Dt6A!PzJ!}(6LIqT()R5jys9m(YH-ka(Nn?~~Rtl-H*pP{zU-MQ? zlXus*&2qLymA^@KO>Y@ZjhbR)e1(|kVQ~2STn}zH$Hv*3wWt5KBjg$eN#@{G$fcMS8-`5K^IA7m_aM6 z`$)$n`bVh3x<&!)d?X1WLQ9uG9!?;qPGiS*BaH;RE}RifZm9eNEHWtim)l0DD^SyZww8iac z7r6e^#bzT+IQYWSF&Kq!LAalh*r_;Wzi*>jtu~LuXq%d^sr49_?y34lr!u2w+EXxL ztvGKYoa^y*IC%Ypz%YnJV8{reNW^fpBHc9m`O*l>0iqm+au0Ze=X^~VrnQF?&PU+5 zvDnPzI3)KOpigkw6k+Ys(1~ggta{l}hmoJQoMZf-VJ+IOf#vtk(!25;+d@FGwm{aR zAx2bT?D_&PU}I*Rt}$?_UtrnE;npz+3Wm#cQDminaPZX-ZsD&rZgNMlOP>~lPs)5- z1VY9g@uu8tU)@>Vy33Lo9Nkp)j+fdu6g^!Frwn87+^Rz~KEqIZNvGPU)wR*jLB$B}I$TO*f~!7t4654oLO6t8V2r?1+T_Q&0K0 z4682u*_{u6j(?P@{;`Y5=-T~Y%Kr<77Z}0&gZ+aQ{5EN9gm5}+3o-ZC$|VI0^CJnl zlu@4piaXoYaQOv8RMg_I3w0k1bN&6lEJ=n~1W@$^LZ*+5?6;J{!0RU%BNqm{<~-t- zYBiVcsKMtWrxI-wsbMy>B;oLhCnBi?O$~EZ4$9!UcL&30S4}6G<>y$P0t(I%#Lna} zX_$_w@IIB}3veH9GP|^0P;_>@eR7vav@g)kd8j3{^_~v_K#JRObGNy!PKV z%zyngxUd z^s@D@xs>D?9|0^XQSe9+5fMBr9-1rL2ipylxZmKI{+KWoVU3B__h9-y+tCNq0iyqW8C?N<_=wTWv36hc-;u6_5$-8<-iG^wVX{rs#%*o<0 zP`zZD%9FKz8kA)Pi`QrR2c(!`3^|x4*s*D2BB*E3p1pCB6wSJ(K~r=?GY2zKWbkSM zk97>~}>cv zb$Jz&BN$J`J1%`SPSlD!*ydwZh|}u@DspA$4$sz zuve=&^SCLUwSd_bGS|G?7q|}mlM8;PN?3s*Qn`LoL_I|_0v+g4G5lm(&>D&~sR6?l znI)Ws=bL^}57Jk}tm&JypgNPrn=57ljDoPx5vC%_rIdlHBI-9tCQd3ccs7 z8t-*ywH72aUrR7)OSDPqV2JeQ%}`Fj)8^<7+S({A|0d~}AU_#mFK*xIuPXctHbR_6 z0>4#tdv;L;zy3>@ngEyuC~{UEld$Xby%R!P6GeG0aQ`p@>*JR7p_5+YHPKN^V4fk3 zP=|o0bY4goP@xf7HieU5*Pudrp}QZK@B~{n6cMl7DMdWz@t^;~@D^eU<>!6(45Z(_ zk$+hp^uOOo|9MRR!MG0pHBKn;ANR0%BC@7!gZmJPZJXt>$m&mX8a!}cI&=T z^1$X1PVvlD`DVXD#eo%T9Hq`v^hcCB+%v=fj3To3%ZWn%=JZC_ zoex%j4J+ zbQX)n1VtYQf2U6; zl+lO7)ctA65@v(JWy3f!Jhj+syx9tcQ)P2qi3?*W-Zw#Ork|#Fs{k`fVV_!Mn!xL3 zIk}JIQwGd7Ve?#cLD_l3;B&IP`k1Ad;eT4RS=pW5A1i9B3J!lo3 z!WN4Denb)1o>9tu9*MQeIgR3$ z0rD%TiSRC-!526-Q_<1bGYn58#9j%95VT-muFHVK2w+EN#G8i;i`sA@UJgGpB~}7x zXT$xV`dKsMX!X;9Ku-Kvd`_&(SCYV;p<-2TVNbPS!mBJ-Wd&_+BDCO7!-ztt23Z4X=cs@kswD@}xU^1g^h~pu=^6pW ze8CszeDle6mmn7p6^EWdfD|dyNB$Hf%@?7eA4}|ajD2dyBKnD5ou30#)271<>qDF}GnvD)t$ z2fj&M*=&%VGF>YIAwtb!y?Ie|YWR?x(XuT5a+5#3i=W?qc_A~KjWxnJccu=Xz$PiiuHzL7#&Jt#VEx6v~-8J%V@+^q|MYi z{c+eNd4k(vCCT3b1G%D0UknFNZ?%lsqRm{_Bk#15n|;|H)9O&HOroVE-FG(hc4&ZE z(2P$V`Y^c7#KE)tx3Id<0tT%cp7~`AFs#cqf_JH!mS_Fm3^W1T!JXma96S=IrQy{} zb0%%7OB-G)J8g)5WpUWTd10Kg^gMRt${vh%)nB};`vmNAbL>TCRA6}wIE<1qWykbg zPcCUTMV-!d>owCDM3^BD{hCpJcQE*pH$gV#ErC;Wx|Pm9SnipSi4GEzX%cltZ8sf0 z4GJEGTyuxoh}YL_^g{rSCj(Mn9xB&ZpEqiyz-a5H?)=3b8E8s zNV4xhy4dT&cqJb_1$w&<_Ly*)afAyxX!#R8gU)gG)(#SXrbXZnoP4uq5;X(XFv+a6 zX>3lBn@9^3=&!a@Iy7C*kVuccxvO@qV6GM z%IEWSgV;mL3SA>lp*KOzvB5IVgDpwgX_;?gI5YK6==zNjtGgy=}3pI7Ml z*K=k&-d*&zJ{n?u+*PW8qBhLLy>UlMZiEIK|oHw$2rs9WFwD^(_d8L4@aT5=s?a8c%PT*VUVg&tO4QDy2SY zjm2bF%vg0dwTFqL)$eqaDox6HxHo5b zNFgp5r*h$E+lpT*h%KuH+&3V2#-tv2SyzkL$JGiwZeF>fbV(hQ2BwSr_!rt3?1T{# z3+p)Tl>z*Z!>MQQ>u0C#>Grq9WuFghUm2<38IZ<^qz{5X#CQaF zf*+9#(YJ9s#v$mL$-q)RasrGY`j8?J&3!QZLlA<|;QEREfPSG;1T6Zobq2^_0kt5q z09VRDG;Z8JCf6j{ENFc;@3BBW=)L0zw=Nv`9rTWlU%SG*pCtHSWjNhK_eeShOUWc1 zguBW=S8?nd=TBUyH^szUGwHcZ_085TFwz#|m8>-DLDz_i63t}Q{&1Hz4#&BBM00Rg zVBLmTo3$&AFIBXyzJFV$-LXKdTj9!w1s4u$sTtwJ%L#eIW7Q-qMV*+xeM-%y0(?Xu zYf$T);aSqS%JCFk#=-}_oMlbLI6SL(vsS@VW3P{axttW?Aj^|nTNjt{WwB<@*PDZT z83dbE=PjR;JkTlb_0}gc$vw%DL8IuHL48?t7bk-p_2$2S%@_`iYL2H6r(tbXtG6$H zi1#UpOr)gY$kAjz^D_2qA(d?Drx*fE7ciOz|S65GQ?@VtM-pB2z zI4+D&hV8ICIAo>$0u9M+c}S*w#r~(Y`X!*Ot*s<>_$|Jy`Jtq%-UyXuOq-?62R=8(;>I?z9KdCKML;#{YLY$;T>XZm?=UMn_|2rJTDP1Hb8tg|jxd^v+7b=!NmtTqBeh&ZS#8&>3NHz5w>{Y4R_ zO^gPq`R-cbRMDwPNbP_#R>)zaj_`d(XF|e#kUT~iLdsnipk{POw`}Y61ZAD0nZ%DK z`9$<-)~~Drk;!X=k_bh1nq3~u>-~rbzMYZ?_?z4aK6~P}R|Rp=V)u!VrbLFxIW+2b z>QCbRY0tN4TkELh&c0Z?EZk3qPr_Z~pM`RmqbUOkJ-FMoK2VOdHC4y-G}8eV+DZWk zX6jN-&=s0$n)ykYm32Cz^-9AHW)kRCfBXP_Rx{TG3mN7#g=+BS3*~Hwshl1}_t0Tr z@>%){i8cncHw7ld83d}Tbd$lY)kp&6w=djR4OnT|iOe!>@!}5DO!8*$5^bG9=g)2C zhntFe*FYJuTv6y}J@zbU^Oo(_A470wLp;z+iI}Hu+#FvD9GC*|JoXx#vUsEWFMWzs zrZu`29dr4^OWAsvC}BUpF4b3865d`bCI=`twM+)7OHA!s+~FKJo5g*Z3)bGBekB6l z{^OH$w2KEi*_gGoh!}k-;;t>d zONzdN&YtPqo8~CDbOb*JqmAK3!_<^zKpEMCm1_Aw;5Ap z5mLu5wB~x0{)K=s#@QHe4QB^QHDEk8EK5WS~XtNf1f;f+>NG|?7@i{z{;oEixJ8NF5> zqrFoEMY^>gJf2r0h7)7!AZa0;Q)Gm-_udiHd6-r+nLkdP8Idjb7YZHg0a|P*pi7*?SHZmWTU_)ek9rzu5jNMxZ1-PQ*8;dpg0KMZ+ zvg<$xcKwT1PCU?+SNM$wAHJ2tf2-A$Hg|CNMu7i3u;2Rm|Lb+l{H9sv<-UiSxL|KC zp<+^oL`w;+0@uOD5|ltr1!It<>CyM9qAyLPU7^`<<=sZwJj}lcAO#Jed;j1|xZP-) z_$diC9(R?o{+&~-z0B_J_6ANFjEe%X=ZqU66Q?A1(h!AWTU?EZ3$shuPcfd!pqaK8 z!fD0;=)T-Z(rPPKxoI++8v5w=@#2 zMjXbSXl5Z|#_JGO8fUn|tFn|N+D7@TQwqfCT14gR8eKfo(XD8)29;&w))lNX3C4^C z4_yvO`*Vokel4~CYWw|m?mdP`6}1AN$VtBqzG;7rd!*;vK*TA97s|PqHCZ{xFnm)~ z9s2x4@urFRS56_BvH!qM3*$k#n1pR|IB6|zmWY+93=<3xqmsN1=9s}qAI$)aN{!JH zA_;b-#~mdM`1_d@qW?<#VVuI_28>DS-W;HRhS3j+m07d#0Xp|#ZnIhhr8t)5s_EE` zT3JNF4UnQUH9EOWEO^G^5&wflY#veqIXg;kE-My3<3l<9gfNQkP1q**CvbxQNd9i4 z?}rC`rg%nf{cI18sklEK1$F*5M?}!fAVS$8bbE-G#XWNyeA8y{>>3X2v0d-+Oj2Nm zDM~hDkKQMEUONW4)V08yH^lSkurW|St2O-qg*X|7z@2eK@Q#PRzc^?S&VF!iHkZ9r zQ|_p96s8ueJgP3de8T?u*X4X7*PB1c+u43Z4}DJ|zhVoT0A8Fiv)KyX%2cjV8ZN3c ztL25YZ~Q;dWu@}E_5AmW*7O3qy%ypGR;@9T0t)F($+h1UowgLH!l=2w zK!qu7u!lkB2db9ff@F80U3Y&HLxo6uuR{t-k=~4>KaMap`91+%-=X4x zPIjb`(iwV6mt`gQh|&>5t)M7K(0ED|DJt@k5JMGy`CcbL;4X9eMpYv9y3t4yjy&B0 zXf?}(|7;DEY^&|$+8O=?lHh`ed24Gb-U*!6TTaZ0@pw}Q7YzJ;?~UHyTPQ)J#Zvh? z@zWJEmhvLkp>o(em;{^vHcBnExu;CTR9eB;(I!)lr!hG6E{)ZFyun7Nb=JW@0qs@d zEkQlh4xOnd+KSSjO@HD@I=o=|<+>iix{rdun$Lsk$f(=9m_IWJCWN&~H&6?b*q;D~ z_z1*N#2($~+O|WY^B2XDwT~$_Z>S36GLjfaX(W-3%cth0B?O@ffccd9nP^2UYXi03 z4uGbbTuq5S1&7(wk?e{h zVAQ9y(!U+Xu-73g-D=uy!XCaY0}{*g46Aw(uj3Y^`bK2@ecVX7t+Z{Sba#VZYI$;U za)t(vXQ(p)x&2Z1>e|kteyh;gzRHrGHZFI%Py~Mt0qoEdxHKWd^)3)GmjLTWKW3do zAjEvy9GP>k;}a@@mp%Hf?5FySdRRTR601M)xPFMIdDtwb#x(F{<^lxbF(}O2M7WWp zl2Z1I|46W47x`fC9WM8*U=}&;9?~EtEz$n{MNV}jhKm(Yw$~vO&R{W4Hb*>XipJ>;XH2Jpx|a+wMXI;lt6wo3Z)Ljs`DHXyJ)$LIq``b zD^gxc6cys%uUQ7+5cWzYV*7mU@Rfg|8&gPjCfdIbLD}~qVEcDktbY!{zmfonO8n{L7g&g|Bl-aN0_nVe5{2&8e+`xB zMjki8%CJ(Aq9@AD?tZ1GGLZ5Aq1*=~L5L@!tSX&ponNexPDz*N=h8YKH9L-P81rF9{!7(z-F7_b$_>=@tomyjdThM!y<6Bae zY{vdG=_1{p8)N}8ioS;C@(dr@R_)}T5C%c>V|b~c;5LhRi;iAu8)R}ulL@=&s@Zk6 z>}ySWoQ>vDwvcTPx>kHaVbZ+SX}@rki*GH~J4+^t9PC z=u|fHt=14)lle{6cYvOX)mZ&GBJ2{g$@KN8b~e?65RAYOh7N;tzih~EAExjN@1q+I z%{fZHMf2P&Y=78aW10S)9?~lu7_`s|<`1A++aoC^NWXxm+jurhppAHvH?dRhvT4g} zhq=&!vD%Yows`SWp3OsVWit8a_qg>5DDv6w@3>Lm9=CAtDXgJv-m&d;~GjW^oz$Nk(#o z1@_a2@uE@10q#}vxN(esT?KbwBA8PA?NrPEpYyT)cg5-dgKbER+m`sAk2Ta?uU_9) zg!RR|*tAsgGaqGH!bakI{!w92PLLRFM>=soXI*OIYUm4;7fv+@-Rlppk~yYy-;f~Y zcJ%Gk`t85CQyCv0$GhmhL<<5aHHdw~BEFM9lm%|p%#Hbwp&mQodTollzGque(8vY{ zR52gtrQ4dcCO!$xA&Ru#v!AX@CL$(HRaHtn!s|1duc@egD!o=UGEWK_r5cS7tNhs` zXU)qVDM>CVNreLwc-GFA*S^Fo;8zo42_DKC(|j8o_}K(;FZ+tK^h}zcEzqyTWWgS@ zh9q-VNo7ZrCv?L8M>F4XBPFc`LGn%7C|ap&BD@1pRflYD?8kcG=Bv?7FhDcF#Y3#* zBRajkVLtbCw0g{{;BLZUXNXE4Z14wHVE*azZ*o4JS@ma$C)d8`c`ZbJk2~_fGvavN z!>{FFkFc8!sb3(TVQQgHCSQ14xZrpu4#;GuWJm0@kuVUqKsRotYGY2ARIOEe##N}v zbX>=47@whw*!`#5H)A98{>QVNI>*K~_FtOT@KY!+UcqjB1B4c-kBRlkrvGYy$QybV zF8{s^o4$h=|CZeN&(Hsd7yXB2N>uui`3|dpKDi%`*(GRz2+1RcH;9hQ4`lzsvXF{^ zASDO;(yU6hckQ&eg3FKILw=zn1_~wR^}Q~zbJj$#j2DQXx|*2syq}!7`gpznAoJzm zJ{9JZ${c8jVh$6aDWuQe$D)R<=VV3+B8O&3?z7tEs@|;vc)&p7En(D+ufG#Db6+i2 zG_pH>tN{ti&V+3C6i?=zx8Hu>Rb89an+j^Ca#Z|_`WR}?UZ%#yU8jLIFGa^8Qht-2 zPIzqsHkga93Dl`Ym)3uh-Nbi}_SsrnFPardtK(KG0R0Alo=5;j>-W%a zv;YBaW_n*32D(HTYQ0$f1D}mzt}0b00pREwqaDs63=9t4-W0$vOrgWA$;f-Z?&gN` z#Y@8Jh((?U{Aty(@Y^H#kv>kR!#)il7cQQrqnK(M8+N!FX;TKysz_yWVeZyih+bxz zPFhwq*I9wiJQZaX@R@Fd zhm)M^g4J!ocM&Sr#Je(})eKrZfmJTtsBOj#%QhS~p?;xq0xat>K!`S6yqJ+fOHe7RiPEXH z=n0VtGLibuH)7tE89ep3(GVosQpm zp|j;a@eEz7Rpe-uw=-^hN9oU9&rT-Yo*rL_J%lQb4~8PawCJ#I-}SFFF?tvaaBG!b zTBym%9f;9t*5>+-4c`T6gEj75YQhMztT$#gMLkh}wXQgjGilvp^{t|I(d@IA0>GVn zVpcietfni2yDnL&wq|Q@girp$h%7qMbnk`ys)1-$xqmNOeHiRAOobh0h4dia@LIh{ zy#XGd*48bZ$YIF~Nt-&b2;LJ)iLy;M0aw48LMd|`3NK3}exvO%Kva$Hkbmypq|qc`#aotE2e&8Cg`toXsxK7lp#v2NQs4T)#v(*T` z4V-l$BJ&{B?HBmT8)3|K-ss)Yn$YH3|v82T4{qFo{drP++b-XdQ8sW`iIaxs@bhmv(W2Fxcau^uSMsEK>Rj z73{pi-93B=GkRE^q(gv}Me`lRD$4u##NtahUMW~WV<_G(mZgpxEkT>ktO&T}AiKv) zYPQQC9FaFTI5u-gy3R1+TJ&fCfwY)wTXYdcPDt(be=m1EX>Vna?{aVX*1{P79o+jr zI=)23ZJRl{?>rL)3bcdo`T_?kA{z$wVkc$8Dd{}$~`4ejC5hO@{QnXc#T z0QlFBFY^6Xn)J?tY@wU`ojVNF&?|( zbnfCK%xS|Q_1F^Kz7K?C~u(8lI(naxFtb;QU!&?z02`H&FF z!mkS)m6y@=PwvK@>EsMeD+WefGIOsvHuV@0?F+bwogS6kg5}ae=zx=nP;tE?I({Q9 zVRtg!inDjc7#8DG$VPEZA`5Im)BVEC9nv_2iK;;wK}ioH&CPgGbexUQ@(Sj9_!r)kvXCJ%encU1>SYu&bJCU4kM% zu&#jOS{6FHo~6ie5+zx|y)N0k&eb>APMu|luTQ!uedH$Hsv?C|)pDP8od%Zf@L%DB z?d11_^zWLo_?E2r{+*gqwzl}c2v(iS;|kx#LLQem@jm+B5D2$HA>`r^fywY7wJ~#Z zlu(rd>NV}eigu2Sg3_d8bT4$Y1!1Cz(0o0K*t*bc)*B~uYRT4w>&?@r zUBxz}*FN1|;CfKaECVr%Gk{uFjmY}Z+SHu@@koWD{1&W1mY!%e<_Q}MIwi={u_m2rB<#9V4J9>?*vl5oRZfXJTmY|e!7f;(GLTw$3dyXdC-ur& zs_ZQKr0CpVi2L-7ErFzqvnpB^fdXWKiYzKQQQ2%ZnB1O5i8%H>MR9pfj2#q3(f2sp zVrO!56^9YP@>1p*qBZ4b(z8B}iwWo#QPzJfZ2n5J5;l5WWJQI2))jQh@YnAnpn|kj!GlSHn`h1%4Pf10 z#$`L|cVl)t_`K}u(j}W>gTh}T{@E_S>wj}-5oWCtG&&=!2_|H?_mnV%zl1v9mRA+J zCMJ^31?>7-WTFszA&y6w3_lSx!8<+n4o@pN{Lvn?<(T0BQ29+UM7(g`QwA~LQZnP4 zU<-r)B?xOkj>kLd9>>fmqNQU{&&ZyHsS0l7`|r20kw*Fg+V}Ep%kOXy>A!Ju{=wRr z>gIY{gR!3yX{l`P-^*cF>v;4mcY)877@BGh6?uPPO0p)^#==jixyOm%O^2i+HnD$i ze?W{vh|)s_^3w|j@ozPP_FI*1=|dX1LRy)u(_anX@r5O@{4qT2{jrrkJ8^;;`Yz`p z>!R$W?6kPNC|ix|@r2;3ey4=Td0YGEQ?Ht>j(7H!;}2=V^6W0W$^`7 zI4ep!?~O!v5~B<=*F@yi7{w_Ts5@e*KyKL4voF&)g4EC{VF$Szr8e2F46~Y@w1hMV zB%|OUt0FB_LN@$5!IPUVer2bGG~Q`Jtd_L+EQLyuIkjw*8Ta0}ElPt!T7GJ#Kxo*& zonOLfp)?We+vTM-Y)^7ym3oj22{2xeP&!pdpt(j%`AtU70i5Ar?K>M$lchY5>M(Uj~|*+YrLz+Z9N3Kui`=?Fe|1= zh!)mB7k+gDHRK;^CKd1GKRWJjSI>*YMszDj=op$RO-x?XI{$YHU5cHrjt6NIvle|B z#L$juDFK31N_xp**g>|YiJyMW_!Wp>UXUE`c*Np>XD~WQ6<0EWeTxkBn;XiVq$xQnv48#Lm*K9f1Q8ZhUc3t@ zaByP4iMp@`I;U1fwS$bkGAwxxx!D;{Fr(r!oG;(WaktP|&V_b?=8BQmip6Luj5$0| zhc~53_*^ZlbQ-2(Y8FF)29@X0^xnMcQ5Se~#b*hLhQt+n2DLTSmsT`OMuM0oSz=k* zm^XohSF%XMksLI`ycclL8ia^bIX9+^&a4uqXvT>sPv0wq!P{{4E3DjB=sm@V$Y7%! zC+sm1RYq9hN$~{yN{e7VltX_cA)c|!n;*q?dYXczgf!fg(noPLrnnxesgD==To z8kL8^Xe6-n;aMKLfz8PlRF#MSv?4>??F%vaeY|2;u^2((FqEY{<}^6LdJYlC1ZqB3 z2{oA5)w({3mp4GtYs<#=m=-G}^`WExESws{F`1^KHG35pCaemZYTNP4S&coDVz1)h z8*Z79OCNUVzXp0;MeWe`E?DxliQF|%2gv+p-JXPDdv`g^VtVM@?JFJ?P6J_C73sK& z0ASccOU!}Lgai6b!cl)%Gh6~G=;U>AUOIwkc2>p3YGZLOhFEDwM3HA02;!~cRX5T<+xEU;Np547z(7REiT>>AxDj?=02(=YF7$%UbodGTeWgW)mhUq%ohVGsscH}xZ zFvAmi7P59!*J~lG8ifrnwf6T!fOnxnfy+8QVkBu4a81qdeDepEiW>$<4BTR0#DoQW#Xh48w zkOr5#77d`5aa;OS*H+0?*2SoI*}r^XC-_7qOqyh=csx#Lg>hkQ;q_?!}lL-SJD0?H4&BRTO`(T7`&1=fH z0g9@7?8b;wGwu11oSm{o@(2a)+v}dEcFaqdFJr`Tp%QNrqmIDFSa17nefwd?;NaEU z(#gt`FJTu}HP<`XFin|1%8^^}AmpUB1EQQ$c0SzBm)=_Eg<(8417DwupI)rljtaNr zZ!AN8cyEV!L^3VFlg#OVE8?Kq_gdBKK8{@L9YI6kM5O`k4C2vLnrurQ>zRO>*pd){ zz3B0|ccsUkB^<*IiL?N3Kcj2iHMHJbD41!e)8V1H5xSTc=e~^O90+yHjLh1Wa+A!h zsoiZ6;mE2e)6``%fiuL#d5-M={fwoxF9fU!#-A*n=IWKM&w6fl-e<0p zdsn$Tzxt~Hkl3`0vvVNwF?#PRg}gj1OfgXZX(wfV=*t!t0bR$4n!F}W{m&0LlNF>A&2Jm-taK&Yln0GU5z zg!R9P+|Jc4c&$~?;e0^r=y@EmV%*K6r^IyM+Jo+v?U}Zaph@_=ol40*wb0{(PeHbw z>xTsnVu8b9`43^L!`Rw3ZM>{%%-%P=J3nCihI4UopHu_=f*oEV;eU>t>SB?$kzDv;~WH^`S`elYG z*-6@0jA_omI-bj}^^@vts~0>)LPgL8s+ErVUw*UB zn`>FfTXiWa>Yw|TgrdG!mqU0}+vBytAJ2b>*|<^jXExZ(40s1!Ut^ay;5%C{%nu$2 zbZvhO{fsa>86G*RgW~X&k394u-+}H!zIo7Z&};6f5()C}?n}|IG45FpuWdi9^=+;x zLEm@I&%xhMM?DW5^0LP-2JU1xXOkf`?vdP!_h6`9Lce+3LqXD#@fSzqSMJfQsX>po z@MJYcqzFT;M4JJ6KWrV@<4Ke*#febLn_ z>w@cZkC(cLHm<6wz6*Xncuo@WbSZYya>K>a#F$Q|dc{UKB&?WBzW0e+N)Jg&82PLQ zj>?XA{Sm?dxM?5gAqP{{fM{M1+0cp!ZwQS$68d&|B}{jputRd}xdt{nA9Q$@l1OjN zwPBRPEZM+OjDqt}$}*WW&=}cSj4W?1h_)37eOx+ZRA=B&{?i+b>yYDNWV}UbYk=)Q zP>aH+hvg2lDxPoOodbaFV4spi`Gh}cc6QhgZ_BsdPLKH=`oZCekYCCWnS}93Y+G@} za!L0GzeR8iHDvG>isJs$IH~dIu+43%6sAgXN?`AKa`S4wTD&sOfq!yL+ooa`CK*a5zP0v<5_Vz--GC62C>eyW3Jv6(Yq3-K%NWL6Xy!!|CEm|)Mz%W>E z8o}p}6cv@1RSD1*Et%D)=A1BlM=CzT0YvvVP&fOXK}KZ{D8k`P?nVeeRZiT)*pEM% z=FU_qeKs+p%;7KvQdJQe#e{H?@5!Jesxq)<)e46sH(6w?SKJ)^FkwkxQ^6~{Jy>!L z?-0%cPaPB9Qg7@EGm^=Q4d9)a>IGPIM!an+Kj=s0)XsqsL{vM{mxvH33e!z(xV#6{ z`Ke{~DFS`$k{wC!l};Mz_P4M{A9wg2cg30(J!DExlI6~DOy0jNOTs*m^C+sdVS>|8 zKQbY|-cZxXWaaYAPh&a(6n8nMC$E#4Ax1dG1^7U`kbyP)eNt<$z# zeKqf8_zvmg@OpT5%}K7@-KjUNJ3r7^Rf>FD;loeDy{U_?lNQ`5X zXHyC%i3!D^8iGWLS`tcKhJXqJ60@d+&adg%I-N)y%VpG8B@euw1mA7gj8|K2kPH>G~2^m))x1XKx$48W}sSyxP{S^wVRF|HV zSk#xKrLp;$DhJ9vDqaY%EILEM2Ie>ubBPA(l^rv|ENJbGe@9V+j@`0`*N(IrXNb+t z205{qs|n4g|1uYbn6-A<23RGq1$3V8EW-~7xP9?syH(BlAPhezomNa`j4br9Fz z)=~FT)xlItaCuX3-KK2-mJdlf2&(s_-7;NWiW66eC_FeWNyhAkMMLJM8Npo?+Ozl3 zBevk_Vd?ByzGrXwCsVhv6s(Tp+}Ppw3y4LwYlS3-2BbkP8R^(QNOla#O~s?%vbkoe zBg7QnQr#UJByEJVsd2iM+}^v!s~Q^P|b?a;Rxpn}(?tsFwEWKETpFp4?3BvCi5gy4)HQYE#UD<7N|{(C=aHd(2(eQrshhDxlelF8qM>` z?!0>eag8!)0GMz9P1*xxHa$t6>2EWBNqBCD`#9Y24Ad)Tu`6xK*_p{(M;4Dbj0LQy z%O9jFpEv&AJWr7I^R~32?HCc~v6<%wf!D(hX9T6A8GT&3cqG%Ov}t_I^NJRnkCk?) z40aie{3tP3S-krhh($@gBH7JJs$BGY!0`02RLo%7Lxm;5!mS%1%yUC9v`4f>ieE4H z#l!OqX^|s43*g(cuhNd>V;JW(jq>3?_#5Zu!R`cQIIF)&sZ$kIb0@Y*8LZGeMsTds znrK>jN8=W3HoVhJ8%0!N;w!@&QL5YHfg-HJ%tTy__Huju0)K2$Wl{|%)5`w*z1p=m zqk(I6-12zJ=u`GR8QMYSslPAtZ@0EflK#cS$XoUTvUzAD5C{~PM{Op$pD8|ftE~PX z{g+?P+@KCOnx(#?cP%8e!)k;X?=ysdA>^SgL=k26OVx%=wa~L|(d(mYv!{8dcze6j z_h|LI<1^Y z5rl?QRzUbq<^7^<3Nrw4iZW@%LvB%uj&Gr+rJ~GIy%hkFrYABRAUnS$q%D0>;?e0F z*YC*NTZCx#;`B%J6dANYbnJuKuiyJ@rPo1!W(yoV9-N|E*bi?ZPSQpCp{sJ6NZ*CU zkKUycUA-@@e-CT-x2UC~bWalsYqBGg!6ArFWmEw1t)0(NT zZ%ah9P*p#+ogxb4pG<{n=s1{w6yf)5Pnc7k->i4J$D=#oy!(LeDbH6emaBR=LFm?bmTzLCYIaUSX9i+(Np3Ech~* zZHTPZ`qMW7@!C0m)ySk|8>=iz9uk3a={c)1BmX_(iy>YbGwBzbB70ITRD;4)n5Re3 zv3feudeh@Wv$Z^3LRkfij>W8`O&Xe0GmItv={wtBH*eWd&MAov7wPat zRX+eoZInHV$FwzpEE#?ASl&^}UDi!0=un=cDFEG_WE^xJtRnhKeVAkBcPLe5t$F(B zdMxkAZQBM_DexyTjp?KgPItFnTep?d7nJi;%7+2_B3wz#V@$6<-6N=m@0Eb_ma<*2 ztl1m5s--y1ew_AvXWGOBMlS{P^oSw+WJ3-`l?LTUxly?Y@u^I6d#dM}QeckO61;u5 z*oLSY({aV(R;c;E4J-16B^vd3ZXp@#!TXInjaahq0>{!8;$%ZPqW!!dTfeZcQFyZ1 z>`NnKReAcFyh{VoCo(Ecg&r#L7$AT&J50!dWuZCSI$7O;2*rs6tQS_bbKP5x$#Btj|uuR!tp8n*%I3T z#I*o#zgxZ75dLNmV{k-117H-Xi89zDKYCfrph%G{*9i8aW)#fi>{Od&bOn&EF~ftt z+7Pq>z)@g8x%{iNrNriHjL8#Tcz|$oqk6D3K2kKbzn0Hlx!8MjN0IXyEo3x@M3g3*q)7 zf=$>mM3McVz#U|myVoDXx{f+xFGNmwCa95_dZ&z|Bvtyn?%{DPH&dD&SoE3s&_z0x z;~M43AnS-z%h+87s-#;(dqrM5{(uxI-x``q{p*WxUWkEWpcdlud)Nt*NWi7ZdDIrC z_*E;|%V30~wZFY1*p<%OpJEBchiO-F5;>!XwzZz1kddp zLZ#w8zx>=scB@Ztd0c#j?z|9PpBNz*-EK)g4%Ib=AD#i#u%c_fz|}vELP1yJH;%_G zBIz&kcdB@=G(LXklqV+FuusvJHyD%Dgh&vGat^kil{edhO2WkgZP$cFd57ALEfGEm zA{ooH`(!1zw_6z}?LjLUIq8nv7yXTl)rjW5#`YLa&C~01FLasqF-bD~i?@MUFJQU& zSK^=jJ}|QE;-6WsfAZ7xKB+J(n3l$B6d_yYh*tf=XlZKuwE1eZmsuk&H(f!fH*$*- z=8VRBrHYD*9hKoEhI<&FNX$4HtbcL+-fc8Vrj^C=axFkI+|CN6am>_(t&OL%n-LR| zXL0(#i=SzkCh-Z&b)93uyM`NMyhTR&m(~3<4n_DN8BWx=fa0lu|1Wo@HZ_;#WnRA` zFqhUtg=`xdz#g5)lATxmS6KhH?*TGIn9kY;$7BRg7*A5X&9B*MBPkOrMH%aA`I`Ybng+8#5_=~W4X{{&s zp|@|-*oP4uBv0IA7toH!!d(J7dy@Ny_DjwVaC~P;D|)N5{HHp?{K9H-kn(a+Nk${B z{~CaG+Xi)9`xa=0zdbJ0|5IlAA7J1gd)GgZAo4rry6_u?XS4cB)X(^@9Ed(@ps{>e z$;(f|5Hm3q2K9j6W_=e0u=dNMOQhZ68_T_L_>>Y5@dZ<#gj*R+J$2&S-1*dXk7=Ic zjqk;++de;1`r?`E$jeg1i2Mzpa9gs94gq1K#1G6!EvdaUQY3boUDqWoRNM3Rt;Ks? z|EIDufroPId>lu~1>khSb`Z}t=!`zW%eR6~<(n0XDNNTWf@b}bdxZX%T;np@o~ z(jpSKP@+_Hy(&v?mP+^bo{8~rj4|)&GoP_^zP~ePd(Lw_=l4G;fL^t`kw|tiVN}*L z&USsIm7Jk{c%)>R9*x(!@`lVOub%65yrN#sRP#t;S$u}Rid7@pCX|9Mh#q$0D>wVy z`ks^`e)vp6hryw}6~U=;H&Wd3y($#i=Gfb3f0I37m4Co6CP43!Z(x-N`X5osp1tms ze%c3}6kDxdVi;xvDg5Kk=TLkvqlYWfL@LvboWsVW+U`h~6rz383{`x@j1I34O>A9u z(OF!w(7xw%ab7W5$HpM}K%Mf9$YGm+jk=D;r>mTjH9CcgYjXwbLtab1OI>AUy5g{C zP+qH{X$!n|DOCvC7Z1h zLb#ijLmCEVemlBALG`lx+>j-CJM z{h@xv#Js&KqkRhBOy1ko*g1^9E1Qrp(!v^?%anZ^SMoN$#p>Wa#eciXlWFTD1ES($ zH&V4-ltR*P33%k}#G;=mJh;o#As5=>+aU21_EK|k|9@jb19hYPwg}ym-xdxYfL#h6fHhzqHN zYkcGRSE)zjf>t}WM{V$3mj0`ekRsBM<`vXf`EFyewPD2G@^lO3*a69qCC@P{(GljB zE`En-IER~AWiM9AR!j4{Uk=#yOt;C+#-Op<(;EA!y|FJxLO9WFXBeaS><3EcaP&*( zzo~{Dmbt3xpYxQDABzsC^mB-j_Y4fixsHDJ@(yo#wk?L1;9ELcW8OHntM9o~DYh@8 zuPLcd@fq&(3&k|dQ~tzN!->&}k}9$L;?Dn7wRQCA2?Hg$*v-@qnn$E{Tf&&2xYXs+ z_LD(>AN;Ua#b*3^n-u!hwIU%`r>>7{oU5eb3t#wbl-7!T;3rgjJ92pfS?_rEApy7Y zS9*>cy#}|gS#39hFKYTV!#^#)X~5`sPNONB&!GZCky=_LR?Jg)3KK5)P-{=pn-RD7 z|KV4UFm2h_XU&_LWA-qv&zCnd!%S81{Fg%;N=8@A{_{GzSaQPzz=BLBF>Q^P|%BeNnwjwq79i}r|@D4J&`6WOqN zeY4?>G@M^Cmc%VrU_17)(9zUH(3Np8iJwT-!F6ng7(=exsw5C*3 z$^`UBU)w+AjcY3CzPctu1(Qyh&@|3*@)ERG>GdpMP7qb49B)w7x`l3AJg7h}x;0XH zOs6_OLo-O7?~z)8VTm_**C=p9U)bW;@Ae%!8vjrG)&fz`lo;@0df-oa--Bn=Is4xK z#g*H=;%p+BqtiVPugD@`558mx$YcUuh-p4BSDQ-0sDU59vNdxwQMcM|u4!j8JDY#` z79(TupPA21fk;WyiB1KNgrKIg*_v#(GB2B@A%#i?(d?zypHcFT)lO%(98W6yOD8?n5M)czS{wx5WqGz2>X%9Wh`BayD&NpQEt}Go42UWTnwA<_|%>>Wwvn$^e4>v zR$*TaG$)R%LWU<(G(D&=EHM@W|V)P*a|Qn z4hw+b3E`aZ&|L|Ph28KG?7aw1*qPfsFcbDhMwm-!oR~lMl;&Nk!8XJQb&MP8{HDZk z@nIuXL@4_N7sa1zs|pLiwv~uL@+mF^IG9+%O0bI^qVyq&3ni{R?O;vVhz!xpO5sA2 zlPwu61)H)UQWF_mNO7=eft6tY3qjn5ACL*xp{QoJiP>sQd;1H>C zumXmzaWkg(sYz|Yx`GcxA$*%sF8G{}N5KsPpCLiSqRSQ*W8W6=(*p?eRqY(+kLsBF zECF0j_>T|>v%g_sCZ}r@ymgC^g`4J*x!=fzKLNa*i0Hg+o}&Y=W@mJx1uo<878fG( z+vDkl-FzEfaG9BzS*t|m?iMT2se)iLW5(_odEUJ)I~zW5%Y{PefPe47&D?g75rz66 D613UA literal 0 HcmV?d00001 diff --git a/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..a2c6fac --- /dev/null +++ b/java/sim-pose-estimation/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/java/sim-pose-estimation/gradlew b/java/sim-pose-estimation/gradlew new file mode 100644 index 0000000..2fe81a7 --- /dev/null +++ b/java/sim-pose-estimation/gradlew @@ -0,0 +1,183 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin or MSYS, switch paths to Windows format before running java +if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=`expr $i + 1` + done + case $i in + 0) set -- ;; + 1) set -- "$args0" ;; + 2) set -- "$args0" "$args1" ;; + 3) set -- "$args0" "$args1" "$args2" ;; + 4) set -- "$args0" "$args1" "$args2" "$args3" ;; + 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=`save "$@"` + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +exec "$JAVACMD" "$@" diff --git a/java/sim-pose-estimation/gradlew.bat b/java/sim-pose-estimation/gradlew.bat new file mode 100644 index 0000000..24467a1 --- /dev/null +++ b/java/sim-pose-estimation/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/java/sim-pose-estimation/settings.gradle b/java/sim-pose-estimation/settings.gradle new file mode 100644 index 0000000..4ee6ec2 --- /dev/null +++ b/java/sim-pose-estimation/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2021' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/java/sim-pose-estimation/src/main/deploy/example.txt b/java/sim-pose-estimation/src/main/deploy/example.txt new file mode 100644 index 0000000..bb82515 --- /dev/null +++ b/java/sim-pose-estimation/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java new file mode 100644 index 0000000..7cd7008 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java @@ -0,0 +1,213 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import org.photonvision.PhotonCamera; +import org.photonvision.SimPhotonCamera; +import org.photonvision.SimVisionSystem; +import org.photonvision.SimVisionTarget; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +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.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +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.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.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N5; + +@SuppressWarnings("PMD.TooManyFields") +public class Drivetrain { + // 3 meters per second. + public static final double kMaxSpeed = 3.0; + // 1/2 rotation per second. + public static final double kMaxAngularSpeed = Math.PI; + + private static final double kTrackWidth = 0.381 * 2; + private static final double kWheelRadius = 0.0508; + private static final int kEncoderResolution = -4096; + + private final PWMVictorSPX m_leftLeader = new PWMVictorSPX(1); + private final PWMVictorSPX m_leftFollower = new PWMVictorSPX(2); + private final PWMVictorSPX m_rightLeader = new PWMVictorSPX(3); + private final PWMVictorSPX m_rightFollower = new PWMVictorSPX(4); + + private final SpeedControllerGroup m_leftGroup = + new SpeedControllerGroup(m_leftLeader, m_leftFollower); + private final SpeedControllerGroup m_rightGroup = + new SpeedControllerGroup(m_rightLeader, m_rightFollower); + + private final Encoder m_leftEncoder = new Encoder(0, 1); + private final Encoder m_rightEncoder = new Encoder(2, 3); + + private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0); + private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final DifferentialDriveKinematics m_kinematics = + new DifferentialDriveKinematics(kTrackWidth); + + 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(m_gyro.getRotation2d(), new Pose2d(), + stateStdDevs, + localMeasurementStdDevs, + visionMeasurementStdDevs); + + private PhotonCamera cam = new PhotonCamera("mainCam"); + + + // Gains are for example purposes only - must be determined for your own + // robot! + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + + // Simulation classes help us simulate our robot + private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); + private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); + private final Field2d m_fieldSim = new Field2d(); + private final LinearSystem m_drivetrainSystem = + LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + private final DifferentialDrivetrainSim m_drivetrainSimulator = + new DifferentialDrivetrainSim( + m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + + double camDiagFOV = 75.0; // degrees + double camPitch = 0.0; // degrees + Transform2d cameraToRobot = new Transform2d(); // meters + double camHeightOffGround = 0.85; // meters + double maxLEDRange = 20; // meters + int camResolutionWidth = 640; // pixels + int camResolutionHeight = 480; // pixels + double minTargetArea = 10; // square pixels + + private final SimVisionSystem simVision = new SimVisionSystem("mainCam", + camDiagFOV, + camPitch, + cameraToRobot, + camHeightOffGround, + maxLEDRange, + camResolutionWidth, + camResolutionHeight, + minTargetArea); + + Pose2d targetPose = new Pose2d(new Translation2d(Units.feetToMeters(54),Units.feetToMeters(10)), + new Rotation2d()); // meters + double targetHeightAboveGround = 2.3; // meters + double targetWidth = 0.54; // meters + double targetHeight = 0.25; // meters + + + 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. + m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + + m_leftEncoder.reset(); + m_rightEncoder.reset(); + + m_rightGroup.setInverted(true); + SmartDashboard.putData("Field", m_fieldSim); + + simVision.addSimVisionTarget( new SimVisionTarget(targetPose, + targetHeightAboveGround, + targetWidth, + targetHeight)); + } + + public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { + var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + double leftOutput = + m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); + double rightOutput = + m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + + m_leftGroup.setVoltage(leftOutput + leftFeedforward); + m_rightGroup.setVoltage(rightOutput + rightFeedforward); + } + + public void drive(double xSpeed, double rot) { + setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot))); + } + + public void updateOdometry() { + m_poseEstimator.update( m_gyro.getRotation2d(), + new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), + m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + + var res = cam.getLatestResult(); + if(res.hasTargets()){ + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis(); + Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget(); + Pose2d camPose = targetPose.transformBy(camToTargetTrans.inverse()); + m_poseEstimator.addVisionMeasurement( camPose.transformBy(cameraToRobot), imageCaptureTime); + } + } + + public void resetOdometry(Pose2d pose) { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + m_drivetrainSimulator.setPose(pose); + m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); + } + + public Pose2d getCtrlsPoseEstimate() { + return m_poseEstimator.getEstimatedPosition(); + } + + public void simulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.setInputs( + m_leftLeader.get() * RobotController.getInputVoltage(), + -m_rightLeader.get() * RobotController.getInputVoltage()); + m_drivetrainSimulator.update(0.02); + + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + + simVision.processFrame(m_drivetrainSimulator.getPose()); + } + + public void periodic() { + updateOdometry(); + m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); + } +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Main.java b/java/sim-pose-estimation/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..8776e5d --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * 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/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..ceed4d9 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +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; + +public class Robot extends TimedRobot { + private final XboxController m_controller = new XboxController(0); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + + private final Drivetrain m_drive = new Drivetrain(); + private final RamseteController m_ramsete = new RamseteController(); + private final Timer m_timer = new Timer(); + private Trajectory m_trajectory; + + @Override + public void robotInit() { + // Flush NetworkTables every loop. This ensures that robot pose and other values + // are sent during every iteration. + setNetworkTablesFlushEnabled(true); + + m_trajectory = + TrajectoryGenerator.generateTrajectory( + new Pose2d(2, 2, new Rotation2d()), + List.of(), + new Pose2d(6, 4, new Rotation2d()), + new TrajectoryConfig(2, 2)); + } + + @Override + public void robotPeriodic() { + m_drive.periodic(); + } + + @Override + public void autonomousInit() { + m_timer.reset(); + m_timer.start(); + m_drive.resetOdometry(m_trajectory.getInitialPose()); + } + + @Override + public void autonomousPeriodic() { + double elapsed = m_timer.get(); + Trajectory.State reference = m_trajectory.sample(elapsed); + ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getCtrlsPoseEstimate(), reference); + m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + @Override + @SuppressWarnings("LocalVariableName") + public void teleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + double xSpeed = + -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; + + // 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. + double rot = + -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; + m_drive.drive(xSpeed, rot); + } + + @Override + public void simulationPeriodic() { + m_drive.simulationPeriodic(); + } +} diff --git a/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json b/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json new file mode 100644 index 0000000..f9fbc4d --- /dev/null +++ b/java/sim-pose-estimation/vendordeps/WPILibOldCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibOldCommands.json", + "name": "WPILib-Old-Commands", + "version": "2020.0.0", + "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-cpp", + "version": "wpilib", + "libName": "wpilibOldCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} diff --git a/java/sim-pose-estimation/vendordeps/photonlib.json b/java/sim-pose-estimation/vendordeps/photonlib.json new file mode 100644 index 0000000..662d44e --- /dev/null +++ b/java/sim-pose-estimation/vendordeps/photonlib.json @@ -0,0 +1,35 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "2021.1.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/lib/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision.lib", + "artifactId": "PhotonLib-cpp", + "version": "2021.1.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision.lib", + "artifactId": "PhotonLib-java", + "version": "2021.1.2" + } + ] +} \ No newline at end of file From b98f431a4c4b7822160d6cd9eef629184756843d Mon Sep 17 00:00:00 2001 From: Chris Date: Sun, 10 Jan 2021 22:21:15 -0600 Subject: [PATCH 2/8] Refactorooski --- .../main/java/frc/robot/AutoController.java | 65 +++++++ .../src/main/java/frc/robot/Constants.java | 53 ++++++ .../src/main/java/frc/robot/Drivetrain.java | 168 +++--------------- .../frc/robot/DrivetrainPoseEstimator.java | 60 +++++++ .../java/frc/robot/OperatorInterface.java | 35 ++++ .../main/java/frc/robot/PoseTelemetry.java | 32 ++++ .../src/main/java/frc/robot/Robot.java | 63 ++----- .../src/main/java/frc/sim/DrivetrainSim.java | 78 ++++++++ 8 files changed, 362 insertions(+), 192 deletions(-) create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/Constants.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java create mode 100644 java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java new file mode 100644 index 0000000..0297d56 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java @@ -0,0 +1,65 @@ +package frc.robot; + +import java.util.List; + +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; + +public class AutoController { + + private Trajectory m_trajectory; + + private RamseteController m_ramsete = new RamseteController(); + + private Timer m_timer = new Timer(); + + boolean isRunning = false; + + + public AutoController(){ + + m_trajectory = + TrajectoryGenerator.generateTrajectory( + new Pose2d(2, 2, new Rotation2d()), + List.of(), + new Pose2d(6, 4, new Rotation2d()), + new TrajectoryConfig(2, 2)); + } + + + Pose2d getInitialPose(){ + return m_trajectory.getInitialPose(); + } + + void startPath(){ + m_timer.reset(); + m_timer.start(); + isRunning = true; + + } + + void stopPath(){ + isRunning = false; + m_timer.reset(); + } + + ChassisSpeeds getCurMotorCmds( Pose2d curEstPose ){ + Trajectory.State reference ; + + if(isRunning){ + double elapsed = m_timer.get(); + reference = m_trajectory.sample(elapsed); + } else { + reference = new Trajectory.State(); + } + + return m_ramsete.calculate(curEstPose, reference); + } + +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..ae49745 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java @@ -0,0 +1,53 @@ +package frc.robot; + +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; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +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; + + + ////////////////////////////////////////////////////////////////// + // 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()); + + // Definition for the opposite-alliance high goal in 2020 & 2021 + public static final double targetHeightAboveGround = 2.3; // meters + public static final double targetWidth = 0.54; // meters + public static final double targetHeight = 0.25; // meters + public static final Pose2d kFarTargetPose = new Pose2d(new Translation2d(Units.feetToMeters(54),Units.feetToMeters(10)), + new Rotation2d(0.0)); + public static final SimVisionTarget kFarTarget = new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight); + + +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java index 7cd7008..4b2b247 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java @@ -4,146 +4,55 @@ package frc.robot; -import org.photonvision.PhotonCamera; -import org.photonvision.SimPhotonCamera; -import org.photonvision.SimVisionSystem; -import org.photonvision.SimVisionTarget; - -import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PWMVictorSPX; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator; 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.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -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.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -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.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.N2; -import edu.wpi.first.wpiutil.math.numbers.N3; -import edu.wpi.first.wpiutil.math.numbers.N5; - -@SuppressWarnings("PMD.TooManyFields") -public class Drivetrain { - // 3 meters per second. - public static final double kMaxSpeed = 3.0; - // 1/2 rotation per second. - public static final double kMaxAngularSpeed = Math.PI; - - private static final double kTrackWidth = 0.381 * 2; - private static final double kWheelRadius = 0.0508; - private static final int kEncoderResolution = -4096; - private final PWMVictorSPX m_leftLeader = new PWMVictorSPX(1); - private final PWMVictorSPX m_leftFollower = new PWMVictorSPX(2); - private final PWMVictorSPX m_rightLeader = new PWMVictorSPX(3); - private final PWMVictorSPX m_rightFollower = new PWMVictorSPX(4); - private final SpeedControllerGroup m_leftGroup = - new SpeedControllerGroup(m_leftLeader, m_leftFollower); - private final SpeedControllerGroup m_rightGroup = - new SpeedControllerGroup(m_rightLeader, m_rightFollower); - - private final Encoder m_leftEncoder = new Encoder(0, 1); - private final Encoder m_rightEncoder = new Encoder(2, 3); +public class Drivetrain { - private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0); - private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0); - private final AnalogGyro m_gyro = new AnalogGyro(0); + PWMVictorSPX m_leftLeader = new PWMVictorSPX(1); + PWMVictorSPX m_leftFollower = new PWMVictorSPX(2); + PWMVictorSPX m_rightLeader = new PWMVictorSPX(3); + PWMVictorSPX m_rightFollower = new PWMVictorSPX(4); - private final DifferentialDriveKinematics m_kinematics = - new DifferentialDriveKinematics(kTrackWidth); + SpeedControllerGroup m_leftGroup = new SpeedControllerGroup(m_leftLeader, m_leftFollower); + SpeedControllerGroup m_rightGroup = new SpeedControllerGroup(m_rightLeader, m_rightFollower); - 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)); + Encoder m_leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB); + Encoder m_rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB); - private final DifferentialDrivePoseEstimator m_poseEstimator = - new DifferentialDrivePoseEstimator(m_gyro.getRotation2d(), new Pose2d(), - stateStdDevs, - localMeasurementStdDevs, - visionMeasurementStdDevs); + DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator(); - private PhotonCamera cam = new PhotonCamera("mainCam"); + DifferentialDriveKinematics m_kinematics = + new DifferentialDriveKinematics(Constants.kTrackWidth); // Gains are for example purposes only - must be determined for your own // robot! - private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); - - // Simulation classes help us simulate our robot - private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); - private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); - private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); - private final Field2d m_fieldSim = new Field2d(); - private final LinearSystem m_drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); - private final DifferentialDrivetrainSim m_drivetrainSimulator = - new DifferentialDrivetrainSim( - m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); - - double camDiagFOV = 75.0; // degrees - double camPitch = 0.0; // degrees - Transform2d cameraToRobot = new Transform2d(); // meters - double camHeightOffGround = 0.85; // meters - double maxLEDRange = 20; // meters - int camResolutionWidth = 640; // pixels - int camResolutionHeight = 480; // pixels - double minTargetArea = 10; // square pixels - - private final SimVisionSystem simVision = new SimVisionSystem("mainCam", - camDiagFOV, - camPitch, - cameraToRobot, - camHeightOffGround, - maxLEDRange, - camResolutionWidth, - camResolutionHeight, - minTargetArea); - - Pose2d targetPose = new Pose2d(new Translation2d(Units.feetToMeters(54),Units.feetToMeters(10)), - new Rotation2d()); // meters - double targetHeightAboveGround = 2.3; // meters - double targetWidth = 0.54; // meters - double targetHeight = 0.25; // meters - + SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + PIDController m_leftPIDController = new PIDController(8.5, 0, 0); + PIDController m_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. - m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); - m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + m_leftEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); + m_rightEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); m_leftEncoder.reset(); m_rightEncoder.reset(); m_rightGroup.setInverted(true); - SmartDashboard.putData("Field", m_fieldSim); - simVision.addSimVisionTarget( new SimVisionTarget(targetPose, - targetHeightAboveGround, - targetWidth, - targetHeight)); } public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { @@ -162,52 +71,19 @@ public void drive(double xSpeed, double rot) { setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot))); } - public void updateOdometry() { - m_poseEstimator.update( m_gyro.getRotation2d(), - new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), - m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - - var res = cam.getLatestResult(); - if(res.hasTargets()){ - double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis(); - Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget(); - Pose2d camPose = targetPose.transformBy(camToTargetTrans.inverse()); - m_poseEstimator.addVisionMeasurement( camPose.transformBy(cameraToRobot), imageCaptureTime); - } - } - public void resetOdometry(Pose2d pose) { m_leftEncoder.reset(); m_rightEncoder.reset(); - m_drivetrainSimulator.setPose(pose); - m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); + poseEst.resetToPose(pose); } public Pose2d getCtrlsPoseEstimate() { - return m_poseEstimator.getEstimatedPosition(); - } - - public void simulationPeriodic() { - // To update our simulation, we set motor voltage inputs, update the - // simulation, and write the simulated positions and velocities to our - // simulated encoder and gyro. We negate the right side so that positive - // voltages make the right side move forward. - m_drivetrainSimulator.setInputs( - m_leftLeader.get() * RobotController.getInputVoltage(), - -m_rightLeader.get() * RobotController.getInputVoltage()); - m_drivetrainSimulator.update(0.02); - - m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); - m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); - m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); - m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); - m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); - - simVision.processFrame(m_drivetrainSimulator.getPose()); + return poseEst.getPoseEst(); } public void periodic() { - updateOdometry(); - m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); + poseEst.update(new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), + m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); } + } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java new file mode 100644 index 0000000..bea4db9 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -0,0 +1,60 @@ +package frc.robot; + +import org.photonvision.PhotonCamera; + +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; + +public class DrivetrainPoseEstimator { + + private final AnalogGyro m_gyro = new AnalogGyro(Constants.kGyroPin); + + 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(m_gyro.getRotation2d(), new Pose2d(), + stateStdDevs, + localMeasurementStdDevs, + visionMeasurementStdDevs); + + private PhotonCamera cam = new PhotonCamera(Constants.kCamName); + + public DrivetrainPoseEstimator(){ + + } + + public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist){ + m_poseEstimator.update( m_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); + } + } + + public void resetToPose(Pose2d pose){ + m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); + } + + public Pose2d getPoseEst() { + return m_poseEstimator.getEstimatedPosition(); + } + +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java new file mode 100644 index 0000000..77fe668 --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java @@ -0,0 +1,35 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.SlewRateLimiter; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID; + + +public class OperatorInterface { + private XboxController m_controller = new XboxController(0); + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + private SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); + private SlewRateLimiter m_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 -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.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 -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + * Drivetrain.kMaxAngularSpeed; + } + +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java new file mode 100644 index 0000000..db6cd3a --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java @@ -0,0 +1,32 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class PoseTelemetry { + + Field2d m_fieldPoseEst = new Field2d(); + Field2d m_fieldPoseAct = new Field2d(); + Field2d m_fieldPoseDes = new Field2d(); + + + public PoseTelemetry(){ + SmartDashboard.putData("Field_EstPose", m_fieldPoseEst); + SmartDashboard.putData("Field_ActPose", m_fieldPoseAct); + SmartDashboard.putData("Field_DesPose", m_fieldPoseDes); + } + + public void setActualPose(Pose2d act_in){ + m_fieldPoseAct.setRobotPose(act_in); + } + + public void setDesiredPose(Pose2d des_in){ + m_fieldPoseDes.setRobotPose(des_in); + } + + public void setEstimatedPose(Pose2d est_in){ + m_fieldPoseEst.setRobotPose(est_in); + } + +} diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java index ceed4d9..58fb099 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java @@ -4,45 +4,25 @@ package frc.robot; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.SlewRateLimiter; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.XboxController; -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; +import frc.sim.DrivetrainSim; + public class Robot extends TimedRobot { - private final XboxController m_controller = new XboxController(0); - // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 - // to 1. - private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); - private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + AutoController autoCtrl = new AutoController(); + Drivetrain m_drive = new Drivetrain(); + OperatorInterface opInf = new OperatorInterface(); - private final Drivetrain m_drive = new Drivetrain(); - private final RamseteController m_ramsete = new RamseteController(); - private final Timer m_timer = new Timer(); - private Trajectory m_trajectory; + DrivetrainSim dtSim = new DrivetrainSim(); @Override public void robotInit() { // Flush NetworkTables every loop. This ensures that robot pose and other values // are sent during every iteration. setNetworkTablesFlushEnabled(true); - - m_trajectory = - TrajectoryGenerator.generateTrajectory( - new Pose2d(2, 2, new Rotation2d()), - List.of(), - new Pose2d(6, 4, new Rotation2d()), - new TrajectoryConfig(2, 2)); } @Override @@ -52,39 +32,30 @@ public void robotPeriodic() { @Override public void autonomousInit() { - m_timer.reset(); - m_timer.start(); - m_drive.resetOdometry(m_trajectory.getInitialPose()); + resetOdometery(); + autoCtrl.startPath(); } @Override public void autonomousPeriodic() { - double elapsed = m_timer.get(); - Trajectory.State reference = m_trajectory.sample(elapsed); - ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getCtrlsPoseEstimate(), reference); + ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(m_drive.getCtrlsPoseEstimate()); m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); } @Override @SuppressWarnings("LocalVariableName") public void teleopPeriodic() { - // Get the x speed. We are inverting this because Xbox controllers return - // negative values when we push forward. - double xSpeed = - -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; - - // 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. - double rot = - -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) - * Drivetrain.kMaxAngularSpeed; - m_drive.drive(xSpeed, rot); + m_drive.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd()); } @Override public void simulationPeriodic() { - m_drive.simulationPeriodic(); + dtSim.update(); + } + + void resetOdometery(){ + Pose2d startPose = autoCtrl.getInitialPose(); + dtSim.resetPose(startPose); + m_drive.resetOdometry(startPose); } } diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java new file mode 100644 index 0000000..182b10a --- /dev/null +++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java @@ -0,0 +1,78 @@ +package frc.sim; + +import org.photonvision.SimVisionSystem; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +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.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 frc.robot.Constants; + + +public class DrivetrainSim { + + // Simulation classes help us simulate our robot + AnalogGyroSim m_gyroSim = new AnalogGyroSim(Constants.kGyroPin); + EncoderSim m_leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA); + EncoderSim m_rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA); + LinearSystem m_drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim(m_drivetrainSystem, DCMotor.getCIM(2), 8, Constants.kTrackWidth, Constants.kWheelRadius, null); + + // Configure these to match your PhotonVision Camera, + // pipeline, and LED setup. + double camDiagFOV = 75.0; // degrees + double camPitch = 0.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); + } + + public void update(){ + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.setInputs( + m_leftLeader.get() * RobotController.getInputVoltage(), + -m_rightLeader.get() * RobotController.getInputVoltage()); + m_drivetrainSimulator.update(0.02); + + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + + simVision.processFrame(m_drivetrainSimulator.getPose()); + + } + + public void resetPose(Pose2d pose){ + m_leftEncoderSim.resetData(); + m_rightEncoderSim.resetData(); + m_drivetrainSimulator.setPose(pose); + } + + +} From d449dfb47cefccdcdc571a88b13a05f4e8860ec8 Mon Sep 17 00:00:00 2001 From: Chris Date: Mon, 11 Jan 2021 18:38:07 -0600 Subject: [PATCH 3/8] More wip debugging and such. Not really working yet, but closer. --- .../main/java/frc/robot/AutoController.java | 47 ++++++++++--- .../src/main/java/frc/robot/Constants.java | 21 ++++-- .../src/main/java/frc/robot/Drivetrain.java | 67 +++++++++++++------ .../frc/robot/DrivetrainPoseEstimator.java | 30 ++++++++- .../java/frc/robot/OperatorInterface.java | 4 +- .../main/java/frc/robot/PoseTelemetry.java | 32 +++++---- .../src/main/java/frc/robot/Robot.java | 20 ++++-- .../src/main/java/frc/sim/DrivetrainSim.java | 59 +++++++++++++--- 8 files changed, 215 insertions(+), 65 deletions(-) diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java index 0297d56..5a3c475 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java @@ -11,6 +11,12 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +/** + * 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 m_trajectory; @@ -21,9 +27,11 @@ public class AutoController { boolean isRunning = false; + Trajectory.State desiredDtState; public AutoController(){ + // Change this trajectory if you need the robot to follow different paths. m_trajectory = TrajectoryGenerator.generateTrajectory( new Pose2d(2, 2, new Rotation2d()), @@ -32,34 +40,55 @@ public AutoController(){ new TrajectoryConfig(2, 2)); } - - Pose2d getInitialPose(){ + /** + * @return The starting (initial) pose of the currently-active trajectory + */ + public Pose2d getInitialPose(){ return m_trajectory.getInitialPose(); } - void startPath(){ + /** + * Starts the controller running. Call this at the start of autonomous + */ + public void startPath(){ m_timer.reset(); m_timer.start(); isRunning = true; } - void stopPath(){ + /** + * Stops the controller from generating commands + */ + public void stopPath(){ isRunning = false; m_timer.reset(); } - ChassisSpeeds getCurMotorCmds( Pose2d curEstPose ){ - Trajectory.State reference ; + /** + * 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 = m_timer.get(); - reference = m_trajectory.sample(elapsed); + desiredDtState = m_trajectory.sample(elapsed); } else { - reference = new Trajectory.State(); + desiredDtState = new Trajectory.State(); } - return m_ramsete.calculate(curEstPose, reference); + return m_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/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java index ae49745..eda0779 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java @@ -9,6 +9,13 @@ import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +/** + * 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 { ////////////////////////////////////////////////////////////////// @@ -17,7 +24,7 @@ public class Constants { 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 kTrackWidth = 0.381 * 2; public static final double kWheelRadius = 0.0508; public static final int kEncoderResolution = -4096; @@ -25,11 +32,17 @@ public class Constants { // Electrical IO ////////////////////////////////////////////////////////////////// public static final int kGyroPin = 0; - public static final int kDtLeftEncoderPinA = 0; - public static final int kDtLeftEncoderPinB = 1; + + 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 @@ -43,7 +56,7 @@ public class Constants { // Definition for the opposite-alliance high goal in 2020 & 2021 public static final double targetHeightAboveGround = 2.3; // meters - public static final double targetWidth = 0.54; // meters + public static final double targetWidth = 0.54; // meters public static final double targetHeight = 0.25; // meters public static final Pose2d kFarTargetPose = new Pose2d(new Translation2d(Units.feetToMeters(54),Units.feetToMeters(10)), new Rotation2d(0.0)); diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java index 4b2b247..55200bc 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java @@ -14,29 +14,38 @@ 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 { - PWMVictorSPX m_leftLeader = new PWMVictorSPX(1); - PWMVictorSPX m_leftFollower = new PWMVictorSPX(2); - PWMVictorSPX m_rightLeader = new PWMVictorSPX(3); - PWMVictorSPX m_rightFollower = new PWMVictorSPX(4); + // PWM motor controller output definitions + PWMVictorSPX m_leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin); + PWMVictorSPX m_leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin); + PWMVictorSPX m_rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin); + PWMVictorSPX m_rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin); SpeedControllerGroup m_leftGroup = new SpeedControllerGroup(m_leftLeader, m_leftFollower); SpeedControllerGroup m_rightGroup = new SpeedControllerGroup(m_rightLeader, m_rightFollower); + // Drivetrain wheel speed sensors + // Used both for speed control and pose estimation. Encoder m_leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB); Encoder m_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 m_kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth); - // Gains are for example purposes only - must be determined for your own - // robot! + // 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 m_feedforward = new SimpleMotorFeedforward(1, 3); PIDController m_leftPIDController = new PIDController(8.5, 0, 0); PIDController m_rightPIDController = new PIDController(8.5, 0, 0); @@ -55,35 +64,51 @@ public Drivetrain() { } - public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); - var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); - double leftOutput = + /** + * 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 = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)); + + // Calculate the feedback (PID) portion of our motor command, based on desired wheel speed + var leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); - double rightOutput = + var rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + // Calculate the feedforward (F) portion of our motor command, based on desired wheel speed + var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + + // Update the motor controllers with our new motor commands m_leftGroup.setVoltage(leftOutput + leftFeedforward); m_rightGroup.setVoltage(rightOutput + rightFeedforward); - } - public void drive(double xSpeed, double rot) { - setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot))); + // Update the pose estimator with the most recent sensor readings. + poseEst.update(new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), m_leftEncoder.getDistance(), m_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) { m_leftEncoder.reset(); m_rightEncoder.reset(); poseEst.resetToPose(pose); } + /** + * @return The current best-guess at drivetrain Pose on the field. + */ public Pose2d getCtrlsPoseEstimate() { return poseEst.getPoseEst(); } - public void periodic() { - poseEst.update(new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), - m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - } - } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java index bea4db9..a246e52 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -15,10 +15,24 @@ import edu.wpi.first.wpiutil.math.numbers.N3; import edu.wpi.first.wpiutil.math.numbers.N5; + +/** + * 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 m_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)); @@ -29,12 +43,17 @@ public class DrivetrainPoseEstimator { localMeasurementStdDevs, visionMeasurementStdDevs); - private PhotonCamera cam = new PhotonCamera(Constants.kCamName); 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 since the previous call to this method. + * @param rightDist Distance (in m) the right wheel has traveled since the previous call to this method. + */ public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist){ m_poseEstimator.update( m_gyro.getRotation2d(), actWheelSpeeds, @@ -49,10 +68,19 @@ public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, } } + /** + * 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, m_gyro.getRotation2d()); } + /** + * @return The current best-guess at drivetrain position on the field. + */ public Pose2d getPoseEst() { return m_poseEstimator.getEstimatedPosition(); } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java index 77fe668..78e14aa 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java @@ -20,7 +20,7 @@ public OperatorInterface(){ public double getFwdRevSpdCmd(){ // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - return -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Drivetrain.kMaxSpeed; + return -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed; } public double getRotateSpdCmd(){ @@ -29,7 +29,7 @@ public double getRotateSpdCmd(){ // mathematics). Xbox controllers return positive values when you pull to // the right by default. return -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) - * Drivetrain.kMaxAngularSpeed; + * Constants.kMaxAngularSpeed; } } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java index db6cd3a..2b9be5a 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java @@ -6,27 +6,33 @@ public class PoseTelemetry { - Field2d m_fieldPoseEst = new Field2d(); - Field2d m_fieldPoseAct = new Field2d(); - Field2d m_fieldPoseDes = new Field2d(); - + Field2d m_field = new Field2d(); + + Pose2d actPose = new Pose2d(); + Pose2d desPose = new Pose2d(); + Pose2d estPose = new Pose2d(); public PoseTelemetry(){ - SmartDashboard.putData("Field_EstPose", m_fieldPoseEst); - SmartDashboard.putData("Field_ActPose", m_fieldPoseAct); - SmartDashboard.putData("Field_DesPose", m_fieldPoseDes); + SmartDashboard.putData("Field", m_field); + update(); + } + + public void update(){ + m_field.getObject("DesPose").setPose(desPose); + m_field.getObject("ActPose").setPose(actPose); + m_field.getObject("Robot").setPose(estPose); } - public void setActualPose(Pose2d act_in){ - m_fieldPoseAct.setRobotPose(act_in); + public void setActualPose(Pose2d in){ + actPose = in; } - public void setDesiredPose(Pose2d des_in){ - m_fieldPoseDes.setRobotPose(des_in); + public void setDesiredPose(Pose2d in){ + desPose = in; } - public void setEstimatedPose(Pose2d est_in){ - m_fieldPoseEst.setRobotPose(est_in); + public void setEstimatedPose(Pose2d in){ + estPose = in; } } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java index 58fb099..1c1a289 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java @@ -18,6 +18,8 @@ public class Robot extends TimedRobot { DrivetrainSim dtSim = new DrivetrainSim(); + PoseTelemetry pt = new PoseTelemetry(); + @Override public void robotInit() { // Flush NetworkTables every loop. This ensures that robot pose and other values @@ -25,11 +27,6 @@ public void robotInit() { setNetworkTablesFlushEnabled(true); } - @Override - public void robotPeriodic() { - m_drive.periodic(); - } - @Override public void autonomousInit() { resetOdometery(); @@ -40,20 +37,29 @@ public void autonomousInit() { public void autonomousPeriodic() { ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(m_drive.getCtrlsPoseEstimate()); m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + pt.setDesiredPose(autoCtrl.getCurPose2d()); } @Override - @SuppressWarnings("LocalVariableName") public void teleopPeriodic() { m_drive.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd()); } + @Override + public void robotPeriodic() { + pt.setEstimatedPose(m_drive.getCtrlsPoseEstimate()); + pt.update(); + } + + @Override public void simulationPeriodic() { dtSim.update(); + pt.setActualPose(dtSim.getCurPose()); } - void resetOdometery(){ + + private void resetOdometery(){ Pose2d startPose = autoCtrl.getInitialPose(); dtSim.resetPose(startPose); m_drive.resetOdometry(startPose); diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java index 182b10a..06fcafd 100644 --- a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java +++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java @@ -2,10 +2,12 @@ import org.photonvision.SimVisionSystem; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.geometry.Pose2d; 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; @@ -13,15 +15,33 @@ import frc.robot.Constants; +/** + * 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 { - // Simulation classes help us simulate our robot + // Simulated Sensors AnalogGyroSim m_gyroSim = new AnalogGyroSim(Constants.kGyroPin); EncoderSim m_leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA); EncoderSim m_rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA); + + // Simulated Motor Controllers + PWMSim m_leftLeader = new PWMSim(Constants.kDtLeftLeaderPin); + PWMSim m_leftFollower = new PWMSim(Constants.kDtLeftFollowerPin); + PWMSim m_rightLeader = new PWMSim(Constants.kDtRightLeaderPin); + PWMSim m_rightFollower = new PWMSim(Constants.kDtRightFollowerPin); + + // Simulation Physics + // Configure these to match your drivetrain's physical dimensions + // and characterization results. LinearSystem m_drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim(m_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 @@ -48,31 +68,54 @@ 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(){ - // To update our simulation, we set motor voltage inputs, update the - // simulation, and write the simulated positions and velocities to our - // simulated encoder and gyro. We negate the right side so that positive - // voltages make the right side move forward. + // 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. + double leftMotorCmd = (m_leftLeader.getSpeed() + m_leftFollower.getSpeed())/2.0; + double rightMotorCmd = (m_rightLeader.getSpeed() + m_rightFollower.getSpeed())/2.0; + + + // Update the physics simulation m_drivetrainSimulator.setInputs( - m_leftLeader.get() * RobotController.getInputVoltage(), - -m_rightLeader.get() * RobotController.getInputVoltage()); + leftMotorCmd * RobotController.getInputVoltage(), + -rightMotorCmd * RobotController.getInputVoltage()); m_drivetrainSimulator.update(0.02); + // Update our sensors based on the simulated motion of the robot m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); - m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + m_gyroSim.setAngle(-m_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(m_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){ m_leftEncoderSim.resetData(); m_rightEncoderSim.resetData(); m_drivetrainSimulator.setPose(pose); } + + /** + * @return The simulated robot's position, in meters. + */ + public Pose2d getCurPose(){ + return m_drivetrainSimulator.getPose(); + } } From b65c96fcbe49efc41e2e9e20a763caf1d53c36c3 Mon Sep 17 00:00:00 2001 From: Chris Date: Mon, 11 Jan 2021 19:25:38 -0600 Subject: [PATCH 4/8] Attempt to colorcode the poses --- java/sim-pose-estimation/imgui.ini | 487 ++++++++++++++++++ .../resources/blue_square.png | Bin 0 -> 3195 bytes .../resources/green_square.png | Bin 0 -> 3110 bytes .../resources/red_square.png | Bin 0 -> 3198 bytes .../frc/robot/DrivetrainPoseEstimator.java | 12 +- 5 files changed, 493 insertions(+), 6 deletions(-) create mode 100644 java/sim-pose-estimation/imgui.ini create mode 100644 java/sim-pose-estimation/resources/blue_square.png create mode 100644 java/sim-pose-estimation/resources/green_square.png create mode 100644 java/sim-pose-estimation/resources/red_square.png diff --git a/java/sim-pose-estimation/imgui.ini b/java/sim-pose-estimation/imgui.ini new file mode 100644 index 0000000..615070b --- /dev/null +++ b/java/sim-pose-estimation/imgui.ini @@ -0,0 +1,487 @@ +[Window][Debug##Default] +Pos=60,60 +Size=400,400 +Collapsed=0 + +[Window][###Other Devices] +Pos=1281,25 +Size=312,868 +Collapsed=0 + +[Window][###Timing] +Pos=6,187 +Size=214,172 +Collapsed=0 + +[Window][###NetworkTables] +Pos=312,346 +Size=937,231 +Collapsed=0 + +[Window][###FMS] +Pos=6,675 +Size=371,232 +Collapsed=0 + +[Window][###Joysticks] +Pos=312,581 +Size=1336,354 +Collapsed=0 + +[Window][###System Joysticks] +Pos=6,437 +Size=313,326 +Collapsed=0 + +[Window][###/SmartDashboard/Field] +Pos=343,34 +Size=571,319 +Collapsed=0 + +[Window][Robot State] +Pos=5,20 +Size=144,144 +Collapsed=0 + +[MainWindow][GLOBAL] +width=1600 +height=900 +maximized=0 +xpos=-1 +ypos=-1 +userScale=4 +style=0 + +[GlassStorage][/SmartDashboard/Field] +image= +top=0 +left=0 +bottom=-1 +right=-1 +width=15.980000 +height=8.210000 + +[GlassStorage][/SmartDashboard/Field###ActPose] +image=.\resources\blue_square.png +width=0.685800 +length=0.820400 + +[GlassStorage][/SmartDashboard/Field###DesPose] +image=.\resources\green_square.png +width=0.685800 +length=0.820400 + +[GlassStorage][/SmartDashboard/Field###Robot] +image=.\resources\red_square.png +width=0.685800 +length=0.820400 + +[GlassStorage][NetworkTables] +tree=1 +connections=0 +flags=0 +timestamp=0 +createNonCanonical=0 + +[GlassStorage][NetworkTables###FMSInfo] +open=0 + +[GlassStorage][NetworkTables###LiveWindow] +open=0 + +[GlassStorage][NetworkTables###SmartDashboard] +open=0 + +[GlassStorage][NetworkTables###photonvision] +open=1 + +[GlassStorage][NetworkTables###photonvision###mainCam] +open=1 + +[GlassStorage][Other Devices###AnalogGyro[0]] +name= +name###open=0 + +[Data Sources][Joystick[0] Button[7]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/d] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Distance] +name= + +[Data Sources][NT:/FMSInfo/FMSControlData] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/i] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/.controllable] +name= + +[Data Sources][NT:/LiveWindow/.status/LW Enabled] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Speed] +name= + +[Data Sources][AGyro Angle[0]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/p] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/.actuator] +name= + +[Data Sources][NT:/FMSInfo/StationNumber] +name= + +[Data Sources][Y Accel[0]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/setpoint] +name= + +[Data Sources][/SmartDashboard/Field/ActPose[0]/rot] +name= + +[Data Sources][FMS:MatchTime] +name= + +[Data Sources][Joystick[0] Axis[5]] +name= + +[Data Sources][/SmartDashboard/Field/DesPose[0]/rot] +name= + +[Data Sources][Joystick[0] Axis[1]] +name= + +[Data Sources][Joystick[0] Button[4]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/Value] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/AnalogGyro[0]/.controllable] +name= + +[Data Sources][Joystick[0] Button[8]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/d] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/i] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Speed] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/p] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/.actuator] +name= + +[Data Sources][Joystick[0] POV[0]] +name= + +[Data Sources][Joystick[0] Axis[2]] +name= + +[Data Sources][Z Accel[0]] +name= + +[Data Sources][NT:/FMSInfo/MatchType] +name= + +[Data Sources][Joystick[0] Button[1]] +name= + +[Data Sources][FMS:AutonomousMode] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/Value] +name= + +[Data Sources][NT:/FMSInfo/MatchNumber] +name= + +[Data Sources][FMS:FMSAttached] +name= + +[Data Sources][Joystick[0] Button[9]] +name= + +[Data Sources][Joystick[0] Button[5]] +name= + +[Data Sources][FMS:AllianceStationID] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Distance] +name= + +[Data Sources][FMS:RobotEnabled] +name= + +[Data Sources][/SmartDashboard/Field/Robot[0]/rot] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[2]/.controllable] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[2]/.controllable] +name= + +[Data Sources][FMS:DSAttached] +name= + +[Data Sources][AGyro Rate[0]] +name= + +[Data Sources][/SmartDashboard/Field/Robot[0]/y] +name= + +[Data Sources][/SmartDashboard/Field/Robot[0]/x] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/setpoint] +name= + +[Data Sources][Joystick[0] Axis[3]] +name= + +[Data Sources][/SmartDashboard/Field/ActPose[0]/x] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/Distance per Tick] +name= + +[Data Sources][/SmartDashboard/Field/ActPose[0]/y] +name= + +[Data Sources][Joystick[0] Button[2]] +name= + +[Data Sources][NT:/FMSInfo/ReplayNumber] +name= + +[Data Sources][/SmartDashboard/Field/DesPose[0]/y] +name= + +[Data Sources][FMS:TestMode] +name= + +[Data Sources][/SmartDashboard/Field/DesPose[0]/x] +name= + +[Data Sources][FMS:EStop] +name= + +[Data Sources][NT:/SmartDashboard/Field/.controllable] +name= + +[Data Sources][Joystick[0] Button[10]] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/AnalogGyro[0]/Value] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[1]/.controllable] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/SpeedControllerGroup[1]/.controllable] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/PIDController[1]/.controllable] +name= + +[Data Sources][Joystick[0] Axis[0]] +name= + +[Data Sources][Joystick[0] Button[6]] +name= + +[Data Sources][X Accel[0]] +name= + +[Data Sources][Joystick[0] Axis[4]] +name= + +[Data Sources][NT:/FMSInfo/IsRedAlliance] +name= + +[Data Sources][NT:/LiveWindow/Ungrouped/Encoder[0]/Distance per Tick] +name= + +[Data Sources][Joystick[0] Button[3]] +name= + +[SimWindow][Mechanism 2D] +name= +visible=0 +enabled=1 + +[HALProvider][Other Devices] +name= +visible=1 +enabled=1 + +[HALProvider][Timing] +name= +visible=1 +enabled=1 + +[NTProviderWindow][/SmartDashboard/Field] +name= +visible=1 +enabled=1 + +[NTProviderWindow][NetworkTables] +name= +visible=1 +enabled=1 + +[NTProvider][/FMSInfo] +name=FMSInfo + +[NTProvider][/SmartDashboard/Field] +name=Field2d + +[NTProvider][/LiveWindow/Ungrouped/PIDController[1]] +name=PIDController + +[NTProvider][/LiveWindow/Ungrouped/SpeedControllerGroup[1]] +name=Speed Controller + +[NTProvider][/LiveWindow/Ungrouped/SpeedControllerGroup[2]] +name=Speed Controller + +[NTProvider][/LiveWindow/Ungrouped/AnalogGyro[0]] +name=Gyro + +[NTProvider][/LiveWindow/Ungrouped/PIDController[2]] +name=PIDController + +[DSManager][FMS] +name= +visible=1 +enabled=1 + +[DSManager][Joysticks] +name= +visible=1 +enabled=1 + +[DSManager][Keyboard 0 Settings] +name= +visible=0 +enabled=1 + +[DSManager][Keyboard 1 Settings] +name= +visible=0 +enabled=1 + +[DSManager][Keyboard 2 Settings] +name= +visible=0 +enabled=1 + +[DSManager][Keyboard 3 Settings] +name= +visible=0 +enabled=1 + +[DSManager][System Joysticks] +name= +visible=1 +enabled=1 + +[Joystick][0] +useGamepad=1 +guid=78696e70757401000000000000000000 + +[KeyboardJoystick][0] +axisCount=3 +buttonCount=4 +povCount=1 +axis0incKey=68 +axis0decKey=65 +axis0keyRate=0.050000 +axis0decayRate=0.050000 +axis1incKey=83 +axis1decKey=87 +axis1keyRate=0.050000 +axis1decayRate=0.050000 +axis2incKey=82 +axis2decKey=69 +axis2keyRate=0.010000 +axis2decayRate=0.000000 +button0=90 +button1=88 +button2=67 +button3=86 +pov0key0=328 +pov0key45=329 +pov0key90=326 +pov0key135=323 +pov0key180=322 +pov0key225=321 +pov0key270=324 +pov0key315=327 + +[KeyboardJoystick][1] +axisCount=2 +buttonCount=4 +povCount=0 +axis0incKey=76 +axis0decKey=74 +axis0keyRate=0.050000 +axis0decayRate=0.050000 +axis1incKey=75 +axis1decKey=73 +axis1keyRate=0.050000 +axis1decayRate=0.050000 +button0=77 +button1=44 +button2=46 +button3=47 + +[KeyboardJoystick][2] +axisCount=2 +buttonCount=6 +povCount=0 +axis0incKey=262 +axis0decKey=263 +axis0keyRate=0.050000 +axis0decayRate=0.050000 +axis1incKey=264 +axis1decKey=265 +axis1keyRate=0.050000 +axis1decayRate=0.050000 +button0=260 +button1=268 +button2=266 +button3=261 +button4=269 +button5=267 + +[KeyboardJoystick][3] +axisCount=0 +buttonCount=0 +povCount=0 + +[DriverStation][Main] +disable=0 +zeroDisconnectedJoysticks=1 +enableDisableKeys=0 +estopKey=0 + +[Mechanism2D][Mechanism2D] +jsonLocation= + diff --git a/java/sim-pose-estimation/resources/blue_square.png b/java/sim-pose-estimation/resources/blue_square.png new file mode 100644 index 0000000000000000000000000000000000000000..be5b5e735a982b25852af9889434b9ad503da500 GIT binary patch literal 3195 zcmeAS@N?(olHy`uVBq!ia0y~yVD@8RV4BLo1QaRv(dh$HjKx9jP7LeL$-D$|SkfJR z9T^xl_H+M9WCij$3p^r=85o30K$!7fntTNV1CM~Gi(^Pd+}mr0f(!}*2R5+(*9vIo z5exY;X{p$*XLlGG7G(TqVwjNpm4RW?8aW1sX;+yUPHd`TV2BLkXHYo(ijg5D(w3n? zXEi&+gw#<*qd_p53Pv-+Xjw2?9FEoqqm|-lyI{0wG}<~GLJj1OS(OL(ux&ZG#C$5S P!@%I_>gTe~DWM4fCrSe0 literal 0 HcmV?d00001 diff --git a/java/sim-pose-estimation/resources/green_square.png b/java/sim-pose-estimation/resources/green_square.png new file mode 100644 index 0000000000000000000000000000000000000000..bae1dc52ab303f0b08fb30895dbbd0ad10879963 GIT binary patch literal 3110 zcmeAS@N?(olHy`uVBq!ia0y~yVD@8RV4BLo1QaRv(dh$HjKx9jP7LeL$-D$|SkfJR z9T^xl_H+M9WCij$3p^r=85o30K$!7fntTNV1NUxE7srr_xVP6Bc^M2i4s6KYDR<_P zfV}tQt{1|>mU^Ewu=7P~&FmSoR c<*kg$!OB}--=91HY}+$s$8OG0`aQYP^LrSDA zLxaw0c7_S5ql!j@U^EqsW`xnQV6-?Ktr12m#nE=bXwzu4bvT3?$Q`qOGA{ID3-9Az SAqDI+FnGH9xvX Date: Mon, 11 Jan 2021 21:20:16 -0600 Subject: [PATCH 5/8] Missed reading docs on what "reset" actually does. Prateek FTW --- .../src/main/java/frc/robot/Constants.java | 2 +- .../main/java/frc/robot/DrivetrainPoseEstimator.java | 7 ++++--- .../src/main/java/frc/sim/DrivetrainSim.java | 11 +++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java index eda0779..3c3807c 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java @@ -26,7 +26,7 @@ public class Constants { public static final double kTrackWidth = 0.381 * 2; public static final double kWheelRadius = 0.0508; - public static final int kEncoderResolution = -4096; + public static final int kEncoderResolution = 4096; ////////////////////////////////////////////////////////////////// // Electrical IO diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java index 6ed4d10..eb5dead 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -36,7 +36,7 @@ public class DrivetrainPoseEstimator { 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(m_gyro.getRotation2d(), new Pose2d(), stateStdDevs, @@ -51,10 +51,11 @@ 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 since the previous call to this method. - * @param rightDist Distance (in m) the right wheel has traveled since the previous call to this method. + * @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( m_gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist); diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java index 06fcafd..082fe3b 100644 --- a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java +++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java @@ -87,10 +87,10 @@ public void update(){ m_drivetrainSimulator.update(0.02); // Update our sensors based on the simulated motion of the robot - m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); - m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); - m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); - m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_leftEncoderSim.setDistance((m_drivetrainSimulator.getLeftPositionMeters())); + m_leftEncoderSim.setRate((m_drivetrainSimulator.getLeftVelocityMetersPerSecond())); + m_rightEncoderSim.setDistance((m_drivetrainSimulator.getRightPositionMeters())); + m_rightEncoderSim.setRate((m_drivetrainSimulator.getRightVelocityMetersPerSecond())); m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); // Gyros have an inverted reference frame for angles, so multiply by -1.0; // Update PhotonVision based on our new robot position. @@ -98,6 +98,7 @@ public void update(){ } + /** * Resets the simulation back to a pre-defined pose * Useful to simulate the action of placing the robot onto a specific @@ -105,8 +106,6 @@ public void update(){ * @param pose */ public void resetPose(Pose2d pose){ - m_leftEncoderSim.resetData(); - m_rightEncoderSim.resetData(); m_drivetrainSimulator.setPose(pose); } From a950ac66bd95a0eceafdccfa3a8414c00e8895cb Mon Sep 17 00:00:00 2001 From: Chris Date: Mon, 11 Jan 2021 22:11:45 -0600 Subject: [PATCH 6/8] Attempt at making names more consistent. --- .../main/java/frc/robot/AutoController.java | 22 +++--- .../src/main/java/frc/robot/Drivetrain.java | 56 +++++++-------- .../frc/robot/DrivetrainPoseEstimator.java | 20 +++--- .../java/frc/robot/OperatorInterface.java | 14 ++-- .../main/java/frc/robot/PoseTelemetry.java | 13 ++-- .../src/main/java/frc/robot/Robot.java | 19 +++-- .../src/main/java/frc/sim/DrivetrainSim.java | 70 ++++++++++++------- 7 files changed, 124 insertions(+), 90 deletions(-) diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java index 5a3c475..d38a30b 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java @@ -19,11 +19,11 @@ */ public class AutoController { - private Trajectory m_trajectory; + private Trajectory trajectory; - private RamseteController m_ramsete = new RamseteController(); + private RamseteController ramsete = new RamseteController(); - private Timer m_timer = new Timer(); + private Timer timer = new Timer(); boolean isRunning = false; @@ -32,7 +32,7 @@ public class AutoController { public AutoController(){ // Change this trajectory if you need the robot to follow different paths. - m_trajectory = + trajectory = TrajectoryGenerator.generateTrajectory( new Pose2d(2, 2, new Rotation2d()), List.of(), @@ -44,15 +44,15 @@ public AutoController(){ * @return The starting (initial) pose of the currently-active trajectory */ public Pose2d getInitialPose(){ - return m_trajectory.getInitialPose(); + return trajectory.getInitialPose(); } /** * Starts the controller running. Call this at the start of autonomous */ public void startPath(){ - m_timer.reset(); - m_timer.start(); + timer.reset(); + timer.start(); isRunning = true; } @@ -62,7 +62,7 @@ public void startPath(){ */ public void stopPath(){ isRunning = false; - m_timer.reset(); + timer.reset(); } /** @@ -75,13 +75,13 @@ public void stopPath(){ public ChassisSpeeds getCurMotorCmds( Pose2d curEstPose ){ if(isRunning){ - double elapsed = m_timer.get(); - desiredDtState = m_trajectory.sample(elapsed); + double elapsed = timer.get(); + desiredDtState = trajectory.sample(elapsed); } else { desiredDtState = new Trajectory.State(); } - return m_ramsete.calculate(curEstPose, desiredDtState); + return ramsete.calculate(curEstPose, desiredDtState); } /** diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java index 55200bc..6a802c8 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java @@ -23,44 +23,44 @@ public class Drivetrain { // PWM motor controller output definitions - PWMVictorSPX m_leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin); - PWMVictorSPX m_leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin); - PWMVictorSPX m_rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin); - PWMVictorSPX m_rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin); + 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 m_leftGroup = new SpeedControllerGroup(m_leftLeader, m_leftFollower); - SpeedControllerGroup m_rightGroup = new SpeedControllerGroup(m_rightLeader, m_rightFollower); + 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 m_leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB); - Encoder m_rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB); + 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 m_kinematics = + 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 m_feedforward = new SimpleMotorFeedforward(1, 3); - PIDController m_leftPIDController = new PIDController(8.5, 0, 0); - PIDController m_rightPIDController = new PIDController(8.5, 0, 0); + 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. - m_leftEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); - m_rightEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); + leftEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); + rightEncoder.setDistancePerPulse(2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution); - m_leftEncoder.reset(); - m_rightEncoder.reset(); + leftEncoder.reset(); + rightEncoder.reset(); - m_rightGroup.setInverted(true); + rightGroup.setInverted(true); } @@ -72,24 +72,22 @@ public Drivetrain() { */ public void drive(double xSpeed, double rot) { // Convert our fwd/rev and rotate commands to wheel speed commands - DifferentialDriveWheelSpeeds speeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)); + 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 = - m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); - var rightOutput = - m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + 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 = m_feedforward.calculate(speeds.leftMetersPerSecond); - var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + var leftFeedforward = feedforward.calculate(speeds.leftMetersPerSecond); + var rightFeedforward = feedforward.calculate(speeds.rightMetersPerSecond); // Update the motor controllers with our new motor commands - m_leftGroup.setVoltage(leftOutput + leftFeedforward); - m_rightGroup.setVoltage(rightOutput + rightFeedforward); + leftGroup.setVoltage(leftOutput + leftFeedforward); + rightGroup.setVoltage(rightOutput + rightFeedforward); // Update the pose estimator with the most recent sensor readings. - poseEst.update(new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + poseEst.update(new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()), leftEncoder.getDistance(), rightEncoder.getDistance()); } /** @@ -99,8 +97,8 @@ public void drive(double xSpeed, double rot) { * @param pose */ public void resetOdometry(Pose2d pose) { - m_leftEncoder.reset(); - m_rightEncoder.reset(); + leftEncoder.reset(); + rightEncoder.reset(); poseEst.resetToPose(pose); } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java index eb5dead..90a6483 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -25,7 +25,7 @@ public class DrivetrainPoseEstimator { //Sensors used as part of the Pose Estimation - private final AnalogGyro m_gyro = new AnalogGyro(Constants.kGyroPin); + 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. @@ -38,7 +38,7 @@ public class DrivetrainPoseEstimator { Matrix visionMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); private final DifferentialDrivePoseEstimator m_poseEstimator = - new DifferentialDrivePoseEstimator(m_gyro.getRotation2d(), new Pose2d(), + new DifferentialDrivePoseEstimator(gyro.getRotation2d(), new Pose2d(), stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs); @@ -56,17 +56,17 @@ public DrivetrainPoseEstimator(){ */ public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist){ - m_poseEstimator.update( m_gyro.getRotation2d(), + 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); - //} + 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); + } } /** @@ -76,7 +76,7 @@ public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, * @param pose */ public void resetToPose(Pose2d pose){ - m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); + m_poseEstimator.resetPosition(pose, gyro.getRotation2d()); } /** diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java index 78e14aa..8cb7cbe 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java @@ -6,12 +6,12 @@ public class OperatorInterface { - private XboxController m_controller = new XboxController(0); + private XboxController opCtrl = new XboxController(0); // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 // to 1. - private SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); - private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + private SlewRateLimiter speedLimiter = new SlewRateLimiter(3); + private SlewRateLimiter rotLimiter = new SlewRateLimiter(3); public OperatorInterface(){ @@ -20,7 +20,7 @@ public OperatorInterface(){ public double getFwdRevSpdCmd(){ // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - return -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed; + return -speedLimiter.calculate(opCtrl.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed; } public double getRotateSpdCmd(){ @@ -28,8 +28,12 @@ public double getRotateSpdCmd(){ // 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 -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight)) + return -rotLimiter.calculate(opCtrl.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed; } + + public boolean getSimKickCmd(){ + return opCtrl.getXButtonPressed(); + } } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java index 2b9be5a..8778655 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java @@ -4,23 +4,26 @@ 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 m_field = new Field2d(); + Field2d field = new Field2d(); Pose2d actPose = new Pose2d(); Pose2d desPose = new Pose2d(); Pose2d estPose = new Pose2d(); public PoseTelemetry(){ - SmartDashboard.putData("Field", m_field); + SmartDashboard.putData("Field", field); update(); } public void update(){ - m_field.getObject("DesPose").setPose(desPose); - m_field.getObject("ActPose").setPose(actPose); - m_field.getObject("Robot").setPose(estPose); + field.getObject("DesPose").setPose(desPose); + field.getObject("ActPose").setPose(actPose); + field.getObject("Robot").setPose(estPose); } public void setActualPose(Pose2d in){ diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java index 1c1a289..a2a6672 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java @@ -13,7 +13,7 @@ public class Robot extends TimedRobot { AutoController autoCtrl = new AutoController(); - Drivetrain m_drive = new Drivetrain(); + Drivetrain dt = new Drivetrain(); OperatorInterface opInf = new OperatorInterface(); DrivetrainSim dtSim = new DrivetrainSim(); @@ -35,25 +35,32 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { - ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(m_drive.getCtrlsPoseEstimate()); - m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(dt.getCtrlsPoseEstimate()); + dt.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); pt.setDesiredPose(autoCtrl.getCurPose2d()); } @Override public void teleopPeriodic() { - m_drive.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd()); + dt.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd()); } @Override public void robotPeriodic() { - pt.setEstimatedPose(m_drive.getCtrlsPoseEstimate()); + 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()); } @@ -62,6 +69,6 @@ public void simulationPeriodic() { private void resetOdometery(){ Pose2d startPose = autoCtrl.getInitialPose(); dtSim.resetPose(startPose); - m_drive.resetOdometry(startPose); + dt.resetOdometry(startPose); } } diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java index 082fe3b..63bfb01 100644 --- a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java +++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java @@ -2,8 +2,12 @@ import org.photonvision.SimVisionSystem; +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; @@ -25,21 +29,21 @@ public class DrivetrainSim { // Simulated Sensors - AnalogGyroSim m_gyroSim = new AnalogGyroSim(Constants.kGyroPin); - EncoderSim m_leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA); - EncoderSim m_rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA); + AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin); + EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA); + EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA); // Simulated Motor Controllers - PWMSim m_leftLeader = new PWMSim(Constants.kDtLeftLeaderPin); - PWMSim m_leftFollower = new PWMSim(Constants.kDtLeftFollowerPin); - PWMSim m_rightLeader = new PWMSim(Constants.kDtRightLeaderPin); - PWMSim m_rightFollower = new PWMSim(Constants.kDtRightFollowerPin); + 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 m_drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); - DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim(m_drivetrainSystem, DCMotor.getCIM(2), 8, Constants.kTrackWidth, Constants.kWheelRadius, null); + 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, @@ -73,28 +77,34 @@ public DrivetrainSim(){ * of robot physics forward by a single 20ms step. */ public void update(){ - // 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. - double leftMotorCmd = (m_leftLeader.getSpeed() + m_leftFollower.getSpeed())/2.0; - double rightMotorCmd = (m_rightLeader.getSpeed() + m_rightFollower.getSpeed())/2.0; + 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 - m_drivetrainSimulator.setInputs( + drivetrainSimulator.setInputs( leftMotorCmd * RobotController.getInputVoltage(), -rightMotorCmd * RobotController.getInputVoltage()); - m_drivetrainSimulator.update(0.02); + drivetrainSimulator.update(0.02); // Update our sensors based on the simulated motion of the robot - m_leftEncoderSim.setDistance((m_drivetrainSimulator.getLeftPositionMeters())); - m_leftEncoderSim.setRate((m_drivetrainSimulator.getLeftVelocityMetersPerSecond())); - m_rightEncoderSim.setDistance((m_drivetrainSimulator.getRightPositionMeters())); - m_rightEncoderSim.setRate((m_drivetrainSimulator.getRightVelocityMetersPerSecond())); - m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); // Gyros have an inverted reference frame for angles, so multiply by -1.0; + 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(m_drivetrainSimulator.getPose()); + simVision.processFrame(drivetrainSimulator.getPose()); } @@ -106,15 +116,27 @@ public void update(){ * @param pose */ public void resetPose(Pose2d pose){ - m_drivetrainSimulator.setPose(pose); + drivetrainSimulator.setPose(pose); } /** * @return The simulated robot's position, in meters. */ public Pose2d getCurPose(){ - return m_drivetrainSimulator.getPose(); + 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); + } } From d6b9404ecd00b916af65710ca7fb48855d5a04b4 Mon Sep 17 00:00:00 2001 From: Chris Date: Mon, 11 Jan 2021 22:13:46 -0600 Subject: [PATCH 7/8] Applied vsCode formatter --- .../main/java/frc/robot/AutoController.java | 38 ++- .../src/main/java/frc/robot/Constants.java | 48 ++-- .../src/main/java/frc/robot/Drivetrain.java | 46 ++-- .../frc/robot/DrivetrainPoseEstimator.java | 70 +++--- .../java/frc/robot/OperatorInterface.java | 16 +- .../main/java/frc/robot/PoseTelemetry.java | 14 +- .../src/main/java/frc/robot/Robot.java | 8 +- .../src/main/java/frc/sim/DrivetrainSim.java | 218 +++++++++--------- 8 files changed, 227 insertions(+), 231 deletions(-) diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java index d38a30b..9967141 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/AutoController.java @@ -13,12 +13,12 @@ /** * 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 + * 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(); @@ -29,28 +29,24 @@ public class AutoController { Trajectory.State desiredDtState; - public AutoController(){ + 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)); + 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(){ + public Pose2d getInitialPose() { return trajectory.getInitialPose(); } /** * Starts the controller running. Call this at the start of autonomous */ - public void startPath(){ + public void startPath() { timer.reset(); timer.start(); isRunning = true; @@ -60,21 +56,22 @@ public void startPath(){ /** * Stops the controller from generating commands */ - public void stopPath(){ + 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. + * 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){ + public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) { + + if (isRunning) { double elapsed = timer.get(); desiredDtState = trajectory.sample(elapsed); } else { @@ -85,9 +82,10 @@ public ChassisSpeeds getCurMotorCmds( Pose2d curEstPose ){ } /** - * @return The position which the auto controller is attempting to move the drivetrain to right now. + * @return The position which the auto controller is attempting to move the + * drivetrain to right now. */ - public Pose2d getCurPose2d(){ + public Pose2d getCurPose2d() { return desiredDtState.poseMeters; } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java index 3c3807c..27a88d8 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Constants.java @@ -10,10 +10,10 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d; /** - * 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 + * 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 { @@ -21,10 +21,10 @@ 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 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 kTrackWidth = 0.381 * 2; public static final double kWheelRadius = 0.0508; public static final int kEncoderResolution = 4096; @@ -33,34 +33,34 @@ public class Constants { ////////////////////////////////////////////////////////////////// public static final int kGyroPin = 0; - public static final int kDtLeftEncoderPinA = 0; - public static final int kDtLeftEncoderPinB = 1; + 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 kDtLeftLeaderPin = 1; + public static final int kDtLeftFollowerPin = 2; + public static final int kDtRightLeaderPin = 3; public static final int kDtRightFollowerPin = 4; - ////////////////////////////////////////////////////////////////// - // Vision Processing + // Vision Processing ////////////////////////////////////////////////////////////////// // Name configured in the PhotonVision GUI for the camera - public static final String kCamName = "mainCam"; + 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()); + // 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()); // Definition for the opposite-alliance high goal in 2020 & 2021 public static final double targetHeightAboveGround = 2.3; // meters - public static final double targetWidth = 0.54; // meters - public static final double targetHeight = 0.25; // meters - public static final Pose2d kFarTargetPose = new Pose2d(new Translation2d(Units.feetToMeters(54),Units.feetToMeters(10)), - new Rotation2d(0.0)); - public static final SimVisionTarget kFarTarget = new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight); + public static final double targetWidth = 0.54; // meters + public static final double targetHeight = 0.25; // meters + public static final Pose2d kFarTargetPose = new Pose2d( + new Translation2d(Units.feetToMeters(54), Units.feetToMeters(10)), new Rotation2d(0.0)); + public static final SimVisionTarget kFarTarget = new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, + targetWidth, targetHeight); - } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java index 6a802c8..76285e9 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Drivetrain.java @@ -15,13 +15,12 @@ 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. + * 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); @@ -39,12 +38,13 @@ public class Drivetrain { // Drivetrain Pose Estimation DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator(); - // Kinematics - defines the physical size and shape of the drivetrain, which is required to convert from + // 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); + DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth); - // Closed-loop PIDF controllers for servoing each side of the drivetrain to a specific speed. + // 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); @@ -65,20 +65,25 @@ public Drivetrain() { } /** - * 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. + * 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 + + // 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 + // 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); @@ -87,13 +92,16 @@ public void drive(double xSpeed, double rot) { 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()); + 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). + * 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) { diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java index 90a6483..8e22169 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/DrivetrainPoseEstimator.java @@ -15,67 +15,67 @@ import edu.wpi.first.wpiutil.math.numbers.N3; import edu.wpi.first.wpiutil.math.numbers.N5; - /** * 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. + * 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 + // 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. + // 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 + // 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); + 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(){ + public DrivetrainPoseEstimator() { } /** - * Perform all periodic pose estimation tasks. + * 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 + * @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){ + public void update(DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) { - m_poseEstimator.update( gyro.getRotation2d(), - actWheelSpeeds, - leftDist, rightDist); + m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist); - var res = cam.getLatestResult(); - if(res.hasTargets()){ + 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); + 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). + * 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){ + public void resetToPose(Pose2d pose) { m_poseEstimator.resetPosition(pose, gyro.getRotation2d()); } @@ -84,6 +84,6 @@ public void resetToPose(Pose2d pose){ */ public Pose2d getPoseEst() { return m_poseEstimator.getEstimatedPosition(); - } - + } + } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java index 8cb7cbe..1a9e280 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/OperatorInterface.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.GenericHID; - public class OperatorInterface { private XboxController opCtrl = new XboxController(0); @@ -12,28 +11,27 @@ public class OperatorInterface { // to 1. private SlewRateLimiter speedLimiter = new SlewRateLimiter(3); private SlewRateLimiter rotLimiter = new SlewRateLimiter(3); - - public OperatorInterface(){ + + public OperatorInterface() { } - public double getFwdRevSpdCmd(){ + 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(){ + 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; + return -rotLimiter.calculate(opCtrl.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed; } - public boolean getSimKickCmd(){ + public boolean getSimKickCmd() { return opCtrl.getXButtonPressed(); } - + } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java index 8778655..8cd3394 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/PoseTelemetry.java @@ -10,32 +10,32 @@ public class PoseTelemetry { Field2d field = new Field2d(); - + Pose2d actPose = new Pose2d(); Pose2d desPose = new Pose2d(); Pose2d estPose = new Pose2d(); - public PoseTelemetry(){ + public PoseTelemetry() { SmartDashboard.putData("Field", field); update(); } - public void update(){ + public void update() { field.getObject("DesPose").setPose(desPose); field.getObject("ActPose").setPose(actPose); field.getObject("Robot").setPose(estPose); } - public void setActualPose(Pose2d in){ + public void setActualPose(Pose2d in) { actPose = in; } - public void setDesiredPose(Pose2d in){ + public void setDesiredPose(Pose2d in) { desPose = in; } - public void setEstimatedPose(Pose2d in){ + public void setEstimatedPose(Pose2d in) { estPose = in; } - + } diff --git a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java index a2a6672..4f024fd 100644 --- a/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java +++ b/java/sim-pose-estimation/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import frc.sim.DrivetrainSim; - public class Robot extends TimedRobot { AutoController autoCtrl = new AutoController(); @@ -48,7 +47,7 @@ public void teleopPeriodic() { @Override public void robotPeriodic() { pt.setEstimatedPose(dt.getCtrlsPoseEstimate()); - pt.update(); + pt.update(); } @Override @@ -58,15 +57,14 @@ public void disabledPeriodic() { @Override public void simulationPeriodic() { - if(opInf.getSimKickCmd()){ + if (opInf.getSimKickCmd()) { dtSim.applyKick(); } dtSim.update(); pt.setActualPose(dtSim.getCurPose()); } - - private void resetOdometery(){ + private void resetOdometery() { Pose2d startPose = autoCtrl.getInitialPose(); dtSim.resetPose(startPose); dt.resetOdometry(startPose); diff --git a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java index 63bfb01..b3ffd72 100644 --- a/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java +++ b/java/sim-pose-estimation/src/main/java/frc/sim/DrivetrainSim.java @@ -18,125 +18,119 @@ import edu.wpi.first.wpiutil.math.numbers.N2; import frc.robot.Constants; - /** * 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. + * 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 = 0.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; + // 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 = 0.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); } - // 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); - } - } From 2c28f98f9486c5ad9d833cd4479f3b60db2149e4 Mon Sep 17 00:00:00 2001 From: Chris Date: Tue, 12 Jan 2021 11:29:04 -0600 Subject: [PATCH 8/8] Revised target settings to match 2021 far field target. --- .../resources/blue_square.png | Bin 3195 -> 1970 bytes .../resources/green_square.png | Bin 3110 -> 1980 bytes .../resources/red_square.png | Bin 3198 -> 1979 bytes .../src/main/java/frc/robot/Constants.java | 22 +++++++++++++----- .../src/main/java/frc/sim/DrivetrainSim.java | 2 +- 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/java/sim-pose-estimation/resources/blue_square.png b/java/sim-pose-estimation/resources/blue_square.png index be5b5e735a982b25852af9889434b9ad503da500..63b52bdd4cc44a0b38b88f4742bc82327052368c 100644 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$gTe~DWM4fCrSe0 diff --git a/java/sim-pose-estimation/resources/green_square.png b/java/sim-pose-estimation/resources/green_square.png index bae1dc52ab303f0b08fb30895dbbd0ad10879963..d70576a43f29e2a3d6f9a8a0975094b66c5e1547 100644 GIT binary patch literal 1980 zcmZuyYg7|w8V+o1Cn7ol4n2$9GF*3SUAbQhM#50Q3kZS`K`v5>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 3110 zcmeAS@N?(olHy`uVBq!ia0y~yVD@8RV4BLo1QaRv(dh$HjKx9jP7LeL$-D$|SkfJR z9T^xl_H+M9WCij$3p^r=85o30K$!7fntTNV1NUxE7srr_xVP6Bc^M2i4s6KYDR<_P zfV}tQt{1|>mU^Ewu=7P~&FmSoR c<*kg$!OB}--=91HY}+$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(@wq5s$8OG0`aQYP^LrSDA zLxaw0c7_S5ql!j@U^EqsW`xnQV6-?Ktr12m#nE=bXwzu4bvT3?$Q`qOGA{ID3-9Az SAqDI+FnGH9xvX