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 0000000..7454180 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ 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" + ] + } + ] +}