From e209aa6572120045bdcf2bac1f2635c176933018 Mon Sep 17 00:00:00 2001 From: Randy Groves Date: Mon, 17 Jan 2022 15:44:49 -0800 Subject: [PATCH] Initial check in - 2022-TestRobot Imported via the GRADLE import from 2022-Template. --- .gitignore | 162 +++++ .vscode/launch.json | 21 + .vscode/settings.json | 29 + .wpilib/wpilib_preferences.json | 6 + README.md | 44 ++ WPILib-License.md | 24 + build.gradle | 87 +++ gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 59536 bytes gradle/wrapper/gradle-wrapper.properties | 5 + gradlew | 234 +++++++ gradlew.bat | 89 +++ settings.gradle | 27 + .../spartronics4915/frc2022/Constants.java | 15 + .../com/spartronics4915/frc2022/Main.java | 23 + .../com/spartronics4915/frc2022/Robot.java | 95 +++ .../frc2022/RobotContainer.java | 45 ++ .../frc2022/commands/ExampleCommand.java | 43 ++ .../frc2022/subsystems/ExampleSubsystem.java | 37 ++ .../lib/hardware/CANCounter.java | 29 + .../lib/hardware/motors/SensorModel.java | 59 ++ .../motors/SpartronicsAnalogEncoder.java | 163 +++++ .../hardware/motors/SpartronicsEncoder.java | 29 + .../lib/hardware/motors/SpartronicsMax.java | 432 ++++++++++++ .../lib/hardware/motors/SpartronicsMotor.java | 188 ++++++ .../lib/hardware/motors/SpartronicsSRX.java | 355 ++++++++++ .../motors/SpartronicsSimulatedMotor.java | 270 ++++++++ .../lib/hardware/sensors/A51IRSensor.java | 23 + .../lib/hardware/sensors/IRSensor.java | 41 ++ .../lib/hardware/sensors/RPLidarA1.java | 622 ++++++++++++++++++ .../lib/hardware/sensors/SpartronicsIMU.java | 8 + .../hardware/sensors/SpartronicsPigeon.java | 28 + .../hardware/sensors/SpartronicsXRS450.java | 16 + .../lib/hardware/sensors/T265Camera.java | 245 +++++++ .../lib/subsystems/SpartronicsSubsystem.java | 153 +++++ .../lib/subsystems/drive/AbstractDrive.java | 141 ++++ .../drive/CharacterizeDriveBaseCommand.java | 104 +++ .../estimator/DrivetrainEstimator.java | 210 ++++++ .../estimator/RobotStateEstimator.java | 284 ++++++++ .../subsystems/estimator/RobotStateMap.java | 171 +++++ .../lib/subsystems/estimator/VisionEvent.java | 13 + .../spartronics4915/lib/util/DeltaTime.java | 45 ++ .../lib/util/Interpolable.java | 27 + .../lib/util/InterpolatingDouble.java | 60 ++ .../lib/util/InterpolatingLong.java | 59 ++ .../lib/util/InterpolatingTreeMap.java | 100 +++ .../lib/util/InverseInterpolable.java | 28 + .../spartronics4915/lib/util/Kinematics.java | 40 ++ .../com/spartronics4915/lib/util/Logger.java | 185 ++++++ .../spartronics4915/lib/util/TriFunction.java | 14 + .../com/spartronics4915/lib/util/Units.java | 55 ++ vendordeps/WPILibNewCommands.json | 37 ++ 51 files changed, 5220 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 README.md create mode 100644 WPILib-License.md create mode 100644 build.gradle create mode 100644 gradle/wrapper/gradle-wrapper.jar create mode 100644 gradle/wrapper/gradle-wrapper.properties create mode 100644 gradlew create mode 100644 gradlew.bat create mode 100644 settings.gradle create mode 100644 src/main/java/com/spartronics4915/frc2022/Constants.java create mode 100644 src/main/java/com/spartronics4915/frc2022/Main.java create mode 100644 src/main/java/com/spartronics4915/frc2022/Robot.java create mode 100644 src/main/java/com/spartronics4915/frc2022/RobotContainer.java create mode 100644 src/main/java/com/spartronics4915/frc2022/commands/ExampleCommand.java create mode 100644 src/main/java/com/spartronics4915/frc2022/subsystems/ExampleSubsystem.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/CANCounter.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsAnalogEncoder.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSimulatedMotor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsPigeon.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsXRS450.java create mode 100644 src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/DrivetrainEstimator.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java create mode 100644 src/main/java/com/spartronics4915/lib/subsystems/estimator/VisionEvent.java create mode 100644 src/main/java/com/spartronics4915/lib/util/DeltaTime.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Interpolable.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java create mode 100644 src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Kinematics.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Logger.java create mode 100644 src/main/java/com/spartronics4915/lib/util/TriFunction.java create mode 100644 src/main/java/com/spartronics4915/lib/util/Units.java create mode 100644 vendordeps/WPILibNewCommands.json diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9535c83 --- /dev/null +++ b/.gitignore @@ -0,0 +1,162 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### 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 +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# Simulation GUI and other tools window save file +*-window.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.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/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4ed293b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,29 @@ +{ + "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 + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests" +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..e579c84 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2022", + "teamNumber": 4915 +} \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..903a72c --- /dev/null +++ b/README.md @@ -0,0 +1,44 @@ +# Spartronics 4915's Test Bed 2022 + +## Usage + +### Prerequisites + +Ensure you have Java 11 installed on your computer and added to `$PATH`. +Visual Studio Code, Git, and the WPILib extension are also helpful for development. + +The (highly) recommended way to install these is through [the WPILib installer](https://docs.wpilib.org/en/latest/docs/zero-to-robot/step-2/wpilib-setup.html +). + +### Installation + +Once your development environment is set up, `clone` this repository to your computer. + +Run `./gradlew tasks` to list available Gradle options. + +Run `./gradlew build` or use the WPILib extension's `Build Robot Code` option to build or compile the codebase. + +Run `./gradlew deploy` or use the WPILib extension's `Deploy Robot Code` option while connected to the robot to build and deploy your code. + +## Style Guide + +- **Indentation**: Four spaces, no tabs. +- **Braces**: Each brace goes on its own line. + - This is verbose, but intentionally so - brace errors are common, + often difficult-to-diagnose, and have caused problems at bad times in the past. +- Line length: Eighty characters or less, as a rule of thumb. + - This improves legibility and makes it easier to view files side-by-side. + - The formatter doesn't actually enforce this until lines are 120 characters, + to give you flexibility around how you'd like to wrap your lines. +- Variable names: `camelCase`. + - **Member variables**: Prefix with `m`, ex. `mMemberVariable`. + - **Final constants**: Prefix with `k`, ex. `kFinalConstant`. + - Parameters: No prefix. +- Class and file names: `PascalCase`. +- Folder names: `lowercase`. +- **Package structure**: `com.spartronics4915.frcXXXX` + +(differences from WPILib are in **bold**) + +A relatively unopinionated Eclipse-style formatter is included in the `.settings` folder. +This can be run at any time by opening Visual Studio Code's Command Palette (`Ctrl+Shift+P`) and selecting `Format Document`. diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..3d5a824 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..4fc0d8f --- /dev/null +++ b/build.gradle @@ -0,0 +1,87 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2022.2.1" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "com.spartronics4915.frc2022.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('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 = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// 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.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'junit:junit:4.12' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// 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) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..7454180f2ae8848c63b8b4dea2cb829da983f2fa GIT binary patch literal 59536 zcma&NbC71ylI~qywr$(CZQJHswz}-9F59+k+g;UV+cs{`J?GrGXYR~=-ydruB3JCa zB64N^cILAcWk5iofq)<(fq;O7{th4@;QxID0)qN`mJ?GIqLY#rX8-|G{5M0pdVW5^ zzXk$-2kQTAC?_N@B`&6-N-rmVFE=$QD?>*=4<|!MJu@}isLc4AW#{m2if&A5T5g&~ ziuMQeS*U5sL6J698wOd)K@oK@1{peP5&Esut<#VH^u)gp`9H4)`uE!2$>RTctN+^u z=ASkePDZA-X8)rp%D;p*~P?*a_=*Kwc<^>QSH|^<0>o37lt^+Mj1;4YvJ(JR-Y+?%Nu}JAYj5 z_Qc5%Ao#F?q32i?ZaN2OSNhWL;2oDEw_({7ZbgUjna!Fqn3NzLM@-EWFPZVmc>(fZ z0&bF-Ch#p9C{YJT9Rcr3+Y_uR^At1^BxZ#eo>$PLJF3=;t_$2|t+_6gg5(j{TmjYU zK12c&lE?Eh+2u2&6Gf*IdKS&6?rYbSEKBN!rv{YCm|Rt=UlPcW9j`0o6{66#y5t9C zruFA2iKd=H%jHf%ypOkxLnO8#H}#Zt{8p!oi6)7#NqoF({t6|J^?1e*oxqng9Q2Cc zg%5Vu!em)}Yuj?kaP!D?b?(C*w!1;>R=j90+RTkyEXz+9CufZ$C^umX^+4|JYaO<5 zmIM3#dv`DGM;@F6;(t!WngZSYzHx?9&$xEF70D1BvfVj<%+b#)vz)2iLCrTeYzUcL z(OBnNoG6Le%M+@2oo)&jdOg=iCszzv59e zDRCeaX8l1hC=8LbBt|k5?CXgep=3r9BXx1uR8!p%Z|0+4Xro=xi0G!e{c4U~1j6!) zH6adq0}#l{%*1U(Cb%4AJ}VLWKBPi0MoKFaQH6x?^hQ!6em@993xdtS%_dmevzeNl z(o?YlOI=jl(`L9^ z0O+H9k$_@`6L13eTT8ci-V0ljDMD|0ifUw|Q-Hep$xYj0hTO@0%IS^TD4b4n6EKDG z??uM;MEx`s98KYN(K0>c!C3HZdZ{+_53DO%9k5W%pr6yJusQAv_;IA}925Y%;+!tY z%2k!YQmLLOr{rF~!s<3-WEUs)`ix_mSU|cNRBIWxOox_Yb7Z=~Q45ZNe*u|m^|)d* zog=i>`=bTe!|;8F+#H>EjIMcgWcG2ORD`w0WD;YZAy5#s{65~qfI6o$+Ty&-hyMyJ z3Ra~t>R!p=5ZpxA;QkDAoPi4sYOP6>LT+}{xp}tk+<0k^CKCFdNYG(Es>p0gqD)jP zWOeX5G;9(m@?GOG7g;e74i_|SmE?`B2i;sLYwRWKLy0RLW!Hx`=!LH3&k=FuCsM=9M4|GqzA)anEHfxkB z?2iK-u(DC_T1};KaUT@3nP~LEcENT^UgPvp!QC@Dw&PVAhaEYrPey{nkcn(ro|r7XUz z%#(=$7D8uP_uU-oPHhd>>^adbCSQetgSG`e$U|7mr!`|bU0aHl_cmL)na-5x1#OsVE#m*+k84Y^+UMeSAa zbrVZHU=mFwXEaGHtXQq`2ZtjfS!B2H{5A<3(nb-6ARVV8kEmOkx6D2x7~-6hl;*-*}2Xz;J#a8Wn;_B5=m zl3dY;%krf?i-Ok^Pal-}4F`{F@TYPTwTEhxpZK5WCpfD^UmM_iYPe}wpE!Djai6_{ z*pGO=WB47#Xjb7!n2Ma)s^yeR*1rTxp`Mt4sfA+`HwZf%!7ZqGosPkw69`Ix5Ku6G z@Pa;pjzV&dn{M=QDx89t?p?d9gna*}jBly*#1!6}5K<*xDPJ{wv4& zM$17DFd~L*Te3A%yD;Dp9UGWTjRxAvMu!j^Tbc}2v~q^59d4bz zvu#!IJCy(BcWTc`;v$9tH;J%oiSJ_i7s;2`JXZF+qd4C)vY!hyCtl)sJIC{ebI*0> z@x>;EzyBv>AI-~{D6l6{ST=em*U( z(r$nuXY-#CCi^8Z2#v#UXOt`dbYN1z5jzNF2 z411?w)whZrfA20;nl&C1Gi+gk<`JSm+{|*2o<< zqM#@z_D`Cn|0H^9$|Tah)0M_X4c37|KQ*PmoT@%xHc3L1ZY6(p(sNXHa&49Frzto& zR`c~ClHpE~4Z=uKa5S(-?M8EJ$zt0&fJk~p$M#fGN1-y$7!37hld`Uw>Urri(DxLa;=#rK0g4J)pXMC zxzraOVw1+kNWpi#P=6(qxf`zSdUC?D$i`8ZI@F>k6k zz21?d+dw7b&i*>Kv5L(LH-?J%@WnqT7j#qZ9B>|Zl+=> z^U-pV@1y_ptHo4hl^cPRWewbLQ#g6XYQ@EkiP z;(=SU!yhjHp%1&MsU`FV1Z_#K1&(|5n(7IHbx&gG28HNT)*~-BQi372@|->2Aw5It z0CBpUcMA*QvsPy)#lr!lIdCi@1k4V2m!NH)%Px(vu-r(Q)HYc!p zJ^$|)j^E#q#QOgcb^pd74^JUi7fUmMiNP_o*lvx*q%_odv49Dsv$NV;6J z9GOXKomA{2Pb{w}&+yHtH?IkJJu~}Z?{Uk++2mB8zyvh*xhHKE``99>y#TdD z&(MH^^JHf;g(Tbb^&8P*;_i*2&fS$7${3WJtV7K&&(MBV2~)2KB3%cWg#1!VE~k#C z!;A;?p$s{ihyojEZz+$I1)L}&G~ml=udD9qh>Tu(ylv)?YcJT3ihapi!zgPtWb*CP zlLLJSRCj-^w?@;RU9aL2zDZY1`I3d<&OMuW=c3$o0#STpv_p3b9Wtbql>w^bBi~u4 z3D8KyF?YE?=HcKk!xcp@Cigvzy=lnFgc^9c%(^F22BWYNAYRSho@~*~S)4%AhEttv zvq>7X!!EWKG?mOd9&n>vvH1p4VzE?HCuxT-u+F&mnsfDI^}*-d00-KAauEaXqg3k@ zy#)MGX!X;&3&0s}F3q40ZmVM$(H3CLfpdL?hB6nVqMxX)q=1b}o_PG%r~hZ4gUfSp zOH4qlEOW4OMUc)_m)fMR_rl^pCfXc{$fQbI*E&mV77}kRF z&{<06AJyJ!e863o-V>FA1a9Eemx6>^F$~9ppt()ZbPGfg_NdRXBWoZnDy2;#ODgf! zgl?iOcF7Meo|{AF>KDwTgYrJLb$L2%%BEtO>T$C?|9bAB&}s;gI?lY#^tttY&hfr# zKhC+&b-rpg_?~uVK%S@mQleU#_xCsvIPK*<`E0fHE1&!J7!xD#IB|SSPW6-PyuqGn3^M^Rz%WT{e?OI^svARX&SAdU77V(C~ zM$H{Kg59op{<|8ry9ecfP%=kFm(-!W&?U0@<%z*+!*<e0XesMxRFu9QnGqun6R_%T+B%&9Dtk?*d$Q zb~>84jEAPi@&F@3wAa^Lzc(AJz5gsfZ7J53;@D<;Klpl?sK&u@gie`~vTsbOE~Cd4 z%kr56mI|#b(Jk&;p6plVwmNB0H@0SmgdmjIn5Ne@)}7Vty(yb2t3ev@22AE^s!KaN zyQ>j+F3w=wnx7w@FVCRe+`vUH)3gW%_72fxzqX!S&!dchdkRiHbXW1FMrIIBwjsai8`CB2r4mAbwp%rrO>3B$Zw;9=%fXI9B{d(UzVap7u z6piC-FQ)>}VOEuPpuqznpY`hN4dGa_1Xz9rVg(;H$5Te^F0dDv*gz9JS<|>>U0J^# z6)(4ICh+N_Q`Ft0hF|3fSHs*?a=XC;e`sJaU9&d>X4l?1W=|fr!5ShD|nv$GK;j46@BV6+{oRbWfqOBRb!ir88XD*SbC(LF}I1h#6@dvK%Toe%@ zhDyG$93H8Eu&gCYddP58iF3oQH*zLbNI;rN@E{T9%A8!=v#JLxKyUe}e}BJpB{~uN zqgxRgo0*-@-iaHPV8bTOH(rS(huwK1Xg0u+e!`(Irzu@Bld&s5&bWgVc@m7;JgELd zimVs`>vQ}B_1(2#rv#N9O`fJpVfPc7V2nv34PC);Dzbb;p!6pqHzvy?2pD&1NE)?A zt(t-ucqy@wn9`^MN5apa7K|L=9>ISC>xoc#>{@e}m#YAAa1*8-RUMKwbm|;5p>T`Z zNf*ph@tnF{gmDa3uwwN(g=`Rh)4!&)^oOy@VJaK4lMT&5#YbXkl`q?<*XtsqD z9PRK6bqb)fJw0g-^a@nu`^?71k|m3RPRjt;pIkCo1{*pdqbVs-Yl>4E>3fZx3Sv44grW=*qdSoiZ9?X0wWyO4`yDHh2E!9I!ZFi zVL8|VtW38}BOJHW(Ax#KL_KQzarbuE{(%TA)AY)@tY4%A%P%SqIU~8~-Lp3qY;U-} z`h_Gel7;K1h}7$_5ZZT0&%$Lxxr-<89V&&TCsu}LL#!xpQ1O31jaa{U34~^le*Y%L za?7$>Jk^k^pS^_M&cDs}NgXlR>16AHkSK-4TRaJSh#h&p!-!vQY%f+bmn6x`4fwTp z$727L^y`~!exvmE^W&#@uY!NxJi`g!i#(++!)?iJ(1)2Wk;RN zFK&O4eTkP$Xn~4bB|q8y(btx$R#D`O@epi4ofcETrx!IM(kWNEe42Qh(8*KqfP(c0 zouBl6>Fc_zM+V;F3znbo{x#%!?mH3`_ANJ?y7ppxS@glg#S9^MXu|FM&ynpz3o&Qh z2ujAHLF3($pH}0jXQsa#?t--TnF1P73b?4`KeJ9^qK-USHE)4!IYgMn-7z|=ALF5SNGkrtPG@Y~niUQV2?g$vzJN3nZ{7;HZHzWAeQ;5P|@Tl3YHpyznGG4-f4=XflwSJY+58-+wf?~Fg@1p1wkzuu-RF3j2JX37SQUc? zQ4v%`V8z9ZVZVqS8h|@@RpD?n0W<=hk=3Cf8R?d^9YK&e9ZybFY%jdnA)PeHvtBe- zhMLD+SSteHBq*q)d6x{)s1UrsO!byyLS$58WK;sqip$Mk{l)Y(_6hEIBsIjCr5t>( z7CdKUrJTrW%qZ#1z^n*Lb8#VdfzPw~OIL76aC+Rhr<~;4Tl!sw?Rj6hXj4XWa#6Tp z@)kJ~qOV)^Rh*-?aG>ic2*NlC2M7&LUzc9RT6WM%Cpe78`iAowe!>(T0jo&ivn8-7 zs{Qa@cGy$rE-3AY0V(l8wjI^uB8Lchj@?L}fYal^>T9z;8juH@?rG&g-t+R2dVDBe zq!K%{e-rT5jX19`(bP23LUN4+_zh2KD~EAYzhpEO3MUG8@}uBHH@4J zd`>_(K4q&>*k82(dDuC)X6JuPrBBubOg7qZ{?x!r@{%0);*`h*^F|%o?&1wX?Wr4b z1~&cy#PUuES{C#xJ84!z<1tp9sfrR(i%Tu^jnXy;4`Xk;AQCdFC@?V%|; zySdC7qS|uQRcH}EFZH%mMB~7gi}a0utE}ZE_}8PQH8f;H%PN41Cb9R%w5Oi5el^fd z$n{3SqLCnrF##x?4sa^r!O$7NX!}&}V;0ZGQ&K&i%6$3C_dR%I7%gdQ;KT6YZiQrW zk%q<74oVBV>@}CvJ4Wj!d^?#Zwq(b$E1ze4$99DuNg?6t9H}k_|D7KWD7i0-g*EO7 z;5{hSIYE4DMOK3H%|f5Edx+S0VI0Yw!tsaRS2&Il2)ea^8R5TG72BrJue|f_{2UHa z@w;^c|K3da#$TB0P3;MPlF7RuQeXT$ zS<<|C0OF(k)>fr&wOB=gP8!Qm>F41u;3esv7_0l%QHt(~+n; zf!G6%hp;Gfa9L9=AceiZs~tK+Tf*Wof=4!u{nIO90jH@iS0l+#%8=~%ASzFv7zqSB^?!@N7)kp0t&tCGLmzXSRMRyxCmCYUD2!B`? zhs$4%KO~m=VFk3Buv9osha{v+mAEq=ik3RdK@;WWTV_g&-$U4IM{1IhGX{pAu%Z&H zFfwCpUsX%RKg);B@7OUzZ{Hn{q6Vv!3#8fAg!P$IEx<0vAx;GU%}0{VIsmFBPq_mb zpe^BChDK>sc-WLKl<6 zwbW|e&d&dv9Wu0goueyu>(JyPx1mz0v4E?cJjFuKF71Q1)AL8jHO$!fYT3(;U3Re* zPPOe%*O+@JYt1bW`!W_1!mN&=w3G9ru1XsmwfS~BJ))PhD(+_J_^N6j)sx5VwbWK| zwRyC?W<`pOCY)b#AS?rluxuuGf-AJ=D!M36l{ua?@SJ5>e!IBr3CXIxWw5xUZ@Xrw z_R@%?{>d%Ld4p}nEsiA@v*nc6Ah!MUs?GA7e5Q5lPpp0@`%5xY$C;{%rz24$;vR#* zBP=a{)K#CwIY%p} zXVdxTQ^HS@O&~eIftU+Qt^~(DGxrdi3k}DdT^I7Iy5SMOp$QuD8s;+93YQ!OY{eB24%xY7ml@|M7I(Nb@K_-?F;2?et|CKkuZK_>+>Lvg!>JE~wN`BI|_h6$qi!P)+K-1Hh(1;a`os z55)4Q{oJiA(lQM#;w#Ta%T0jDNXIPM_bgESMCDEg6rM33anEr}=|Fn6)|jBP6Y}u{ zv9@%7*#RI9;fv;Yii5CI+KrRdr0DKh=L>)eO4q$1zmcSmglsV`*N(x=&Wx`*v!!hn6X-l0 zP_m;X??O(skcj+oS$cIdKhfT%ABAzz3w^la-Ucw?yBPEC+=Pe_vU8nd-HV5YX6X8r zZih&j^eLU=%*;VzhUyoLF;#8QsEfmByk+Y~caBqSvQaaWf2a{JKB9B>V&r?l^rXaC z8)6AdR@Qy_BxQrE2Fk?ewD!SwLuMj@&d_n5RZFf7=>O>hzVE*seW3U?_p|R^CfoY`?|#x9)-*yjv#lo&zP=uI`M?J zbzC<^3x7GfXA4{FZ72{PE*-mNHyy59Q;kYG@BB~NhTd6pm2Oj=_ zizmD?MKVRkT^KmXuhsk?eRQllPo2Ubk=uCKiZ&u3Xjj~<(!M94c)Tez@9M1Gfs5JV z->@II)CDJOXTtPrQudNjE}Eltbjq>6KiwAwqvAKd^|g!exgLG3;wP+#mZYr`cy3#39e653d=jrR-ulW|h#ddHu(m9mFoW~2yE zz5?dB%6vF}+`-&-W8vy^OCxm3_{02royjvmwjlp+eQDzFVEUiyO#gLv%QdDSI#3W* z?3!lL8clTaNo-DVJw@ynq?q!%6hTQi35&^>P85G$TqNt78%9_sSJt2RThO|JzM$iL zg|wjxdMC2|Icc5rX*qPL(coL!u>-xxz-rFiC!6hD1IR%|HSRsV3>Kq~&vJ=s3M5y8SG%YBQ|{^l#LGlg!D?E>2yR*eV%9m$_J6VGQ~AIh&P$_aFbh zULr0Z$QE!QpkP=aAeR4ny<#3Fwyw@rZf4?Ewq`;mCVv}xaz+3ni+}a=k~P+yaWt^L z@w67!DqVf7D%7XtXX5xBW;Co|HvQ8WR1k?r2cZD%U;2$bsM%u8{JUJ5Z0k= zZJARv^vFkmWx15CB=rb=D4${+#DVqy5$C%bf`!T0+epLJLnh1jwCdb*zuCL}eEFvE z{rO1%gxg>1!W(I!owu*mJZ0@6FM(?C+d*CeceZRW_4id*D9p5nzMY&{mWqrJomjIZ z97ZNnZ3_%Hx8dn;H>p8m7F#^2;T%yZ3H;a&N7tm=Lvs&lgJLW{V1@h&6Vy~!+Ffbb zv(n3+v)_D$}dqd!2>Y2B)#<+o}LH#%ogGi2-?xRIH)1!SD)u-L65B&bsJTC=LiaF+YOCif2dUX6uAA|#+vNR z>U+KQekVGon)Yi<93(d!(yw1h3&X0N(PxN2{%vn}cnV?rYw z$N^}_o!XUB!mckL`yO1rnUaI4wrOeQ(+&k?2mi47hzxSD`N#-byqd1IhEoh!PGq>t z_MRy{5B0eKY>;Ao3z$RUU7U+i?iX^&r739F)itdrTpAi-NN0=?^m%?{A9Ly2pVv>Lqs6moTP?T2-AHqFD-o_ znVr|7OAS#AEH}h8SRPQ@NGG47dO}l=t07__+iK8nHw^(AHx&Wb<%jPc$$jl6_p(b$ z)!pi(0fQodCHfM)KMEMUR&UID>}m^(!{C^U7sBDOA)$VThRCI0_+2=( zV8mMq0R(#z;C|7$m>$>`tX+T|xGt(+Y48@ZYu#z;0pCgYgmMVbFb!$?%yhZqP_nhn zy4<#3P1oQ#2b51NU1mGnHP$cf0j-YOgAA}A$QoL6JVLcmExs(kU{4z;PBHJD%_=0F z>+sQV`mzijSIT7xn%PiDKHOujX;n|M&qr1T@rOxTdxtZ!&u&3HHFLYD5$RLQ=heur zb>+AFokUVQeJy-#LP*^)spt{mb@Mqe=A~-4p0b+Bt|pZ+@CY+%x}9f}izU5;4&QFE zO1bhg&A4uC1)Zb67kuowWY4xbo&J=%yoXlFB)&$d*-}kjBu|w!^zbD1YPc0-#XTJr z)pm2RDy%J3jlqSMq|o%xGS$bPwn4AqitC6&e?pqWcjWPt{3I{>CBy;hg0Umh#c;hU3RhCUX=8aR>rmd` z7Orw(5tcM{|-^J?ZAA9KP|)X6n9$-kvr#j5YDecTM6n z&07(nD^qb8hpF0B^z^pQ*%5ePYkv&FabrlI61ntiVp!!C8y^}|<2xgAd#FY=8b*y( zuQOuvy2`Ii^`VBNJB&R!0{hABYX55ooCAJSSevl4RPqEGb)iy_0H}v@vFwFzD%>#I>)3PsouQ+_Kkbqy*kKdHdfkN7NBcq%V{x^fSxgXpg7$bF& zj!6AQbDY(1u#1_A#1UO9AxiZaCVN2F0wGXdY*g@x$ByvUA?ePdide0dmr#}udE%K| z3*k}Vv2Ew2u1FXBaVA6aerI36R&rzEZeDDCl5!t0J=ug6kuNZzH>3i_VN`%BsaVB3 zQYw|Xub_SGf{)F{$ZX5`Jc!X!;eybjP+o$I{Z^Hsj@D=E{MnnL+TbC@HEU2DjG{3-LDGIbq()U87x4eS;JXnSh;lRlJ z>EL3D>wHt-+wTjQF$fGyDO$>d+(fq@bPpLBS~xA~R=3JPbS{tzN(u~m#Po!?H;IYv zE;?8%^vle|%#oux(Lj!YzBKv+Fd}*Ur-dCBoX*t{KeNM*n~ZPYJ4NNKkI^MFbz9!v z4(Bvm*Kc!-$%VFEewYJKz-CQN{`2}KX4*CeJEs+Q(!kI%hN1!1P6iOq?ovz}X0IOi z)YfWpwW@pK08^69#wSyCZkX9?uZD?C^@rw^Y?gLS_xmFKkooyx$*^5#cPqntNTtSG zlP>XLMj2!VF^0k#ole7`-c~*~+_T5ls?x4)ah(j8vo_ zwb%S8qoaZqY0-$ZI+ViIA_1~~rAH7K_+yFS{0rT@eQtTAdz#8E5VpwnW!zJ_^{Utv zlW5Iar3V5t&H4D6A=>?mq;G92;1cg9a2sf;gY9pJDVKn$DYdQlvfXq}zz8#LyPGq@ z+`YUMD;^-6w&r-82JL7mA8&M~Pj@aK!m{0+^v<|t%APYf7`}jGEhdYLqsHW-Le9TL z_hZZ1gbrz7$f9^fAzVIP30^KIz!!#+DRLL+qMszvI_BpOSmjtl$hh;&UeM{ER@INV zcI}VbiVTPoN|iSna@=7XkP&-4#06C};8ajbxJ4Gcq8(vWv4*&X8bM^T$mBk75Q92j z1v&%a;OSKc8EIrodmIiw$lOES2hzGDcjjB`kEDfJe{r}yE6`eZL zEB`9u>Cl0IsQ+t}`-cx}{6jqcANucqIB>Qmga_&<+80E2Q|VHHQ$YlAt{6`Qu`HA3 z03s0-sSlwbvgi&_R8s={6<~M^pGvBNjKOa>tWenzS8s zR>L7R5aZ=mSU{f?ib4Grx$AeFvtO5N|D>9#)ChH#Fny2maHWHOf2G=#<9Myot#+4u zWVa6d^Vseq_0=#AYS(-m$Lp;*8nC_6jXIjEM`omUmtH@QDs3|G)i4j*#_?#UYVZvJ z?YjT-?!4Q{BNun;dKBWLEw2C-VeAz`%?A>p;)PL}TAZn5j~HK>v1W&anteARlE+~+ zj>c(F;?qO3pXBb|#OZdQnm<4xWmn~;DR5SDMxt0UK_F^&eD|KZ=O;tO3vy4@4h^;2 zUL~-z`-P1aOe?|ZC1BgVsL)2^J-&vIFI%q@40w0{jjEfeVl)i9(~bt2z#2Vm)p`V_ z1;6$Ae7=YXk#=Qkd24Y23t&GvRxaOoad~NbJ+6pxqzJ>FY#Td7@`N5xp!n(c!=RE& z&<<@^a$_Ys8jqz4|5Nk#FY$~|FPC0`*a5HH!|Gssa9=~66&xG9)|=pOOJ2KE5|YrR zw!w6K2aC=J$t?L-;}5hn6mHd%hC;p8P|Dgh6D>hGnXPgi;6r+eA=?f72y9(Cf_ho{ zH6#)uD&R=73^$$NE;5piWX2bzR67fQ)`b=85o0eOLGI4c-Tb@-KNi2pz=Ke@SDcPn za$AxXib84`!Sf;Z3B@TSo`Dz7GM5Kf(@PR>Ghzi=BBxK8wRp>YQoXm+iL>H*Jo9M3 z6w&E?BC8AFTFT&Tv8zf+m9<&S&%dIaZ)Aoqkak_$r-2{$d~0g2oLETx9Y`eOAf14QXEQw3tJne;fdzl@wV#TFXSLXM2428F-Q}t+n2g%vPRMUzYPvzQ9f# zu(liiJem9P*?0%V@RwA7F53r~|I!Ty)<*AsMX3J{_4&}{6pT%Tpw>)^|DJ)>gpS~1rNEh z0$D?uO8mG?H;2BwM5a*26^7YO$XjUm40XmBsb63MoR;bJh63J;OngS5sSI+o2HA;W zdZV#8pDpC9Oez&L8loZO)MClRz!_!WD&QRtQxnazhT%Vj6Wl4G11nUk8*vSeVab@N#oJ}`KyJv+8Mo@T1-pqZ1t|?cnaVOd;1(h9 z!$DrN=jcGsVYE-0-n?oCJ^4x)F}E;UaD-LZUIzcD?W^ficqJWM%QLy6QikrM1aKZC zi{?;oKwq^Vsr|&`i{jIphA8S6G4)$KGvpULjH%9u(Dq247;R#l&I0{IhcC|oBF*Al zvLo7Xte=C{aIt*otJD}BUq)|_pdR>{zBMT< z(^1RpZv*l*m*OV^8>9&asGBo8h*_4q*)-eCv*|Pq=XNGrZE)^(SF7^{QE_~4VDB(o zVcPA_!G+2CAtLbl+`=Q~9iW`4ZRLku!uB?;tWqVjB0lEOf}2RD7dJ=BExy=<9wkb- z9&7{XFA%n#JsHYN8t5d~=T~5DcW4$B%3M+nNvC2`0!#@sckqlzo5;hhGi(D9=*A4` z5ynobawSPRtWn&CDLEs3Xf`(8^zDP=NdF~F^s&={l7(aw&EG}KWpMjtmz7j_VLO;@ zM2NVLDxZ@GIv7*gzl1 zjq78tv*8#WSY`}Su0&C;2F$Ze(q>F(@Wm^Gw!)(j;dk9Ad{STaxn)IV9FZhm*n+U} zi;4y*3v%A`_c7a__DJ8D1b@dl0Std3F||4Wtvi)fCcBRh!X9$1x!_VzUh>*S5s!oq z;qd{J_r79EL2wIeiGAqFstWtkfIJpjVh%zFo*=55B9Zq~y0=^iqHWfQl@O!Ak;(o*m!pZqe9 z%U2oDOhR)BvW8&F70L;2TpkzIutIvNQaTjjs5V#8mV4!NQ}zN=i`i@WI1z0eN-iCS z;vL-Wxc^Vc_qK<5RPh(}*8dLT{~GzE{w2o$2kMFaEl&q zP{V=>&3kW7tWaK-Exy{~`v4J0U#OZBk{a9{&)&QG18L@6=bsZ1zC_d{{pKZ-Ey>I> z;8H0t4bwyQqgu4hmO`3|4K{R*5>qnQ&gOfdy?z`XD%e5+pTDzUt3`k^u~SaL&XMe= z9*h#kT(*Q9jO#w2Hd|Mr-%DV8i_1{J1MU~XJ3!WUplhXDYBpJH><0OU`**nIvPIof z|N8@I=wA)sf45SAvx||f?Z5uB$kz1qL3Ky_{%RPdP5iN-D2!p5scq}buuC00C@jom zhfGKm3|f?Z0iQ|K$Z~!`8{nmAS1r+fp6r#YDOS8V*;K&Gs7Lc&f^$RC66O|)28oh`NHy&vq zJh+hAw8+ybTB0@VhWN^0iiTnLsCWbS_y`^gs!LX!Lw{yE``!UVzrV24tP8o;I6-65 z1MUiHw^{bB15tmrVT*7-#sj6cs~z`wk52YQJ*TG{SE;KTm#Hf#a~|<(|ImHH17nNM z`Ub{+J3dMD!)mzC8b(2tZtokKW5pAwHa?NFiso~# z1*iaNh4lQ4TS)|@G)H4dZV@l*Vd;Rw;-;odDhW2&lJ%m@jz+Panv7LQm~2Js6rOW3 z0_&2cW^b^MYW3)@o;neZ<{B4c#m48dAl$GCc=$>ErDe|?y@z`$uq3xd(%aAsX)D%l z>y*SQ%My`yDP*zof|3@_w#cjaW_YW4BdA;#Glg1RQcJGY*CJ9`H{@|D+*e~*457kd z73p<%fB^PV!Ybw@)Dr%(ZJbX}xmCStCYv#K3O32ej{$9IzM^I{6FJ8!(=azt7RWf4 z7ib0UOPqN40X!wOnFOoddd8`!_IN~9O)#HRTyjfc#&MCZ zZAMzOVB=;qwt8gV?{Y2?b=iSZG~RF~uyx18K)IDFLl})G1v@$(s{O4@RJ%OTJyF+Cpcx4jmy|F3euCnMK!P2WTDu5j z{{gD$=M*pH!GGzL%P)V2*ROm>!$Y=z|D`!_yY6e7SU$~a5q8?hZGgaYqaiLnkK%?0 zs#oI%;zOxF@g*@(V4p!$7dS1rOr6GVs6uYCTt2h)eB4?(&w8{#o)s#%gN@BBosRUe z)@P@8_Zm89pr~)b>e{tbPC~&_MR--iB{=)y;INU5#)@Gix-YpgP<-c2Ms{9zuCX|3 z!p(?VaXww&(w&uBHzoT%!A2=3HAP>SDxcljrego7rY|%hxy3XlODWffO_%g|l+7Y_ zqV(xbu)s4lV=l7M;f>vJl{`6qBm>#ZeMA}kXb97Z)?R97EkoI?x6Lp0yu1Z>PS?2{ z0QQ(8D)|lc9CO3B~e(pQM&5(1y&y=e>C^X$`)_&XuaI!IgDTVqt31wX#n+@!a_A0ZQkA zCJ2@M_4Gb5MfCrm5UPggeyh)8 zO9?`B0J#rkoCx(R0I!ko_2?iO@|oRf1;3r+i)w-2&j?=;NVIdPFsB)`|IC0zk6r9c zRrkfxWsiJ(#8QndNJj@{@WP2Ackr|r1VxV{7S&rSU(^)-M8gV>@UzOLXu9K<{6e{T zXJ6b92r$!|lwjhmgqkdswY&}c)KW4A)-ac%sU;2^fvq7gfUW4Bw$b!i@duy1CAxSn z(pyh$^Z=&O-q<{bZUP+$U}=*#M9uVc>CQVgDs4swy5&8RAHZ~$)hrTF4W zPsSa~qYv_0mJnF89RnnJTH`3}w4?~epFl=D(35$ zWa07ON$`OMBOHgCmfO(9RFc<)?$x)N}Jd2A(<*Ll7+4jrRt9w zwGxExUXd9VB#I|DwfxvJ;HZ8Q{37^wDhaZ%O!oO(HpcqfLH%#a#!~;Jl7F5>EX_=8 z{()l2NqPz>La3qJR;_v+wlK>GsHl;uRA8%j`A|yH@k5r%55S9{*Cp%uw6t`qc1!*T za2OeqtQj7sAp#Q~=5Fs&aCR9v>5V+s&RdNvo&H~6FJOjvaj--2sYYBvMq;55%z8^o z|BJDA4vzfow#DO#ZQHh;Oq_{r+qP{R9ox2TOgwQiv7Ow!zjN+A@BN;0tA2lUb#+zO z(^b89eV)D7UVE+h{mcNc6&GtpOqDn_?VAQ)Vob$hlFwW%xh>D#wml{t&Ofmm_d_+; zKDxzdr}`n2Rw`DtyIjrG)eD0vut$}dJAZ0AohZ+ZQdWXn_Z@dI_y=7t3q8x#pDI-K z2VVc&EGq445Rq-j0=U=Zx`oBaBjsefY;%)Co>J3v4l8V(T8H?49_@;K6q#r~Wwppc z4XW0(4k}cP=5ex>-Xt3oATZ~bBWKv)aw|I|Lx=9C1s~&b77idz({&q3T(Y(KbWO?+ zmcZ6?WeUsGk6>km*~234YC+2e6Zxdl~<_g2J|IE`GH%n<%PRv-50; zH{tnVts*S5*_RxFT9eM0z-pksIb^drUq4>QSww=u;UFCv2AhOuXE*V4z?MM`|ABOC4P;OfhS(M{1|c%QZ=!%rQTDFx`+}?Kdx$&FU?Y<$x;j7z=(;Lyz+?EE>ov!8vvMtSzG!nMie zsBa9t8as#2nH}n8xzN%W%U$#MHNXmDUVr@GX{?(=yI=4vks|V)!-W5jHsU|h_&+kY zS_8^kd3jlYqOoiI`ZqBVY!(UfnAGny!FowZWY_@YR0z!nG7m{{)4OS$q&YDyw6vC$ zm4!$h>*|!2LbMbxS+VM6&DIrL*X4DeMO!@#EzMVfr)e4Tagn~AQHIU8?e61TuhcKD zr!F4(kEebk(Wdk-?4oXM(rJwanS>Jc%<>R(siF+>+5*CqJLecP_we33iTFTXr6W^G z7M?LPC-qFHK;E!fxCP)`8rkxZyFk{EV;G-|kwf4b$c1k0atD?85+|4V%YATWMG|?K zLyLrws36p%Qz6{}>7b>)$pe>mR+=IWuGrX{3ZPZXF3plvuv5Huax86}KX*lbPVr}L z{C#lDjdDeHr~?l|)Vp_}T|%$qF&q#U;ClHEPVuS+Jg~NjC1RP=17=aQKGOcJ6B3mp z8?4*-fAD~}sX*=E6!}^u8)+m2j<&FSW%pYr_d|p_{28DZ#Cz0@NF=gC-o$MY?8Ca8 zr5Y8DSR^*urS~rhpX^05r30Ik#2>*dIOGxRm0#0YX@YQ%Mg5b6dXlS!4{7O_kdaW8PFSdj1=ryI-=5$fiieGK{LZ+SX(1b=MNL!q#lN zv98?fqqTUH8r8C7v(cx#BQ5P9W>- zmW93;eH6T`vuJ~rqtIBg%A6>q>gnWb3X!r0wh_q;211+Om&?nvYzL1hhtjB zK_7G3!n7PL>d!kj){HQE zE8(%J%dWLh1_k%gVXTZt zEdT09XSKAx27Ncaq|(vzL3gm83q>6CAw<$fTnMU05*xAe&rDfCiu`u^1)CD<>sx0i z*hr^N_TeN89G(nunZoLBf^81#pmM}>JgD@Nn1l*lN#a=B=9pN%tmvYFjFIoKe_(GF z-26x{(KXdfsQL7Uv6UtDuYwV`;8V3w>oT_I<`Ccz3QqK9tYT5ZQzbop{=I=!pMOCb zCU68`n?^DT%^&m>A%+-~#lvF!7`L7a{z<3JqIlk1$<||_J}vW1U9Y&eX<}l8##6i( zZcTT@2`9(Mecptm@{3A_Y(X`w9K0EwtPq~O!16bq{7c0f7#(3wn-^)h zxV&M~iiF!{-6A@>o;$RzQ5A50kxXYj!tcgme=Qjrbje~;5X2xryU;vH|6bE(8z^<7 zQ>BG7_c*JG8~K7Oe68i#0~C$v?-t@~@r3t2inUnLT(c=URpA9kA8uq9PKU(Ps(LVH zqgcqW>Gm?6oV#AldDPKVRcEyQIdTT`Qa1j~vS{<;SwyTdr&3*t?J)y=M7q*CzucZ&B0M=joT zBbj@*SY;o2^_h*>R0e({!QHF0=)0hOj^B^d*m>SnRrwq>MolNSgl^~r8GR#mDWGYEIJA8B<|{{j?-7p zVnV$zancW3&JVDtVpIlI|5djKq0(w$KxEFzEiiL=h5Jw~4Le23@s(mYyXWL9SX6Ot zmb)sZaly_P%BeX_9 zw&{yBef8tFm+%=--m*J|o~+Xg3N+$IH)t)=fqD+|fEk4AAZ&!wcN5=mi~Vvo^i`}> z#_3ahR}Ju)(Px7kev#JGcSwPXJ2id9%Qd2A#Uc@t8~egZ8;iC{e! z%=CGJOD1}j!HW_sgbi_8suYnn4#Ou}%9u)dXd3huFIb!ytlX>Denx@pCS-Nj$`VO&j@(z!kKSP0hE4;YIP#w9ta=3DO$7f*x zc9M4&NK%IrVmZAe=r@skWD`AEWH=g+r|*13Ss$+{c_R!b?>?UaGXlw*8qDmY#xlR= z<0XFbs2t?8i^G~m?b|!Hal^ZjRjt<@a? z%({Gn14b4-a|#uY^=@iiKH+k?~~wTj5K1A&hU z2^9-HTC)7zpoWK|$JXaBL6C z#qSNYtY>65T@Zs&-0cHeu|RX(Pxz6vTITdzJdYippF zC-EB+n4}#lM7`2Ry~SO>FxhKboIAF#Z{1wqxaCb{#yEFhLuX;Rx(Lz%T`Xo1+a2M}7D+@wol2)OJs$TwtRNJ={( zD@#zTUEE}#Fz#&(EoD|SV#bayvr&E0vzmb%H?o~46|FAcx?r4$N z&67W3mdip-T1RIxwSm_&(%U|+WvtGBj*}t69XVd&ebn>KOuL(7Y8cV?THd-(+9>G7*Nt%T zcH;`p={`SOjaf7hNd(=37Lz3-51;58JffzIPgGs_7xIOsB5p2t&@v1mKS$2D$*GQ6 zM(IR*j4{nri7NMK9xlDy-hJW6sW|ZiDRaFiayj%;(%51DN!ZCCCXz+0Vm#};70nOx zJ#yA0P3p^1DED;jGdPbQWo0WATN=&2(QybbVdhd=Vq*liDk`c7iZ?*AKEYC#SY&2g z&Q(Ci)MJ{mEat$ZdSwTjf6h~roanYh2?9j$CF@4hjj_f35kTKuGHvIs9}Re@iKMxS-OI*`0S z6s)fOtz}O$T?PLFVSeOjSO26$@u`e<>k(OSP!&YstH3ANh>)mzmKGNOwOawq-MPXe zy4xbeUAl6tamnx))-`Gi2uV5>9n(73yS)Ukma4*7fI8PaEwa)dWHs6QA6>$}7?(L8 ztN8M}?{Tf!Zu22J5?2@95&rQ|F7=FK-hihT-vDp!5JCcWrVogEnp;CHenAZ)+E+K5 z$Cffk5sNwD_?4+ymgcHR(5xgt20Z8M`2*;MzOM#>yhk{r3x=EyM226wb&!+j`W<%* zSc&|`8!>dn9D@!pYow~(DsY_naSx7(Z4i>cu#hA5=;IuI88}7f%)bRkuY2B;+9Uep zpXcvFWkJ!mQai63BgNXG26$5kyhZ2&*3Q_tk)Ii4M>@p~_~q_cE!|^A;_MHB;7s#9 zKzMzK{lIxotjc};k67^Xsl-gS!^*m*m6kn|sbdun`O?dUkJ{0cmI0-_2y=lTAfn*Y zKg*A-2sJq)CCJgY0LF-VQvl&6HIXZyxo2#!O&6fOhbHXC?%1cMc6y^*dOS{f$=137Ds1m01qs`>iUQ49JijsaQ( zksqV9@&?il$|4Ua%4!O15>Zy&%gBY&wgqB>XA3!EldQ%1CRSM(pp#k~-pkcCg4LAT zXE=puHbgsw)!xtc@P4r~Z}nTF=D2~j(6D%gTBw$(`Fc=OOQ0kiW$_RDd=hcO0t97h zb86S5r=>(@VGy1&#S$Kg_H@7G^;8Ue)X5Y+IWUi`o;mpvoV)`fcVk4FpcT|;EG!;? zHG^zrVVZOm>1KFaHlaogcWj(v!S)O(Aa|Vo?S|P z5|6b{qkH(USa*Z7-y_Uvty_Z1|B{rTS^qmEMLEYUSk03_Fg&!O3BMo{b^*`3SHvl0 zhnLTe^_vVIdcSHe)SQE}r~2dq)VZJ!aSKR?RS<(9lzkYo&dQ?mubnWmgMM37Nudwo z3Vz@R{=m2gENUE3V4NbIzAA$H1z0pagz94-PTJyX{b$yndsdKptmlKQKaaHj@3=ED zc7L?p@%ui|RegVYutK$64q4pe9+5sv34QUpo)u{1ci?)_7gXQd{PL>b0l(LI#rJmN zGuO+%GO`xneFOOr4EU(Wg}_%bhzUf;d@TU+V*2#}!2OLwg~%D;1FAu=Un>OgjPb3S z7l(riiCwgghC=Lm5hWGf5NdGp#01xQ59`HJcLXbUR3&n%P(+W2q$h2Qd z*6+-QXJ*&Kvk9ht0f0*rO_|FMBALen{j7T1l%=Q>gf#kma zQlg#I9+HB+z*5BMxdesMND`_W;q5|FaEURFk|~&{@qY32N$G$2B=&Po{=!)x5b!#n zxLzblkq{yj05#O7(GRuT39(06FJlalyv<#K4m}+vs>9@q-&31@1(QBv82{}Zkns~K ze{eHC_RDX0#^A*JQTwF`a=IkE6Ze@j#-8Q`tTT?k9`^ZhA~3eCZJ-Jr{~7Cx;H4A3 zcZ+Zj{mzFZbVvQ6U~n>$U2ZotGsERZ@}VKrgGh0xM;Jzt29%TX6_&CWzg+YYMozrM z`nutuS)_0dCM8UVaKRj804J4i%z2BA_8A4OJRQ$N(P9Mfn-gF;4#q788C@9XR0O3< zsoS4wIoyt046d+LnSCJOy@B@Uz*#GGd#+Ln1ek5Dv>(ZtD@tgZlPnZZJGBLr^JK+!$$?A_fA3LOrkoDRH&l7 zcMcD$Hsjko3`-{bn)jPL6E9Ds{WskMrivsUu5apD z?grQO@W7i5+%X&E&p|RBaEZ(sGLR@~(y^BI@lDMot^Ll?!`90KT!JXUhYS`ZgX3jnu@Ja^seA*M5R@f`=`ynQV4rc$uT1mvE?@tz)TN<=&H1%Z?5yjxcpO+6y_R z6EPuPKM5uxKpmZfT(WKjRRNHs@ib)F5WAP7QCADvmCSD#hPz$V10wiD&{NXyEwx5S z6NE`3z!IS^$s7m}PCwQutVQ#~w+V z=+~->DI*bR2j0^@dMr9`p>q^Ny~NrAVxrJtX2DUveic5vM%#N*XO|?YAWwNI$Q)_) zvE|L(L1jP@F%gOGtnlXtIv2&1i8q<)Xfz8O3G^Ea~e*HJsQgBxWL(yuLY+jqUK zRE~`-zklrGog(X}$9@ZVUw!8*=l`6mzYLtsg`AvBYz(cxmAhr^j0~(rzXdiOEeu_p zE$sf2(w(BPAvO5DlaN&uQ$4@p-b?fRs}d7&2UQ4Fh?1Hzu*YVjcndqJLw0#q@fR4u zJCJ}>_7-|QbvOfylj+e^_L`5Ep9gqd>XI3-O?Wp z-gt*P29f$Tx(mtS`0d05nHH=gm~Po_^OxxUwV294BDKT>PHVlC5bndncxGR!n(OOm znsNt@Q&N{TLrmsoKFw0&_M9$&+C24`sIXGWgQaz=kY;S{?w`z^Q0JXXBKFLj0w0U6P*+jPKyZHX9F#b0D1$&(- zrm8PJd?+SrVf^JlfTM^qGDK&-p2Kdfg?f>^%>1n8bu&byH(huaocL>l@f%c*QkX2i znl}VZ4R1en4S&Bcqw?$=Zi7ohqB$Jw9x`aM#>pHc0x z0$!q7iFu zZ`tryM70qBI6JWWTF9EjgG@>6SRzsd}3h+4D8d~@CR07P$LJ}MFsYi-*O%XVvD@yT|rJ+Mk zDllJ7$n0V&A!0flbOf)HE6P_afPWZmbhpliqJuw=-h+r;WGk|ntkWN(8tKlYpq5Ow z(@%s>IN8nHRaYb*^d;M(D$zGCv5C|uqmsDjwy4g=Lz>*OhO3z=)VD}C<65;`89Ye} zSCxrv#ILzIpEx1KdLPlM&%Cctf@FqTKvNPXC&`*H9=l=D3r!GLM?UV zOxa(8ZsB`&+76S-_xuj?G#wXBfDY@Z_tMpXJS7^mp z@YX&u0jYw2A+Z+bD#6sgVK5ZgdPSJV3>{K^4~%HV?rn~4D)*2H!67Y>0aOmzup`{D zzDp3c9yEbGCY$U<8biJ_gB*`jluz1ShUd!QUIQJ$*1;MXCMApJ^m*Fiv88RZ zFopLViw}{$Tyhh_{MLGIE2~sZ)t0VvoW%=8qKZ>h=adTe3QM$&$PO2lfqH@brt!9j ziePM8$!CgE9iz6B<6_wyTQj?qYa;eC^{x_0wuwV~W+^fZmFco-o%wsKSnjXFEx02V zF5C2t)T6Gw$Kf^_c;Ei3G~uC8SM-xyycmXyC2hAVi-IfXqhu$$-C=*|X?R0~hu z8`J6TdgflslhrmDZq1f?GXF7*ALeMmOEpRDg(s*H`4>_NAr`2uqF;k;JQ+8>A|_6ZNsNLECC%NNEb1Y1dP zbIEmNpK)#XagtL4R6BC{C5T(+=yA-(Z|Ap}U-AfZM#gwVpus3(gPn}Q$CExObJ5AC z)ff9Yk?wZ}dZ-^)?cbb9Fw#EjqQ8jxF4G3=L?Ra zg_)0QDMV1y^A^>HRI$x?Op@t;oj&H@1xt4SZ9(kifQ zb59B*`M99Td7@aZ3UWvj1rD0sE)d=BsBuW*KwkCds7ay(7*01_+L}b~7)VHI>F_!{ zyxg-&nCO?v#KOUec0{OOKy+sjWA;8rTE|Lv6I9H?CI?H(mUm8VXGwU$49LGpz&{nQp2}dinE1@lZ1iox6{ghN&v^GZv9J${7WaXj)<0S4g_uiJ&JCZ zr8-hsu`U%N;+9N^@&Q0^kVPB3)wY(rr}p7{p0qFHb3NUUHJb672+wRZs`gd1UjKPX z4o6zljKKA+Kkj?H>Ew63o%QjyBk&1!P22;MkD>sM0=z_s-G{mTixJCT9@_|*(p^bz zJ8?ZZ&;pzV+7#6Mn`_U-)k8Pjg?a;|Oe^us^PoPY$Va~yi8|?+&=y$f+lABT<*pZr zP}D{~Pq1Qyni+@|aP;ixO~mbEW9#c0OU#YbDZIaw=_&$K%Ep2f%hO^&P67hApZe`x zv8b`Mz@?M_7-)b!lkQKk)JXXUuT|B8kJlvqRmRpxtQDgvrHMXC1B$M@Y%Me!BSx3P z#2Eawl$HleZhhTS6Txm>lN_+I`>eV$&v9fOg)%zVn3O5mI*lAl>QcHuW6!Kixmq`X zBCZ*Ck6OYtDiK!N47>jxI&O2a9x7M|i^IagRr-fmrmikEQGgw%J7bO|)*$2FW95O4 zeBs>KR)izRG1gRVL;F*sr8A}aRHO0gc$$j&ds8CIO1=Gwq1%_~E)CWNn9pCtBE}+`Jelk4{>S)M)`Ll=!~gnn1yq^EX(+y*ik@3Ou0qU`IgYi3*doM+5&dU!cho$pZ zn%lhKeZkS72P?Cf68<#kll_6OAO26bIbueZx**j6o;I0cS^XiL`y+>{cD}gd%lux} z)3N>MaE24WBZ}s0ApfdM;5J_Ny}rfUyxfkC``Awo2#sgLnGPewK};dORuT?@I6(5~ z?kE)Qh$L&fwJXzK){iYx!l5$Tt|^D~MkGZPA}(o6f7w~O2G6Vvzdo*a;iXzk$B66$ zwF#;wM7A+(;uFG4+UAY(2`*3XXx|V$K8AYu#ECJYSl@S=uZW$ksfC$~qrrbQj4??z-)uz0QL}>k^?fPnJTPw% zGz)~?B4}u0CzOf@l^um}HZzbaIwPmb<)< zi_3@E9lc)Qe2_`*Z^HH;1CXOceL=CHpHS{HySy3T%<^NrWQ}G0i4e1xm_K3(+~oi$ zoHl9wzb?Z4j#90DtURtjtgvi7uw8DzHYmtPb;?%8vb9n@bszT=1qr)V_>R%s!92_` zfnHQPANx z<#hIjIMm#*(v*!OXtF+w8kLu`o?VZ5k7{`vw{Yc^qYclpUGIM_PBN1+c{#Vxv&E*@ zxg=W2W~JuV{IuRYw3>LSI1)a!thID@R=bU+cU@DbR^_SXY`MC7HOsCN z!dO4OKV7(E_Z8T#8MA1H`99?Z!r0)qKW_#|29X3#Jb+5+>qUidbeP1NJ@)(qi2S-X zao|f0_tl(O+$R|Qwd$H{_ig|~I1fbp_$NkI!0E;Y z6JrnU{1Ra6^on{9gUUB0mwzP3S%B#h0fjo>JvV~#+X0P~JV=IG=yHG$O+p5O3NUgG zEQ}z6BTp^Fie)Sg<){Z&I8NwPR(=mO4joTLHkJ>|Tnk23E(Bo`FSbPc05lF2-+)X? z6vV3*m~IBHTy*^E!<0nA(tCOJW2G4DsH7)BxLV8kICn5lu6@U*R`w)o9;Ro$i8=Q^V%uH8n3q=+Yf;SFRZu z!+F&PKcH#8cG?aSK_Tl@K9P#8o+jry@gdexz&d(Q=47<7nw@e@FFfIRNL9^)1i@;A z28+$Z#rjv-wj#heI|<&J_DiJ*s}xd-f!{J8jfqOHE`TiHHZVIA8CjkNQ_u;Ery^^t zl1I75&u^`1_q)crO+JT4rx|z2ToSC>)Or@-D zy3S>jW*sNIZR-EBsfyaJ+Jq4BQE4?SePtD2+jY8*%FsSLZ9MY>+wk?}}}AFAw)vr{ml)8LUG-y9>^t!{~|sgpxYc0Gnkg`&~R z-pilJZjr@y5$>B=VMdZ73svct%##v%wdX~9fz6i3Q-zOKJ9wso+h?VME7}SjL=!NUG{J?M&i!>ma`eoEa@IX`5G>B1(7;%}M*%-# zfhJ(W{y;>MRz!Ic8=S}VaBKqh;~7KdnGEHxcL$kA-6E~=!hrN*zw9N+_=odt<$_H_8dbo;0=42wcAETPCVGUr~v(`Uai zb{=D!Qc!dOEU6v)2eHSZq%5iqK?B(JlCq%T6av$Cb4Rko6onlG&?CqaX7Y_C_cOC3 zYZ;_oI(}=>_07}Oep&Ws7x7-R)cc8zfe!SYxJYP``pi$FDS)4Fvw5HH=FiU6xfVqIM!hJ;Rx8c0cB7~aPtNH(Nmm5Vh{ibAoU#J6 zImRCr?(iyu_4W_6AWo3*vxTPUw@vPwy@E0`(>1Qi=%>5eSIrp^`` zK*Y?fK_6F1W>-7UsB)RPC4>>Ps9)f+^MqM}8AUm@tZ->j%&h1M8s*s!LX5&WxQcAh z8mciQej@RPm?660%>{_D+7er>%zX_{s|$Z+;G7_sfNfBgY(zLB4Ey}J9F>zX#K0f6 z?dVNIeEh?EIShmP6>M+d|0wMM85Sa4diw1hrg|ITJ}JDg@o8y>(rF9mXk5M z2@D|NA)-7>wD&wF;S_$KS=eE84`BGw3g0?6wGxu8ys4rwI?9U=*^VF22t3%mbGeOh z`!O-OpF7#Vceu~F`${bW0nYVU9ecmk31V{tF%iv&5hWofC>I~cqAt@u6|R+|HLMMX zVxuSlMFOK_EQ86#E8&KwxIr8S9tj_goWtLv4f@!&h8;Ov41{J~496vp9vX=(LK#j! zAwi*21RAV-LD>9Cw3bV_9X(X3)Kr0-UaB*7Y>t82EQ%!)(&(XuAYtTsYy-dz+w=$ir)VJpe!_$ z6SGpX^i(af3{o=VlFPC);|J8#(=_8#vdxDe|Cok+ANhYwbE*FO`Su2m1~w+&9<_9~ z-|tTU_ACGN`~CNW5WYYBn^B#SwZ(t4%3aPp z;o)|L6Rk569KGxFLUPx@!6OOa+5OjQLK5w&nAmwxkC5rZ|m&HT8G%GVZxB_@ME z>>{rnXUqyiJrT(8GMj_ap#yN_!9-lO5e8mR3cJiK3NE{_UM&=*vIU`YkiL$1%kf+1 z4=jk@7EEj`u(jy$HnzE33ZVW_J4bj}K;vT?T91YlO(|Y0FU4r+VdbmQ97%(J5 zkK*Bed8+C}FcZ@HIgdCMioV%A<*4pw_n}l*{Cr4}a(lq|injK#O?$tyvyE`S%(1`H z_wwRvk#13ElkZvij2MFGOj`fhy?nC^8`Zyo%yVcUAfEr8x&J#A{|moUBAV_^f$hpaUuyQeY3da^ zS9iRgf87YBwfe}>BO+T&Fl%rfpZh#+AM?Dq-k$Bq`vG6G_b4z%Kbd&v>qFjow*mBl z-OylnqOpLg}or7_VNwRg2za3VBK6FUfFX{|TD z`Wt0Vm2H$vdlRWYQJqDmM?JUbVqL*ZQY|5&sY*?!&%P8qhA~5+Af<{MaGo(dl&C5t zE%t!J0 zh6jqANt4ABdPxSTrVV}fLsRQal*)l&_*rFq(Ez}ClEH6LHv{J#v?+H-BZ2)Wy{K@9 z+ovXHq~DiDvm>O~r$LJo!cOuwL+Oa--6;UFE2q@g3N8Qkw5E>ytz^(&($!O47+i~$ zKM+tkAd-RbmP{s_rh+ugTD;lriL~`Xwkad#;_aM?nQ7L_muEFI}U_4$phjvYgleK~`Fo`;GiC07&Hq1F<%p;9Q;tv5b?*QnR%8DYJH3P>Svmv47Y>*LPZJy8_{9H`g6kQpyZU{oJ`m%&p~D=K#KpfoJ@ zn-3cqmHsdtN!f?~w+(t+I`*7GQA#EQC^lUA9(i6=i1PqSAc|ha91I%X&nXzjYaM{8$s&wEx@aVkQ6M{E2 zfzId#&r(XwUNtPcq4Ngze^+XaJA1EK-%&C9j>^9(secqe{}z>hR5CFNveMsVA)m#S zk)_%SidkY-XmMWlVnQ(mNJ>)ooszQ#vaK;!rPmGKXV7am^_F!Lz>;~{VrIO$;!#30XRhE1QqO_~#+Ux;B_D{Nk=grn z8Y0oR^4RqtcYM)7a%@B(XdbZCOqnX#fD{BQTeLvRHd(irHKq=4*jq34`6@VAQR8WG z^%)@5CXnD_T#f%@-l${>y$tfb>2LPmc{~5A82|16mH)R?&r#KKLs7xpN-D`=&Cm^R zvMA6#Ahr<3X>Q7|-qfTY)}32HkAz$_mibYV!I)u>bmjK`qwBe(>za^0Kt*HnFbSdO z1>+ryKCNxmm^)*$XfiDOF2|{-v3KKB?&!(S_Y=Ht@|ir^hLd978xuI&N{k>?(*f8H z=ClxVJK_%_z1TH0eUwm2J+2To7FK4o+n_na)&#VLn1m;!+CX+~WC+qg1?PA~KdOlC zW)C@pw75_xoe=w7i|r9KGIvQ$+3K?L{7TGHwrQM{dCp=Z*D}3kX7E-@sZnup!BImw z*T#a=+WcTwL78exTgBn|iNE3#EsOorO z*kt)gDzHiPt07fmisA2LWN?AymkdqTgr?=loT7z@d`wnlr6oN}@o|&JX!yPzC*Y8d zu6kWlTzE1)ckyBn+0Y^HMN+GA$wUO_LN6W>mxCo!0?oiQvT`z$jbSEu&{UHRU0E8# z%B^wOc@S!yhMT49Y)ww(Xta^8pmPCe@eI5C*ed96)AX9<>))nKx0(sci8gwob_1}4 z0DIL&vsJ1_s%<@y%U*-eX z5rN&(zef-5G~?@r79oZGW1d!WaTqQn0F6RIOa9tJ=0(kdd{d1{<*tHT#cCvl*i>YY zH+L7jq8xZNcTUBqj(S)ztTU!TM!RQ}In*n&Gn<>(60G7}4%WQL!o>hbJqNDSGwl#H z`4k+twp0cj%PsS+NKaxslAEu9!#U3xT1|_KB6`h=PI0SW`P9GTa7caD1}vKEglV8# zjKZR`pluCW19c2fM&ZG)c3T3Um;ir3y(tSCJ7Agl6|b524dy5El{^EQBG?E61H0XY z`bqg!;zhGhyMFl&(o=JWEJ8n~z)xI}A@C0d2hQGvw7nGv)?POU@(kS1m=%`|+^ika zXl8zjS?xqW$WlO?Ewa;vF~XbybHBor$f<%I&*t$F5fynwZlTGj|IjZtVfGa7l&tK} zW>I<69w(cZLu)QIVG|M2xzW@S+70NinQzk&Y0+3WT*cC)rx~04O-^<{JohU_&HL5XdUKW!uFy|i$FB|EMu0eUyW;gsf`XfIc!Z0V zeK&*hPL}f_cX=@iv>K%S5kL;cl_$v?n(Q9f_cChk8Lq$glT|=e+T*8O4H2n<=NGmn z+2*h+v;kBvF>}&0RDS>)B{1!_*XuE8A$Y=G8w^qGMtfudDBsD5>T5SB;Qo}fSkkiV ze^K^M(UthkwrD!&*tTsu>Dacdj_q`~V%r_twr$(Ct&_dKeeXE?fA&4&yASJWJ*}~- zel=@W)tusynfC_YqH4ll>4Eg`Xjs5F7Tj>tTLz<0N3)X<1px_d2yUY>X~y>>93*$) z5PuNMQLf9Bu?AAGO~a_|J2akO1M*@VYN^VxvP0F$2>;Zb9;d5Yfd8P%oFCCoZE$ z4#N$^J8rxYjUE_6{T%Y>MmWfHgScpuGv59#4u6fpTF%~KB^Ae`t1TD_^Ud#DhL+Dm zbY^VAM#MrAmFj{3-BpVSWph2b_Y6gCnCAombVa|1S@DU)2r9W<> zT5L8BB^er3zxKt1v(y&OYk!^aoQisqU zH(g@_o)D~BufUXcPt!Ydom)e|aW{XiMnes2z&rE?og>7|G+tp7&^;q?Qz5S5^yd$i z8lWr4g5nctBHtigX%0%XzIAB8U|T6&JsC4&^hZBw^*aIcuNO47de?|pGXJ4t}BB`L^d8tD`H`i zqrP8?#J@8T#;{^B!KO6J=@OWKhAerih(phML`(Rg7N1XWf1TN>=Z3Do{l_!d~DND&)O)D>ta20}@Lt77qSnVsA7>)uZAaT9bsB>u&aUQl+7GiY2|dAEg@%Al3i316y;&IhQL^8fw_nwS>f60M_-m+!5)S_6EPM7Y)(Nq^8gL7(3 zOiot`6Wy6%vw~a_H?1hLVzIT^i1;HedHgW9-P#)}Y6vF%C=P70X0Tk^z9Te@kPILI z_(gk!k+0%CG)%!WnBjjw*kAKs_lf#=5HXC00s-}oM-Q1aXYLj)(1d!_a7 z*Gg4Fe6F$*ujVjI|79Z5+Pr`us%zW@ln++2l+0hsngv<{mJ%?OfSo_3HJXOCys{Ug z00*YR-(fv<=&%Q!j%b-_ppA$JsTm^_L4x`$k{VpfLI(FMCap%LFAyq;#ns5bR7V+x zO!o;c5y~DyBPqdVQX)8G^G&jWkBy2|oWTw>)?5u}SAsI$RjT#)lTV&Rf8;>u*qXnb z8F%Xb=7#$m)83z%`E;49)t3fHInhtc#kx4wSLLms!*~Z$V?bTyUGiS&m>1P(952(H zuHdv=;o*{;5#X-uAyon`hP}d#U{uDlV?W?_5UjJvf%11hKwe&(&9_~{W)*y1nR5f_ z!N(R74nNK`y8>B!0Bt_Vr!;nc3W>~RiKtGSBkNlsR#-t^&;$W#)f9tTlZz>n*+Fjz z3zXZ;jf(sTM(oDzJt4FJS*8c&;PLTW(IQDFs_5QPy+7yhi1syPCarvqrHFcf&yTy)^O<1EBx;Ir`5W{TIM>{8w&PB>ro4;YD<5LF^TjTb0!zAP|QijA+1Vg>{Afv^% zmrkc4o6rvBI;Q8rj4*=AZacy*n8B{&G3VJc)so4$XUoie0)vr;qzPZVbb<#Fc=j+8CGBWe$n|3K& z_@%?{l|TzKSlUEO{U{{%Fz_pVDxs7i9H#bnbCw7@4DR=}r_qV!Zo~CvD4ZI*+j3kO zW6_=|S`)(*gM0Z;;}nj`73OigF4p6_NPZQ-Od~e$c_);;4-7sR>+2u$6m$Gf%T{aq zle>e3(*Rt(TPD}03n5)!Ca8Pu!V}m6v0o1;5<1h$*|7z|^(3$Y&;KHKTT}hV056wuF0Xo@mK-52~r=6^SI1NC%c~CC?n>yX6wPTgiWYVz!Sx^atLby9YNn1Rk{g?|pJaxD4|9cUf|V1_I*w zzxK)hRh9%zOl=*$?XUjly5z8?jPMy%vEN)f%T*|WO|bp5NWv@B(K3D6LMl!-6dQg0 zXNE&O>Oyf%K@`ngCvbGPR>HRg5!1IV$_}m@3dWB7x3t&KFyOJn9pxRXCAzFr&%37wXG;z^xaO$ekR=LJG ztIHpY8F5xBP{mtQidqNRoz= z@){+N3(VO5bD+VrmS^YjG@+JO{EOIW)9=F4v_$Ed8rZtHvjpiEp{r^c4F6Ic#ChlC zJX^DtSK+v(YdCW)^EFcs=XP7S>Y!4=xgmv>{S$~@h=xW-G4FF9?I@zYN$e5oF9g$# zb!eVU#J+NjLyX;yb)%SY)xJdvGhsnE*JEkuOVo^k5PyS=o#vq!KD46UTW_%R=Y&0G zFj6bV{`Y6)YoKgqnir2&+sl+i6foAn-**Zd1{_;Zb7Ki=u394C5J{l^H@XN`_6XTKY%X1AgQM6KycJ+= zYO=&t#5oSKB^pYhNdzPgH~aEGW2=ec1O#s-KG z71}LOg@4UEFtp3GY1PBemXpNs6UK-ax*)#$J^pC_me;Z$Je(OqLoh|ZrW*mAMBFn< zHttjwC&fkVfMnQeen8`Rvy^$pNRFVaiEN4Pih*Y3@jo!T0nsClN)pdrr9AYLcZxZ| zJ5Wlj+4q~($hbtuY zVQ7hl>4-+@6g1i`1a)rvtp-;b0>^`Dloy(#{z~ytgv=j4q^Kl}wD>K_Y!l~ zp(_&7sh`vfO(1*MO!B%<6E_bx1)&s+Ae`O)a|X=J9y~XDa@UB`m)`tSG4AUhoM=5& znWoHlA-(z@3n0=l{E)R-p8sB9XkV zZ#D8wietfHL?J5X0%&fGg@MH~(rNS2`GHS4xTo7L$>TPme+Is~!|79=^}QbPF>m%J zFMkGzSndiPO|E~hrhCeo@&Ea{M(ieIgRWMf)E}qeTxT8Q#g-!Lu*x$v8W^M^>?-g= zwMJ$dThI|~M06rG$Sv@C@tWR>_YgaG&!BAbkGggVQa#KdtDB)lMLNVLN|51C@F^y8 zCRvMB^{GO@j=cHfmy}_pCGbP%xb{pNN>? z?7tBz$1^zVaP|uaatYaIN+#xEN4jBzwZ|YI_)p(4CUAz1ZEbDk>J~Y|63SZaak~#0 zoYKruYsWHoOlC1(MhTnsdUOwQfz5p6-D0}4;DO$B;7#M{3lSE^jnTT;ns`>!G%i*F?@pR1JO{QTuD0U+~SlZxcc8~>IB{)@8p`P&+nDxNj`*gh|u?yrv$phpQcW)Us)bi`kT%qLj(fi{dWRZ%Es2!=3mI~UxiW0$-v3vUl?#g{p6eF zMEUAqo5-L0Ar(s{VlR9g=j7+lt!gP!UN2ICMokAZ5(Agd>})#gkA2w|5+<%-CuEP# zqgcM}u@3(QIC^Gx<2dbLj?cFSws_f3e%f4jeR?4M^M3cx1f+Qr6ydQ>n)kz1s##2w zk}UyQc+Z5G-d-1}{WzjkLXgS-2P7auWSJ%pSnD|Uivj5u!xk0 z_^-N9r9o;(rFDt~q1PvE#iJZ_f>J3gcP$)SOqhE~pD2|$=GvpL^d!r z6u=sp-CrMoF7;)}Zd7XO4XihC4ji?>V&(t^?@3Q&t9Mx=qex6C9d%{FE6dvU6%d94 zIE;hJ1J)cCqjv?F``7I*6bc#X)JW2b4f$L^>j{*$R`%5VHFi*+Q$2;nyieduE}qdS{L8y8F08yLs?w}{>8>$3236T-VMh@B zq-nujsb_1aUv_7g#)*rf9h%sFj*^mIcImRV*k~Vmw;%;YH(&ylYpy!&UjUVqqtfG` zox3esju?`unJJA_zKXRJP)rA3nXc$m^{S&-p|v|-0x9LHJm;XIww7C#R$?00l&Yyj z=e}gKUOpsImwW?N)+E(awoF@HyP^EhL+GlNB#k?R<2>95hz!h9sF@U20DHSB3~WMa zk90+858r@-+vWwkawJ)8ougd(i#1m3GLN{iSTylYz$brAsP%=&m$mQQrH$g%3-^VR zE%B`Vi&m8f3T~&myTEK28BDWCVzfWir1I?03;pX))|kY5ClO^+bae z*7E?g=3g7EiisYOrE+lA)2?Ln6q2*HLNpZEWMB|O-JI_oaHZB%CvYB(%=tU= zE*OY%QY58fW#RG5=gm0NR#iMB=EuNF@)%oZJ}nmm=tsJ?eGjia{e{yuU0l3{d^D@)kVDt=1PE)&tf_hHC%0MB znL|CRCPC}SeuVTdf>-QV70`0(EHizc21s^sU>y%hW0t!0&y<7}Wi-wGy>m%(-jsDj zP?mF|>p_K>liZ6ZP(w5(|9Ga%>tLgb$|doDDfkdW>Z z`)>V2XC?NJT26mL^@ zf+IKr27TfM!UbZ@?zRddC7#6ss1sw%CXJ4FWC+t3lHZupzM77m^=9 z&(a?-LxIq}*nvv)y?27lZ{j zifdl9hyJudyP2LpU$-kXctshbJDKS{WfulP5Dk~xU4Le4c#h^(YjJit4#R8_khheS z|8(>2ibaHES4+J|DBM7I#QF5u-*EdN{n=Kt@4Zt?@Tv{JZA{`4 zU#kYOv{#A&gGPwT+$Ud}AXlK3K7hYzo$(fBSFjrP{QQ zeaKg--L&jh$9N}`pu{Bs>?eDFPaWY4|9|foN%}i;3%;@4{dc+iw>m}{3rELqH21G! z`8@;w-zsJ1H(N3%|1B@#ioLOjib)j`EiJqPQVSbPSPVHCj6t5J&(NcWzBrzCiDt{4 zdlPAUKldz%6x5II1H_+jv)(xVL+a;P+-1hv_pM>gMRr%04@k;DTokASSKKhU1Qms| zrWh3a!b(J3n0>-tipg{a?UaKsP7?+|@A+1WPDiQIW1Sf@qDU~M_P65_s}7(gjTn0X zucyEm)o;f8UyshMy&>^SC3I|C6jR*R_GFwGranWZe*I>K+0k}pBuET&M~ z;Odo*ZcT?ZpduHyrf8E%IBFtv;JQ!N_m>!sV6ly$_1D{(&nO~w)G~Y`7sD3#hQk%^ zp}ucDF_$!6DAz*PM8yE(&~;%|=+h(Rn-=1Wykas_-@d&z#=S}rDf`4w(rVlcF&lF! z=1)M3YVz7orwk^BXhslJ8jR);sh^knJW(Qmm(QdSgIAIdlN4Te5KJisifjr?eB{FjAX1a0AB>d?qY4Wx>BZ8&}5K0fA+d{l8 z?^s&l8#j7pR&ijD?0b%;lL9l$P_mi2^*_OL+b}4kuLR$GAf85sOo02?Y#90}CCDiS zZ%rbCw>=H~CBO=C_JVV=xgDe%b4FaEFtuS7Q1##y686r%F6I)s-~2(}PWK|Z8M+Gu zl$y~5@#0Ka%$M<&Cv%L`a8X^@tY&T7<0|(6dNT=EsRe0%kp1Qyq!^43VAKYnr*A5~ zsI%lK1ewqO;0TpLrT9v}!@vJK{QoVa_+N4FYT#h?Y8rS1S&-G+m$FNMP?(8N`MZP zels(*?kK{{^g9DOzkuZXJ2;SrOQsp9T$hwRB1(phw1c7`!Q!by?Q#YsSM#I12RhU{$Q+{xj83axHcftEc$mNJ8_T7A-BQc*k(sZ+~NsO~xAA zxnbb%dam_fZlHvW7fKXrB~F&jS<4FD2FqY?VG?ix*r~MDXCE^WQ|W|WM;gsIA4lQP zJ2hAK@CF*3*VqPr2eeg6GzWFlICi8S>nO>5HvWzyZTE)hlkdC_>pBej*>o0EOHR|) z$?};&I4+_?wvL*g#PJ9)!bc#9BJu1(*RdNEn>#Oxta(VWeM40ola<0aOe2kSS~{^P zDJBd}0L-P#O-CzX*%+$#v;(x%<*SPgAje=F{Zh-@ucd2DA(yC|N_|ocs*|-!H%wEw z@Q!>siv2W;C^^j^59OAX03&}&D*W4EjCvfi(ygcL#~t8XGa#|NPO+*M@Y-)ctFA@I z-p7npT1#5zOLo>7q?aZpCZ=iecn3QYklP;gF0bq@>oyBq94f6C=;Csw3PkZ|5q=(c zfs`aw?II0e(h=|7o&T+hq&m$; zBrE09Twxd9BJ2P+QPN}*OdZ-JZV7%av@OM7v!!NL8R;%WFq*?{9T3{ct@2EKgc8h) zMxoM$SaF#p<`65BwIDfmXG6+OiK0e)`I=!A3E`+K@61f}0e z!2a*FOaDrOe>U`q%K!QN`&=&0C~)CaL3R4VY(NDt{Xz(Xpqru5=r#uQN1L$Je1*dkdqQ*=lofQaN%lO!<5z9ZlHgxt|`THd>2 zsWfU$9=p;yLyJyM^t zS2w9w?Bpto`@H^xJpZDKR1@~^30Il6oFGfk5%g6w*C+VM)+%R@gfIwNprOV5{F^M2 zO?n3DEzpT+EoSV-%OdvZvNF+pDd-ZVZ&d8 zKeIyrrfPN=EcFRCPEDCVflX#3-)Ik_HCkL(ejmY8vzcf-MTA{oHk!R2*36`O68$7J zf}zJC+bbQk--9Xm!u#lgLvx8TXx2J258E5^*IZ(FXMpq$2LUUvhWQPs((z1+2{Op% z?J}9k5^N=z;7ja~zi8a_-exIqWUBJwohe#4QJ`|FF*$C{lM18z^#hX6!5B8KAkLUX ziP=oti-gpV(BsLD{0(3*dw}4JxK23Y7M{BeFPucw!sHpY&l%Ws4pSm`+~V7;bZ%Dx zeI)MK=4vC&5#;2MT7fS?^ch9?2;%<8Jlu-IB&N~gg8t;6S-#C@!NU{`p7M8@2iGc& zg|JPg%@gCoCQ&s6JvDU&`X2S<57f(k8nJ1wvBu{8r?;q3_kpZZ${?|( z+^)UvR33sjSd)aT!UPkA;ylO6{aE3MQa{g%Mcf$1KONcjO@&g5zPHWtzM1rYC{_K> zgQNcs<{&X{OA=cEWw5JGqpr0O>x*Tfak2PE9?FuWtz^DDNI}rwAaT0(bdo-<+SJ6A z&}S%boGMWIS0L}=S>|-#kRX;e^sUsotry(MjE|3_9duvfc|nwF#NHuM-w7ZU!5ei8 z6Mkf>2)WunY2eU@C-Uj-A zG(z0Tz2YoBk>zCz_9-)4a>T46$(~kF+Y{#sA9MWH%5z#zNoz)sdXq7ZR_+`RZ%0(q zC7&GyS_|BGHNFl8Xa%@>iWh%Gr?=J5<(!OEjauj5jyrA-QXBjn0OAhJJ9+v=!LK`` z@g(`^*84Q4jcDL`OA&ZV60djgwG`|bcD*i50O}Q{9_noRg|~?dj%VtKOnyRs$Uzqg z191aWoR^rDX#@iSq0n z?9Sg$WSRPqSeI<}&n1T3!6%Wj@5iw5`*`Btni~G=&;J+4`7g#OQTa>u`{4ZZ(c@s$ zK0y;ySOGD-UTjREKbru{QaS>HjN<2)R%Nn-TZiQ(Twe4p@-saNa3~p{?^V9Nixz@a zykPv~<@lu6-Ng9i$Lrk(xi2Tri3q=RW`BJYOPC;S0Yly%77c727Yj-d1vF!Fuk{Xh z)lMbA69y7*5ufET>P*gXQrxsW+ zz)*MbHZv*eJPEXYE<6g6_M7N%#%mR{#awV3i^PafNv(zyI)&bH?F}2s8_rR(6%!V4SOWlup`TKAb@ee>!9JKPM=&8g#BeYRH9FpFybxBXQI2|g}FGJfJ+ zY-*2hB?o{TVL;Wt_ek;AP5PBqfDR4@Z->_182W z{P@Mc27j6jE*9xG{R$>6_;i=y{qf(c`5w9fa*`rEzX6t!KJ(p1H|>J1pC-2zqWENF zmm=Z5B4u{cY2XYl(PfrInB*~WGWik3@1oRhiMOS|D;acnf-Bs(QCm#wR;@Vf!hOPJ zgjhDCfDj$HcyVLJ=AaTbQ{@vIv14LWWF$=i-BDoC11}V;2V8A`S>_x)vIq44-VB-v z*w-d}$G+Ql?En8j!~ZkCpQ$|cA0|+rrY>tiCeWxkRGPoarxlGU2?7%k#F693RHT24 z-?JsiXlT2PTqZqNb&sSc>$d;O4V@|b6VKSWQb~bUaWn1Cf0+K%`Q&Wc<>mQ>*iEGB zbZ;aYOotBZ{vH3y<0A*L0QVM|#rf*LIsGx(O*-7)r@yyBIzJnBFSKBUSl1e|8lxU* zzFL+YDVVkIuzFWeJ8AbgN&w(4-7zbiaMn{5!JQXu)SELk*CNL+Fro|2v|YO)1l15t zs(0^&EB6DPMyaqvY>=KL>)tEpsn;N5Q#yJj<9}ImL((SqErWN3Q=;tBO~ExTCs9hB z2E$7eN#5wX4<3m^5pdjm#5o>s#eS_Q^P)tm$@SawTqF*1dj_i#)3};JslbLKHXl_N z)Fxzf>FN)EK&Rz&*|6&%Hs-^f{V|+_vL1S;-1K-l$5xiC@}%uDuwHYhmsV?YcOUlk zOYkG5v2+`+UWqpn0aaaqrD3lYdh0*!L`3FAsNKu=Q!vJu?Yc8n|CoYyDo_`r0mPoo z8>XCo$W4>l(==h?2~PoRR*kEe)&IH{1sM41mO#-36`02m#nTX{r*r`Q5rZ2-sE|nA zhnn5T#s#v`52T5|?GNS`%HgS2;R(*|^egNPDzzH_z^W)-Q98~$#YAe)cEZ%vge965AS_am#DK#pjPRr-!^za8>`kksCAUj(Xr*1NW5~e zpypt_eJpD&4_bl_y?G%>^L}=>xAaV>KR6;^aBytqpiHe%!j;&MzI_>Sx7O%F%D*8s zSN}cS^<{iiK)=Ji`FpO#^zY!_|D)qeRNAtgmH)m;qC|mq^j(|hL`7uBz+ULUj37gj zksdbnU+LSVo35riSX_4z{UX=%n&}7s0{WuZYoSfwAP`8aKN9P@%e=~1`~1ASL-z%# zw>DO&ixr}c9%4InGc*_y42bdEk)ZdG7-mTu0bD@_vGAr*NcFoMW;@r?@LUhRI zCUJgHb`O?M3!w)|CPu~ej%fddw20lod?Ufp8Dmt0PbnA0J%KE^2~AIcnKP()025V> zG>noSM3$5Btmc$GZoyP^v1@Poz0FD(6YSTH@aD0}BXva?LphAiSz9f&Y(aDAzBnUh z?d2m``~{z;{}kZJ>a^wYI?ry(V9hIoh;|EFc0*-#*`$T0DRQ1;WsqInG;YPS+I4{g zJGpKk%%Sdc5xBa$Q^_I~(F97eqDO7AN3EN0u)PNBAb+n+ zWBTxQx^;O9o0`=g+Zrt_{lP!sgWZHW?8bLYS$;1a@&7w9rD9|Ge;Gb?sEjFoF9-6v z#!2)t{DMHZ2@0W*fCx;62d#;jouz`R5Y(t{BT=$N4yr^^o$ON8d{PQ=!O zX17^CrdM~7D-;ZrC!||<+FEOxI_WI3CA<35va%4v>gc zEX-@h8esj=a4szW7x{0g$hwoWRQG$yK{@3mqd-jYiVofJE!Wok1* znV7Gm&Ssq#hFuvj1sRyHg(6PFA5U*Q8Rx>-blOs=lb`qa{zFy&n4xY;sd$fE+<3EI z##W$P9M{B3c3Si9gw^jlPU-JqD~Cye;wr=XkV7BSv#6}DrsXWFJ3eUNrc%7{=^sP> zrp)BWKA9<}^R9g!0q7yWlh;gr_TEOD|#BmGq<@IV;ueg+D2}cjpp+dPf&Q(36sFU&K8}hA85U61faW&{ zlB`9HUl-WWCG|<1XANN3JVAkRYvr5U4q6;!G*MTdSUt*Mi=z_y3B1A9j-@aK{lNvx zK%p23>M&=KTCgR!Ee8c?DAO2_R?B zkaqr6^BSP!8dHXxj%N1l+V$_%vzHjqvu7p@%Nl6;>y*S}M!B=pz=aqUV#`;h%M0rU zHfcog>kv3UZAEB*g7Er@t6CF8kHDmKTjO@rejA^ULqn!`LwrEwOVmHx^;g|5PHm#B zZ+jjWgjJ!043F+&#_;D*mz%Q60=L9Ove|$gU&~As5^uz@2-BfQ!bW)Khn}G+Wyjw- z19qI#oB(RSNydn0t~;tAmK!P-d{b-@@E5|cdgOS#!>%#Rj6ynkMvaW@37E>@hJP^8 z2zk8VXx|>#R^JCcWdBCy{0nPmYFOxN55#^-rlqobe0#L6)bi?E?SPymF*a5oDDeSd zO0gx?#KMoOd&G(2O@*W)HgX6y_aa6iMCl^~`{@UR`nMQE`>n_{_aY5nA}vqU8mt8H z`oa=g0SyiLd~BxAj2~l$zRSDHxvDs;I4>+M$W`HbJ|g&P+$!U7-PHX4RAcR0szJ*( ze-417=bO2q{492SWrqDK+L3#ChUHtz*@MP)e^%@>_&#Yk^1|tv@j4%3T)diEX zATx4K*hcO`sY$jk#jN5WD<=C3nvuVsRh||qDHnc~;Kf59zr0;c7VkVSUPD%NnnJC_ zl3F^#f_rDu8l}l8qcAz0FFa)EAt32IUy_JLIhU_J^l~FRH&6-ivSpG2PRqzDdMWft>Zc(c)#tb%wgmWN%>IOPm zZi-noqS!^Ftb81pRcQi`X#UhWK70hy4tGW1mz|+vI8c*h@ zfFGJtW3r>qV>1Z0r|L>7I3un^gcep$AAWfZHRvB|E*kktY$qQP_$YG60C@X~tTQjB3%@`uz!qxtxF+LE!+=nrS^07hn` zEgAp!h|r03h7B!$#OZW#ACD+M;-5J!W+{h|6I;5cNnE(Y863%1(oH}_FTW})8zYb$7czP zg~Szk1+_NTm6SJ0MS_|oSz%e(S~P-&SFp;!k?uFayytV$8HPwuyELSXOs^27XvK-D zOx-Dl!P|28DK6iX>p#Yb%3`A&CG0X2S43FjN%IB}q(!hC$fG}yl1y9W&W&I@KTg6@ zK^kpH8=yFuP+vI^+59|3%Zqnb5lTDAykf z9S#X`3N(X^SpdMyWQGOQRjhiwlj!0W-yD<3aEj^&X%=?`6lCy~?`&WSWt z?U~EKFcCG_RJ(Qp7j=$I%H8t)Z@6VjA#>1f@EYiS8MRHZphp zMA_5`znM=pzUpBPO)pXGYpQ6gkine{6u_o!P@Q+NKJ}k!_X7u|qfpAyIJb$_#3@wJ z<1SE2Edkfk9C!0t%}8Yio09^F`YGzpaJHGk*-ffsn85@)%4@`;Fv^8q(-Wk7r=Q8p zT&hD`5(f?M{gfzGbbwh8(}G#|#fDuk7v1W)5H9wkorE0ZZjL0Q1=NRGY>zwgfm81DdoaVwNH;or{{eSyybt)m<=zXoA^RALYG-2t zouH|L*BLvmm9cdMmn+KGopyR@4*=&0&4g|FLoreZOhRmh=)R0bg~ zT2(8V_q7~42-zvb)+y959OAv!V$u(O3)%Es0M@CRFmG{5sovIq4%8Ahjk#*5w{+)+ zMWQoJI_r$HxL5km1#6(e@{lK3Udc~n0@g`g$s?VrnQJ$!oPnb?IHh-1qA`Rz$)Ai< z6w$-MJW-gKNvOhL+XMbE7&mFt`x1KY>k4(!KbbpZ`>`K@1J<(#vVbjx@Z@(6Q}MF# zMnbr-f55(cTa^q4+#)=s+ThMaV~E`B8V=|W_fZWDwiso8tNMTNse)RNBGi=gVwgg% zbOg8>mbRN%7^Um-7oj4=6`$|(K7!+t^90a{$18Z>}<#!bm%ZEFQ{X(yBZMc>lCz0f1I2w9Sq zuGh<9<=AO&g6BZte6hn>Qmvv;Rt)*cJfTr2=~EnGD8P$v3R|&1RCl&7)b+`=QGapi zPbLg_pxm`+HZurtFZ;wZ=`Vk*do~$wB zxoW&=j0OTbQ=Q%S8XJ%~qoa3Ea|au5o}_(P;=!y-AjFrERh%8la!z6Fn@lR?^E~H12D?8#ht=1F;7@o4$Q8GDj;sSC%Jfn01xgL&%F2 zwG1|5ikb^qHv&9hT8w83+yv&BQXOQyMVJSBL(Ky~p)gU3#%|blG?IR9rP^zUbs7rOA0X52Ao=GRt@C&zlyjNLv-} z9?*x{y(`509qhCV*B47f2hLrGl^<@SuRGR!KwHei?!CM10Tq*YDIoBNyRuO*>3FU? zHjipIE#B~y3FSfOsMfj~F9PNr*H?0oHyYB^G(YyNh{SxcE(Y-`x5jFMKb~HO*m+R% zrq|ic4fzJ#USpTm;X7K+E%xsT_3VHKe?*uc4-FsILUH;kL>_okY(w`VU*8+l>o>Jm ziU#?2^`>arnsl#)*R&nf_%>A+qwl%o{l(u)M?DK1^mf260_oteV3#E_>6Y4!_hhVD zM8AI6MM2V*^_M^sQ0dmHu11fy^kOqXqzpr?K$`}BKWG`=Es(9&S@K@)ZjA{lj3ea7_MBP zk(|hBFRjHVMN!sNUkrB;(cTP)T97M$0Dtc&UXSec<+q?y>5=)}S~{Z@ua;1xt@=T5 zI7{`Z=z_X*no8s>mY;>BvEXK%b`a6(DTS6t&b!vf_z#HM{Uoy_5fiB(zpkF{})ruka$iX*~pq1ZxD?q68dIo zIZSVls9kFGsTwvr4{T_LidcWtt$u{kJlW7moRaH6+A5hW&;;2O#$oKyEN8kx`LmG)Wfq4ykh+q{I3|RfVpkR&QH_x;t41Uw z`P+tft^E2B$domKT@|nNW`EHwyj>&}K;eDpe z1bNOh=fvIfk`&B61+S8ND<(KC%>y&?>opCnY*r5M+!UrWKxv0_QvTlJc>X#AaI^xo zaRXL}t5Ej_Z$y*|w*$6D+A?Lw-CO-$itm^{2Ct82-<0IW)0KMNvJHgBrdsIR0v~=H z?n6^}l{D``Me90`^o|q!olsF?UX3YSq^6Vu>Ijm>>PaZI8G@<^NGw{Cx&%|PwYrfw zR!gX_%AR=L3BFsf8LxI|K^J}deh0ZdV?$3r--FEX`#INxsOG6_=!v)DI>0q|BxT)z z-G6kzA01M?rba+G_mwNMQD1mbVbNTWmBi*{s_v_Ft9m2Avg!^78(QFu&n6mbRJ2bA zv!b;%yo{g*9l2)>tsZJOOp}U~8VUH`}$ z8p_}t*XIOehezolNa-a2x0BS})Y9}&*TPgua{Ewn-=wVrmJUeU39EKx+%w%=ixQWK zDLpwaNJs65#6o7Ln7~~X+p_o2BR1g~VCfxLzxA{HlWAI6^H;`juI=&r1jQrUv_q0Z z1Ja-tjdktrrP>GOC*#p?*xfQU5MqjMsBe!9lh(u8)w$e@Z|>aUHI5o;MGw*|Myiz3 z-f0;pHg~Q#%*Kx8MxH%AluVXjG2C$)WL-K63@Q`#y9_k_+}eR(x4~dp7oV-ek0H>I zgy8p#i4GN{>#v=pFYUQT(g&b$OeTy-X_#FDgNF8XyfGY6R!>inYn8IR2RDa&O!(6< znXs{W!bkP|s_YI*Yx%4stI`=ZO45IK6rBs`g7sP40ic}GZ58s?Mc$&i`kq_tfci>N zIHrC0H+Qpam1bNa=(`SRKjixBTtm&e`j9porEci!zdlg1RI0Jw#b(_Tb@RQK1Zxr_ z%7SUeH6=TrXt3J@js`4iDD0=IoHhK~I7^W8^Rcp~Yaf>2wVe|Hh1bUpX9ATD#moByY57-f2Ef1TP^lBi&p5_s7WGG9|0T}dlfxOx zXvScJO1Cnq`c`~{Dp;{;l<-KkCDE+pmexJkd}zCgE{eF=)K``-qC~IT6GcRog_)!X z?fK^F8UDz$(zFUrwuR$qro5>qqn>+Z%<5>;_*3pZ8QM|yv9CAtrAx;($>4l^_$_-L z*&?(77!-=zvnCVW&kUcZMb6;2!83si518Y%R*A3JZ8Is|kUCMu`!vxDgaWjs7^0j( ziTaS4HhQ)ldR=r)_7vYFUr%THE}cPF{0H45FJ5MQW^+W>P+eEX2kLp3zzFe*-pFVA zdDZRybv?H|>`9f$AKVjFWJ=wegO7hOOIYCtd?Vj{EYLT*^gl35|HQ`R=ti+ADm{jyQE7K@kdjuqJhWVSks>b^ zxha88-h3s;%3_5b1TqFCPTxVjvuB5U>v=HyZ$?JSk+&I%)M7KE*wOg<)1-Iy)8-K! z^XpIt|0ibmk9RtMmlUd7#Ap3Q!q9N4atQy)TmrhrFhfx1DAN`^vq@Q_SRl|V z#lU<~n67$mT)NvHh`%als+G-)x1`Y%4Bp*6Un5Ri9h=_Db zA-AdP!f>f0m@~>7X#uBM?diI@)Egjuz@jXKvm zJo+==juc9_<;CqeRaU9_Mz@;3e=E4=6TK+c`|uu#pIqhSyNm`G(X)&)B`8q0RBv#> z`gGlw(Q=1Xmf55VHj%C#^1lpc>LY8kfA@|rlC1EA<1#`iuyNO z(=;irt{_&K=i4)^x%;U(Xv<)+o=dczC5H3W~+e|f~{*ucxj@{Yi-cw^MqYr3fN zF5D+~!wd$#al?UfMnz(@K#wn`_5na@rRr8XqN@&M&FGEC@`+OEv}sI1hw>Up0qAWf zL#e4~&oM;TVfjRE+10B_gFlLEP9?Q-dARr3xi6nQqnw>k-S;~b z;!0s2VS4}W8b&pGuK=7im+t(`nz@FnT#VD|!)eQNp-W6)@>aA+j~K*H{$G`y2|QHY z|Hmy+CR@#jWY4~)lr1qBJB_RfHJFfP<}pK5(#ZZGSqcpyS&}01LnTWk5fzmXMGHkJ zTP6L^B+uj;lmB_W<~4=${+v0>z31M!-_O@o-O9GyW)j_mjx}!0@br_LE-7SIuPP84 z;5=O(U*g_um0tyG|61N@d9lEuOeiRd+#NY^{nd5;-CVlw&Ap7J?qwM^?E29wvS}2d zbzar4Fz&RSR(-|s!Z6+za&Z zY#D<5q_JUktIzvL0)yq_kLWG6DO{ri=?c!y!f(Dk%G{8)k`Gym%j#!OgXVDD3;$&v@qy#ISJfp=Vm>pls@9-mapVQChAHHd-x+OGx)(*Yr zC1qDUTZ6mM(b_hi!TuFF2k#8uI2;kD70AQ&di$L*4P*Y-@p`jdm%_c3f)XhYD^6M8&#Y$ZpzQMcR|6nsH>b=*R_Von!$BTRj7yGCXokoAQ z&ANvx0-Epw`QIEPgI(^cS2f(Y85yV@ygI{ewyv5Frng)e}KCZF7JbR(&W618_dcEh(#+^zZFY;o<815<5sOHQdeax9_!PyM&;{P zkBa5xymca0#)c#tke@3KNEM8a_mT&1gm;p&&JlMGH(cL(b)BckgMQ^9&vRwj!~3@l zY?L5}=Jzr080OGKb|y`ee(+`flQg|!lo6>=H)X4`$Gz~hLmu2a%kYW_Uu8x09Pa0J zKZ`E$BKJ=2GPj_3l*TEcZ*uYRr<*J^#5pILTT;k_cgto1ZL-%slyc16J~OH-(RgDA z%;EjEnoUkZ&acS{Q8`{i6T5^nywgqQI5bDIymoa7CSZG|WWVk>GM9)zy*bNih|QIm z%0+(Nnc*a_xo;$=!HQYaapLms>J1ToyjtFByY`C2H1wT#178#4+|{H0BBqtCdd$L% z_3Hc60j@{t9~MjM@LBalR&6@>B;9?r<7J~F+WXyYu*y3?px*=8MAK@EA+jRX8{CG?GI-< z54?Dc9CAh>QTAvyOEm0^+x;r2BWX|{3$Y7)L5l*qVE*y0`7J>l2wCmW zL1?|a`pJ-l{fb_N;R(Z9UMiSj6pQjOvQ^%DvhIJF!+Th7jO2~1f1N+(-TyCFYQZYw z4)>7caf^Ki_KJ^Zx2JUb z&$3zJy!*+rCV4%jqwyuNY3j1ZEiltS0xTzd+=itTb;IPYpaf?8Y+RSdVdpacB(bVQ zC(JupLfFp8y43%PMj2}T|VS@%LVp>hv4Y!RPMF?pp8U_$xCJ)S zQx!69>bphNTIb9yn*_yfj{N%bY)t{L1cs8<8|!f$;UQ*}IN=2<6lA;x^(`8t?;+ST zh)z4qeYYgZkIy{$4x28O-pugO&gauRh3;lti9)9Pvw+^)0!h~%m&8Q!AKX%urEMnl z?yEz?g#ODn$UM`+Q#$Q!6|zsq_`dLO5YK-6bJM6ya>}H+vnW^h?o$z;V&wvuM$dR& zeEq;uUUh$XR`TWeC$$c&Jjau2it3#%J-y}Qm>nW*s?En?R&6w@sDXMEr#8~$=b(gk zwDC3)NtAP;M2BW_lL^5ShpK$D%@|BnD{=!Tq)o(5@z3i7Z){} zGr}Exom_qDO{kAVkZ*MbLNHE666Kina#D{&>Jy%~w7yX$oj;cYCd^p9zy z8*+wgSEcj$4{WxKmCF(5o7U4jqwEvO&dm1H#7z}%VXAbW&W24v-tS6N3}qrm1OnE)fUkoE8yMMn9S$?IswS88tQWm4#Oid#ckgr6 zRtHm!mfNl-`d>O*1~d7%;~n+{Rph6BBy^95zqI{K((E!iFQ+h*C3EsbxNo_aRm5gj zKYug($r*Q#W9`p%Bf{bi6;IY0v`pB^^qu)gbg9QHQ7 zWBj(a1YSu)~2RK8Pi#C>{DMlrqFb9e_RehEHyI{n?e3vL_}L>kYJC z_ly$$)zFi*SFyNrnOt(B*7E$??s67EO%DgoZL2XNk8iVx~X_)o++4oaK1M|ou73vA0K^503j@uuVmLcHH4ya-kOIDfM%5%(E z+Xpt~#7y2!KB&)PoyCA+$~DXqxPxxALy!g-O?<9+9KTk4Pgq4AIdUkl`1<1#j^cJg zgU3`0hkHj_jxV>`Y~%LAZl^3o0}`Sm@iw7kwff{M%VwtN)|~!p{AsfA6vB5UolF~d zHWS%*uBDt<9y!9v2Xe|au&1j&iR1HXCdyCjxSgG*L{wmTD4(NQ=mFjpa~xooc6kju z`~+d{j7$h-;HAB04H!Zscu^hZffL#9!p$)9>sRI|Yovm)g@F>ZnosF2EgkU3ln0bR zTA}|+E(tt)!SG)-bEJi_0m{l+(cAz^pi}`9=~n?y&;2eG;d9{M6nj>BHGn(KA2n|O zt}$=FPq!j`p&kQ8>cirSzkU0c08%8{^Qyqi-w2LoO8)^E7;;I1;HQ6B$u0nNaX2CY zSmfi)F`m94zL8>#zu;8|{aBui@RzRKBlP1&mfFxEC@%cjl?NBs`cr^nm){>;$g?rhKr$AO&6qV_Wbn^}5tfFBry^e1`%du2~o zs$~dN;S_#%iwwA_QvmMjh%Qo?0?rR~6liyN5Xmej8(*V9ym*T`xAhHih-v$7U}8=dfXi2i*aAB!xM(Xekg*ix@r|ymDw*{*s0?dlVys2e)z62u1 z+k3esbJE=-P5S$&KdFp+2H7_2e=}OKDrf( z9-207?6$@f4m4B+9E*e((Y89!q?zH|mz_vM>kp*HGXldO0Hg#!EtFhRuOm$u8e~a9 z5(roy7m$Kh+zjW6@zw{&20u?1f2uP&boD}$#Zy)4o&T;vyBoqFiF2t;*g=|1=)PxB z8eM3Mp=l_obbc?I^xyLz?4Y1YDWPa+nm;O<$Cn;@ane616`J9OO2r=rZr{I_Kizyc zP#^^WCdIEp*()rRT+*YZK>V@^Zs=ht32x>Kwe zab)@ZEffz;VM4{XA6e421^h~`ji5r%)B{wZu#hD}f3$y@L0JV9f3g{-RK!A?vBUA}${YF(vO4)@`6f1 z-A|}e#LN{)(eXloDnX4Vs7eH|<@{r#LodP@Nz--$Dg_Par%DCpu2>2jUnqy~|J?eZ zBG4FVsz_A+ibdwv>mLp>P!(t}E>$JGaK$R~;fb{O3($y1ssQQo|5M;^JqC?7qe|hg zu0ZOqeFcp?qVn&Qu7FQJ4hcFi&|nR!*j)MF#b}QO^lN%5)4p*D^H+B){n8%VPUzi! zDihoGcP71a6!ab`l^hK&*dYrVYzJ0)#}xVrp!e;lI!+x+bfCN0KXwUAPU9@#l7@0& QuEJmfE|#`Dqx|px0L@K;Y5)KL literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..f959987 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100644 index 0000000..c53aefa --- /dev/null +++ b/gradlew @@ -0,0 +1,234 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original 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 POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${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 "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# 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 ;; #( + MSYS* | 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" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..107acd3 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,89 @@ +@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 Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@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 execute + +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 execute + +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 + +: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 %* + +: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/settings.gradle b/settings.gradle new file mode 100644 index 0000000..c363694 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2022' + 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/src/main/java/com/spartronics4915/frc2022/Constants.java b/src/main/java/com/spartronics4915/frc2022/Constants.java new file mode 100644 index 0000000..2fe2037 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/Constants.java @@ -0,0 +1,15 @@ +package com.spartronics4915.frc2022; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This class should not be used for any other + * purpose. All constants should be declared globally (i.e. public static). + * Do not put anything functional in this class. + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. + */ +public final class Constants +{ + +} diff --git a/src/main/java/com/spartronics4915/frc2022/Main.java b/src/main/java/com/spartronics4915/frc2022/Main.java new file mode 100644 index 0000000..c80af63 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/Main.java @@ -0,0 +1,23 @@ +package com.spartronics4915.frc2022; + +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/src/main/java/com/spartronics4915/frc2022/Robot.java b/src/main/java/com/spartronics4915/frc2022/Robot.java new file mode 100644 index 0000000..a655232 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/Robot.java @@ -0,0 +1,95 @@ +package com.spartronics4915.frc2022; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class Robot extends TimedRobot +{ + private Command mAutonomousCommand; + private RobotContainer mRobotContainer; + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() + { + // Instantiate our RobotContainer. This will perform all our button + // bindings, and put our autonomous chooser on the dashboard. + mRobotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. + * Use this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + *

+ * This runs after the mode specific periodic functions, + * but before LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() + { + // Runs the Scheduler. This is responsible for polling buttons, + // adding newly-scheduled commands, running already-scheduled commands, + // removing finished or interrupted commands, and running subsystem + // periodic() methods. + // This must be called from the robot's periodic block in order for + // anything in the command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** This function is called once each time the robot enters disabled mode. */ + @Override + public void disabledInit() {} + + /** This function is called periodically while the robot is in disabled mode. */ + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() + { + mAutonomousCommand = mRobotContainer.getAutonomousCommand(); + + if (mAutonomousCommand != null) + { + mAutonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopInit() + { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (mAutonomousCommand != null) { + mAutonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once each time the robot enters test mode. */ + @Override + public void testInit() + { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} +} diff --git a/src/main/java/com/spartronics4915/frc2022/RobotContainer.java b/src/main/java/com/spartronics4915/frc2022/RobotContainer.java new file mode 100644 index 0000000..811306a --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/RobotContainer.java @@ -0,0 +1,45 @@ +package com.spartronics4915.frc2022; + +import com.spartronics4915.frc2022.commands.ExampleCommand; +import com.spartronics4915.frc2022.subsystems.ExampleSubsystem; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; + +/** + * This class is where the bulk of the robot should be declared. + * Since command-based is a "declarative" paradigm, very little robot logic + * should actually be handled in the {@link Robot} periodic methods + * (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer +{ + // The robot's subsystems and commands are defined here... + public final ExampleSubsystem mExampleSubsystem; + public final ExampleCommand mAutoCommand; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() + { + // ...and constructed here. + mExampleSubsystem = new ExampleSubsystem(); + mAutoCommand = new ExampleCommand(mExampleSubsystem); + + configureButtonBindings(); + } + + /** Use this method to define your button ==> command mappings. */ + private void configureButtonBindings() {} + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() + { + return mAutoCommand; + } +} diff --git a/src/main/java/com/spartronics4915/frc2022/commands/ExampleCommand.java b/src/main/java/com/spartronics4915/frc2022/commands/ExampleCommand.java new file mode 100644 index 0000000..8748690 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/commands/ExampleCommand.java @@ -0,0 +1,43 @@ +package com.spartronics4915.frc2022.commands; + +import com.spartronics4915.frc2022.subsystems.ExampleSubsystem; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * Detailed description of ExampleCommand. + */ +public class ExampleCommand extends CommandBase +{ + private final ExampleSubsystem mExampleSubsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) + { + mExampleSubsystem = subsystem; + addRequirements(mExampleSubsystem); // Declares subsystem dependencies + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Returns true when the command should end. + @Override + public boolean isFinished() + { + return false; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} +} diff --git a/src/main/java/com/spartronics4915/frc2022/subsystems/ExampleSubsystem.java b/src/main/java/com/spartronics4915/frc2022/subsystems/ExampleSubsystem.java new file mode 100644 index 0000000..82d367f --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2022/subsystems/ExampleSubsystem.java @@ -0,0 +1,37 @@ +package com.spartronics4915.frc2022.subsystems; + +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; + +/** + * Detailed description of ExampleSubsystem. + */ +public class ExampleSubsystem extends SpartronicsSubsystem +{ + // The subsystem's hardware is defined here... + + /** Creates a new ExampleSubsystem. */ + public ExampleSubsystem() + { + boolean success = true; + try + { + // ...and constructed here. + } + catch (Exception exception) + { + logException("Could not construct hardware: ", exception); + success = false; + } + logInitialized(success); + } + + // Subsystem methods - actions the robot can take - should be placed here. + + /** This method will be called once per scheduler run. */ + @Override + public void periodic() {} + + /** This method will be called once per scheduler run during simulation. */ + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/CANCounter.java b/src/main/java/com/spartronics4915/lib/hardware/CANCounter.java new file mode 100644 index 0000000..34a69af --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/CANCounter.java @@ -0,0 +1,29 @@ +package com.spartronics4915.lib.hardware; + +public class CANCounter +{ + private static int sFoundCANDevices = 0; + private static int sTotalCANDevices = 0; + + public static void addDevice(boolean errored) + { + sFoundCANDevices += errored ? 0 : 1; + sTotalCANDevices++; + } + + public static int getFoundDevices() + { + return sFoundCANDevices; + } + + public static int getTotalDevices() + { + return sTotalCANDevices; + } + + public static String getStatusMessage() + { + return sFoundCANDevices == sTotalCANDevices ? "OK" + : sFoundCANDevices + "/" + sTotalCANDevices; + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java new file mode 100644 index 0000000..ec43338 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SensorModel.java @@ -0,0 +1,59 @@ +package com.spartronics4915.lib.hardware.motors; + +public class SensorModel +{ + private final double mToCustomUnitsMultiplier; + + /** + * This is a convenience constructor for sensors connected to a wheel. Use the + * more direct constructor if your sensor is not connected to a wheel. + * + * @param wheelDiameterMeters The diameter of your wheel in meters. + * @param nativeUnitsPerRevolution The number of native units per wheel revolution. + */ + public static SensorModel fromWheelDiameter(double wheelDiameterMeters, + double nativeUnitsPerRevolution) + { + return new SensorModel((1 / nativeUnitsPerRevolution) * (wheelDiameterMeters * Math.PI)); + } + + /** + * @param nativeUnitsToCustomUnitsMultiplier A number that, when multiplied with some + * amount of native units, converts to custom + * units. This factor will also be used to + * convert to related units, like custom + * units/sec. + */ + public static SensorModel fromMultiplier(double nativeUnitsToCustomUnitsMultiplier) + { + return new SensorModel(nativeUnitsToCustomUnitsMultiplier); + } + + /** + * @param nativeUnitsPerRevolution The number of native units per wheel revolution. + */ + public static SensorModel toRadians(double nativeUnitsPerRevolution) + { + return new SensorModel((1 / nativeUnitsPerRevolution) * 2 * Math.PI); + } + + public static SensorModel toDegrees(double nativeUnitsPerRevolution) + { + return new SensorModel(360 / nativeUnitsPerRevolution); + } + + private SensorModel(double nativeUnitsToCustomUnitsMultiplier) + { + mToCustomUnitsMultiplier = nativeUnitsToCustomUnitsMultiplier; + } + + public double toCustomUnits(double nativeUnits) + { + return nativeUnits * mToCustomUnitsMultiplier; + } + + public double toNativeUnits(double customUnits) + { + return customUnits / mToCustomUnitsMultiplier; + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsAnalogEncoder.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsAnalogEncoder.java new file mode 100644 index 0000000..c6c2fb9 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsAnalogEncoder.java @@ -0,0 +1,163 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.spartronics4915.lib.hardware.motors; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogTrigger; +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; + +/** + * Class for supporting continuous analog encoders, such as the US Digital MA3. + */ +public class SpartronicsAnalogEncoder implements Sendable, AutoCloseable { + private final AnalogInput m_analogInput; + private AnalogTrigger m_analogTrigger; + private Counter m_counter; + private double m_positionOffset; + private double m_distancePerRotation = 1.0; + private double m_lastPosition; + + protected SimDevice m_simDevice; + protected SimDouble m_simPosition; + + /** + * Construct a new AnalogEncoder attached to a specific AnalogInput. + * + * @param analogInput the analog input to attach to + */ + public SpartronicsAnalogEncoder(AnalogInput analogInput) { + m_analogInput = analogInput; + init(); + } + + private void init() { + m_analogTrigger = new AnalogTrigger(m_analogInput); + m_counter = new Counter(); + + m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel()); + + if (m_simDevice != null) { + m_simPosition = m_simDevice.createDouble("Position", false, 0.0); + } + + // Limits need to be 25% from each end + m_analogTrigger.setLimitsVoltage(1.195,3.585); + m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse); + m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse); + + SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel()); + } + + /** + * Get the encoder value since the last reset. + * + *

This is reported in rotations since the last reset. + * + * @return the encoder value in rotations + */ + public double get() { + if (m_simPosition != null) { + return m_simPosition.get(); + } + + // As the values are not atomic, keep trying until we get 2 reads of the same + // value. If we don't within 10 attempts, warn + for (int i = 0; i < 10; i++) { + double counter = m_counter.get(); + double pos = m_analogInput.getVoltage()/4.78; + double counter2 = m_counter.get(); + double pos2 = m_analogInput.getVoltage()/4.78; + if (counter == counter2 && pos == pos2) { + double position = counter + pos - m_positionOffset; + m_lastPosition = position; + return position; + } + } + + DriverStation.reportWarning( + "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false); + return m_lastPosition; + } + + /** + * Get the offset of position relative to the last reset. + * + *

getPositionInRotation() - getPositionOffset() will give an encoder absolute + * position relative to the last reset. This could potentially be negative, + * which needs to be accounted for. + * + * @return the position offset + */ + public double getPositionOffset() { + return m_positionOffset; + } + + /** + * Set the distance per rotation of the encoder. This sets the multiplier used + * to determine the distance driven based on the rotation value from the + * encoder. Set this value based on the how far the mechanism travels in 1 + * rotation of the encoder, and factor in gearing reductions following the + * encoder shaft. This distance can be in any units you like, linear or angular. + * + * @param distancePerRotation the distance per rotation of the encoder + */ + public void setDistancePerRotation(double distancePerRotation) { + m_distancePerRotation = distancePerRotation; + } + + /** + * Get the distance per rotation for this encoder. + * + * @return The scale factor that will be used to convert rotation to useful + * units. + */ + public double getDistancePerRotation() { + return m_distancePerRotation; + } + + /** + * Get the distance the sensor has driven since the last reset as scaled by the + * value from {@link #setDistancePerRotation(double)}. + * + * @return The distance driven since the last reset + */ + public double getDistance() { + return get() * getDistancePerRotation(); + } + + /** + * Reset the Encoder distance to zero. + */ + public void reset() { + m_counter.reset(); + m_positionOffset = m_analogInput.getVoltage(); + } + + @Override + public void close() { + m_counter.close(); + m_analogTrigger.close(); + if (m_simDevice != null) { + m_simDevice.close(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("AbsoluteEncoder"); + builder.addDoubleProperty("Distance", this::getDistance, null); + builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null); + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java new file mode 100644 index 0000000..db28e09 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsEncoder.java @@ -0,0 +1,29 @@ +package com.spartronics4915.lib.hardware.motors; + +public interface SpartronicsEncoder +{ + + /** + * @return Velocity in custom units/second. + */ + double getVelocity(); + + /** + * @return Position in custom units. + */ + double getPosition(); + + /** + * Sets the "direction" (phase) of this encoder. + * + * @param isReversed If true, the sensor's output is reversed. + */ + void setPhase(boolean isReversed); + + /** + * Sets the current position (The value stored, not PID target) + * + * @param TargetPosition position + */ + boolean setPosition(double position); +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java new file mode 100644 index 0000000..3c8124a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMax.java @@ -0,0 +1,432 @@ +package com.spartronics4915.lib.hardware.motors; + +import com.revrobotics.CANAnalog; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANError; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANAnalog.AnalogMode; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.spartronics4915.lib.hardware.CANCounter; +import com.spartronics4915.lib.util.Logger; + +import edu.wpi.first.wpilibj.RobotBase; + +public class SpartronicsMax implements SpartronicsMotor +{ + + private static final int kVelocitySlotIdx = 0; + private static final int kPositionSlotIdx = 1; + + private final double kRPMtoRPS = 1.0 / 60.0; + + private final CANSparkMax mSparkMax; + private final CANSparkMax mFollower; + private final SpartronicsEncoder mEncoder; + private final SensorModel mSensorModel; + private final boolean mHadStartupError; + + private boolean mBrakeMode = false; + /** Volts */ + private double mVoltageCompSaturation = 12.0; + /** Native units/sec, converted to meters on get and set */ + private double mMotionProfileCruiseVelocity = 0.0; + /** Native units/sec^2, converted to meters on get and set */ + private double mMotionProfileAcceleration = 0.0; + private boolean mUseMotionProfileForPosition = false; + + private CANEncoder mEncoderSensor; + private CANAnalog mAnalogSensor; + private CANPIDController mPIDController; + private AnalogMode mAnalogMode; + private FeedbackSensorType mFeedbackSensor; + + public class InternalEncoder implements SpartronicsEncoder + { + + @Override + public double getVelocity() + { + return mSensorModel.toCustomUnits(mEncoderSensor.getVelocity()); + } + + @Override + public double getPosition() + { + return mSensorModel.toCustomUnits(mEncoderSensor.getPosition()); + } + + @Override + public void setPhase(boolean isReversed) + { + mSparkMax.setInverted(isReversed); + } + + @Override + public boolean setPosition(double position) + { + mEncoderSensor.setPosition(position); + return true; + } + } + + public class AnalogEncoder implements SpartronicsEncoder + { + + @Override + public double getVelocity() + { + return mSensorModel.toCustomUnits(mAnalogSensor.getVelocity()); + } + + @Override + public double getPosition() + { + return mSensorModel.toCustomUnits(mAnalogSensor.getPosition()); + } + + @Override + public void setPhase(boolean isReversed) + { + mSparkMax.setInverted(isReversed); + } + + @Override + public boolean setPosition(double position) + { + return false; + } + } + + public static enum FeedbackSensorType + { + kInternal, kAnalogRelative, kAnalogAbsolute + } + + // Temporary!!!! + public static SpartronicsMotor makeMotorBrushed(int deviceNumber) + { + if (RobotBase.isSimulation()) + { + return new SpartronicsSimulatedMotor(deviceNumber); + } + + Logger.warning("You're using a **temporary** Spark Max brushed constructor! Revert to brushless when you plug in a brushless motor, or be smitten by the Rev Robotics (tm) gods!!!1!"); + + return new SpartronicsMax(new CANSparkMax(deviceNumber, MotorType.kBrushed), SensorModel.fromMultiplier(1), FeedbackSensorType.kInternal, null); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel, + FeedbackSensorType feedbackSensor) + { + if (RobotBase.isSimulation()) + { + return new SpartronicsSimulatedMotor(deviceNumber); + } + return new SpartronicsMax(new CANSparkMax(deviceNumber, MotorType.kBrushless), sensorModel, + feedbackSensor, null); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel) + { + return makeMotor(deviceNumber, sensorModel, FeedbackSensorType.kInternal); + } + + public static SpartronicsMotor makeMotor(int deviceNumber) + { + return makeMotor(deviceNumber, SensorModel.fromMultiplier(1)); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel, + FeedbackSensorType feedbackSensor, int followerDeviceNumber) + { + if (RobotBase.isSimulation()) + { + return new SpartronicsSimulatedMotor(deviceNumber, followerDeviceNumber); + } + + // We only use SPARK MAXes for brushless motors + // If that changes we can make motor type configurable + var master = new CANSparkMax(deviceNumber, MotorType.kBrushless); + CANSparkMax follower = new CANSparkMax(followerDeviceNumber, MotorType.kBrushless); + follower.follow(master); + return new SpartronicsMax(master, sensorModel, feedbackSensor, follower); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel, + int followerDeviceNumber) + { + return makeMotor(deviceNumber, sensorModel, FeedbackSensorType.kInternal, followerDeviceNumber); + } + + private SpartronicsMax(CANSparkMax spark, SensorModel sensorModel, + FeedbackSensorType feedbackSensor, CANSparkMax follower) + { + mSparkMax = spark; + mFollower = follower; + mSensorModel = sensorModel; + mPIDController = mSparkMax.getPIDController(); + mFeedbackSensor = feedbackSensor; + + CANError err; + switch (feedbackSensor) + { + case kInternal: + mEncoderSensor = mSparkMax.getEncoder(); + err = mEncoderSensor.setVelocityConversionFactor(kRPMtoRPS); // Set conversion + // factor. + mEncoder = new InternalEncoder(); + mPIDController.setFeedbackDevice(mEncoderSensor); + break; + case kAnalogRelative: + mAnalogMode = AnalogMode.kRelative; + mAnalogSensor = mSparkMax.getAnalog(mAnalogMode); + err = mAnalogSensor.setVelocityConversionFactor(kRPMtoRPS); + mEncoder = new AnalogEncoder(); + mPIDController.setFeedbackDevice(mAnalogSensor); + break; + case kAnalogAbsolute: + mAnalogMode = AnalogMode.kAbsolute; + mAnalogSensor = mSparkMax.getAnalog(mAnalogMode); + err = mAnalogSensor.setVelocityConversionFactor(kRPMtoRPS); + mEncoder = new AnalogEncoder(); + mPIDController.setFeedbackDevice(mAnalogSensor); + break; + default: + mEncoder = new InternalEncoder(); + err = CANError.kError; // stops errors. Should never happen + } + if (err != CANError.kOk) + { + Logger.error("SparkMax on with ID " + mSparkMax.getDeviceId() + + " returned a non-OK error code on sensor configuration... Is the motor controller plugged in?"); + mHadStartupError = true; + } + else + { + mHadStartupError = false; + } + CANCounter.addDevice(mHadStartupError); + + mSparkMax.enableVoltageCompensation(mVoltageCompSaturation); + } + + @Override + public SpartronicsEncoder getEncoder() + { + return mEncoder; + } + + /** + * @return An object for interfacing with a connected analog sensor. + */ + public CANAnalog getAnalog() + { + return mSparkMax.getAnalog(AnalogMode.kAbsolute); + } + + @Override + public boolean hadStartupError() + { + return false;//mHadStartupError; Change back when comp season is over! + } + + @Override + public double getVoltageOutput() + { + return mSparkMax.getBusVoltage() * mSparkMax.getAppliedOutput(); + } + + @Override + public boolean getOutputInverted() + { + return mSparkMax.getInverted(); + } + + @Override + public void setOutputInverted(boolean inverted) + { + mSparkMax.setInverted(inverted); + } + + @Override + public boolean getBrakeMode() + { + return mBrakeMode; + } + + @Override + public void setBrakeMode(boolean mode) + { + mBrakeMode = mode; + mSparkMax.setIdleMode(mode ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public double getVoltageCompSaturation() + { + return mVoltageCompSaturation; + } + + @Override + public void setVoltageCompSaturation(double voltage) + { + mVoltageCompSaturation = voltage; + mSparkMax.enableVoltageCompensation(mVoltageCompSaturation); + } + + @Override + public double getMotionProfileCruiseVelocity() + { + return mSensorModel.toCustomUnits(mMotionProfileCruiseVelocity); + } + + @Override + public void setMotionProfileCruiseVelocity(double velocityMetersPerSecond) + { // Set to slot + mMotionProfileCruiseVelocity = mSensorModel.toNativeUnits(velocityMetersPerSecond); + mPIDController.setSmartMotionMaxVelocity(mMotionProfileCruiseVelocity, + kPositionSlotIdx); + } + + @Override + public double getMotionProfileMaxAcceleration() + { + return mSensorModel.toCustomUnits(mMotionProfileAcceleration); + } + + @Override + public void setMotionProfileMaxAcceleration(double accelerationMetersPerSecondSq) + { + mMotionProfileAcceleration = mSensorModel.toNativeUnits(accelerationMetersPerSecondSq); + mPIDController.setSmartMotionMaxAccel(mMotionProfileAcceleration, kPositionSlotIdx); + } + + @Override + public void setUseMotionProfileForPosition(boolean useMotionProfile) + { + mUseMotionProfileForPosition = useMotionProfile; + mPIDController.setSmartMotionAllowedClosedLoopError(2.0/360.0, kPositionSlotIdx); + } + + @Override + public void setPercentOutput(double dutyCycle, double arbitraryFeedForwardVolts) + { + mPIDController.setReference(dutyCycle, ControlType.kDutyCycle, 0, + arbitraryFeedForwardVolts); + } + + @Override + public void setPercentOutput(double dutyCycle) + { + setPercentOutput(dutyCycle, 0.0); + } + + @Override + public void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts) + { + double velocityNative = mSensorModel.toNativeUnits(velocityMetersPerSecond); + mPIDController.setReference(velocityNative, ControlType.kVelocity, kVelocitySlotIdx, + arbitraryFeedForwardVolts); + } + + @Override + public void setVelocityGains(double kP, double kD) + { + setVelocityGains(kP, 0, kD, 0); + } + + @Override + public void setVelocityGains(double kP, double kI, double kD, double kF) + { + mPIDController.setP(kP, kVelocitySlotIdx); + mPIDController.setI(kI, kVelocitySlotIdx); + mPIDController.setD(kD, kVelocitySlotIdx); + mPIDController.setFF(kF, kVelocitySlotIdx); + } + + @Override + public void setPosition(double positionMeters) + { + double positionNativeUnits = mSensorModel.toNativeUnits(positionMeters); + mPIDController.setReference(positionNativeUnits, + mUseMotionProfileForPosition ? ControlType.kSmartMotion : ControlType.kPosition, + kPositionSlotIdx); + } + + @Override + public void setPositionGains(double kP, double kD) + { + setPositionGains(kP, 0, kD, 0); + } + + @Override + public void setPositionGains(double kP, double kI, double kD, double kF) + { + mPIDController.setP(kP, kPositionSlotIdx); + mPIDController.setI(kI, kPositionSlotIdx); + mPIDController.setD(kD, kPositionSlotIdx); + mPIDController.setFF(kF, kPositionSlotIdx); + } + + @Override + public SensorModel getSensorModel() + { + return mSensorModel; + } + + @Override + public void setVelocity(double velocityMetersPerSecond) + { + setVelocity(velocityMetersPerSecond, 0.0); + } + + @Override + public void setNeutral() + { + mPIDController.setReference(0.0, ControlType.kDutyCycle, 0); + } + + public FeedbackSensorType getFeedbackSensor() + { + return mFeedbackSensor; + } + + @Override + public double getOutputCurrent() + { + return mSparkMax.getOutputCurrent(); + } + + @Override + public SpartronicsMotor getFollower() + { + return new SpartronicsMax(mFollower, this.mSensorModel, this.mFeedbackSensor, null); + } + + @Override + public int getDeviceNumber() + { + return mSparkMax.getDeviceId(); + } + + @Override + public void setSoftLimits(double forwardLimitCustomUnits, double reverseLimitCustomUnits) + { + mSparkMax.enableSoftLimit(SoftLimitDirection.kForward, true); + mSparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true); + + mSparkMax.setSoftLimit(SoftLimitDirection.kForward, (float) mSensorModel.toNativeUnits(forwardLimitCustomUnits)); + mSparkMax.setSoftLimit(SoftLimitDirection.kReverse, (float) mSensorModel.toNativeUnits(reverseLimitCustomUnits)); + } + + @Override + public void setStatorCurrentLimit(int limitAmps) + { + mSparkMax.setSmartCurrentLimit(limitAmps); + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java new file mode 100644 index 0000000..80ebe75 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsMotor.java @@ -0,0 +1,188 @@ +package com.spartronics4915.lib.hardware.motors; + +import com.spartronics4915.lib.util.Logger; + +public interface SpartronicsMotor +{ + + /** + * @return The encoder attached to the motor. + */ + SpartronicsEncoder getEncoder(); + + /** + * @return A {@link SensorModel} that converts between native units and meters. + */ + SensorModel getSensorModel(); + + /** + * This method is useful for detecting unplugged motor controllers. + * + * @return If true, the motor controller returned some type of error on startup. + */ + boolean hadStartupError(); + + /** + * @return Current output of the motor controller in Volts. + */ + double getVoltageOutput(); + + /** + * @return Is the motor output inverted. + */ + boolean getOutputInverted(); + + /** + * @param inverted If true, the motor output is inverted. + */ + void setOutputInverted(boolean inverted); + + /** + * @return Are the motor leads electrically commonized to reduce movement. + */ + boolean getBrakeMode(); + + /** + * @param mode If true, commonize the motor leads to reduce movement. + */ + void setBrakeMode(boolean mode); + + /** + * @return The max voltage output given to the motor. + */ + double getVoltageCompSaturation(); + + /** + * @param voltage Sets the max voltage output given to the motor, in Volts. + */ + void setVoltageCompSaturation(double voltage); + + /** + * @return The max velocity in meters/s that the on board motion profile + * generator will use. + */ + double getMotionProfileCruiseVelocity(); + + /** + * @param velocityCustomUnitsPerSecond The max velocity in custom units/s that the on board + * motion profile generator will use. + */ + void setMotionProfileCruiseVelocity(double velocityCustomUnitsPerSecond); + + /** + * @return The max acceleration in custom units/s^2 that the on board motion profile + * generator will use. + */ + double getMotionProfileMaxAcceleration(); + + /** + * @param accelerationCustomUnitsSecSq The max acceleration in custom units/s^2 that the on + * board motion profile generator will use. + */ + void setMotionProfileMaxAcceleration(double accelerationCustomUnitsSecSq); + + /** + * @param useMotionProfile If true, we will use the motion profile to get to + * positions passed to + * {@link SpartronicsMotor#setPosition(double) + * setPosition}. + */ + void setUseMotionProfileForPosition(boolean useMotionProfile); + + /** + * Sets the output as a percentage (like setOpenLoop). + * + * @param dutyCycle Output in perecnt. + * @param arbitraryFeedforward Additional arbitrary feedforward in Volts. + */ + void setPercentOutput(double dutyCycle, double arbitraryFeedForwardVolts); + + /** + * Sets the output as a percentage (like setOpenLoop). + * + * @param dutyCycle Output in percent. + */ + void setPercentOutput(double dutyCycle); + + /** + * Sets the target output velocity. + * + * @param velocityCustomUnitsPerSecond Velocity in custom units/s. + */ + void setVelocity(double velocityCustomUnitsPerSecond); + + /** + * Sets the target output velocity. + * + * @param velocityCustomUnitsPerSecond Velocity in custom units/s. + * @param arbitraryFeedForwardVolts Additional arbitrary feedforward in Volts. + */ + void setVelocity(double velocityCustomUnitsPerSecond, double arbitraryFeedForwardVolts); + + /** + * Sets the PD gains for the built in velocity PID controller. + */ + void setVelocityGains(double kP, double kD); + /** + * Sets the PID gains for the built in velocity PID controller. + */ + void setVelocityGains(double kP, double kI, double kD, double kF); + + /** + * Sets the target position. + * + * @param positionCustomUnits Target position in custom units. + */ + void setPosition(double positionCustomUnits); + + /** + * Sets the PID gains for the built in position PID controller. + */ + void setPositionGains(double kP, double kD); + /** + * Sets the PID gains for the built in position PID controller. + */ + void setPositionGains(double kP, double kI, double kD, double kF); + + /** + * Turns the motor off. + */ + void setNeutral(); + + /** + * @return The output current of the motor in amps. + */ + double getOutputCurrent(); + + /** + * @return The motor following this motor, or null. + */ + SpartronicsMotor getFollower(); + + /** + * @return The device ID + */ + int getDeviceNumber(); + + /** + * @param forwardLimitCustomUnits Forward soft limit position in custom units. + * @param reverseLimitCustomUnits Reverse soft limit position in custom units. + */ + void setSoftLimits(double forwardLimitCustomUnits, double reverseLimitCustomUnits); + + /** + * @param limitAmps Max stator current in amps. + */ + default void setStatorCurrentLimit(int limitAmps) + { + Logger.warning("Stator current limit not implemented for device number " + getDeviceNumber() + "!"); + } + + /** + * @param limitAmps Max input current in amps. + */ + default void setSupplyCurrentLimit(int limitAmps, double maxTimeAtLimit) + { + Logger.warning("Supply current limit not implemented for device number " + getDeviceNumber() + "!"); + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java new file mode 100644 index 0000000..18d11f7 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSRX.java @@ -0,0 +1,355 @@ +package com.spartronics4915.lib.hardware.motors; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.spartronics4915.lib.hardware.CANCounter; +import com.spartronics4915.lib.util.Logger; + +import edu.wpi.first.wpilibj.RobotBase; + +public class SpartronicsSRX implements SpartronicsMotor +{ + private static final int kVelocitySlotIdx = 0; + private static final int kPositionSlotIdx = 1; + + // This conversion could go into the sensor model, but the per 100ms thing is + // Talon-only, so it's not worth it. + private static final double kMetersPer100msToMetersPerSecond = 10; + private static final double kMetersPerSecondToMetersPer100ms = 1 + / kMetersPer100msToMetersPerSecond; + + private final TalonSRX mTalonSRX; + private final TalonSRX mFollower; + private final SpartronicsEncoder mEncoder; + private final SensorModel mSensorModel; + private final boolean mHadStartupError; + + private boolean mBrakeMode = false; + /** Volts */ + private double mVoltageCompSaturation = 12.0; + /** Native units/sec, converted to meters on get and set */ + private double mMotionProfileCruiseVelocity = 0.0; + /** Native units/sec^2, converted to meters on get and set */ + private double mMotionProfileAcceleration = 0.0; + private boolean mUseMotionProfileForPosition = false; + + private ControlMode mLastControlMode = null; + + public class SpartronicsSRXEncoder implements SpartronicsEncoder + { + + @Override + public double getVelocity() + { + return mSensorModel.toCustomUnits(mTalonSRX.getSelectedSensorVelocity()) + * kMetersPer100msToMetersPerSecond; + } + + @Override + public double getPosition() + { + return mSensorModel.toCustomUnits(mTalonSRX.getSelectedSensorPosition()); + } + + @Override + public void setPhase(boolean isReversed) + { + mTalonSRX.setSensorPhase(isReversed); + } + + @Override + public boolean setPosition(double position) + { + mTalonSRX.getSensorCollection() + .setQuadraturePosition((int) mSensorModel.toNativeUnits(position), 0); + return true; + } + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel, + FeedbackDevice feedbackDevice) + { + if (RobotBase.isSimulation()) + { + return new SpartronicsSimulatedMotor(deviceNumber); + } + return new SpartronicsSRX(new TalonSRX(deviceNumber), sensorModel, feedbackDevice, null); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel) + { + return makeMotor(deviceNumber, sensorModel, FeedbackDevice.QuadEncoder); + } + + public static SpartronicsMotor makeMotor(int deviceNumber) + { + return makeMotor(deviceNumber, SensorModel.fromMultiplier(1)); + } + + public static SpartronicsMotor makeMotor(int deviceNumber, SensorModel sensorModel, + int followerDeviceNumber) + { + if (RobotBase.isSimulation()) + { + return new SpartronicsSimulatedMotor(deviceNumber, followerDeviceNumber); + } + var master = new TalonSRX(deviceNumber); + var follower = new TalonSRX(followerDeviceNumber); + follower.follow(master); + + return new SpartronicsSRX(master, sensorModel, FeedbackDevice.QuadEncoder, follower); + } + + private SpartronicsSRX(TalonSRX talon, SensorModel sensorModel, FeedbackDevice encoder, + TalonSRX follower) + { + mTalonSRX = talon; + mFollower = follower; + mSensorModel = sensorModel; + + ErrorCode err = mTalonSRX.configSelectedFeedbackSensor(encoder, 0, 5); + if (err != ErrorCode.OK) + { + Logger.error("TalonSRX on with ID " + mTalonSRX.getDeviceID() + + " returned a non-OK error code on sensor configuration... Is the encoder plugged in?"); + mHadStartupError = true; + } + else + { + mHadStartupError = false; + } + CANCounter.addDevice(mHadStartupError); + + mEncoder = new SpartronicsSRXEncoder(); + + mTalonSRX.configFactoryDefault(); + mTalonSRX.configVoltageCompSaturation(mVoltageCompSaturation); + mTalonSRX.enableVoltageCompensation(true); + } + + @Override + public SpartronicsEncoder getEncoder() + { + return mEncoder; + } + + @Override + public boolean hadStartupError() + { + return false;//mHadStartupError; Change back when comp season is over! + } + + @Override + public double getVoltageOutput() + { + return mTalonSRX.getMotorOutputVoltage(); + } + + @Override + public boolean getOutputInverted() + { + return mTalonSRX.getInverted(); + } + + @Override + public void setOutputInverted(boolean inverted) + { + mTalonSRX.setInverted(inverted); + } + + @Override + public boolean getBrakeMode() + { + return mBrakeMode; + } + + @Override + public void setBrakeMode(boolean mode) + { + mBrakeMode = mode; + mTalonSRX.setNeutralMode(mode ? NeutralMode.Brake : NeutralMode.Coast); + } + + @Override + public double getVoltageCompSaturation() + { + return mVoltageCompSaturation; + } + + @Override + public void setVoltageCompSaturation(double voltage) + { + mVoltageCompSaturation = voltage; + mTalonSRX.configVoltageCompSaturation(mVoltageCompSaturation); + mTalonSRX.enableVoltageCompensation(true); + } + + @Override + public double getMotionProfileCruiseVelocity() + { + return mSensorModel.toCustomUnits(mMotionProfileCruiseVelocity) + * kMetersPer100msToMetersPerSecond; + } + + @Override + public void setMotionProfileCruiseVelocity(double velocityMetersPerSecond) + { + mMotionProfileCruiseVelocity = mSensorModel + .toNativeUnits(velocityMetersPerSecond * kMetersPerSecondToMetersPer100ms); + mTalonSRX.configMotionCruiseVelocity((int) mMotionProfileCruiseVelocity); + } + + @Override + public double getMotionProfileMaxAcceleration() + { + return mSensorModel.toCustomUnits(mMotionProfileAcceleration) + * kMetersPer100msToMetersPerSecond; + } + + @Override + public void setMotionProfileMaxAcceleration(double accelerationMetersPerSecondSq) + { + mMotionProfileAcceleration = mSensorModel + .toNativeUnits(accelerationMetersPerSecondSq * kMetersPerSecondToMetersPer100ms); + mTalonSRX.configMotionAcceleration((int) mMotionProfileAcceleration); + } + + @Override + public void setUseMotionProfileForPosition(boolean useMotionProfile) + { + mUseMotionProfileForPosition = useMotionProfile; + } + + @Override + public void setPercentOutput(double dutyCycle, double arbitraryFeedForwardVolts) + { + mLastControlMode = ControlMode.PercentOutput; + mTalonSRX.set(ControlMode.PercentOutput, dutyCycle, DemandType.ArbitraryFeedForward, + arbitraryFeedForwardVolts / mVoltageCompSaturation); + } + + @Override + public void setPercentOutput(double dutyCycle) + { + setPercentOutput(dutyCycle, 0.0); + } + + @Override + public void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts) + { + if (mLastControlMode != ControlMode.Velocity) + { + mTalonSRX.selectProfileSlot(kVelocitySlotIdx, 0); + mLastControlMode = ControlMode.Velocity; + } + + double velocityNative = mSensorModel + .toNativeUnits(velocityMetersPerSecond * kMetersPerSecondToMetersPer100ms); + mTalonSRX.set(ControlMode.Velocity, velocityNative, DemandType.ArbitraryFeedForward, + arbitraryFeedForwardVolts / mVoltageCompSaturation); + } + + @Override + public void setVelocityGains(double kP, double kD) + { + setVelocityGains(kP, 0, kD, 0); + } + + @Override + public void setVelocityGains(double kP, double kI, double kD, double kF) + { + mTalonSRX.config_kP(kVelocitySlotIdx, kP); + mTalonSRX.config_kI(kVelocitySlotIdx, kI); + mTalonSRX.config_kD(kVelocitySlotIdx, kD); + mTalonSRX.config_kF(kVelocitySlotIdx, kF); + } + + @Override + public void setPosition(double positionMeters) + { + if (mLastControlMode != ControlMode.Position) + { + mTalonSRX.selectProfileSlot(kPositionSlotIdx, 0); + mLastControlMode = ControlMode.Position; + } + + double positionNative = mSensorModel.toNativeUnits(positionMeters); + mTalonSRX.set(mUseMotionProfileForPosition ? ControlMode.MotionMagic : ControlMode.Position, + positionNative); + } + + @Override + public void setPositionGains(double kP, double kD) + { + setPositionGains(kP, 0, kD, 0); + } + + @Override + public void setPositionGains(double kP, double kI, double kD, double kF) + { + mTalonSRX.config_kP(kPositionSlotIdx, kP); + mTalonSRX.config_kI(kPositionSlotIdx, kI); + mTalonSRX.config_kD(kPositionSlotIdx, kD); + mTalonSRX.config_kF(kPositionSlotIdx, kF); + } + + @Override + public SensorModel getSensorModel() + { + return mSensorModel; + } + + @Override + public void setVelocity(double velocityMetersPerSecond) + { + setVelocity(velocityMetersPerSecond, 0.0); + } + + @Override + public void setNeutral() + { + mTalonSRX.set(ControlMode.Disabled, 0.0, DemandType.Neutral, 0.0); + } + + @Override + public double getOutputCurrent() + { + return mTalonSRX.getStatorCurrent(); + } + + @Override + public SpartronicsMotor getFollower() + { + return new SpartronicsSRX(mFollower, mSensorModel, FeedbackDevice.None, null); + } + + @Override + public int getDeviceNumber() + { + return mTalonSRX.getDeviceID(); + } + + @Override + public void setSoftLimits(double forwardLimitCustomUnits, double reverseLimitCustomUnits) + { + mTalonSRX.configForwardSoftLimitEnable(true); + mTalonSRX.configReverseSoftLimitEnable(true); + + mTalonSRX.configForwardSoftLimitThreshold( + (int) Math.round(mSensorModel.toNativeUnits(forwardLimitCustomUnits))); + mTalonSRX.configReverseSoftLimitThreshold( + (int) Math.round(mSensorModel.toNativeUnits(reverseLimitCustomUnits))); + } + + @Override + public void setSupplyCurrentLimit(int limitAmps, double maxTimeAtLimit) + { + mTalonSRX.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, limitAmps, limitAmps, maxTimeAtLimit)); + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSimulatedMotor.java b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSimulatedMotor.java new file mode 100644 index 0000000..94418be --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/motors/SpartronicsSimulatedMotor.java @@ -0,0 +1,270 @@ +package com.spartronics4915.lib.hardware.motors; + +import java.util.HashMap; + +/** + * This class provides a simulated, easy-to-inspect, implementor of SpartronicsMotor. + */ +// TODO: Keep track of motor state +public class SpartronicsSimulatedMotor implements SpartronicsMotor +{ + private static HashMap sMotors = new HashMap<>(); + + public static SpartronicsSimulatedMotor getFromId(int deviceId) + { + var dev = sMotors.get(deviceId); + if (dev == null) + { + throw new RuntimeException("Simulated motor with ID " + deviceId + " not found"); + } + return dev; + } + + public static void resetGlobalState() + { + sMotors = new HashMap<>(); + } + + private double mMotionProfileMaxAcceleration = 0; + private boolean mOutputInverted = false; + private boolean mBrakeMode = false; + private double mVoltageCompSaturation = 0; + private double mMotionProfileCruiseVelocity = 0; + private double mOutputCurrent = 0; + + private final SpartronicsSimulatedMotor mFollower; + private final int mDeviceNumber; + + @Override + public SpartronicsEncoder getEncoder() + { + return new SpartronicsEncoder() + { + private double mPosition = 0; + + @Override + public void setPhase(boolean isReversed) + { + } + + @Override + public double getVelocity() + { + return 0; + } + + @Override + public double getPosition() + { + return mPosition; + } + + @Override + public boolean setPosition(double position) + { + mPosition = position; + return true; + } + }; + } + + public SpartronicsSimulatedMotor(int deviceNumber) + { + this(deviceNumber, -1); + } + + public SpartronicsSimulatedMotor(int deviceNumber, int followerDeviceNumber) + { + if (sMotors.get(deviceNumber) != null) + { + throw new RuntimeException( + "Can't instantiate a duplicate motor with device ID " + deviceNumber); + } + sMotors.put(deviceNumber, this); + + mDeviceNumber = deviceNumber; + + if (followerDeviceNumber != -1) + { + mFollower = new SpartronicsSimulatedMotor(followerDeviceNumber); + } + else + { + mFollower = null; + } + } + + @Override + public SensorModel getSensorModel() + { + return SensorModel.fromMultiplier(1); + } + + @Override + public boolean hadStartupError() + { + return false; + } + + @Override + public double getVoltageOutput() + { + return 0; + } + + @Override + public boolean getOutputInverted() + { + return mOutputInverted; + } + + @Override + public void setOutputInverted(boolean inverted) + { + mOutputInverted = inverted; + } + + @Override + public boolean getBrakeMode() + { + return mBrakeMode; + } + + @Override + public void setBrakeMode(boolean mode) + { + mBrakeMode = mode; + } + + @Override + public double getVoltageCompSaturation() + { + return mVoltageCompSaturation; + } + + @Override + public void setVoltageCompSaturation(double voltage) + { + mVoltageCompSaturation = voltage; + } + + @Override + public double getMotionProfileCruiseVelocity() + { + return mMotionProfileCruiseVelocity; + } + + @Override + public void setMotionProfileCruiseVelocity(double velocityMetersPerSecond) + { + mMotionProfileCruiseVelocity = velocityMetersPerSecond; + } + + @Override + public double getMotionProfileMaxAcceleration() + { + return mMotionProfileMaxAcceleration; + } + + @Override + public void setMotionProfileMaxAcceleration(double accelerationMetersSecSq) + { + mMotionProfileMaxAcceleration = accelerationMetersSecSq; + } + + @Override + public void setUseMotionProfileForPosition(boolean useMotionProfile) + { + + } + + @Override + public void setPercentOutput(double dutyCycle, double arbitraryFeedForwardVolts) + { + + } + + @Override + public void setPercentOutput(double dutyCycle) + { + + } + + @Override + public void setVelocity(double velocityMetersPerSecond) + { + + } + + @Override + public void setVelocity(double velocityMetersPerSecond, double arbitraryFeedForwardVolts) + { + + } + + @Override + public void setVelocityGains(double kP, double kD) + { + + } + + @Override + public void setVelocityGains(double kP, double kI, double kD, double kF) + { + + } + + @Override + public void setPosition(double positionMeters) + { + + } + + @Override + public void setPositionGains(double kP, double kD) + { + + } + + @Override + public void setPositionGains(double kP, double kI, double kD, double kF) + { + + } + + @Override + public void setNeutral() + { + + } + + @Override + public double getOutputCurrent() + { + return mOutputCurrent; + } + + public void setOutputCurrent(double current) + { + mOutputCurrent = current; + } + + @Override + public SpartronicsMotor getFollower() + { + return mFollower; + } + + @Override + public int getDeviceNumber() + { + return mDeviceNumber; + } + + @Override + public void setSoftLimits(double forwardLimitCustomUnits, double reverseLimitCustomUnits) + { + + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java new file mode 100644 index 0000000..9b23fe3 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/A51IRSensor.java @@ -0,0 +1,23 @@ +package com.spartronics4915.lib.hardware.sensors; + +public class A51IRSensor extends IRSensor +{ + public A51IRSensor(int port) + { + super(port); + } + + // How did we get here? We measured 9 seperate centimeter values and got the + // voltage. + // We then used a cubic polynomial regression to find the equation below. + // Range: 10 cm - 80 cm + + @Override + public double getDistance() + { + double volt = getVoltage(); + double cm = 56.2958 - (79.12745 * volt) + (43.04363 * Math.pow(volt, 2.0)) + - (7.753581 * Math.pow(volt, 3.0)); + return cm / 2.54; + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java new file mode 100644 index 0000000..d531651 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/IRSensor.java @@ -0,0 +1,41 @@ +package com.spartronics4915.lib.hardware.sensors; + +import edu.wpi.first.wpilibj.AnalogInput; + +public abstract class IRSensor +{ + AnalogInput mAnalogInput; + + public IRSensor(int port) + { + mAnalogInput = new AnalogInput(port); + mAnalogInput.setAverageBits(12); + } + + public double getVoltage() + { + double v = mAnalogInput.getAverageVoltage(); + if (v < .001) + v = .001; + return v; + } + + public abstract double getDistance(); // inches + + public boolean isTargetInVoltageRange(double min, double max) + { + double v = getVoltage(); + return v > min && v < max; + } + + /** + * @param minDist in inches + * @param maxDist in inches + * @return is within the distance + */ + public boolean isTargetInDistanceRange(double minDist, double maxDist) + { + double d = getDistance(); + return d > minDist && d < maxDist; + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java new file mode 100644 index 0000000..d22de20 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/RPLidarA1.java @@ -0,0 +1,622 @@ +package com.spartronics4915.lib.hardware.sensors; + +import java.io.IOException; +import java.io.InputStream; +import java.io.OutputStream; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Formatter; +import java.util.List; +import java.util.Optional; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.function.Consumer; + +import com.fazecast.jSerialComm.SerialPort; +import com.spartronics4915.lib.subsystems.estimator.RobotStateMap; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * This is a single-file Java driver for the RPLidarA1. This driver supports + * starting and stopping scans, getting individual points in a stream, getting + * device-relative pointclouds, and getting robot-relative pointclouds. + * + * This driver may work with the A2, but it does not use in express scan mode, + * which means that you cannot use the A2 at its highest rated speed. + * + * @author Declan Freeman-Gleason + */ +public class RPLidarA1 +{ + private static enum OutgoingPacket + { + RESET(0x20), STOP(0x5A), SCAN(0x20), GET_INFO(0x50), GET_HEALTH(0xF0); + + public final byte header; + + private OutgoingPacket(int header) + { + this.header = (byte) header; + } + } + + private static enum IncomingPacket + { + INFO(0x04, 20), HEALTH(0x06, 3), SCAN(0x81, 5); + + public final byte header; + public final int size; + + private IncomingPacket(int header, int size) + { + this.header = (byte) header; + this.size = size; + } + } + + public static class DeviceHealth + { + + public static enum HealthStatus + { + GOOD, WARNING, ERROR + } + + public final HealthStatus status; + public final int errorCode; + + public DeviceHealth(int status, int errorCode) + { + this.status = HealthStatus.values()[status]; // With throw if out of range + this.errorCode = errorCode; + } + + @Override + public String toString() + { + return status + (status != HealthStatus.GOOD ? " (Error code " + errorCode + ")" : ""); + } + } + + public static class DeviceInfo + { + public final int model; + public final int firmwareMinor; + public final int firmwareMajor; + public final int hardwareRevision; + public final byte[] serialNumber; + + public DeviceInfo(int model, int firmwareMinor, int firmwareMajor, int hardwareRevision, + byte[] serialNumber) + { + this.model = model; + this.firmwareMinor = firmwareMinor; + this.firmwareMajor = firmwareMajor; + this.hardwareRevision = hardwareRevision; + this.serialNumber = serialNumber; + } + + @Override + public String toString() + { + try (var formatter = new Formatter()) + { + for (byte b : serialNumber) + { + formatter.format("%02x", b); + } + return "RPLidar model " + model + ", firmware version " + firmwareMajor + "." + + firmwareMinor + ", hardware revision " + hardwareRevision + ", serial number " + + formatter.toString(); + } + } + } + + /* + * This is lowest level user-facing representation of what we get back from the + * sensor. + */ + public static class Measurement + { + /** Is this the start of a new scan */ + public final boolean start; + /** Scan quality from 0 (lowest) to 255 (highest) */ + public final int quality; + /** Angle in degrees */ + public final Rotation2d angle; + /** Distance in meters */ + public final double distance; + /** Timer.getFPGATimestamp() when the measurement arrived */ + public final double timestamp; + + public Measurement(boolean start, int quality, double angleDegrees, double distanceMeters, + double timestampSeconds) + { + this.start = start; + this.quality = quality; + this.angle = Rotation2d.fromDegrees(angleDegrees); + this.distance = distanceMeters; + this.timestamp = timestampSeconds; + } + + public Translation2d getAsPoint() + { + return new Translation2d(angle.getCos() * distance, angle.getSin() * distance); + } + + public boolean isInvalid() + { + return distance == 0; + } + + public String toString() + { + return "Starting: " + start + ", Quality: " + quality + ", Angle: " + angle + + ", Distance: " + distance + ", Timestamp: " + timestamp; + } + } + + // We have to make these aliases because Java type erasure + public static interface MeasurementConsumer extends Consumer + { + } + + public static interface PointConsumer extends Consumer + { + } + + public static interface PointcloudConsumer extends Consumer> + { + } + + private static final String kPortDescription = "CP2102 USB to UART Bridge Controller"; + private static final byte kSyncByteZero = (byte) 0xA5; + private static final byte kSyncByteOne = (byte) 0x5A; + /** Seconds */ + private static final double kSendBlockingRetryDelay = 0.02; + /** Seconds */ + private static final double kReadThreadDelay = 0.02; + /** Seconds */ + private static final double kSendTimeout = 2; + + private Consumer mMeasurementConsumer; + private final SerialPort mSerialPort; + private InputStream mInStream; + private OutputStream mOutStream; + + // We keep a large buffer as a member because the entire packet isn't always + // available + private byte[] mReadBuffer = new byte[2048]; + private int mEndOfDataIndex = 0; + + private Object mLastRecievedHeaderLock = new Object(); + private int mLastRecievedHeader = 0; + + private Optional mLastDeviceHealth = Optional.empty(); + private Optional mLastDeviceInfo = Optional.empty(); + + // There are two modes: + // 1. Request/response mode, where a each request has a response. + // 2. Many response mode, where one request has many responses. + // + // The first mode is used for all request types except the scan request. The + // scan request puts us into the second mode and we can only get out by sending + // the stop request. + private AtomicBoolean mInScanMode = new AtomicBoolean(false); + private AtomicBoolean mIsStarted = new AtomicBoolean(false); + + // This list will be empty unless the user uses one of the pointcloud ctors + private List mCurrentPointcloud = new ArrayList<>(); + + public RPLidarA1() + { + mMeasurementConsumer = (m) -> {}; + mSerialPort = Arrays.stream(SerialPort.getCommPorts()) + .filter( + (SerialPort p) -> p.getPortDescription().equals(kPortDescription) && !p.isOpen()) + .findFirst().orElseThrow(() -> new RuntimeException("No RPLidar device found")); + + mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY); + mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED); + mSerialPort.openPort(); + + mInStream = mSerialPort.getInputStream(); + mOutStream = mSerialPort.getOutputStream(); + + new Thread(this::readData).start(); + + stop(); + } + + /** + * @param measurementConsumer A callback to recieve measurements. Note that this + * will be called from an independent read thread. + */ + public void setCallback(MeasurementConsumer measurementConsumer) + { + synchronized (mMeasurementConsumer) + { + mMeasurementConsumer = measurementConsumer; + } + } + + /** + * @param pointConsumer A callback that accepts single sensor-relative points, + * in meters. Note that this will be called from an + * independant read thread. + */ + public void setCallback(PointConsumer pointConsumer) + { + setCallback((Measurement m) -> { pointConsumer.accept(m.getAsPoint()); }); + } + + /** + * @param pointcloudConsumer A callback that accepts a field-relative + * pointcloud, with all coordinates in meters. + * @param robotStateMap A robot state map to transform the pointcloud with. + * @param vehicleToLidar Transformation from the vehicle to the lidar sensor + * (i.e. the sensor's offset from the vehicle's + * center). + */ + public void setCallback(PointcloudConsumer pointcloudConsumer, RobotStateMap robotStateMap, + Transform2d vehicleToLidar) + { + setCallback((Measurement m) -> { + if (m.start && mCurrentPointcloud.size() > 0) + { + pointcloudConsumer.accept(new ArrayList<>(mCurrentPointcloud)); + mCurrentPointcloud.clear(); + } + Translation2d point = m.getAsPoint(); + point = robotStateMap.getFieldToVehicle(m.timestamp).transformBy(vehicleToLidar) + .transformBy(new Transform2d(point, new Rotation2d())).getTranslation(); + mCurrentPointcloud.add(point); + }); + } + + /** + * @param pointcloudConsumer A callback that accepts a sensor-relative + * pointcloud, with all coordinates in meters. This is + * susceptible to motion smear if you move during a + * scan. + */ + public void setCallback(PointcloudConsumer pointcloudConsumer) + { + setCallback(pointcloudConsumer, new RobotStateMap(), new Transform2d()); + } + + public void start() + { + mInScanMode.set(false); + mIsStarted.set(true); + + mSerialPort.clearDTR(); + sendData(OutgoingPacket.SCAN, IncomingPacket.SCAN, kSendTimeout); + } + + public void stop() + { + mInScanMode.set(false); + mIsStarted.set(false); + + sendData(OutgoingPacket.RESET); + Timer.delay(0.002); // Docs say to sleep 2ms after a reset + sendData(OutgoingPacket.STOP); + Timer.delay(0.02); // This is also advised by the docs + mSerialPort.setDTR(); + } + + public Optional getHealth() + { + sendData(OutgoingPacket.GET_HEALTH, IncomingPacket.HEALTH, kSendTimeout); + synchronized (mLastDeviceHealth) + { + return mLastDeviceHealth; + } + } + + public Optional getInfo() + { + sendData(OutgoingPacket.GET_INFO, IncomingPacket.INFO, kSendTimeout); + synchronized (mLastDeviceInfo) + { + return mLastDeviceInfo; + } + } + + private void readData() + { + try + { + // Should we allow users to stop the read thread? + while (true) + { + if (mInStream.available() > 0 && mIsStarted.get()) + { + // Note that there is a chance packets get truncated if we exceed the read + // buffer. + // This shouldn't happen in practice though because messages aren't greater than + // 84 bytes. + int totalRead = mInStream.read(mReadBuffer, mEndOfDataIndex, + mReadBuffer.length - mEndOfDataIndex); + mEndOfDataIndex += totalRead; + + // Here we parse the message and shift everything over such that the bytes we + // just read are cleared from the buffer. + int totalUsed = parseData(); + for (int i = 0; i < mEndOfDataIndex - totalUsed; i++) + { + mReadBuffer[i] = mReadBuffer[i + totalUsed]; + } + mEndOfDataIndex -= totalUsed; + } + else + { + Timer.delay(kReadThreadDelay); + } + } + } + catch (IOException e) + { + throw new RuntimeException(e); + } + } + + /** + * @return The number of bytes that were parsed and used + */ + private int parseData() + { + int offset = 0; + while (true) + { + if (mInScanMode.get()) + { + // There are no more complete scan packets to parse so we just return + if (offset + IncomingPacket.SCAN.size > mEndOfDataIndex) + { + return offset; + } + + if (parseScan(offset, IncomingPacket.SCAN.size)) + { + offset += IncomingPacket.SCAN.size; + } + else + { + // Bad packet + System.err.println("Got a bad scan packet"); + offset += 1; + } + } + else + { + // Check if we have consumed enough bytes to get to the response length field + // (which is 6 bytes in: 2 bytes for the start flags and 4 bytes for the length + // field itself) + if (offset + 6 > mEndOfDataIndex) + { + return offset; + } + + if (mReadBuffer[offset] == kSyncByteZero && mReadBuffer[offset + 1] == kSyncByteOne) + { + // Packet length is a 30-bit unsigned integer. The 2 least significant bytes + // after those 30 bytes give the current send mode. + + // First convert the bytes to an integer + int packetLength = ByteBuffer + .wrap(Arrays.copyOfRange(mReadBuffer, offset + 2, offset + 6)) + .order(ByteOrder.LITTLE_ENDIAN).getInt(); + // Then mask off the first two (least significant) bits + packetLength &= 0x3FFFFFFF; + + // Get the packet header + byte header = mReadBuffer[offset + 6]; + + // 7 is the first two sync bytes, plus the 4 bytes for packet length/send mode, + // plus 1 byte for the data type (header) of the packet. + // Adding this to offset gives us the starting address of the actual packet + // data. + int packetDataOffset = 7; + + if (offset + packetDataOffset + packetLength > mEndOfDataIndex) + { + return offset; + } + + if (parsePacket(offset + packetDataOffset, packetLength, header)) + { + synchronized (mLastRecievedHeaderLock) + { + // AND wiht 0xFF is to convert an unsigned to signed integer + mLastRecievedHeader = header & 0xFF; + offset += packetDataOffset + packetLength; + } + } + else + { + // The packet was bad; this sometimes happens on startup + offset += 2; + } + } + else + { + // We should only get here on startup; getting to this point in other situations + // means that we probably got a malformed packet, or that we parsed a packet + // wrong which screwed up our offset + offset++; + } + } + } + } + + private boolean parsePacket(int offset, int length, byte header) + { + // Can't do a switch because these aren't constant expressions + if (header == IncomingPacket.INFO.header) + { + return parseDeviceInfo(offset, length); + } + else if (header == IncomingPacket.HEALTH.header) + { + return parseDeviceHealth(offset, length); + } + else if (header == IncomingPacket.SCAN.header) + { + // We only get here when we recieve the initial scan packet + if (parseScan(offset, length)) + { + mInScanMode.set(true); + return true; + } + } + return false; + } + + private boolean parseScan(int offset, int length) + { + if (length != IncomingPacket.SCAN.size) + { + return false; + } + + // The least significant bit of byte zero indicates if this is thes start of a + // new scan. + // The second to least significant bit should be the oppositive of the least + // significant bit. + // The remaining 6 bits are an unsigned integer that represents the quality of + // the point. + byte byteZero = mReadBuffer[offset]; + // The least significant bit of byte one should always be 1; it is a check bit. + // The remaining 6 bits are part of a fixed-point number representing the angle + // of the sensor at this point. + byte byteOne = mReadBuffer[offset + 1]; + + boolean isStart = (byteZero & 0x01) == 1; + boolean notIsStart = (byteZero & 0x02) >> 1 == 1; + + if (isStart == notIsStart) + { + return false; + } + + if ((byteOne & 0x01) != 1) + { + // This is probably not a scan packet, which means we have the wrong offset + // That only really happens on startup + return false; + } + + double timestamp = Timer.getFPGATimestamp(); + int quality = (byteZero & 0xFF) >> 2; // Convert to signed int and get rid of the last two + // bits + int angle = ((byteOne & 0xFF) | ((mReadBuffer[offset + 2] & 0xFF) << 8)) >> 1; // Convert to + // signed int + // and + // extract + // from + // multiple + // bytes + int distance = ((mReadBuffer[offset + 3] & 0xFF) | ((mReadBuffer[offset + 4] & 0xFF) << 8)); // Same + // as + // above + + // We negate because the angle they provide is counter clockwise positive + double angleDegrees = -1 * angle / 64d; + double distanceMeters = (distance / 4d) / 1000d; + + var m = new Measurement(isStart, quality, angleDegrees, distanceMeters, timestamp); + synchronized (mMeasurementConsumer) + { + mMeasurementConsumer.accept(m); + } + + return true; + } + + private boolean parseDeviceHealth(int offset, int length) + { + if (length != IncomingPacket.HEALTH.size) + { + System.err.println("Bad health packet"); + return false; + } + + // AND by 0xFF is to convert unsigned integers to signed integers + // The bit shift and OR is to convert the little endian bytes to integers (this + // can be dne with a ByteBuffer, but it's slower and much less concise) + DeviceHealth h = new DeviceHealth(mReadBuffer[offset] & 0xFF, + (mReadBuffer[offset + 1] & 0xFF) | ((mReadBuffer[offset + 2] & 0xFF) << 8)); + synchronized (mLastDeviceHealth) + { + mLastDeviceHealth = Optional.of(h); + } + return true; + } + + private boolean parseDeviceInfo(int offset, int length) + { + if (length != IncomingPacket.INFO.size) + { + System.err.println("Bad device info packet"); + return false; + } + + byte[] serialNumber = new byte[16]; + for (int i = 0; i < serialNumber.length; i++) + { + serialNumber[i] = mReadBuffer[offset + i + 4]; + } + + synchronized (mLastDeviceInfo) + { + // AND by 0xFF to convert to signed integer from unsigned + mLastDeviceInfo = Optional + .of(new DeviceInfo(mReadBuffer[offset] & 0xFF, mReadBuffer[offset + 1] & 0xFF, + mReadBuffer[offset + 2] & 0xFF, mReadBuffer[offset + 3] & 0xFF, serialNumber)); + } + return true; + } + + /** + * Sends data and blocks until the expected packet is recieved, or the timeout + * is hit. + */ + private void sendData(OutgoingPacket packet, IncomingPacket expected, double timeoutSeconds) + { + sendData(packet); + + synchronized (mLastRecievedHeaderLock) + { + double endTime = Timer.getFPGATimestamp() + timeoutSeconds; + while (endTime >= Timer.getFPGATimestamp() && mLastRecievedHeader != expected.header) + { + Timer.delay(kSendBlockingRetryDelay); + } + } + } + + /** + * Sends a packet without blocking. + */ + private void sendData(OutgoingPacket packet) + { + try + { + mOutStream.write(new byte[] {kSyncByteZero, packet.header}); + mOutStream.flush(); + } + catch (IOException e) + { + throw new RuntimeException(e); + } + } + +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java new file mode 100644 index 0000000..1fece3f --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsIMU.java @@ -0,0 +1,8 @@ +package com.spartronics4915.lib.hardware.sensors; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface SpartronicsIMU +{ + Rotation2d getYaw(); +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsPigeon.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsPigeon.java new file mode 100644 index 0000000..eb92e8f --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsPigeon.java @@ -0,0 +1,28 @@ +package com.spartronics4915.lib.hardware.sensors; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.sensors.PigeonIMU; +import com.spartronics4915.lib.hardware.CANCounter; +import edu.wpi.first.math.geometry.Rotation2d; + +public class SpartronicsPigeon implements SpartronicsIMU +{ + PigeonIMU mIMU; + + public SpartronicsPigeon(int canID) + { + PigeonIMU imu = new PigeonIMU(canID); + ErrorCode err = imu.configFactoryDefault(); + + boolean hadError = err != ErrorCode.OK; + CANCounter.addDevice(hadError); + + mIMU = imu; + } + + @Override + public Rotation2d getYaw() + { + return Rotation2d.fromDegrees(mIMU.getFusedHeading()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsXRS450.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsXRS450.java new file mode 100644 index 0000000..90c6504 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/SpartronicsXRS450.java @@ -0,0 +1,16 @@ +package com.spartronics4915.lib.hardware.sensors; + +import edu.wpi.first.math.geometry.Rotation2d; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; + +public class SpartronicsXRS450 implements SpartronicsIMU +{ + private ADXRS450_Gyro mGyro = new ADXRS450_Gyro(); + + @Override + public Rotation2d getYaw() + { + return Rotation2d.fromDegrees(-mGyro.getAngle()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java b/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java new file mode 100644 index 0000000..446ed7b --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/hardware/sensors/T265Camera.java @@ -0,0 +1,245 @@ +package com.spartronics4915.lib.hardware.sensors; + +import java.nio.file.Paths; + +import java.util.function.Consumer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Provides a convenient Java interface to the Intel RealSense + * T265 V-SLAM camera. Only the subset of the librealsense that is useful + * to robot tracking is exposed in this class. + *

+ * We employ JNI to call librealsense. There are Java bindings for + * librealsense, but they are not complete and do not support our usecase. + *

+ * This class works entirely in 2d, even though the tracking camera supports + * giving us a third dimension (Z). + *

+ * The coordinate system is as follows: + * + X == Camera forwards + * + Y == Camera Left (left is from the perspective of a viewer standing behind the camera) + *

+ * All distance units are meters. All time units are seconds. + */ +public class T265Camera { + private static UnsatisfiedLinkError mLinkError = null; + + static { + try { + System.load(Paths.get(System.getProperty("user.home"), "libspartronicsnative.so").toAbsolutePath().toString()); + + // Cleanup is quite tricky for us, because the native code has no idea when Java + // will be done. (This is why smart pointers don't really make sense in the native code.) + // Even worse, trying to cleanup with atexit in the native code is too late and + // results in unfinished callbacks blocking. As a result a shutdown hook is our + // best option. + Runtime.getRuntime().addShutdownHook(new Thread(() -> T265Camera.cleanup())); + } catch (UnsatisfiedLinkError e) { + mLinkError = e; + } + } + + public static enum PoseConfidence { + Failed, Low, Medium, High, + } + + public static class CameraUpdate { + /** + * The robot's pose in meters. + */ + public final Pose2d pose; + /** + * The robot's velocity in meters/sec and radians/sec. + */ + public final ChassisSpeeds velocity; + public final PoseConfidence confidence; + + public CameraUpdate(Pose2d pose, ChassisSpeeds velocity, PoseConfidence confidence) { + this.pose = pose; + this.velocity = velocity; + this.confidence = confidence; + } + } + + private long mNativeCameraObjectPointer = 0; + private boolean mIsStarted = false; + private Transform2d mRobotOffset; + private Pose2d mOrigin = new Pose2d(); + private Pose2d mLastRecievedPose = new Pose2d(); + private Consumer mPoseConsumer = null; + + /** + * This method constructs a T265 camera and sets it up with the right info. + * {@link T265Camera#start start} will not be called, you must call it + * yourself. + * + * @param robotOffset Offset of the center of the robot from the center + * of the camera. + * @param odometryCovariance Covariance of the odometry input when doing + * sensor fusion (you probably want to tune this). + */ + public T265Camera(Transform2d robotOffset, double odometryCovariance) { + this(robotOffset, odometryCovariance, ""); + } + + /** + * This method constructs a T265 camera and sets it up with the right info. + * {@link T265Camera#start start} will not be called, you must call it + * yourself. + * + * @param robotOffsetMeters Offset of the center of the robot from the center + * of the camera. Units are meters. + * @param odometryCovariance Covariance of the odometry input when doing + * sensor fusion (you probably want to tune this) + * @param relocMapPath path (including filename) to a relocalization map + * to load. + */ + public T265Camera(Transform2d robotOffsetMeters, double odometryCovariance, String relocMapPath) { + if (mLinkError != null) { + throw mLinkError; + } + + mNativeCameraObjectPointer = newCamera(relocMapPath); + setOdometryInfo((float) robotOffsetMeters.getTranslation().getX(), + (float) robotOffsetMeters.getTranslation().getY(), + (float) robotOffsetMeters.getRotation().getRadians(), odometryCovariance); + mRobotOffset = robotOffsetMeters; + } + + /** + * This allows the user-provided pose receive callback to receive data. + * This will also reset the camera's pose to (0, 0) at 0 degrees. + *

+ * This will not restart the camera following exportRelocalizationMap. You will + * have to call {@link T265Camera#free()} and make a new {@link T265Camera}. + * This is related to what appears to be a bug in librealsense. + * + * @param poseConsumer A method to be called every time we receive a pose from + * from a different thread! You must synchronize + * memory access across threads! + *

+ * Received poses are in meters. + */ + public synchronized void start(Consumer poseConsumer) { + if (mIsStarted) + throw new RuntimeException("T265 camera is already started"); + mPoseConsumer = poseConsumer; + mIsStarted = true; + } + + /** + * This allows the callback to receivez data, but it does not internally stop the + * camera. + */ + public synchronized void stop() { + mIsStarted = false; + } + + /** + * Exports a binary relocalization map file to the given path. + * This will stop the camera. Because of a librealsense bug the camera isn't + * restarted after you call this method. TODO: Fix that. + * + * @param path Path (with filename) to export to + */ + public native void exportRelocalizationMap(String path); + + /** + * Sends robot velocity as computed from wheel encoders. + * + * @param velocity The robot's translational velocity in meters/sec. + */ + public void sendOdometry(Twist2d velocity) { + Pose2d transVel = new Pose2d().exp(velocity); + // Only 1 odometry sensor is supported for now (index 0) + sendOdometryRaw(0, (float) transVel.getTranslation().getX(), + (float) transVel.getTranslation().getY()); + } + + /** + * This zeroes the camera pose to the provided new pose. + * + * @param newPose The pose the camera should be zeroed to. + */ + public synchronized void setPose(Pose2d newPose) { + mOrigin = newPose; + } + + /** + * This will free the underlying native objects. You probably don't want to use + * this; on program shutdown the native code will gracefully stop and delete any + * remaining objects. + */ + public native void free(); + + private native void setOdometryInfo(float robotOffsetX, float robotOffsetY, + float robotOffsetRads, double measurementCovariance); + + private native void sendOdometryRaw(int sensorIndex, float xVel, float yVel); + + private native long newCamera(String mapPath); + + private static native void cleanup(); + + private synchronized void consumePoseUpdate(float x, float y, float radians, float dx, float dy, + float dtheta, int confOrdinal) { + // First we apply an offset to go from the camera coordinate system to the + // robot coordinate system with an origin at the center of the robot. This + // is not a directional transformation. + // Then we transform the pose our camera is giving us so that it reports is + // the robot's pose, not the camera's. This is a directional transformation. + final Pose2d currentPose = new Pose2d(x - mRobotOffset.getTranslation().getX(), + y - mRobotOffset.getTranslation().getY(), new Rotation2d(radians)) + .transformBy(mRobotOffset); + + mLastRecievedPose = currentPose; + + if (!mIsStarted) + return; + + // See + // https://github.com/IntelRealSense/librealsense/blob/7f2ba0de8769620fd672f7b44101f0758e7e2fb3/include/librealsense2/h/rs_types.h#L115 + // for ordinals + PoseConfidence confidence; + switch (confOrdinal) { + case 0x0: + confidence = PoseConfidence.Failed; + break; + case 0x1: + confidence = PoseConfidence.Low; + break; + case 0x2: + confidence = PoseConfidence.Medium; + break; + case 0x3: + confidence = PoseConfidence.High; + break; + default: + throw new RuntimeException( + "Unknown confidence ordinal \"" + confOrdinal + "\" passed from native code"); + } + + final Pose2d transformedPose = mOrigin.transformBy(new Transform2d(currentPose.getTranslation(), currentPose.getRotation())); + + mPoseConsumer.accept(new CameraUpdate(transformedPose, + new ChassisSpeeds(dx, dy, dtheta), confidence)); + } + + /** + * Thrown if something goes wrong in the native code + */ + public static class CameraJNIException extends RuntimeException { + + // This must be static _and_ have this constructor if you want it to be + // thrown from native code + public CameraJNIException(String message) { + super(message); + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java b/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java new file mode 100644 index 0000000..8cc0614 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/SpartronicsSubsystem.java @@ -0,0 +1,153 @@ +package com.spartronics4915.lib.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.spartronics4915.lib.util.Logger; + + +/** + * We introduce an abstract SpartronicsSubsystem class to provide uniformity + * in logging, smartdashboard and initialization behavior to all subclasses. + * + * The Subsystem abstract class, which serves as a basic framework for all robot + * subsystems. Each subsystem outputs commands to SmartDashboard, has a stop + * routine (for after each match), and a routine to zero all sensors, which helps + * with calibration. + *

+ * All Subsystems only have one instance (after all, one robot does not have two + * drivetrains), and functions get the instance of the drivetrain and act + * accordingly. + */ +public abstract class SpartronicsSubsystem extends SubsystemBase +{ + // We install a default command for diagnostic purposes. Subclasses + // should override the default command during construction. + private class SpartronicsMissingDefaultCommand extends CommandBase + { + SpartronicsMissingDefaultCommand() + { + this.addRequirements(SpartronicsSubsystem.this); + } + }; + + // All subsystems should set mInitialized upon successful init. + private boolean mInitialized = false; + private String mName = null; + + public boolean isInitialized() + { + return mInitialized; + } + + protected SpartronicsSubsystem() + { + // NB: SubsystemBase posts some interesting info to dashboard + // using our class name. + String classname = this.getClass().getName(); + int tail = classname.lastIndexOf('.'); + if (tail == -1) + { + this.mName = classname; + } + else + { + this.mName = classname.substring(tail + 1); + } + this.setDefaultCommand(new SpartronicsMissingDefaultCommand()); + } + + public String getClassName() + { + return this.mName; + } + + public void logInitialized(boolean success) + { + this.mInitialized = success; + if (success) + this.logNotice("init SUCCEEDED"); + else + this.logWarning("init FAILED"); + SmartDashboard.putString(this.mName + "/Status", this.mInitialized ? "OK" : "ERROR"); + } + + public void dashboardPutString(String nm, String value) + { + SmartDashboard.putString(this.mName + "/" + nm, value); + } + + public String dashboardGetString(String nm, String defValue) + { + return SmartDashboard.getString(this.mName + "/" + nm, defValue); + } + + public void dashboardPutNumber(String nm, Number value) + { + SmartDashboard.putNumber(this.mName + "/" + nm, value.doubleValue()); + } + + public Number dashboardGetNumber(String nm, Number defaultValue) + { + return SmartDashboard.getNumber(this.mName + "/" + nm, defaultValue.doubleValue()); + } + + public void dashboardPutBoolean(String nm, Boolean value) + { + SmartDashboard.putBoolean(this.mName + "/" + nm, value); + } + + public boolean dashboardGetBoolean(String nm, Boolean defValue) + { + return SmartDashboard.getBoolean(this.mName + "/" + nm, defValue); + } + + // log methods are for conventionalizing format across subsystems + public void logException(String msg, Throwable e) + { + Logger.logThrowableCrash(this.getClassName() + " " + msg, e); + } + + public void logError(String msg) + { + Logger.error(this.getClassName() + " " + msg); + } + + public void logWarning(String msg) + { + Logger.warning(this.getClassName() + " " + msg); + } + + public void logNotice(String msg) + { + Logger.notice(this.getClassName() + " " + msg); + } + + public void logInfo(String msg) + { + Logger.info(this.getClassName() + " " + msg); + } + + public void logDebug(String msg) + { + Logger.debug(this.getClassName() + " " + msg); + } + + public void zeroSensors() + { + } + + @Override + public void periodic() + { + Command currentCommand = CommandScheduler.getInstance().requiring(this); + + // this.mName/currentCommand contains the current command for + // a subsystem. + dashboardPutString("currentCommand", + currentCommand != null ? currentCommand.getName() : "none"); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java new file mode 100644 index 0000000..24e3960 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/AbstractDrive.java @@ -0,0 +1,141 @@ +package com.spartronics4915.lib.subsystems.drive; + +import com.spartronics4915.lib.hardware.motors.SpartronicsMotor; +import com.spartronics4915.lib.hardware.motors.SpartronicsSimulatedMotor; +import com.spartronics4915.lib.hardware.sensors.SpartronicsIMU; +import edu.wpi.first.math.geometry.Rotation2d; +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; + +public abstract class AbstractDrive extends SpartronicsSubsystem +{ + protected final SpartronicsMotor mLeftMotor, mRightMotor; + protected final SpartronicsIMU mIMU; + + protected Rotation2d mIMUOffset = new Rotation2d(); + + /** + * This constructor will set up everything you need. It's protected to allow for + * a singleton drivetrain. + */ + protected AbstractDrive(SpartronicsMotor leftMotor, SpartronicsMotor rightMotor, + SpartronicsIMU imu) + { + if (leftMotor.hadStartupError() || rightMotor.hadStartupError()) + { + mLeftMotor = new SpartronicsSimulatedMotor(leftMotor.getDeviceNumber(), leftMotor.getFollower().getDeviceNumber()); + mRightMotor = new SpartronicsSimulatedMotor(rightMotor.getDeviceNumber(), rightMotor.getFollower().getDeviceNumber()); + mIMU = new SpartronicsIMU() + { + @Override + public Rotation2d getYaw() + { + return new Rotation2d(); + } + }; + + logInitialized(false); + } + else + { + mLeftMotor = leftMotor; + mRightMotor = rightMotor; + mIMU = imu; + + logInitialized(true); + } + } + + /** + * This sets the heading of the IMU, but this should almost exclusively be + * called by RobotStateEstimator, because that is the single source of truth for + * robot heading. + * + * @param heading Heading to set the IMU to. + */ + public void setIMUHeading(Rotation2d heading) + { + mIMUOffset = mIMU.getYaw().minus(heading); + } + + /** + * This gets the heading of the IMU, but this should almost exclusively be + * called by RobotStateMap, which is the single source of truth for all matters + * of robot pose (including heading). + * + * @return Heading of the IMU. + */ + public Rotation2d getIMUHeading() + { + return mIMU.getYaw().rotateBy(mIMUOffset); + } + + public double getTurretAngle() + { + return 0; + } + + public void arcadeDrive(double linearPercent, double rotationPercent) + { + double maxInput = Math.copySign(Math.max(Math.abs(linearPercent), + Math.abs(rotationPercent)), linearPercent); + + double leftMotorOutput, rightMotorOutput; + + if (linearPercent >= 0.0) + { + // First quadrant, else second quadrant + if (rotationPercent >= 0.0) + { + leftMotorOutput = maxInput; + rightMotorOutput = linearPercent - rotationPercent; + } + else + { + leftMotorOutput = linearPercent + rotationPercent; + rightMotorOutput = maxInput; + } + } + else + { + // Third quadrant, else fourth quadrant + if (rotationPercent >= 0.0) + { + leftMotorOutput = linearPercent + rotationPercent; + rightMotorOutput = maxInput; + } + else + { + leftMotorOutput = maxInput; + rightMotorOutput = linearPercent - rotationPercent; + } + } + + tankDrive(leftMotorOutput, rightMotorOutput); + } + + public void tankDrive(double leftPercent, double rightPercent) + { + mLeftMotor.setPercentOutput(leftPercent); + mRightMotor.setPercentOutput(rightPercent); + } + + public SpartronicsMotor getLeftMotor() + { + return mLeftMotor; + } + + public SpartronicsMotor getRightMotor() + { + return mRightMotor; + } + + @Override + public void periodic() + { + dashboardPutNumber("imuHeading", mIMU.getYaw().getDegrees()); + dashboardPutNumber("imuHeadingAdjusted", getIMUHeading().getDegrees()); + + dashboardPutNumber("leftSpeed", getLeftMotor().getEncoder().getVelocity()); + dashboardPutNumber("rightSpeed", getRightMotor().getEncoder().getVelocity()); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java b/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java new file mode 100644 index 0000000..10f8319 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/drive/CharacterizeDriveBaseCommand.java @@ -0,0 +1,104 @@ +package com.spartronics4915.lib.subsystems.drive; + +import java.util.Set; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class CharacterizeDriveBaseCommand implements Command +{ + private final AbstractDrive mDrive; + private final NetworkTableEntry mAutoSpeedEntry = NetworkTableInstance.getDefault() + .getEntry("/robot/autospeed"); + private final NetworkTableEntry mTelemetryEntry = NetworkTableInstance.getDefault() + .getEntry("/robot/telemetry"); + private final NetworkTableEntry rotateEntry = NetworkTableInstance.getDefault() + .getEntry("/robot/rotate"); + + /** Meters */ + private final double mWheelCircumference; + + private Number[] mOutputArray = new Number[10]; + private double mLeftInitialPosition = 0.0; + private double mRightInitialPosition = 0.0; + + /** + * This feeds this Python script: + * https://github.com/robotpy/robot-characterization + */ + public CharacterizeDriveBaseCommand(AbstractDrive drive, double wheelDiameterMeters) + { + NetworkTableInstance.getDefault().setUpdateRate(0.010); + + mDrive = drive; + mWheelCircumference = wheelDiameterMeters * Math.PI; + } + + @Override + public boolean isFinished() + { + // This must be manually stopped by the user via the Disable button + return false; + } + + @Override + public void execute() + { + double now = Timer.getFPGATimestamp(); + + // radians and radians/s + double leftPosition = (mDrive.getLeftMotor().getEncoder().getPosition() + - mLeftInitialPosition) / mWheelCircumference * (2 * Math.PI); + double leftVelocity = mDrive.getLeftMotor().getEncoder().getVelocity() / mWheelCircumference + * (2 * Math.PI); + + // radians and radians/s + double rightPosition = (mDrive.getRightMotor().getEncoder().getPosition() + - mRightInitialPosition) / mWheelCircumference * (2 * Math.PI); + double rightVelocity = mDrive.getLeftMotor().getEncoder().getVelocity() + / mWheelCircumference * (2 * Math.PI); + + // volts + double battery = RobotController.getBatteryVoltage(); + + // volts + double leftMotorVolts = mDrive.getLeftMotor().getVoltageOutput(); + double rightMotorVolts = mDrive.getRightMotor().getVoltageOutput(); + + // percent output on [-1 1] + double autospeed = mAutoSpeedEntry.getDouble(0.0); + + mDrive.tankDrive((rotateEntry.getBoolean(false) ? -1 : 1) * autospeed, autospeed); + + // send telemetry data array back to NT + mOutputArray[0] = now; + mOutputArray[1] = battery; + mOutputArray[2] = autospeed; + mOutputArray[3] = leftMotorVolts; + mOutputArray[4] = rightMotorVolts; + mOutputArray[5] = leftPosition; + mOutputArray[6] = rightPosition; + mOutputArray[7] = leftVelocity; + mOutputArray[8] = rightVelocity; + mOutputArray[9] = mDrive.getIMUHeading().getRadians(); + + mTelemetryEntry.setNumberArray(mOutputArray); + } + + @Override + public void initialize() + { + mLeftInitialPosition = mDrive.getLeftMotor().getEncoder().getPosition(); + mRightInitialPosition = mDrive.getRightMotor().getEncoder().getPosition(); + } + + @Override + public Set getRequirements() + { + return Set.of(mDrive); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/DrivetrainEstimator.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/DrivetrainEstimator.java new file mode 100644 index 0000000..612eb44 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/DrivetrainEstimator.java @@ -0,0 +1,210 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import java.util.Map; +import java.util.TreeMap; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import com.spartronics4915.lib.util.Logger; + +import edu.wpi.first.math.estimator.ExtendedKalmanFilter; +import edu.wpi.first.math.math.Discretization; +import edu.wpi.first.math.math.StateSpaceUtil; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N6; + +/** + * Wraps the Extended Kalman Filter I wrote and contributed to the prerelease version of WPILibStateSpace to fuse encoder odometry, VSLAM, and PnP to localize the robot. + * + * We also do latency compensation for the vision. + * + * Our state-space system is: + * + * x = [[x, y, dtheta]]^T in the field coordinate system + * + * u = [[v_l, v_r, theta]]^T Not actual inputs, but the dynamics math required + * to use actual inputs like voltage is gross. + * + * y = [[x_s, y_s, theta_s, x_r, y_r, theta_r]]^T All the subscript s ones are + * measurements from vSLAM, while the sub r ones are from retroreflective tape + * vision. + */ +public class DrivetrainEstimator +{ + private static final double kNominalDt = 0.01; + private static final int kMaxPastObserverStates = 200; + + private final ExtendedKalmanFilter mObserver; + private final ExtendedKalmanFilter mVisionObserver; + private final Matrix mMeasurementStdDevs; + + private final Pose2d mBeginningRobotPose; + private final double mSlamStdDevsPerMeter; + + private final TreeMap mPastObserverStates; + + public DrivetrainEstimator(Matrix stateStdDevs, Matrix measurementStdDevs, + double slamStdDevsPerMeter, Pose2d beginningRobotPose) + { + mObserver = new ExtendedKalmanFilter(Nat.N3(), Nat.N3(), Nat.N3(), + this::f, (x, u) -> x, stateStdDevs, + new MatBuilder<>(Nat.N3(), Nat.N1()).fill(measurementStdDevs.get(0, 0), + measurementStdDevs.get(1, 0), measurementStdDevs.get(2, 0)), + kNominalDt); + mVisionObserver = new ExtendedKalmanFilter<>(Nat.N3(), Nat.N3(), Nat.N6(), + this::f, this::h, stateStdDevs, measurementStdDevs, kNominalDt); + mMeasurementStdDevs = measurementStdDevs; + + mBeginningRobotPose = beginningRobotPose; + mSlamStdDevsPerMeter = slamStdDevsPerMeter; + + mPastObserverStates = new TreeMap<>(); + } + + private final Matrix f(Matrix x, Matrix u) + { + // Diff drive forward kinematics: + // v_c = (v_l + v_r) / 2 + var newPose = new Pose2d(x.get(0, 0), x.get(1, 0), new Rotation2d(x.get(2, 0))).exp( + new Twist2d((u.get(0, 0) + u.get(1, 0)) / 2, 0.0, u.get(2, 0)) + ); + + return new MatBuilder<>(Nat.N3(), Nat.N1()).fill(newPose.getTranslation().getX(), + newPose.getTranslation().getY(), x.get(2, 0) + u.get(2, 0)); + } + + private final Matrix h(Matrix x, Matrix u) + { + return new MatBuilder<>(Nat.N6(), Nat.N1()).fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), + x.get(0, 0), x.get(1, 0), x.get(2, 0)); + } + + public synchronized void addVisionMeasurement(Pose2d visionRobotPose, double timestampSeconds) + { + var low = mPastObserverStates.floorEntry(timestampSeconds); + var high = mPastObserverStates.ceilingEntry(timestampSeconds); + + Map.Entry closestEntry = null; + if (low != null && high != null) + { + closestEntry = Math.abs(timestampSeconds - low.getKey()) < Math + .abs(timestampSeconds - high.getKey()) + ? low + : high; + } + else + { + closestEntry = low != null ? low : high; + } + + if (closestEntry == null) + { + Logger.warning("Observer state map was empty; ignorning vision update"); + return; + } + + var tailMap = mPastObserverStates.tailMap(closestEntry.getKey(), true); + for (var st : tailMap.values()) + { + if (visionRobotPose != null) + { + mObserver.setP(st.errorCovariances); + mObserver.setXhat(st.xHat); + } + update(st.slamMeasurements, visionRobotPose, st.inputs); + + visionRobotPose = null; + } + } + + /** + * @param slamRobotPose The robot pose just given by the vSLAM camera. + * @return The estimated pose of the robot. + */ + public synchronized Pose2d update(Pose2d slamRobotPose, double dleftMeters, double drightMeters, + double dthetaRadians, double timeSeconds) + { + var u = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(dleftMeters, drightMeters, dthetaRadians); + mPastObserverStates.put(timeSeconds, new ObserverState(mObserver, u, slamRobotPose)); + + if (mPastObserverStates.size() > kMaxPastObserverStates) + { + mPastObserverStates.remove(mPastObserverStates.firstKey()); + } + + return update(slamRobotPose, null, u); + } + + private Pose2d update(Pose2d slamRobotPose, Pose2d visionRobotPose, Matrix u) + { + double distFromBeginningMeters = slamRobotPose.getTranslation().getDistance(mBeginningRobotPose.getTranslation()); + + if (visionRobotPose != null) + { + mVisionObserver.setP(mObserver.getP()); + mVisionObserver.setXhat(mObserver.getXhat()); + + var measStdDevs = new MatBuilder<>(Nat.N6(), Nat.N1()).fill( + mSlamStdDevsPerMeter * distFromBeginningMeters, + mSlamStdDevsPerMeter * distFromBeginningMeters, + mSlamStdDevsPerMeter * distFromBeginningMeters, + mMeasurementStdDevs.get(3, 0), + mMeasurementStdDevs.get(4, 0), + mMeasurementStdDevs.get(5, 0)); + var contR = StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), measStdDevs); + var discR = Discretization.discretizeR(contR, kNominalDt); + + var y = new MatBuilder<>(Nat.N6(), Nat.N1()).fill(slamRobotPose.getTranslation().getX(), + slamRobotPose.getTranslation().getY(), slamRobotPose.getRotation().getRadians(), + visionRobotPose.getTranslation().getX(), visionRobotPose.getTranslation().getY(), + visionRobotPose.getRotation().getRadians()); + + mVisionObserver.correct(Nat.N6(), u, y, this::h, discR); + mVisionObserver.predict(u, kNominalDt); + + mObserver.setP(mVisionObserver.getP()); + mObserver.setXhat(mVisionObserver.getXhat()); + } + else + { + var y = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(slamRobotPose.getTranslation().getX(), + slamRobotPose.getTranslation().getY(), slamRobotPose.getRotation().getRadians()); + + var measStdDevs = new MatBuilder<>(Nat.N3(), Nat.N1()).fill( + mSlamStdDevsPerMeter * distFromBeginningMeters, + mSlamStdDevsPerMeter * distFromBeginningMeters, + mSlamStdDevsPerMeter * distFromBeginningMeters); + var contR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), measStdDevs); + var discR = Discretization.discretizeR(contR, kNominalDt); + + mObserver.correct(Nat.N3(), u, y, (x, _u) -> x, discR); + mObserver.predict(u, kNominalDt); + } + + return new Pose2d(mObserver.getXhat(0), mObserver.getXhat(1), + new Rotation2d(mObserver.getXhat(2))); + } + + private static class ObserverState + { + public final Matrix xHat; + public final Matrix errorCovariances; + public final Matrix inputs; + public final Pose2d slamMeasurements; + + public ObserverState(ExtendedKalmanFilter observer, Matrix u, + Pose2d y) + { + xHat = observer.getXhat(); + errorCovariances = observer.getP(); + + inputs = u; + slamMeasurements = y; + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java new file mode 100644 index 0000000..3cb11a6 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateEstimator.java @@ -0,0 +1,284 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import com.spartronics4915.lib.util.Kinematics; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import com.spartronics4915.lib.hardware.sensors.T265Camera; +import com.spartronics4915.lib.hardware.sensors.T265Camera.CameraJNIException; +import com.spartronics4915.lib.hardware.sensors.T265Camera.CameraUpdate; +import com.spartronics4915.lib.subsystems.SpartronicsSubsystem; +import com.spartronics4915.lib.subsystems.drive.AbstractDrive; +import com.spartronics4915.lib.util.Logger; +import com.spartronics4915.lib.util.Units; + +/** + * This loop keeps track of robot state whenever the robot is enabled. + */ +public class RobotStateEstimator extends SpartronicsSubsystem +{ + /** + * The SLAM camera/encoder RobotStateMap objects represent two views of the + * robot's current state (state is pose, velocity, and distance driven). + * + * All length units are meters. + */ + private RobotStateMap mEncoderStateMap = new RobotStateMap(); + private RobotStateMap mCameraStateMap = new RobotStateMap(); + private RobotStateMap mVisionResetEncoderStateMap = new RobotStateMap(); + private RobotStateMap mFusedStateMap = new RobotStateMap(); + + private AbstractDrive mDrive; + private Kinematics mKinematics; + private T265Camera mSLAMCamera; + private final VisionEvent mVisionEventListener; + private DrivetrainEstimator mEKF; + private Notifier mNotifier; + + private final EstimatorSource mBestEstimatorSource; + + public enum EstimatorSource + { + EncoderOdometry, + VisualSLAM, + VisionResetEncoderOdometry, + Fused + } + + /** Meters */ + private double mLeftPrevDist = 0.0, mRightPrevDist = 0.0; + private double mPrevHeading = 0.0; + + public RobotStateEstimator(AbstractDrive driveSubsystem, Kinematics kinematics, + T265Camera slamra, DrivetrainEstimator ekfEstimator, EstimatorSource bestEstimatorSource) + { + mBestEstimatorSource = bestEstimatorSource; + + mDrive = driveSubsystem; + mKinematics = kinematics; + mSLAMCamera = slamra; + mEKF = ekfEstimator; + + mVisionEventListener = new VisionEvent() + { + @Override + public void run() + { + if (mBestEstimatorSource == EstimatorSource.VisionResetEncoderOdometry) + { + Transform2d deltaSinceVisionEvent = mEncoderStateMap.getLatestFieldToVehicle() + .minus(mEncoderStateMap.getFieldToVehicle(this.mTimestamp)); + mVisionResetEncoderStateMap.reset(Timer.getFPGATimestamp(), + this.mVisionEstimate.transformBy(deltaSinceVisionEvent)); + } + else if (mBestEstimatorSource == EstimatorSource.Fused) + { + mEKF.addVisionMeasurement(this.mVisionEstimate, this.mTimestamp); + } + } + }; + + resetRobotStateMaps(); + + if (mSLAMCamera == null) + { + logInitialized(false); + } + else + { + logInitialized(true); + } + + // Run this at 100 Hz + this.mNotifier = new Notifier(this::run); + this.mNotifier.startPeriodic(1 / 100.0); + } + + public VisionEvent getVisionListener() + { + return mVisionEventListener; + } + + public RobotStateMap getEncoderRobotStateMap() + { + return mEncoderStateMap; + } + + public RobotStateMap getBestRobotStateMap() + { + switch (mBestEstimatorSource) + { + case VisionResetEncoderOdometry: + return mVisionResetEncoderStateMap; + case EncoderOdometry: + return mEncoderStateMap; + case Fused: + return mFusedStateMap; + case VisualSLAM: + return mCameraStateMap; + default: + Logger.warning("Unknown EstimatorSource " + mBestEstimatorSource.name()); + return mEncoderStateMap; + + } + } + + public void resetRobotStateMaps() + { + resetRobotStateMaps(new Pose2d()); + } + + public synchronized void resetRobotStateMaps(Pose2d pose) + { + double time = Timer.getFPGATimestamp(); + mEncoderStateMap.reset(time, pose); + mCameraStateMap.reset(time, pose); + mVisionResetEncoderStateMap.reset(time, pose); + mFusedStateMap.reset(time, pose); + + mLeftPrevDist = mDrive.getLeftMotor().getEncoder().getPosition(); + mRightPrevDist = mDrive.getRightMotor().getEncoder().getPosition(); + + if (mSLAMCamera != null) + { + mSLAMCamera.setPose(pose); + } + mDrive.setIMUHeading(pose.getRotation()); + } + + @Override + public void periodic() + { + super.periodic(); + + final RobotStateMap.State estate = mEncoderStateMap.getLatestState(); + Pose2d epose = estate.pose; + SmartDashboard.putNumber("RobotState/timeStamp", Timer.getFPGATimestamp()); // Important for + // vision sync + SmartDashboard.putString("RobotState/encoderPose", + Units.metersToInches(epose.getTranslation().getX()) + " " + + Units.metersToInches(epose.getTranslation().getY()) + " " + + epose.getRotation().getDegrees()); + + final RobotStateMap.State bestState = getBestRobotStateMap().getLatestState(); + Pose2d cpose = bestState.pose; + + // NB: other tools (like Dashboard and Vision) depend on the structure + // and id of RobotState/pose. Change with caution. + SmartDashboard.putString("RobotState/pose", + Units.metersToInches(cpose.getTranslation().getX()) + " " + + Units.metersToInches(cpose.getTranslation().getY()) + " " + + cpose.getRotation().getDegrees()); + } + + public void stop() + { + if (mSLAMCamera != null) + { + mSLAMCamera.stop(); + } + } + + public void run() + { + if (DriverStation.isDisabled()) + return; + + double ts = Timer.getFPGATimestamp(); + + final RobotStateMap.State last = mEncoderStateMap.getLatestState(); + + /* + * There are two ways to measure current velocity: + * Method 1, integrationVelocity + * Look at the distance traveled since last measurement, consider + * current gyro heading rather than our stored state + * Divide by delta time to produce a velocity. Note that + * 254's implementation doesn't include time computations explicitly. + * In method 1, the implicit time is the time between samples which relates + * to the looper time interval. Thus: leftDelta is measured in + * meters/loopinterval. To the degree that the loop interval isn't a + * constant the result will be noisy. OTH: we can interpret this + * velocity as also a distance traveled since last loop. + */ + final Twist2d iVal; + synchronized (this) + { + final double leftDist = mDrive.getLeftMotor().getEncoder().getPosition(); + final double rightDist = mDrive.getRightMotor().getEncoder().getPosition(); + final double leftDelta = leftDist - mLeftPrevDist; + final double rightDelta = rightDist - mRightPrevDist; + final Rotation2d heading = mDrive.getIMUHeading(); + mLeftPrevDist = leftDist; + mRightPrevDist = rightDist; + iVal = mKinematics.forwardKinematics(last.pose.getRotation(), leftDelta, rightDelta, + heading); + + if (mBestEstimatorSource == EstimatorSource.Fused) + { + var ekfPose = mEKF.update(mCameraStateMap.getLatestFieldToVehicle(), leftDist, rightDist, heading.getRadians() - mPrevHeading, ts); + + mPrevHeading = heading.getRadians(); + mFusedStateMap.addObservations(ts, ekfPose); + } + } + + /* + * integrateForward: given a last state and a current velocity, + * estimate a new state (P2 = P1 + dPdt * dt) + */ + Pose2d nextP = mKinematics.integrateForwardKinematics(last.pose, iVal); + + /* record the new state estimate */ + mEncoderStateMap.addObservations(ts, nextP); + + if (mBestEstimatorSource == EstimatorSource.VisionResetEncoderOdometry) + { + nextP = mKinematics.integrateForwardKinematics( + mVisionResetEncoderStateMap.getLatestFieldToVehicle(), iVal); + mVisionResetEncoderStateMap.addObservations(ts, nextP); + } + + // We convert meters/loopinterval and radians/loopinterval to meters/sec and + // radians/sec + final double loopintervalToSeconds = 1 / (ts - last.timestamp); + final Twist2d normalizedIVal = new Twist2d(iVal.dx * loopintervalToSeconds, iVal.dy * loopintervalToSeconds, iVal.dtheta * loopintervalToSeconds); + + if (mSLAMCamera != null) + { + try + { + // Sometimes (for unknown reasons) the native code can't send odometry info. + // We throw a Java exception when this happens, but we'd like to ignore that + // exception in this situation. + mSLAMCamera.sendOdometry(normalizedIVal); + } + catch (CameraJNIException e) + { + Logger.exception(e); + } + } + } + + public void enable() + { + if (mSLAMCamera == null) + { + return; + } + + // Callback is called from a different thread... We avoid data races + // because RobotSteteMap is thread-safe + mSLAMCamera.stop(); + mSLAMCamera.start((CameraUpdate update) -> { + mCameraStateMap.addObservations(Timer.getFPGATimestamp(), update.pose); + SmartDashboard.putString("RobotState/cameraConfidence", update.confidence.toString()); + }); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java new file mode 100644 index 0000000..207909d --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/RobotStateMap.java @@ -0,0 +1,171 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; +import com.spartronics4915.lib.util.Interpolable; +import com.spartronics4915.lib.util.InterpolatingDouble; +import com.spartronics4915.lib.util.InterpolatingTreeMap; + +/** + * Container for time-indexed state estimates of a robot. + * Used to monitor robot state history and to infer state at + * intermediate points in time. Currently we don't support an + * explicit extrapolate method. Position can be extrapolated by + * applying the velocity * dtime to the sample pose. + */ +public class RobotStateMap +{ + private static final int kObservationBufferSize = 300; + + static public class State implements Interpolable + { + public Pose2d pose; + public double timestamp; + + /** + * default constructor + */ + public State() + { + this.pose = new Pose2d(); + this.timestamp = 0; + } + + /** + * copy constructor + */ + public State(State other) + { + this.pose = other.pose; + this.timestamp = other.timestamp; + } + + /** + * constructor variant that doesn't care about turretAngle + * @param p + * @param ts + */ + public State(Pose2d p, double ts) + { + this.pose = p; + this.timestamp = ts; + } + + /** + * constructor variant that does care about turretAngle + * @param p + * @param turretAngle + * @param ts + */ + public State(Pose2d p, double turretAngle, double ts) + { + this.pose = p; + this.timestamp = ts; + } + + @Override + public State interpolate(final State other, double pct) + { + if (pct <= 0) + return new State(this); + else if (pct >= 0) + return new State(other); + else + { + final State s = new State(interpolatePose(this.pose, other.pose, pct), + this.timestamp + pct * (other.timestamp - this.timestamp)); + return s; + } + } + } + + private InterpolatingTreeMap mStateMap; + private double mDistanceDriven; + + public RobotStateMap() + { + reset(0, new Pose2d()); + } + + /** + * Resets the field to robot transform (robot's position on the field) + * Default value of turretAngle reset is currently zero. + */ + public synchronized void reset(double startTime, Pose2d initialPose) + { + mStateMap = new InterpolatingTreeMap<>(kObservationBufferSize); + mStateMap.put(new InterpolatingDouble(startTime), new State(initialPose, startTime)); + mDistanceDriven = 0.0; + } + + /** + * Resets the field to robot transform (robot's position on the field) + * as well as the turretAngle. + */ + public synchronized void reset(double startTime, Pose2d initialPose, + double turretAngle) + { + mStateMap = new InterpolatingTreeMap<>(kObservationBufferSize); + mStateMap.put(new InterpolatingDouble(startTime), + new State(initialPose, turretAngle, startTime)); + mDistanceDriven = 0.0; + } + + public synchronized void resetDistanceDriven() + { + mDistanceDriven = 0.0; + } + + public synchronized void addObservations(double timestamp, Pose2d pose) + { + InterpolatingDouble ts = new InterpolatingDouble(timestamp); + mStateMap.put(ts, new State(pose, timestamp)); + } + + /** + * Returns the robot's state on the field at a certain time. Linearly + * interpolates between stored robot state to fill in the gaps. + */ + public synchronized State get(double ts) + { + return mStateMap.getInterpolated(new InterpolatingDouble(ts)); + } + + /** + * Returns the robot's position on the field at a certain time. Linearly + * interpolates between stored robot positions to fill in the gaps. + */ + public synchronized Pose2d getFieldToVehicle(double timestamp) + { + return this.get(timestamp).pose; + } + + public synchronized Pose2d getLatestFieldToVehicle() + { + return mStateMap.lastEntry().getValue().pose; + } + + public synchronized State getLatestState() + { + return mStateMap.lastEntry().getValue(); + } + + public synchronized double getDistanceDriven() + { + return mDistanceDriven; + } + + private static Pose2d interpolatePose(Pose2d startPose, Pose2d endPose, double t) + { + if (t <= 0) + { + return startPose; + } + else if (t >= 1) + { + return endPose; + } + final Twist2d twist = startPose.log(endPose); + return startPose.exp(new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t)); + } +} diff --git a/src/main/java/com/spartronics4915/lib/subsystems/estimator/VisionEvent.java b/src/main/java/com/spartronics4915/lib/subsystems/estimator/VisionEvent.java new file mode 100644 index 0000000..8420ea6 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/subsystems/estimator/VisionEvent.java @@ -0,0 +1,13 @@ +package com.spartronics4915.lib.subsystems.estimator; + +import edu.wpi.first.math.geometry.Pose2d; + +public class VisionEvent implements Runnable +{ + public Pose2d mVisionEstimate = null; + public double mTimestamp = 0; + + public void run() + { + }; // override me +} diff --git a/src/main/java/com/spartronics4915/lib/util/DeltaTime.java b/src/main/java/com/spartronics4915/lib/util/DeltaTime.java new file mode 100644 index 0000000..6991d00 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/DeltaTime.java @@ -0,0 +1,45 @@ +package com.spartronics4915.lib.util; + +public class DeltaTime +{ + /** Seconds */ + private double mStartTime = -1; + /** Seconds */ + double mCurrentTime = -1; + /** Seconds */ + double mDeltaTime = 0.0; + + public double getDeltaTimeSeconds() + { + return mDeltaTime; + } + + public double getCurrentTimeSeconds() + { + return mCurrentTime; + } + + public double getStartTime() + { + if (mStartTime < 0) + throw new RuntimeException("Can't get start time if DeltaTime has never been updated"); + + return mStartTime; + } + + public double updateTime(double newTimeSeconds) + { + if (mCurrentTime < 0) + mDeltaTime = 0; + else + mDeltaTime = newTimeSeconds - mCurrentTime; + + mCurrentTime = newTimeSeconds; + return mDeltaTime; + } + + public void reset() + { + mCurrentTime = -1; + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/Interpolable.java b/src/main/java/com/spartronics4915/lib/util/Interpolable.java new file mode 100644 index 0000000..83c5472 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Interpolable.java @@ -0,0 +1,27 @@ +package com.spartronics4915.lib.util; + +/** + * Interpolable is an interface used by an Interpolating Tree as the Value type. + * Given two end points and an + * interpolation parameter on [0, 1], it calculates a new Interpolable + * representing the interpolated value. + * + * @param The Type of Interpolable + * @see InterpolatingTreeMap + */ +public interface Interpolable +{ + + /** + * Interpolates between this value and an other value according to a given + * parameter. If x is 0, the method should + * return this value. If x is 1, the method should return the other value. If 0 + * < x < 1, the return value should be + * interpolated proportionally between the two. + * + * @param endValue The value of the upper bound + * @param x The requested value. Should be between 0 and 1. + * @return Interpolable The estimated average between the surrounding data + */ + public T interpolate(T endValue, double x); +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java new file mode 100644 index 0000000..2e5f312 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingDouble.java @@ -0,0 +1,60 @@ +package com.spartronics4915.lib.util; + +/** + * A Double that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingDouble implements Interpolable, + InverseInterpolable, Comparable +{ + + public Double value = 0.0; + + public InterpolatingDouble(Double val) + { + value = val; + } + + @Override + public InterpolatingDouble interpolate(InterpolatingDouble other, double x) + { + Double dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingDouble(searchY); + } + + @Override + public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) + { + double upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) + { + return 0; + } + double query_to_lower = query.value - value; + if (query_to_lower <= 0) + { + return 0; + } + return query_to_lower / upper_to_lower; + } + + @Override + public int compareTo(InterpolatingDouble other) + { + if (other.value < value) + { + return 1; + } + else if (other.value > value) + { + return -1; + } + else + { + return 0; + } + } + +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java new file mode 100644 index 0000000..24a200c --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingLong.java @@ -0,0 +1,59 @@ +package com.spartronics4915.lib.util; + +/** + * A Long that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingLong implements Interpolable, + InverseInterpolable, Comparable +{ + + public Long value = 0L; + + public InterpolatingLong(Long val) + { + value = val; + } + + @Override + public InterpolatingLong interpolate(InterpolatingLong other, double x) + { + Long dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingLong(searchY.longValue()); + } + + @Override + public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) + { + long upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) + { + return 0; + } + long query_to_lower = query.value - value; + if (query_to_lower <= 0) + { + return 0; + } + return query_to_lower / (double) upper_to_lower; + } + + @Override + public int compareTo(InterpolatingLong other) + { + if (other.value < value) + { + return 1; + } + else if (other.value > value) + { + return -1; + } + else + { + return 0; + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java b/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java new file mode 100644 index 0000000..dac175e --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InterpolatingTreeMap.java @@ -0,0 +1,100 @@ +package com.spartronics4915.lib.util; + +import java.util.Map; +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined + * by making a guess from points that are + * defined. This uses linear interpolation. + * + * @param The type of the key (must implement InverseInterpolable) + * @param The type of the value (must implement Interpolable) + */ +public class InterpolatingTreeMap & Comparable, V extends Interpolable> + extends TreeMap +{ + + private static final long serialVersionUID = 8347275262778054124L; + + int max_; + + public InterpolatingTreeMap(int maximumSize) + { + max_ = maximumSize; + } + + public InterpolatingTreeMap() + { + this(0); + } + + /** + * Inserts a key value pair, and trims the tree if a max size is specified + * + * @param key Key for inserted data + * @param value Value for inserted data + * @return the value + */ + @Override + public V put(K key, V value) + { + if (max_ > 0 && max_ <= size()) + { + // "Prune" the tree if it is oversize + K first = firstKey(); + remove(first); + } + + super.put(key, value); + + return value; + } + + @Override + public void putAll(Map map) + { + Logger.error("Unimplemented Method"); + } + + /** + * @param key Lookup for a value (does not have to exist) + * @return V or null; V if it is Interpolable or exists, null if it is at a + * bound and cannot average + */ + public V getInterpolated(K key) + { + V gotval = get(key); + if (gotval == null) + { + /** Get surrounding keys for interpolation */ + K topBound = ceilingKey(key); + K bottomBound = floorKey(key); + + /** + * If attempting interpolation at ends of tree, return the nearest data point + */ + if (topBound == null && bottomBound == null) + { + return null; + } + else if (topBound == null) + { + return get(bottomBound); + } + else if (bottomBound == null) + { + return get(topBound); + } + + /** Get surrounding values for interpolation */ + V topElem = get(topBound); + V bottomElem = get(bottomBound); + return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); + } + else + { + return gotval; + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java b/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java new file mode 100644 index 0000000..35db4cf --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/InverseInterpolable.java @@ -0,0 +1,28 @@ +package com.spartronics4915.lib.util; + +/** + * InverseInterpolable is an interface used by an Interpolating Tree as the Key + * type. Given two endpoint keys and a + * third query key, an InverseInterpolable object can calculate the + * interpolation parameter of the query key on the + * interval [0, 1]. + * + * @param The Type of InverseInterpolable + * @see InterpolatingTreeMap + */ +public interface InverseInterpolable +{ + + /** + * Given this point (lower), a query point (query), and an upper point (upper), + * estimate how far (on [0, 1]) between + * 'lower' and 'upper' the query point lies. + * + * @param upper + * @param query + * @return The interpolation parameter on [0, 1] representing how far between + * this point and the upper point the + * query point lies. + */ + public double inverseInterpolate(T upper, T query); +} diff --git a/src/main/java/com/spartronics4915/lib/util/Kinematics.java b/src/main/java/com/spartronics4915/lib/util/Kinematics.java new file mode 100644 index 0000000..27a7f14 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Kinematics.java @@ -0,0 +1,40 @@ +package com.spartronics4915.lib.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; + +/** + * Provides forward and inverse kinematics equations for the robot modeling the + * wheelbase as a differential drive (with a corrective factor to account for + * skidding). + */ +public class Kinematics +{ + // TODO: Deduplicate kinematics once we add in path following stuff (i.e. remove this file) + + private final double mTrackWidthInches, mScrubFactor; + + public Kinematics(double trackWidthInches, double scrubFactor) + { + mTrackWidthInches = trackWidthInches; + mScrubFactor = scrubFactor; + } + + public Twist2d forwardKinematics(Rotation2d prevHeading, double leftWheelDelta, double rightWheelDelta, + Rotation2d currentHeading) + { + final double dx = (leftWheelDelta + rightWheelDelta) / 2.0; + final double dy = 0.0; + return new Twist2d(dx, dy, prevHeading.unaryMinus().plus(currentHeading).getRadians()); + } + + /** + * For convenience, integrate forward kinematics with a Twist2d and previous + * rotation. + */ + public Pose2d integrateForwardKinematics(Pose2d currentPose, Twist2d forwardKinematics) + { + return currentPose.exp(forwardKinematics); + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/Logger.java b/src/main/java/com/spartronics4915/lib/util/Logger.java new file mode 100644 index 0000000..fff755b --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Logger.java @@ -0,0 +1,185 @@ +package com.spartronics4915.lib.util; + +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import java.io.StringWriter; +import java.nio.file.Paths; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.UUID; + +/** + * Tracks start-up and caught crash events, logging them to a file which dosn't + * roll over + */ +public class Logger +{ + /** + * a ClientWrite subclass can intercept logging messages usually sent + * to stdout. This may be of use for unit testing. + */ + public static abstract class ClientWriter + { + public void writeln(String str, Throwable Exception) {} + } + + private static final UUID sRunInstanceUUID = UUID.randomUUID(); + public static int sVerbosity = 2; // 0: notices and above, 1: info and above, 2: all + private static final DateFormat sDateFormat = new SimpleDateFormat("hh:mm:ss"); + private static ClientWriter sClientWriter = null; + + public static void installClientWriter(ClientWriter w) + { + sClientWriter = w; + } + + public static void setVerbosity(String nm) + { + if (nm.equals("NOTICE")) + sVerbosity = 0; + else if (nm.equals("INFO")) + sVerbosity = 1; + else if (nm.equals("DEBUG")) + sVerbosity = 2; + else + error("Logger: unknown verbosity level:" + nm); + } + + public static void logRobotStartup() + { + notice("robot startup"); + } + + public static void logRobotConstruction() + { + notice("robot construction"); + } + + public static void logRobotInit() + { + notice("robot init"); + } + + public static void logTeleopInit() + { + notice("teleop init"); + } + + public static void logAutoInit() + { + notice("auto init"); + } + + public static void logDisabledInit() + { + notice("disabled init"); + } + + public static void logTestInit() + { + notice("test init"); + } + + public static void logThrowableCrash(Throwable throwable) + { + logMarker("Exception", throwable); + } + + public static void logThrowableCrash(String msg, Throwable throwable) + { + logMarker("ERROR " + msg, throwable); + } + + public static void exception(Throwable e) + { + StringWriter sw = new StringWriter(); + e.printStackTrace(new PrintWriter(sw)); + logMarker("EXCEPT " + e.getMessage() + " trace:\n" + sw.toString()); + } + + public static void error(String m) + { + logMarker("ERROR " + m); + } + + public static void warning(String m) + { + logMarker("WARNING " + m); + } + + public static void notice(String m) + { + logMarker("NOTICE " + m); + } + + public static void info(String m) + { + if (sVerbosity > 0) + { + printMarker("INFO " + m); + } + } + + public static void debug(String m) + { + if (sVerbosity > 1) + { + printMarker("DEBUG " + m); + } + } + + private static String getTimeStamp() + { + Date now = new Date(); + String nowstr = sDateFormat.format(now) + " "; + return nowstr; + } + + private static void logMarker(String mark) + { + logMarker(mark, null); + } + + private static void printMarker(String mark) + { + if(sClientWriter != null) + { + sClientWriter.writeln(mark, null); + } + else + System.out.println(mark); + } + + private static void logMarker(String mark, Throwable nullableException) + { + if(sClientWriter != null) + { + sClientWriter.writeln(mark, nullableException); + } + else + { + printMarker(mark); + if (nullableException != null) + nullableException.printStackTrace(); + } + try (PrintWriter writer = new PrintWriter(new FileWriter( + Paths.get(System.getProperty("user.home"), "crash_tracking.txt").toString(), true))) + { + writer.print(sRunInstanceUUID.toString()); + writer.print(", "); + writer.print(mark); + if (nullableException != null) + { + writer.print(", "); + nullableException.printStackTrace(writer); + } + writer.println(); + } + catch (IOException e) + { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/com/spartronics4915/lib/util/TriFunction.java b/src/main/java/com/spartronics4915/lib/util/TriFunction.java new file mode 100644 index 0000000..4eebb25 --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/TriFunction.java @@ -0,0 +1,14 @@ +package com.spartronics4915.lib.util; + +import java.util.Objects; +import java.util.function.Function; + +@FunctionalInterface +public interface TriFunction { + public R apply(T t, U u, V v); + + public default TriFunction andThen(Function after) { + Objects.requireNonNull(after); + return (T t, U u, V v) -> after.apply(apply(t, u, v)); + } +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/lib/util/Units.java b/src/main/java/com/spartronics4915/lib/util/Units.java new file mode 100644 index 0000000..1e87a8a --- /dev/null +++ b/src/main/java/com/spartronics4915/lib/util/Units.java @@ -0,0 +1,55 @@ +package com.spartronics4915.lib.util; + +public class Units +{ + + public static double rpmToRadsPerSec(double rpm) + { + return rpm * 2.0 * Math.PI / 60.0; + } + + public static double radsPerSecToRpm(double radsPerSec) + { + return radsPerSec * 60.0 / (2.0 * Math.PI); + } + + public static double inchesToMeters(double inches) + { + return inches * 0.0254; + } + + public static double millimetersToInches(double millimeters) + { + return metersToInches(millimeters / 1000); + } + + public static double inchesToMillimeters(double inches) + { + return inchesToMeters(inches) * 1000; + } + + public static double metersToInches(double meters) + { + return meters / 0.0254; + } + + public static double feetToMeters(double feet) + { + return inchesToMeters(feet * 12.0); + } + + public static double metersToFeet(double meters) + { + return metersToInches(meters) / 12.0; + } + + public static double degreesToRadians(double degrees) + { + return Math.toRadians(degrees); + } + + public static double radiansToDegrees(double radians) + { + return Math.toDegrees(radians); + } +} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..d7bd9b0 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "2020.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +}