diff --git a/.gitignore b/.gitignore index 924a5f8b..57d300ee 100644 --- a/.gitignore +++ b/.gitignore @@ -27,6 +27,8 @@ bun.lock .DS_Store .vscode +*/.idea/ + # Build files .gradle diff --git a/examples/stage2/templates/ctre/.gitignore b/examples/stage2/templates/ctre/.gitignore new file mode 100644 index 00000000..34cbaac1 --- /dev/null +++ b/examples/stage2/templates/ctre/.gitignore @@ -0,0 +1,187 @@ +# 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/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/examples/stage2/templates/ctre/.wpilib/wpilib_preferences.json b/examples/stage2/templates/ctre/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..14c708c9 --- /dev/null +++ b/examples/stage2/templates/ctre/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2027_alpha7", + "teamNumber": 9999 +} \ No newline at end of file diff --git a/examples/stage2/templates/ctre/WPILib-License.md b/examples/stage2/templates/ctre/WPILib-License.md new file mode 100644 index 00000000..eb3061b0 --- /dev/null +++ b/examples/stage2/templates/ctre/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2026 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/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/config.json b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/config.json new file mode 100644 index 00000000..4b2b531d --- /dev/null +++ b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/config.json @@ -0,0 +1,72 @@ +{ + "name": "Midtide", + "rotations": [ + {"axis": "x", + "degrees": 90}, + {"axis": "z", + "degrees": 90} + ], + "position": [ + 0, + 0, + 0 + ], + "components": [ + { + "name": "elevator", + "zeroedRotations": [ + {"axis": "x", + "degrees": 90}, + {"axis": "z", + "degrees": 90} + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { + "name": "carriage", + "zeroedRotations": [ + {"axis": "x", + "degrees": 90}, + {"axis": "z", + "degrees": 90} + ], + "zeroedPosition": [ + 0, + 0, + -0.5627090346 + ] + }, + { + "name": "arm", + "zeroedRotations": [ + {"axis": "x", + "degrees": 90}, + {"axis": "z", + "degrees": 90} + ], + "zeroedPosition": [ + 0, + 0, + -1.06854 + ] + }, + { + "name": "intake", + "zeroedRotations": [ + {"axis": "x", + "degrees": 90}, + {"axis": "z", + "degrees": 90} + ], + "zeroedPosition": [ + -0.324690, + 0, + -0.298374 + ] + } + ] +} diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model.glb new file mode 100644 index 00000000..60561f37 Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model.glb differ diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_0.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_0.glb new file mode 100644 index 00000000..44d47ef3 Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_0.glb differ diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_1.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_1.glb new file mode 100644 index 00000000..fd1a8d9a Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_1.glb differ diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_2.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_2.glb new file mode 100644 index 00000000..2a328bc2 Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_2.glb differ diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3.glb new file mode 100644 index 00000000..acb65b64 Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3.glb differ diff --git a/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3middle.glb b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3middle.glb new file mode 100644 index 00000000..c54aec11 Binary files /dev/null and b/examples/stage2/templates/ctre/advantagescope_models/Robot_midtide/model_3middle.glb differ diff --git a/examples/stage2/templates/ctre/build.gradle b/examples/stage2/templates/ctre/build.gradle new file mode 100644 index 00000000..54b69dd6 --- /dev/null +++ b/examples/stage2/templates/ctre/build.gradle @@ -0,0 +1,152 @@ +plugins { + id "java" + id "application" + id "com.diffplug.spotless" version "8.0.0" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-7" +} + +java { + sourceCompatibility = JavaVersion.VERSION_25 + targetCompatibility = JavaVersion.VERSION_25 +} + +def ROBOT_MAIN_CLASS = "first.Main" + +// Define my targets (SystemCore) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + systemcore(getTargetTypeClass('SystemCore')) { + // 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.wpilib.getTeamNumber() + // Use the default systemcore host name. This must be called after setting team + // as happens on the line above + useDefaultSystemcoreHostName() + debug = project.wpilib.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + wpilibJava(getArtifactTypeClass('WPILibJavaArtifact')) { + // Set to true to use debug including JNI, which will drastically impact performance. + debugJni = false + } + + // Static files artifact + wpilibStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/systemcore/deploy' + deleteOldFiles = false // Change to true to delete files on systemcore that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.systemcore.artifacts.wpilibJava + +// Set to true to use debug for simulation including JNI +wpi.java.runSimWithDebugJni = 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 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + systemcoreDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.systemcore) + systemcoreDebug wpi.java.vendor.jniDebug(wpi.platforms.systemcore) + + systemcoreRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.systemcore) + systemcoreRelease wpi.java.vendor.jniRelease(wpi.platforms.systemcore) + + 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 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +application.mainClass = ROBOT_MAIN_CLASS + +deployArtifact.configureApplication(application) +wpi.java.configureApplication(application) +wpi.java.configureTestTasks(test) + +jar { + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } +} + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} + +project.compileJava.dependsOn(spotlessApply) +spotless { + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**', '**/bin/**' + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree('.') { + include '**/*.gradle' + exclude '**/build/**', '**/build-*/**' + } + greclipse() + leadingTabsToSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + format 'xml', { + target fileTree('.') { + include '**/*.xml' + exclude '**/build/**', '**/build-*/**' + } + eclipseWtp('xml') + trimTrailingWhitespace() + leadingTabsToSpaces(2) + endWithNewline() + } + format 'misc', { + target fileTree('.') { + include '**/*.md', '**/.gitignore' + exclude '**/build/**', '**/build-*/**' + } + trimTrailingWhitespace() + leadingTabsToSpaces(2) + endWithNewline() + } +} diff --git a/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.jar b/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..d997cfc6 Binary files /dev/null and b/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.jar differ diff --git a/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.properties b/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..42bb5ac9 --- /dev/null +++ b/examples/stage2/templates/ctre/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-9.4.1-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/examples/stage2/templates/ctre/gradlew b/examples/stage2/templates/ctre/gradlew new file mode 100644 index 00000000..739907df --- /dev/null +++ b/examples/stage2/templates/ctre/gradlew @@ -0,0 +1,248 @@ +#!/bin/sh + +# +# Copyright © 2015 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. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# 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/2d6327017519d23b96af35865dc997fcb544fb40/platforms/jvm/plugins-application/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 + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s\n' "$PWD" ) || exit + +# 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 + + + +# 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 + if ! command -v java >/dev/null 2>&1 + then + 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 +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + 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" ) + + 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 + + +# 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"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -jar "$APP_HOME/gradle/wrapper/gradle-wrapper.jar" \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# 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/examples/stage2/templates/ctre/gradlew.bat b/examples/stage2/templates/ctre/gradlew.bat new file mode 100644 index 00000000..c4bdd3ab --- /dev/null +++ b/examples/stage2/templates/ctre/gradlew.bat @@ -0,0 +1,93 @@ +@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 +@rem SPDX-License-Identifier: Apache-2.0 +@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=. +@rem This is normally unused +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% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 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! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/examples/stage2/templates/ctre/settings.gradle b/examples/stage2/templates/ctre/settings.gradle new file mode 100644 index 00000000..a17bda05 --- /dev/null +++ b/examples/stage2/templates/ctre/settings.gradle @@ -0,0 +1,38 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + String wpilibYear = '2027_alpha7' + File wpilibHome + def os = OperatingSystem.current() + if (os.isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + wpilibHome = new File(homeRoot, wpilibYear) + } else if (os.isLinux()) { + String xdgDataHome = System.getenv('XDG_DATA_HOME') + if (xdgDataHome == null || xdgDataHome.trim().isEmpty() || !new File(xdgDataHome).isAbsolute()) { + xdgDataHome = new File(System.getProperty("user.home"), ".local/share").getPath() + } + def homeRoot = new File(xdgDataHome, "wpilib") + wpilibHome = new File(homeRoot, wpilibYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, ".wpilib") + wpilibHome = new File(homeRoot, wpilibYear) + } + def wpilibHomeMaven = new File(wpilibHome, 'maven') + maven { + name = 'wpilibHome' + url = wpilibHomeMaven + } + mavenLocal() + gradlePluginPortal() + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/examples/stage2/templates/ctre/simgui-ds.json b/examples/stage2/templates/ctre/simgui-ds.json new file mode 100644 index 00000000..74014e8b --- /dev/null +++ b/examples/stage2/templates/ctre/simgui-ds.json @@ -0,0 +1,59 @@ +{ + "keyboardJoysticks": [{ + "axisConfig": [{ + "decKey": 546, + "incKey": 549 + }, { + "decKey": 568, + "incKey": 564 + }, { + "decKey": 550, + "decayRate": 0, + "incKey": 563, + "keyRate": 0.01 + }], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [571, 569, 548, 567], + "povConfig": [{ + "keyDown": 614, + "keyDownLeft": 613, + "keyDownRight": 615, + "keyLeft": 616, + "keyRight": 618, + "keyUp": 620, + "keyUpLeft": 619, + "keyUpRight": 621 + }], + "povCount": 1 + }, { + "axisConfig": [{ + "decKey": 555, + "incKey": 557 + }, { + "decKey": 554, + "incKey": 556 + }], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [558, 597, 599, 600], + "povCount": 0 + }, { + "axisConfig": [{ + "decKey": 513, + "incKey": 514 + }, { + "decKey": 515, + "incKey": 516 + }], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [521, 519, 517, 522, 520, 518], + "povCount": 0 + }, { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + }], + "robotJoysticks": [{"guid": "0300fa675e040000ff02000000007801"}] +} diff --git a/examples/stage2/templates/ctre/src/main/deploy/example.txt b/examples/stage2/templates/ctre/src/main/deploy/example.txt new file mode 100644 index 00000000..c9bc6a2f --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the Systemcore into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/examples/stage2/templates/ctre/src/main/java/first/Main.java b/examples/stage2/templates/ctre/src/main/java/first/Main.java new file mode 100644 index 00000000..f9d9f6fc --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/Main.java @@ -0,0 +1,27 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first; + +import first.robot.Robot; +import org.wpilib.framework.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/examples/stage2/templates/ctre/src/main/java/first/robot/FieldConstants.java b/examples/stage2/templates/ctre/src/main/java/first/robot/FieldConstants.java new file mode 100644 index 00000000..b09f32bb --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/FieldConstants.java @@ -0,0 +1,44 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot; + +import org.wpilib.fields.Fields; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; + +public class FieldConstants { + + public static final double fieldWidth = Fields.FRC_2025_REEFSCAPE_WELDED.width; + public static final double fieldLength = Fields.FRC_2025_REEFSCAPE_WELDED.length; + + public static final Translation2d BLUE_REEF_CENTER = new Translation2d(4.284788, 0); + public static final Translation2d RED_REEF_CENTER = mirrorX(BLUE_REEF_CENTER); + + public static Translation2d mirrorX(Translation2d translation) { + return new Translation2d(-translation.getX(), translation.getY()); + } + + public static Pose2d mirrorX(Pose2d pose) { + return new Pose2d(mirrorX(pose.getTranslation()), Rotation2d.PI.minus(pose.getRotation())); + } + + public static Translation2d mirrorY(Translation2d translation) { + return new Translation2d(translation.getX(), -translation.getY()); + } + + public static Pose2d mirrorY(Pose2d pose) { + return new Pose2d(mirrorY(pose.getTranslation()), pose.getRotation().unaryMinus()); + } + + public static Translation2d rotateAboutCenter(Translation2d translation) { + return mirrorX(mirrorY(translation)); + } + + public static Pose2d rotateAboutCenter(Pose2d pose) { + return mirrorX(mirrorY(pose)); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/Poses.java b/examples/stage2/templates/ctre/src/main/java/first/robot/Poses.java new file mode 100644 index 00000000..0a2166e5 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/Poses.java @@ -0,0 +1,95 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot; + +import java.util.List; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; + +public class Poses { + + public static final Pose2d BLUE_REEF_A = + new Pose2d( + FieldConstants.BLUE_REEF_CENTER.plus(new Translation2d(1.43, -0.164309)), + Rotation2d.ZERO); + public static final Pose2d BLUE_REEF_B = FieldConstants.mirrorY(BLUE_REEF_A); + public static final Pose2d BLUE_REEF_C = + BLUE_REEF_A.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(60)); + public static final Pose2d BLUE_REEF_D = + BLUE_REEF_B.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(60)); + public static final Pose2d BLUE_REEF_E = + BLUE_REEF_A.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(120)); + public static final Pose2d BLUE_REEF_F = + BLUE_REEF_B.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(120)); + public static final Pose2d BLUE_REEF_G = + BLUE_REEF_A.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_REEF_H = + BLUE_REEF_B.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_REEF_I = + BLUE_REEF_A.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(240)); + public static final Pose2d BLUE_REEF_J = + BLUE_REEF_B.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(240)); + public static final Pose2d BLUE_REEF_K = + BLUE_REEF_A.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(300)); + public static final Pose2d BLUE_REEF_L = + BLUE_REEF_B.rotateAround(FieldConstants.BLUE_REEF_CENTER, Rotation2d.fromDegrees(300)); + + public static final List BLUE_REEF_POSES = + List.of( + BLUE_REEF_A, + BLUE_REEF_B, + BLUE_REEF_C, + BLUE_REEF_D, + BLUE_REEF_E, + BLUE_REEF_F, + BLUE_REEF_G, + BLUE_REEF_H, + BLUE_REEF_I, + BLUE_REEF_J, + BLUE_REEF_K, + BLUE_REEF_L); + public static final List BLUE_REEF_LEFT_POSES = + List.of(BLUE_REEF_A, BLUE_REEF_C, BLUE_REEF_E, BLUE_REEF_G, BLUE_REEF_I, BLUE_REEF_K); + public static final List BLUE_REEF_RIGHT_POSES = + List.of(BLUE_REEF_B, BLUE_REEF_D, BLUE_REEF_F, BLUE_REEF_H, BLUE_REEF_J, BLUE_REEF_L); + + public static final Pose2d RED_REEF_A = FieldConstants.rotateAboutCenter(BLUE_REEF_A); + public static final Pose2d RED_REEF_B = FieldConstants.rotateAboutCenter(BLUE_REEF_B); + public static final Pose2d RED_REEF_C = FieldConstants.rotateAboutCenter(BLUE_REEF_C); + public static final Pose2d RED_REEF_D = FieldConstants.rotateAboutCenter(BLUE_REEF_D); + public static final Pose2d RED_REEF_E = FieldConstants.rotateAboutCenter(BLUE_REEF_E); + public static final Pose2d RED_REEF_F = FieldConstants.rotateAboutCenter(BLUE_REEF_F); + public static final Pose2d RED_REEF_G = FieldConstants.rotateAboutCenter(BLUE_REEF_G); + public static final Pose2d RED_REEF_H = FieldConstants.rotateAboutCenter(BLUE_REEF_H); + public static final Pose2d RED_REEF_I = FieldConstants.rotateAboutCenter(BLUE_REEF_I); + public static final Pose2d RED_REEF_J = FieldConstants.rotateAboutCenter(BLUE_REEF_J); + public static final Pose2d RED_REEF_K = FieldConstants.rotateAboutCenter(BLUE_REEF_K); + public static final Pose2d RED_REEF_L = FieldConstants.rotateAboutCenter(BLUE_REEF_L); + + public static final List RED_REEF_POSES = + List.of( + RED_REEF_A, + RED_REEF_B, + RED_REEF_C, + RED_REEF_D, + RED_REEF_E, + RED_REEF_F, + RED_REEF_G, + RED_REEF_H, + RED_REEF_I, + RED_REEF_J, + RED_REEF_K, + RED_REEF_L); + public static final List RED_REEF_LEFT_POSES = + List.of(RED_REEF_A, RED_REEF_C, RED_REEF_E, RED_REEF_G, RED_REEF_I, RED_REEF_K); + public static final List RED_REEF_RIGHT_POSES = + List.of(RED_REEF_B, RED_REEF_D, RED_REEF_F, RED_REEF_H, RED_REEF_J, RED_REEF_L); + + public static final Transform2d REEF_PREALIGN_TRANSFORM = + new Transform2d(0.3, 0, Rotation2d.ZERO); +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/Robot.java b/examples/stage2/templates/ctre/src/main/java/first/robot/Robot.java new file mode 100644 index 00000000..42c9baa2 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/Robot.java @@ -0,0 +1,65 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot; + +import first.robot.mechanisms.*; +import first.robot.simulation.CoralSim; +import first.robot.simulation.RobotVisualization; +import org.wpilib.command3.Scheduler; +import org.wpilib.framework.OpModeRobot; + +public class Robot extends OpModeRobot { + + public final CoralSim coralSim; + + public final Drive drive; + public final Elevator elevator; + public final Arm arm; + public final Superstructure superstructure; + public final Claw claw; + public final Intake intake; + public final Indexer indexer; + public final RobotVisualization robotVisualization; + + public Robot() { + drive = new Drive(); + elevator = new Elevator(); + arm = new Arm(); + superstructure = new Superstructure(elevator, arm); + coralSim = new CoralSim(drive::getPose, elevator::getPosition, arm::getPosition); + claw = new Claw(); + intake = new Intake(); + indexer = new Indexer(); + + // robotVisualization = new RobotVisualization(drive::getPose, elevator::getPosition, + // arm::getPosition, intake::getPivotPosition); + // robotVisualization = new RobotVisualization(drive::getPose, () -> 0, () -> 0, () -> 0); + // robotVisualization = new RobotVisualization(drive::getPose, () -> Math.max(0, + // Math.sin(Timer.getTimestamp()) * 2), () -> Timer.getTimestamp()/2 * Math.PI, () -> + // Math.sin(Timer.getTimestamp()) * Math.PI); + robotVisualization = + new RobotVisualization(drive::getPose, elevator::getPosition, arm::getPosition, () -> 0); + } + + @Override + public void robotPeriodic() { + drive.periodic(); + elevator.periodic(); + arm.periodic(); + claw.periodic(); + intake.periodic(); + indexer.periodic(); + + robotVisualization.periodic(); + + Scheduler.getDefault().run(); + } + + @Override + public void simulationPeriodic() { + coralSim.periodic(); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Arm.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Arm.java new file mode 100644 index 00000000..5051f23b --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Arm.java @@ -0,0 +1,202 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import first.robot.Robot; +import first.robot.simulation.ArmSim; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.math.util.Units; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; + +public class Arm implements Mechanism { + + private static class Constants { + static final int MOTOR_ID = 22; + static final CANBus BUS = new CANBus(CANPort.CAN_S1); + + static final InvertedValue MOTOR_INVERSION = InvertedValue.Clockwise_Positive; + + static final double GEAR_RATIO = + (56.0 / 16.0) * (84.0 / 10.0); // Two stage, 56:16, 84:10 reductions + + static final double kS = 0; + static final double kG = 0.777; + static final double kV = 3.464; + static final double kA = 0.192; + static final double kP = 60; + static final double kI = 0; + static final double kD = 7; + + static final double POSITION_TOLERANCE = Units.degreesToRotations(3); + } + + private TalonFX motor; + private ArmSim sim; + + private VoltageOut voltageRequest; + private PositionVoltage positionRequest; + + private StatusSignal appliedVoltageSignal; + private StatusSignal positionSignal; + private StatusSignal velocitySignal; + private StatusSignal statorCurrentSignal; + private StatusSignal supplyCurrentSignal; + + private BaseStatusSignal[] signals; + + private double setpoint = 0; + + private TelemetryTable table = Telemetry.getTable("Arm"); + + public Arm() { + // Construct and configure motors + motor = new TalonFX(Constants.MOTOR_ID, Constants.BUS); + + TalonFXConfiguration configuration = new TalonFXConfiguration(); + configuration.MotorOutput.withNeutralMode(NeutralModeValue.Brake) + .withInverted(Constants.MOTOR_INVERSION); + configuration.Feedback.withSensorToMechanismRatio(Constants.GEAR_RATIO); + configuration.Slot0.withKS(Constants.kS) + .withKG(Constants.kG) + .withKV(Constants.kV) + .withKA(Constants.kA) + .withKP(Constants.kP) + .withKI(Constants.kI) + .withKD(Constants.kD) + .withGravityType(GravityTypeValue.Arm_Cosine); + + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(60) + .withSupplyCurrentLimitEnable(true); + + configuration.withCurrentLimits(currentLimitsConfigs); + + motor.getConfigurator().apply(configuration); + + // Set up simulation + sim = new ArmSim(motor); + + // Set up requests + voltageRequest = new VoltageOut(0); + positionRequest = new PositionVoltage(0); + + // Set up status signals + appliedVoltageSignal = motor.getMotorVoltage(); + positionSignal = motor.getPosition(); + velocitySignal = motor.getVelocity(); + statorCurrentSignal = motor.getStatorCurrent(); + supplyCurrentSignal = motor.getSupplyCurrent(); + + signals = + new BaseStatusSignal[] { + appliedVoltageSignal, + positionSignal, + velocitySignal, + statorCurrentSignal, + supplyCurrentSignal + }; + } + + public void periodic() { + // Update simulation + if (Robot.isSimulation()) { + sim.periodic(); + } + + BaseStatusSignal.refreshAll(signals); + + table.log("Applied Voltage", Volts.of(getAppliedVoltage())); + table.log("Position", Rotations.of(getPosition())); + table.log("Velocity", RotationsPerSecond.of(getVelocity())); + table.log("At Setpoint", isAtSetpoint()); + table.log("Active Commands", getRunningCommands().toString()); + } + + /** + * @param voltage the voltage to apply to the motor + * @return a command + */ + public Command setVoltage(double voltage) { + return run(coro -> { + motor.setControl(voltageRequest.withOutput(voltage)); + }) + .named("Set Voltage " + voltage + "V"); + } + + /** + * @param position the position for the arm to target, in rotations + * @return a command + */ + public Command setPosition(double position) { + return run(coro -> { + setpoint = position; + motor.setControl(positionRequest.withPosition(position)); + table.log("Setpoint", Rotations.of(position)); + + coro.waitUntil(() -> isAtPosition(position)); + }) + .named("Set Position " + position + "rot"); + } + + /** + * @return the current position of the arm, in rotations + */ + public double getPosition() { + return positionSignal.getValueAsDouble(); + } + + /** + * @return the current velocity, in rotations per second + */ + public double getVelocity() { + return velocitySignal.getValueAsDouble(); + } + + /** + * @return the current voltage being applied to the arm motor + */ + public double getAppliedVoltage() { + return appliedVoltageSignal.getValueAsDouble(); + } + + /** + * @param position the position to compare against, in rotations + * @return whether the arm is at that positoin + */ + public boolean isAtPosition(double position) { + return Math.abs(getPosition() - position) < Constants.POSITION_TOLERANCE; + } + + /** + * @return whether the arm is at its current setpoint + */ + public boolean isAtSetpoint() { + return isAtPosition(setpoint); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Claw.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Claw.java new file mode 100644 index 00000000..a10b3ca7 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Claw.java @@ -0,0 +1,88 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import first.robot.Robot; +import first.robot.simulation.ClawSim; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.hardware.discrete.DigitalInput; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; + +public class Claw implements Mechanism { + private final TalonFX motor; + private final DigitalInput sensor; + + private final ClawSim sim; + + private VoltageOut voltageRequest; + + private StatusSignal appliedVoltageSignal; + private StatusSignal statorCurrentSignal; + private StatusSignal supplyCurrentSignal; + + private BaseStatusSignal[] signals; + + private TelemetryTable table = Telemetry.getTable("Claw"); + + public Claw() { + motor = new TalonFX(23, new CANBus(CANPort.CAN_S2)); + sensor = new DigitalInput(1); + + sim = new ClawSim(motor); + + voltageRequest = new VoltageOut(0); + + appliedVoltageSignal = motor.getMotorVoltage(); + statorCurrentSignal = motor.getStatorCurrent(); + supplyCurrentSignal = motor.getSupplyCurrent(); + + signals = + new BaseStatusSignal[] {appliedVoltageSignal, statorCurrentSignal, supplyCurrentSignal}; + } + + public void periodic() { + // Update simulation + if (Robot.isSimulation()) { + sim.periodic(); + } + + BaseStatusSignal.refreshAll(signals); + + table.log("Applied Voltage", Volts.of(appliedVoltageSignal.getValueAsDouble())); + table.log("Stator Current", Amps.of(statorCurrentSignal.getValueAsDouble())); + table.log("Supply Current", Amps.of(supplyCurrentSignal.getValueAsDouble())); + table.log("Active Commands", getRunningCommands().toString()); + + table.log("Has Coral", hasCoral()); + } + + /** + * @param voltage the voltage to apply to the motors + * @return a command + */ + public Command setVoltage(double voltage) { + return run((coro) -> { + motor.setControl(voltageRequest.withOutput(voltage)); + }) + .named("Set Voltage: " + voltage + "V"); + } + + public boolean hasCoral() { + return sensor.get(); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Drive.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Drive.java new file mode 100644 index 00000000..31574874 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Drive.java @@ -0,0 +1,443 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ParentConfiguration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.*; +import java.util.Set; +import java.util.function.Supplier; +import org.wpilib.command3.*; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.util.Units; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; + +public class Drive implements Mechanism { + + private final SwerveDrivetrain swerve; + + private final SwerveDrivetrainConstants constants; + private final SwerveModuleConstantsFactory< + ParentConfiguration, ParentConfiguration, ParentConfiguration> + moduleConstantsFactory; + + private final double DRIVE_WIDTH = Units.inchesToMeters(22.729228); + private final double DRIVE_LENGTH = Units.inchesToMeters(22.729228); + + private static final double MAX_VELOCITY = Units.feetToMeters(14.9); + private static final double MAX_ACCELERATION = 5; + + private Pose2d currentPose = new Pose2d(); + + private TelemetryTable table = Telemetry.getTable("Drive"); + private TelemetryTable autoAlignTable = table.getTable("AutoAlign"); + + public Drive() { + constants = + new SwerveDrivetrainConstants().withNetwork(new CANBus(CANPort.CAN_S0)).withPigeon2Id(4); + + moduleConstantsFactory = + new SwerveModuleConstantsFactory<>() + .withDriveMotorGearRatio((54.0 / 12.0) * (25.0 / 35.0) * (30.0 / 15.0)) + .withSteerMotorGearRatio(287.0 / 11.0) + .withCouplingGearRatio(3.857142857142857) + .withWheelRadius(Units.inchesToMeters(2)) + // .withSteerMotorGains(new Slot0Configs() + // .withKP(0.05).withKD(0) + // .withKS(0.2).withKV(0.72972) + // + // .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)) + .withSteerMotorGains( + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.49) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)) + // .withDriveMotorGains(new Slot0Configs() + // .withKP(8).withKD(0) + // .withKS(0.2)) + .withDriveMotorGains( + new Slot0Configs().withKP(0.3).withKI(0).withKD(0).withKS(0.2).withKV(0.125)) + // .withDriveMotorGains(new Slot0Configs() + // .withKP(0.3).withKI(0).withKD(0) + // .withKS(0.1498).withKV(3.16356)) + .withSteerMotorClosedLoopOutput(SwerveModuleConstants.ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(SwerveModuleConstants.ClosedLoopOutputType.Voltage) + .withSlipCurrent(Amps.of(45)) + .withSpeedAt12Volts(MAX_VELOCITY) + .withDriveMotorType(SwerveModuleConstants.DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SwerveModuleConstants.SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SwerveModuleConstants.SteerFeedbackType.RemoteCANcoder) + .withDriveMotorInitialConfigs(new TalonFXConfiguration()) + .withSteerMotorInitialConfigs(new TalonFXConfiguration()) + .withEncoderInitialConfigs(new CANcoderConfiguration()) + .withSteerInertia(KilogramSquareMeters.of(0.01)) + .withDriveInertia(KilogramSquareMeters.of(0.03)) + .withSteerFrictionVoltage(Volts.of(0.1)) + .withDriveFrictionVoltage(Volts.of(0.2)); + + var moduleConstants = + new SwerveModuleConstants[] { + moduleConstantsFactory.createModuleConstants( + 10, 11, 10, 0, DRIVE_LENGTH / 2, DRIVE_WIDTH / 2, false, false, false), + moduleConstantsFactory.createModuleConstants( + 12, 13, 12, 0, DRIVE_LENGTH / 2, -DRIVE_WIDTH / 2, false, false, false), + moduleConstantsFactory.createModuleConstants( + 14, 15, 14, 0, -DRIVE_LENGTH / 2, DRIVE_WIDTH / 2, false, false, false), + moduleConstantsFactory.createModuleConstants( + 16, 17, 16, 0, -DRIVE_LENGTH / 2, -DRIVE_WIDTH / 2, false, false, false) + }; + + swerve = + new SwerveDrivetrain<>( + TalonFX::new, TalonFX::new, CANcoder::new, constants, moduleConstants); + } + + public void periodic() { + // Update sim 5x per loop for greater PID realism + swerve.updateSimState(1.0 / 50.0 / 5.0, 12); + swerve.updateSimState(1.0 / 50.0 / 5.0, 12); + swerve.updateSimState(1.0 / 50.0 / 5.0, 12); + swerve.updateSimState(1.0 / 50.0 / 5.0, 12); + swerve.updateSimState(1.0 / 50.0 / 5.0, 12); + + currentPose = swerve.getState().Pose; + + table.log("Pose", currentPose); + table.log("ModulesReal", swerve.getState().ModuleVelocities); + table.log("ModulesTargets", swerve.getState().ModuleTargets); + table.log("Velocity", swerve.getState().Velocity); + table.log("Active Commands", getRunningCommands().toString()); + } + + /** + * @return the current pose of the drivetrain + */ + public Pose2d getPose() { + // return swerve.getState().Pose; + return currentPose; + } + + /** + * @return the current heading of the drivetrain + */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * @return Field-relative robot velocity + */ + public ChassisVelocities getVelocity() { + return swerve.getState().Velocity.toFieldRelative(getHeading()); + } + + /** + * Creates a teleop drive command + * + * @param controller the controller to use for inputs, the left stick controls translation, while + * the right stick controls rotation + * @return a command + */ + public Command getDriveCommand(CommandXboxController controller) { + return runRepeatedly( + () -> { + double x = -MathUtil.applyDeadband(controller.getLeftY(), 0.1) * MAX_VELOCITY; + double y = -MathUtil.applyDeadband(controller.getLeftX(), 0.1) * MAX_VELOCITY; + double rotation = -MathUtil.applyDeadband(controller.getRightX(), 0.10) * 10; + + var request = new SwerveRequest.FieldCentric(); + request.withForwardPerspective( + SwerveRequest.ForwardPerspectiveValue.OperatorPerspective); + request.withVelocityX(x); + request.withVelocityY(y); + request.withRotationalRate(rotation); + request.withDeadband(0.1 * 4.54); + + swerve.setControl(request); + + table.log( + "DesiredVelocity", + new ChassisVelocities(x, y, rotation).toRobotRelative(getHeading())); + }) + .named("Teleop Drive"); + } + + public static class AutoAlignCommand implements Command { + + private final Drive drive; + + private final SwerveRequest.ApplyFieldVelocity swerveRequest = + new SwerveRequest.ApplyFieldVelocity() + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity); + + private Pose2d targetPose = new Pose2d(); + private Supplier poseSupplier = + () -> { + return new Pose2d(); + }; + private boolean usePoseSupplier = false; + + private double accelerationLimit = MAX_ACCELERATION; + private double velocityLimit = MAX_VELOCITY; + private double positionTolerance = Units.inchesToMeters(2); + private double angleTolerance = Units.degreesToRadians(3); + private boolean runContinuously = false; + private boolean ignoreRotation = false; + private double targetVelocity = 0; + + private double angularVelocityLimit = 5; + private double angularAccelerationLimit = 45; + + private ProfiledPIDController headingController; + + public AutoAlignCommand(Drive drive, Pose2d target) { + this.drive = drive; + this.targetPose = target; + this.usePoseSupplier = false; + + headingController = + new ProfiledPIDController( + 6, + 0, + 0.2, + new TrapezoidProfile.Constraints(angularVelocityLimit, angularAccelerationLimit)); + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + public AutoAlignCommand(Drive drive, Supplier poseSupplier) { + this.drive = drive; + this.poseSupplier = poseSupplier; + this.usePoseSupplier = true; + + headingController = + new ProfiledPIDController( + 6, + 0, + 0.2, + new TrapezoidProfile.Constraints(angularVelocityLimit, angularAccelerationLimit)); + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void run(Coroutine coroutine) { + if (usePoseSupplier) { + targetPose = poseSupplier.get(); + } + + headingController.reset(drive.getHeading().getRadians(), drive.getVelocity().omega); + + Translation2d lastVelocityVector = + new Translation2d(drive.getVelocity().vx, drive.getVelocity().vy); + Translation2d lastRealVelocity = + new Translation2d(drive.getVelocity().vx, drive.getVelocity().vy); + + while (runContinuously || !atPosition()) { + var currentPose = drive.getPose(); + + if (usePoseSupplier) { + targetPose = poseSupplier.get(); + } + + drive.autoAlignTable.log("At Target", atPosition()); + drive.autoAlignTable.log( + "Position in Tolerance", + targetPose.getTranslation().getDistance(drive.getPose().getTranslation()) + <= positionTolerance); + drive.autoAlignTable.log( + "Angle in Tolerance", + (ignoreRotation + || Math.abs( + targetPose.getRotation().minus(drive.getPose().getRotation()).getRadians()) + <= angleTolerance)); + + drive.autoAlignTable.log("Target Pose", targetPose); + + Translation2d vectorToTarget = + targetPose.getTranslation().minus(currentPose.getTranslation()); + double distanceToEnd = vectorToTarget.getNorm(); + + drive.autoAlignTable.log("Distance", Meters.of(distanceToEnd)); + + // ChassisVelocities currentVelocity = drive.getVelocity(); + // Translation2d currentVelocityAsTranslation = new + // Translation2d(currentVelocity.vx, currentVelocity.vy); + // + // double velocityTowardsTarget = + // vectorToTarget.div(distanceToEnd).dot(currentVelocityAsTranslation); + // + // double maxDistanceForCurrentSpeed = (Math.pow(targetVelocity,2) - + // Math.pow(velocityTowardsTarget,2)) / (2 * -accelerationLimit); + + double velocityForDistance = + Math.sqrt(Math.pow(targetVelocity, 2) - 2 * -accelerationLimit * distanceToEnd); + + drive.autoAlignTable.log("Velocity for Distance", MetersPerSecond.of(velocityForDistance)); + + double velocityToTarget = Math.min(velocityForDistance * 0.85, velocityLimit); + + drive.autoAlignTable.log("Targeted Velocity", MetersPerSecond.of(velocityToTarget)); + + Translation2d velocities = vectorToTarget.div(distanceToEnd).times(velocityToTarget); + + lastVelocityVector = + MathUtil.slewRateLimit(lastVelocityVector, velocities, 0.02, accelerationLimit); + + double omega = 0; + + if (!ignoreRotation) { + omega = + headingController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + } + + ChassisVelocities chassisVelocities = + new ChassisVelocities(lastVelocityVector.getX(), lastVelocityVector.getY(), omega); + + drive.table.log("DesiredVelocity", chassisVelocities); + + swerveRequest.withVelocity(chassisVelocities); + drive.swerve.setControl(swerveRequest); + + coroutine.yield(); + } + + // If command ends naturally, have the robot stop + swerveRequest.withVelocity(new ChassisVelocities()); + drive.swerve.setControl(swerveRequest); + } + + @Override + public String name() { + return "AutoAlign"; + } + + @Override + public Set requirements() { + return Set.of(drive); + } + + /** + * Sets a static target pose for the command + * + * @param pose The pose to target + * @return this command + */ + public AutoAlignCommand withTargetPose(Pose2d pose) { + this.targetPose = pose; + this.usePoseSupplier = false; + return this; + } + + /** + * Sets a pose supplier for the command to use, which will be evaluated continuously + * + * @param supplier The supplier to use for the pose target + * @return this command + */ + public AutoAlignCommand withTargetPoseSupplier(Supplier supplier) { + this.poseSupplier = supplier; + this.usePoseSupplier = true; + return this; + } + + /** + * Sets the maximum allowed acceleration for the robot to use + * + * @param accelerationLimit The maximum allowed acceleration + * @return this command + */ + public AutoAlignCommand withAccelerationLimit(double accelerationLimit) { + this.accelerationLimit = accelerationLimit; + return this; + } + + /** + * Sets the maximum allowed velocity for the robot to use + * + * @param velocityLimit The maximum allowed velocity + * @return this command + */ + public AutoAlignCommand withVelocityLimit(double velocityLimit) { + this.velocityLimit = velocityLimit; + return this; + } + + /** + * Sets the tolerance for the final position of the robot. Used to determine whether + * + * @param tolerance + * @return + */ + public AutoAlignCommand withPositionTolerance(double tolerance) { + this.positionTolerance = tolerance; + return this; + } + + public AutoAlignCommand withAngleTolerance(double tolerance) { + this.angleTolerance = tolerance; + return this; + } + + public AutoAlignCommand withRunningContinuously(boolean runContinuously) { + this.runContinuously = runContinuously; + return this; + } + + public AutoAlignCommand withIgnoringRotation(boolean ignoringRotation) { + this.ignoreRotation = ignoringRotation; + return this; + } + + public AutoAlignCommand withTargetVelocity(double targetVelocity) { + this.targetVelocity = targetVelocity; + return this; + } + + public AutoAlignCommand withAngularAccelerationLimit(double angularAccelerationLimit) { + this.angularAccelerationLimit = angularAccelerationLimit; + return this; + } + + public AutoAlignCommand withAngularVelocityLimit(double angularVelocityLimit) { + this.angularVelocityLimit = angularVelocityLimit; + return this; + } + + public boolean atPosition() { + return targetPose.getTranslation().getDistance(drive.getPose().getTranslation()) + <= positionTolerance + && (ignoreRotation + || Math.abs( + targetPose.getRotation().minus(drive.getPose().getRotation()).getRadians()) + <= angleTolerance); + } + + public Command waitUntilAtPosition() { + return Command.waitUntil(this::atPosition).named("Wait Until At Position"); + } + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Elevator.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Elevator.java new file mode 100644 index 00000000..fa43c71e --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Elevator.java @@ -0,0 +1,224 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import first.robot.Robot; +import first.robot.simulation.ElevatorSim; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.math.util.Units; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; +import org.wpilib.units.measure.*; + +public class Elevator implements Mechanism { + + private static class Constants { + static final int LEADER_ID = 20; + static final int FOLLOWER_ID = 21; + static final CANBus BUS = new CANBus(CANPort.CAN_S1); + + static final InvertedValue LEADER_INVERSION = InvertedValue.Clockwise_Positive; + static final MotorAlignmentValue FOLLOWER_ALIGNMENT = MotorAlignmentValue.Opposed; + + static final double GEAR_RATIO = + (44.0 / 14.0) * (42.0 / 22.0); // Two stage, 14:44, 22:44 reductions + static final double PULLEY_CIRCUMFERENCE = + 24 * 0.005; // Convert 24t pulley to circumference, in m + + static final double kS = 0; + static final double kG = 0.26; + static final double kV = 5.94; + static final double kA = 0.03; + static final double kP = 18.3; + static final double kI = 0; + static final double kD = 0.7; + ; + + static final double POSITION_TOLERANCE = Units.inchesToMeters(0.5); + } + + private final TalonFX leader; + private final TalonFX follower; + + private final ElevatorSim sim; + + private final VoltageOut voltageRequest; + private final PositionVoltage positionRequest; + + private final StatusSignal appliedVoltageSignal; + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + private final StatusSignal leaderStatorCurrentSignal; + private final StatusSignal followerStatorCurrentSignal; + private final StatusSignal leaderSupplyCurrentSignal; + private final StatusSignal followerSupplyCurrentSignal; + + private final BaseStatusSignal[] signals; + + private double setpoint = 0; + + private TelemetryTable table = Telemetry.getTable("Elevator"); + + public Elevator() { + // Construct and configure motors + leader = new TalonFX(Constants.LEADER_ID, Constants.BUS); + follower = new TalonFX(Constants.FOLLOWER_ID, Constants.BUS); + + TalonFXConfiguration leaderConfiguration = new TalonFXConfiguration(); + leaderConfiguration + .MotorOutput + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(Constants.LEADER_INVERSION); + leaderConfiguration.Feedback.withSensorToMechanismRatio(Constants.GEAR_RATIO); + leaderConfiguration + .Slot0 + .withKS(Constants.kS) + .withKG(Constants.kG) + .withKV(Constants.kV) + .withKA(Constants.kA) + .withKP(Constants.kP) + .withKI(Constants.kI) + .withKD(Constants.kD) + .withGravityType(GravityTypeValue.Elevator_Static); + + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(60) + .withSupplyCurrentLimitEnable(true); + + leaderConfiguration.withCurrentLimits(currentLimitsConfigs); + + leader.getConfigurator().apply(leaderConfiguration); + + follower.getConfigurator().apply(currentLimitsConfigs); + + follower.setControl(new Follower(leader.getDeviceID(), Constants.FOLLOWER_ALIGNMENT)); + + // Set up simulation + sim = new ElevatorSim(leader, follower); + + // Set up requests + voltageRequest = new VoltageOut(0); + positionRequest = new PositionVoltage(0); + + // Set up status signals + appliedVoltageSignal = leader.getMotorVoltage(); + positionSignal = leader.getPosition(); + velocitySignal = leader.getVelocity(); + leaderStatorCurrentSignal = leader.getStatorCurrent(); + followerStatorCurrentSignal = follower.getStatorCurrent(); + leaderSupplyCurrentSignal = leader.getSupplyCurrent(); + followerSupplyCurrentSignal = follower.getSupplyCurrent(); + + signals = + new BaseStatusSignal[] { + appliedVoltageSignal, + positionSignal, + velocitySignal, + leaderStatorCurrentSignal, + followerStatorCurrentSignal, + leaderSupplyCurrentSignal, + followerSupplyCurrentSignal + }; + } + + public void periodic() { + // Update simulation + if (Robot.isSimulation()) { + sim.periodic(); + } + + BaseStatusSignal.refreshAll(signals); + + table.log("Applied Voltage", Volts.of(getAppliedVoltage())); + table.log("Position", Meters.of(getPosition())); + table.log("Velocity", MetersPerSecond.of(getVelocity())); + table.log("At Setpoint", isAtSetpoint()); + table.log("Active Commands", getRunningCommands().toString()); + } + + /** + * @param voltage the voltage to apply to the motors + * @return a command + */ + public Command setVoltage(double voltage) { + return run(coro -> { + leader.setControl(voltageRequest.withOutput(voltage)); + }) + .named("Set Voltage " + voltage + "V"); + } + + /** + * @param position the position for the elevator to target, in meters + * @return a command + */ + public Command setPosition(double position) { + return run(coro -> { + setpoint = position; + leader.setControl( + positionRequest.withPosition(position / Constants.PULLEY_CIRCUMFERENCE)); + table.log("Setpoint", Meters.of(position)); + + coro.waitUntil(() -> isAtPosition(position)); + }) + .named("Set Position " + position + "m"); + } + + /** + * @return the position of the elevator, in meters + */ + public double getPosition() { + return positionSignal.getValueAsDouble() * Constants.PULLEY_CIRCUMFERENCE; + } + + /** + * @return the velocity of the elevator, in meters per second + */ + public double getVelocity() { + return velocitySignal.getValueAsDouble() * Constants.PULLEY_CIRCUMFERENCE; + } + + /** + * @return the current voltage being applied to the arm motor + */ + public double getAppliedVoltage() { + return appliedVoltageSignal.getValueAsDouble(); + } + + /** + * @param position the position to compare against, in meters + * @return whether the elevator is at that position + */ + public boolean isAtPosition(double position) { + return Math.abs(getPosition() - position) < Constants.POSITION_TOLERANCE; + } + + /** + * @return whether the elevator is at its current setpoint + */ + public boolean isAtSetpoint() { + return isAtPosition(setpoint); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Indexer.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Indexer.java new file mode 100644 index 00000000..0172bf0a --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Indexer.java @@ -0,0 +1,117 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; +import first.robot.Robot; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.hardware.discrete.DigitalInput; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.FlywheelSim; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; + +public class Indexer implements Mechanism { + private final TalonFX motor; + private final DigitalInput sensor; + + private final TalonFXSimState simState; + + private final FlywheelSim sim; + + private final VoltageOut voltageRequest; + + private final StatusSignal appliedVoltageSignal; + private final StatusSignal statorCurrentSignal; + private final StatusSignal supplyCurrentSignal; + + private BaseStatusSignal[] signals; + + private TelemetryTable table = Telemetry.getTable("Indexer"); + + private static class Constants { + static final int MOTOR_ID = 35; + static final CANBus BUS = new CANBus(CANPort.CAN_S1); + + static final double GEAR_RATIO = (48.0 / 12.0); // One stage, 48:12 reduction + + static final double J = 0.00001; + } + + public Indexer() { + motor = new TalonFX(Constants.MOTOR_ID, Constants.BUS); + sensor = new DigitalInput(0); + + simState = motor.getSimState(); + + sim = + new FlywheelSim( + Models.flywheelFromPhysicalConstants( + DCMotor.getKrakenX44(2), Constants.J, Constants.GEAR_RATIO), + DCMotor.getKrakenX44(1)); + + voltageRequest = new VoltageOut(0); + + appliedVoltageSignal = motor.getMotorVoltage(); + statorCurrentSignal = motor.getStatorCurrent(); + supplyCurrentSignal = motor.getSupplyCurrent(); + + signals = + new BaseStatusSignal[] {appliedVoltageSignal, statorCurrentSignal, supplyCurrentSignal}; + } + + public void periodic() { + // Update simulation + if (Robot.isSimulation()) { + simPeriodic(); + } + + BaseStatusSignal.refreshAll(signals); + + table.log("Applied Voltage", Volts.of(appliedVoltageSignal.getValueAsDouble())); + table.log("Stator Current", Amps.of(statorCurrentSignal.getValueAsDouble())); + table.log("Supply Current", Amps.of(supplyCurrentSignal.getValueAsDouble())); + table.log("Active Commands", getRunningCommands().toString()); + + table.log("Has Coral", sensor.get()); + } + + public void simPeriodic() { + sim.setInputVoltage(simState.getMotorVoltage()); + sim.update(0.02); + + simState.setRotorVelocity( + Units.radiansToRotations(sim.getAngularVelocity()) * Constants.GEAR_RATIO); + } + + /** + * @param voltage the voltage to apply to the motors + * @return a command + */ + public Command setVoltage(double voltage) { + return run((coro) -> { + motor.setControl(voltageRequest.withOutput(voltage)); + }) + .named("Set Voltage: " + voltage + "V"); + } + + public boolean hasCoral() { + return sensor.get(); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Intake.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Intake.java new file mode 100644 index 00000000..20c8e8cf --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Intake.java @@ -0,0 +1,323 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import static org.wpilib.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import first.robot.Robot; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.hardware.bus.CANPort; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.FlywheelSim; +import org.wpilib.simulation.SingleJointedArmSim; +import org.wpilib.telemetry.Telemetry; +import org.wpilib.telemetry.TelemetryTable; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; + +public class Intake implements Mechanism { + private final TalonFX pivot; + private final TalonFX rollers; + + // Sim stuff + private final TalonFXSimState pivotSimState; + private final TalonFXSimState rollersSimState; + + private final SingleJointedArmSim pivotSim; + private final FlywheelSim rollersSim; + + private double pivotTargetPosition = 0; + + private final StatusSignal pivotAppliedVoltageSignal; + private final StatusSignal pivotStatorCurrentSignal; + private final StatusSignal pivotSupplyCurrentSignal; + private final StatusSignal pivotPositionSignal; + private final StatusSignal pivotVelocitySignal; + + private final StatusSignal rollersAppliedVoltageSignal; + private final StatusSignal rollersStatorCurrentSignal; + private final StatusSignal rollersSupplyCurrentSignal; + private final StatusSignal rollersVelocitySignal; + + private final BaseStatusSignal[] statusSignals; + + private final VoltageOut pivotVoltageRequest = new VoltageOut(0); + private final PositionVoltage pivotPositionRequest = new PositionVoltage(0); + private final VoltageOut rollersVoltageRequest = new VoltageOut(0); + + private final TelemetryTable table = Telemetry.getTable("Intake"); + private final TelemetryTable pivotTable = table.getTable("Pivot"); + private final TelemetryTable rollersTable = table.getTable("Rollers"); + + private static class Constants { + static final int PIVOT_ID = 30; + static final int ROLLERS_ID = 31; + + static final CANBus BUS = new CANBus(CANPort.CAN_S1); + + static final InvertedValue PIVOT_INVERSION = InvertedValue.Clockwise_Positive; + + static final double PIVOT_RATIO = + (60.0 / 16.0) + * (70.0 / 18.0) + * (42.0 / 10.0); // Three stage, 60:16, 70:18, 42:10 reductions + + static final double PIVOT_kS = 0; + static final double PIVOT_kV = 5.621; + static final double PIVOT_kA = 0.03; + // static final double PIVOT_kP = 19; //92 + static final double PIVOT_kP = 25; + static final double PIVOT_kI = 0; + // static final double PIVOT_kD = 0.2; //0.5 + static final double PIVOT_kD = 0.5; + + static final double PIVOT_POSITION_TOLERANCE = Units.degreesToRotations(5); + + static final double PIVOT_ARM_LENGTH = Units.inchesToMeters(5.2692); + static final double PIVOT_ARM_MASS = Units.lbsToKilograms(12.296); + static final double PIVOT_ARM_MIN_ANGLE = Units.degreesToRotations(-3.206); + static final double PIVOT_ARM_MAX_ANGLE = Units.degreesToRotations(138.469); + + static final InvertedValue ROLLERS_INVERSION = InvertedValue.Clockwise_Positive; + + static final double ROLLERS_RATIO = (42.0 / 12.0); // Single stage, 42:12 reduction + + static final double ROLLERS_J = 0.0002; + } + + public Intake() { + pivot = new TalonFX(Constants.PIVOT_ID, Constants.BUS); + rollers = new TalonFX(Constants.ROLLERS_ID, Constants.BUS); + + TalonFXConfiguration pivotConfiguration = new TalonFXConfiguration(); + pivotConfiguration + .MotorOutput + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(Constants.PIVOT_INVERSION); + pivotConfiguration.Feedback.withSensorToMechanismRatio(Constants.PIVOT_RATIO); + pivotConfiguration + .Slot0 + .withKS(Constants.PIVOT_kS) + .withKV(Constants.PIVOT_kV) + .withKA(Constants.PIVOT_kA) + .withKP(Constants.PIVOT_kP) + .withKI(Constants.PIVOT_kI) + .withKD(Constants.PIVOT_kD); + + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(60) + .withSupplyCurrentLimitEnable(true); + + pivotConfiguration.withCurrentLimits(currentLimitsConfigs); + + pivot.getConfigurator().apply(pivotConfiguration); + + pivot.setPosition(Constants.PIVOT_ARM_MAX_ANGLE); + // pivot.setPosition(0.38); + + TalonFXConfiguration rollersConfiguration = new TalonFXConfiguration(); + rollersConfiguration.withCurrentLimits(currentLimitsConfigs); + rollersConfiguration + .MotorOutput + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(Constants.ROLLERS_INVERSION); + + rollers.getConfigurator().apply(rollersConfiguration); + + // Create status signals + + pivotAppliedVoltageSignal = pivot.getMotorVoltage(); + pivotStatorCurrentSignal = pivot.getStatorCurrent(); + pivotSupplyCurrentSignal = pivot.getSupplyCurrent(); + pivotPositionSignal = pivot.getPosition(); + pivotVelocitySignal = pivot.getVelocity(); + + rollersAppliedVoltageSignal = rollers.getMotorVoltage(); + rollersStatorCurrentSignal = rollers.getStatorCurrent(); + rollersSupplyCurrentSignal = rollers.getSupplyCurrent(); + rollersVelocitySignal = rollers.getVelocity(); + + statusSignals = + new BaseStatusSignal[] { + pivotAppliedVoltageSignal, + pivotStatorCurrentSignal, + pivotSupplyCurrentSignal, + pivotPositionSignal, + pivotVelocitySignal, + rollersAppliedVoltageSignal, + rollersStatorCurrentSignal, + rollersSupplyCurrentSignal, + rollersVelocitySignal + }; + + // Setup sim + + pivotSimState = pivot.getSimState(); + // pivotSimState.Orientation = ChassisReference.CounterClockwise_Positive; + pivotSimState.Orientation = ChassisReference.Clockwise_Positive; + rollersSimState = rollers.getSimState(); + + pivotSim = + new SingleJointedArmSim( + DCMotor.getKrakenX44(1), + Constants.PIVOT_RATIO, + SingleJointedArmSim.estimateMOI(Constants.PIVOT_ARM_LENGTH, Constants.PIVOT_ARM_MASS), + Constants.PIVOT_ARM_LENGTH, + Units.rotationsToRadians(Constants.PIVOT_ARM_MIN_ANGLE), + Units.rotationsToRadians(Constants.PIVOT_ARM_MAX_ANGLE), + true, + Units.rotationsToRadians(Constants.PIVOT_ARM_MAX_ANGLE)); + + rollersSim = + new FlywheelSim( + Models.flywheelFromPhysicalConstants( + DCMotor.getKrakenX60(1), Constants.ROLLERS_J, Constants.ROLLERS_RATIO), + DCMotor.getKrakenX60(1)); + } + + public void periodic() { + // Update simulation + if (Robot.isSimulation()) { + simPeriodic(); + } + + BaseStatusSignal.refreshAll(statusSignals); + + pivotTable.log("Applied Voltage", Volts.of(getPivotAppliedVoltage())); + pivotTable.log("Stator Current", Amps.of(pivotStatorCurrentSignal.getValueAsDouble())); + pivotTable.log("Supply Current", Amps.of(pivotSupplyCurrentSignal.getValueAsDouble())); + pivotTable.log("Target Position", Rotations.of(pivotTargetPosition)); + pivotTable.log("Position", Rotations.of(getPivotPosition())); + pivotTable.log("Velocity", RotationsPerSecond.of(getPivotVelocity())); + pivotTable.log("At Setpoint", isPivotAtSetpoint()); + + rollersTable.log("Applied Voltage", Volts.of(getRollersAppliedVoltage())); + rollersTable.log("Stator Current", Amps.of(rollersStatorCurrentSignal.getValueAsDouble())); + rollersTable.log("Supply Current", Amps.of(rollersSupplyCurrentSignal.getValueAsDouble())); + rollersTable.log("Velocity", RotationsPerSecond.of(getRollersVelocity())); + + table.log("Active Commands", getRunningCommands().toString()); + } + + public void simPeriodic() { + pivotSim.setInputVoltage(pivotSimState.getMotorVoltage()); + pivotSim.update(0.02); + + pivotSimState.setRawRotorPosition( + Units.radiansToRotations(pivotSim.getAngle()) * Constants.PIVOT_RATIO); + pivotSimState.setRotorVelocity( + Units.radiansToRotations(pivotSim.getVelocity()) * Constants.PIVOT_RATIO); + + pivotTable.log("SimPosition", Radians.of(pivotSim.getAngle())); + + rollersSim.setInputVoltage(rollersSimState.getMotorVoltage()); + rollersSim.update(0.02); + + rollersSimState.setRotorVelocity( + Units.radiansToRotations(rollersSim.getAngularVelocity()) * Constants.ROLLERS_RATIO); + } + + /** + * @param voltage voltage to set + * @return a command + */ + public Command setRollerVoltage(double voltage) { + return run(coro -> rollers.setControl(rollersVoltageRequest.withOutput(voltage))) + .named("Set Roller Voltage " + voltage + "V"); + } + + /** + * @param voltage voltage to set + * @return a command + */ + public Command setPivotVoltage(double voltage) { + return run(coro -> pivot.setControl(pivotVoltageRequest.withOutput(voltage))) + .named("Set Pivot Voltage " + voltage + "V"); + } + + /** + * @param position position to target, in rotations + * @return a command + */ + public Command setPivotPosition(double position) { + return run(coro -> { + pivotTargetPosition = position; + pivot.setControl(pivotPositionRequest.withPosition(position)); + coro.waitUntil(() -> isPivotAtPosition(position)); + }) + .named("Set Pivot Position " + position + "rot"); + } + + /** + * @return the current position of the pivot, in rotations + */ + public double getPivotPosition() { + return pivotPositionSignal.getValueAsDouble(); + } + + /** + * @return the current velocity of the pivot, in rotations per second + */ + public double getPivotVelocity() { + return pivotVelocitySignal.getValueAsDouble(); + } + + /** + * @return the current voltage being applied to the pivot motor + */ + public double getPivotAppliedVoltage() { + return pivotAppliedVoltageSignal.getValueAsDouble(); + } + + /** + * @return the current velocity of the pivot, in rotations per second + */ + public double getRollersVelocity() { + return rollersVelocitySignal.getValueAsDouble(); + } + + /** + * @return the current voltage being applied to the pivot motor + */ + public double getRollersAppliedVoltage() { + return rollersAppliedVoltageSignal.getValueAsDouble(); + } + + /** + * @param position the position to compare against, in rotations + * @return whether the arm is at that positoin + */ + public boolean isPivotAtPosition(double position) { + return Math.abs(getPivotPosition() - position) < Constants.PIVOT_POSITION_TOLERANCE; + } + + /** + * @return whether the arm is at its current setpoint + */ + public boolean isPivotAtSetpoint() { + return isPivotAtPosition(pivotTargetPosition); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Superstructure.java b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Superstructure.java new file mode 100644 index 00000000..483ca177 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Superstructure.java @@ -0,0 +1,52 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.mechanisms; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; + +public class Superstructure implements Mechanism { + + private final Elevator elevator; + private final Arm arm; + + public Superstructure(Elevator elevator, Arm arm) { + this.elevator = elevator; + this.arm = arm; + } + + public Command setPosition(double elevatorPosition, double armPosition) { + return run((coro) -> { + coro.awaitAll(elevator.setPosition(elevatorPosition), arm.setPosition(armPosition)); + }) + .named("Set Position"); + } + + public Command setPosition(Positions position) { + return setPosition(position.elevatorSetpoint, position.armSetpoint); + } + + public enum Positions { + STOW(0, 0.75), + HANDOFF_PREP(0.5, 0.25), + HANDOFF(0.38, 0.25), + L4_PREP(1.2, 0.6), + L4_SCORE(1.2, 0.525), + L3_PREP(0.5, 0.6), + L3_SCORE(0.5, 0.55), + L2_PREP(0.1, 0.6), + L2_SCORE(0.1, 0.55), + ; + + private double elevatorSetpoint; + private double armSetpoint; + + Positions(double elevatorSetpoint, double armSetpoint) { + this.elevatorSetpoint = elevatorSetpoint; + this.armSetpoint = armSetpoint; + } + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ArmTest.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ArmTest.java new file mode 100644 index 00000000..23b865f9 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ArmTest.java @@ -0,0 +1,27 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Robot; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Utility; + +@Utility +public class ArmTest extends PeriodicOpMode { + private final Robot robot; + + private final CommandGamepad controller; + + public ArmTest(Robot robot) { + this.robot = robot; + + controller = new CommandGamepad(0); + + controller.button(0).onTrue(robot.arm.setPosition(1)); + controller.button(1).onTrue(robot.arm.setPosition(0)); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/BasicScoringTeleop.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/BasicScoringTeleop.java new file mode 100644 index 00000000..be5b2569 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/BasicScoringTeleop.java @@ -0,0 +1,58 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import static org.wpilib.units.Units.*; + +import first.robot.Poses; +import first.robot.Robot; +import first.robot.mechanisms.Drive.AutoAlignCommand; +import first.robot.mechanisms.Superstructure; +import org.wpilib.command3.Command; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Teleop; + +@Teleop +public class BasicScoringTeleop extends PeriodicOpMode { + private final Robot robot; + + private final CommandXboxController controller; + + public BasicScoringTeleop(Robot robot) { + this.robot = robot; + + controller = new CommandXboxController(0); + + robot.drive.setDefaultCommand(robot.drive.getDriveCommand(controller)); + + controller + .a() + .whileTrue( + Command.noRequirements( + (coro) -> { + coro.fork( + new AutoAlignCommand( + robot.drive, + Poses.BLUE_REEF_A.transformBy(Poses.REEF_PREALIGN_TRANSFORM)) + .withRunningContinuously(true)); + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.L4_PREP)); + + AutoAlignCommand finalAlign = + new AutoAlignCommand(robot.drive, Poses.BLUE_REEF_A) + .withRunningContinuously(true); + coro.fork(finalAlign); + coro.waitUntil(finalAlign::atPosition); + + coro.fork( + robot.superstructure.setPosition(Superstructure.Positions.L4_SCORE)); + coro.wait(Seconds.of(0.25)); + coro.await(robot.claw.setVoltage(-6)); + }) + .named("AutoAlign and Score")); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/DriveTest.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/DriveTest.java new file mode 100644 index 00000000..4bdb1aff --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/DriveTest.java @@ -0,0 +1,26 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Robot; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Utility; + +@Utility +public class DriveTest extends PeriodicOpMode { + private final Robot robot; + + private final CommandXboxController controller; + + public DriveTest(Robot robot) { + this.robot = robot; + + controller = new CommandXboxController(0); + + robot.drive.setDefaultCommand(robot.drive.getDriveCommand(controller)); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorArmPoseTuning.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorArmPoseTuning.java new file mode 100644 index 00000000..af0fa206 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorArmPoseTuning.java @@ -0,0 +1,77 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Poses; +import first.robot.Robot; +import first.robot.mechanisms.Drive; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Utility; +import org.wpilib.tunable.TunableDouble; +import org.wpilib.tunable.Tunables; + +@Utility +public class ElevatorArmPoseTuning extends PeriodicOpMode { + private final Robot robot; + + private final CommandXboxController controller; + + private final TunableDouble elevatorPositionTunable; + private final TunableDouble armPositionTunable; + + public ElevatorArmPoseTuning(Robot robot) { + this.robot = robot; + + controller = new CommandXboxController(0); + + robot.drive.setDefaultCommand(robot.drive.getDriveCommand(controller)); + + elevatorPositionTunable = Tunables.addDouble("Elevator Setpoint", 0); + armPositionTunable = Tunables.addDouble("Arm Setpoint", 0); + + controller + .a() + .onTrue( + robot + .superstructure + .run( + (coro) -> { + coro.await( + robot.superstructure.setPosition( + Math.clamp(elevatorPositionTunable.get(), 0, 1.347), + Math.clamp(armPositionTunable.get(), 0, 1.5))); + }) + .named("Set Superstructure Position from Tunables")); + + controller + .leftBumper() + .whileTrue( + new Drive.AutoAlignCommand( + robot.drive, () -> robot.drive.getPose().nearest(Poses.RED_REEF_LEFT_POSES)) + .withRunningContinuously(true)); + controller + .rightBumper() + .whileTrue( + new Drive.AutoAlignCommand( + robot.drive, () -> robot.drive.getPose().nearest(Poses.RED_REEF_RIGHT_POSES)) + .withRunningContinuously(true)); + + // Scheduler.getDefault().addPeriodic(() -> { + // Telemetry.log("elev", elevatorPositionTunable.hasChanged()); + // Telemetry.log("arm", armPositionTunable.hasChanged()); + // Telemetry.log("elevSetpoint", elevatorPositionTunable.get()); + // Telemetry.log("armSetpoint", armPositionTunable.get()); + // }); + + } + + @Override + public void close() { + Tunables.remove("Elevator Setpoint"); + Tunables.remove("Arm Setpoint"); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorTest.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorTest.java new file mode 100644 index 00000000..0c7b9508 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/ElevatorTest.java @@ -0,0 +1,27 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Robot; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Utility; + +@Utility +public class ElevatorTest extends PeriodicOpMode { + private final Robot robot; + + private final CommandGamepad controller; + + public ElevatorTest(Robot robot) { + this.robot = robot; + + controller = new CommandGamepad(0); + + controller.button(0).onTrue(robot.elevator.setPosition(1)); + controller.button(1).onTrue(robot.elevator.setPosition(0)); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullCoralTeleop.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullCoralTeleop.java new file mode 100644 index 00000000..b1e66c44 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullCoralTeleop.java @@ -0,0 +1,239 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import static org.wpilib.units.Units.Seconds; + +import first.robot.Poses; +import first.robot.Robot; +import first.robot.mechanisms.Drive; +import first.robot.mechanisms.Superstructure; +import java.util.ArrayList; +import java.util.List; +import org.wpilib.command3.*; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Teleop; +import org.wpilib.telemetry.Telemetry; + +@Teleop +public class FullCoralTeleop extends PeriodicOpMode { + private final Robot robot; + + private CommandXboxController controller; + + private Trigger stowButton; + private Trigger l2Button; + private Trigger l3Button; + private Trigger l4Button; + private Trigger leftAlignButton; + private Trigger rightAlignButton; + + private int selectedLevel = 4; + + public FullCoralTeleop(Robot robot) { + this.robot = robot; + + controller = new CommandXboxController(0); + + stowButton = controller.x(); + l2Button = controller.a(); + l3Button = controller.b(); + l4Button = controller.y(); + leftAlignButton = controller.leftBumper(); + rightAlignButton = controller.rightBumper(); + + l2Button.onTrue(Command.noRequirements((coro) -> selectedLevel = 2).named("Select L2")); + l3Button.onTrue(Command.noRequirements((coro) -> selectedLevel = 3).named("Select L3")); + l4Button.onTrue(Command.noRequirements((coro) -> selectedLevel = 4).named("Select L4")); + + robot.drive.setDefaultCommand(robot.drive.getDriveCommand(controller)); + + StateMachine stateMachine = new StateMachine("Teleop State Machine"); + + StateMachine.State stowState = + stateMachine.addState( + Command.noRequirements( + (coro) -> { + coro.await(robot.superstructure.setPosition(Superstructure.Positions.STOW)); + while (true) { + coro.yield(); + } + }) + .named("Stow")); + StateMachine.State handoff = + stateMachine.addState( + Command.noRequirements( + (coro) -> { + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF_PREP)); + coro.waitUntil(robot.indexer::hasCoral); + coro.await(robot.claw.setVoltage(6)); + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF)); + robot.coralSim.attemptHandoff(); + coro.waitUntil(robot.claw::hasCoral); + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF_PREP)); + while (true) { + coro.yield(); + } + }) + .named("Handoff")); + StateMachine.State scoreL2Left = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_LEFT_POSES, + Superstructure.Positions.L2_PREP, + Superstructure.Positions.L2_SCORE) + .named("Score L2 Left")); + StateMachine.State scoreL2Right = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_RIGHT_POSES, + Superstructure.Positions.L2_PREP, + Superstructure.Positions.L2_SCORE) + .named("Score L2 Right")); + StateMachine.State scoreL3Left = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_LEFT_POSES, + Superstructure.Positions.L3_PREP, + Superstructure.Positions.L3_SCORE) + .named("Score L2 Left")); + StateMachine.State scoreL3Right = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_RIGHT_POSES, + Superstructure.Positions.L3_PREP, + Superstructure.Positions.L3_SCORE) + .named("Score L3 Right")); + StateMachine.State scoreL4Left = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_LEFT_POSES, + Superstructure.Positions.L4_PREP, + Superstructure.Positions.L4_SCORE) + .named("Score L4 Left")); + StateMachine.State scoreL4Right = + stateMachine.addState( + createScoreCommand( + Poses.RED_REEF_RIGHT_POSES, + Superstructure.Positions.L4_PREP, + Superstructure.Positions.L4_SCORE) + .named("Score L4 Right")); + + stateMachine.setInitialState(stowState); + + stowState.switchTo(handoff).when(robot.indexer::hasCoral); + + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL2Left) + .when(leftAlignButton.and(() -> selectedLevel == 2).and(robot.claw::hasCoral)); + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL2Right) + .when(rightAlignButton.and(() -> selectedLevel == 2).and(robot.claw::hasCoral)); + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL3Left) + .when(leftAlignButton.and(() -> selectedLevel == 3).and(robot.claw::hasCoral)); + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL3Right) + .when(rightAlignButton.and(() -> selectedLevel == 3).and(robot.claw::hasCoral)); + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL4Left) + .when(leftAlignButton.and(() -> selectedLevel == 4).and(robot.claw::hasCoral)); + stateMachine + .switchFromAny(stowState, handoff) + .to(scoreL4Right) + .when(rightAlignButton.and(() -> selectedLevel == 4).and(robot.claw::hasCoral)); + + stateMachine + .switchFromAny( + scoreL2Left, scoreL2Right, scoreL3Left, scoreL3Right, scoreL4Left, scoreL4Right) + .to(handoff) + .whenComplete(); + + stateMachine + .switchFromAny( + scoreL2Left, scoreL2Right, scoreL3Left, scoreL3Right, scoreL4Left, scoreL4Right) + .to(stowState) + .when(leftAlignButton.or(rightAlignButton).negate()); + + stateMachine.switchFromAny().to(stowState).when(stowButton); + + // RobotModeTriggers.disabled().onFalse(stateMachine); + Scheduler.getDefault().schedule(stateMachine); + // RobotModeTriggers.teleop().onTrue(stateMachine); + + Scheduler.getDefault() + .addPeriodic( + () -> { + // Telemetry.log("State Machine State", stateMachine.) + + var commands = new ArrayList<>(Scheduler.getDefault().getRunningCommands()); + for (int i = 0; i < commands.size(); i++) { + Telemetry.log("ActiveCommands/" + i, commands.get(i).name()); + } + Telemetry.log("StateMachine", stateMachine); + Telemetry.log("Selected Coral Level", selectedLevel); + }); + + Scheduler.getDefault() + .addEventListener( + (event) -> { + if (event instanceof SchedulerEvent.Canceled) { + System.out.println( + "CANCELLED: " + ((SchedulerEvent.Canceled) event).command().name()); + } else if (event instanceof SchedulerEvent.Interrupted) { + System.out.println( + "INTERRUPTED: " + + ((SchedulerEvent.Interrupted) event).command().name() + + " by " + + ((SchedulerEvent.Interrupted) event).interrupter().name()); + } else if (event instanceof SchedulerEvent.Completed) { + System.out.println( + "COMPLETED: " + ((SchedulerEvent.Completed) event).command().name()); + } + }); + } + + private NeedsNameBuilderStage createScoreCommand( + List poses, + Superstructure.Positions prepPosition, + Superstructure.Positions scorePosition) { + return Command.noRequirements( + (coro) -> { + coro.fork( + new Drive.AutoAlignCommand( + robot.drive, + () -> + robot + .drive + .getPose() + .nearest(poses) + .transformBy(Poses.REEF_PREALIGN_TRANSFORM)) + .withRunningContinuously(true)); + coro.await(robot.superstructure.setPosition(prepPosition)); + + Drive.AutoAlignCommand finalAlign = + new Drive.AutoAlignCommand(robot.drive, () -> robot.drive.getPose().nearest(poses)) + .withRunningContinuously(true); + coro.fork(finalAlign); + coro.waitUntil(finalAlign::atPosition); + + coro.fork(robot.superstructure.setPosition(scorePosition)); + coro.wait(Seconds.of(0.25)); + coro.await(robot.claw.setVoltage(-6)); + robot.coralSim.attemptScore(); + }); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullScoringTeleop.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullScoringTeleop.java new file mode 100644 index 00000000..233863ff --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/FullScoringTeleop.java @@ -0,0 +1,87 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import static org.wpilib.units.Units.Seconds; + +import first.robot.Poses; +import first.robot.Robot; +import first.robot.mechanisms.Drive; +import first.robot.mechanisms.Superstructure; +import java.util.List; +import org.wpilib.command3.Command; +import org.wpilib.command3.NeedsNameBuilderStage; +import org.wpilib.command3.button.CommandXboxController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Teleop; + +@Teleop +public class FullScoringTeleop extends PeriodicOpMode { + private final Robot robot; + + private final CommandXboxController controller; + + public FullScoringTeleop(Robot robot) { + this.robot = robot; + + controller = new CommandXboxController(0); + + robot.drive.setDefaultCommand(robot.drive.getDriveCommand(controller)); + + controller + .leftBumper() + .whileTrue( + createScoreCommand(Poses.BLUE_REEF_LEFT_POSES).named("Autoalign and score (left)")); + controller + .rightBumper() + .whileTrue( + createScoreCommand(Poses.BLUE_REEF_RIGHT_POSES).named("Autoalign and score (right)")); + + controller + .a() + .onTrue( + Command.noRequirements( + (coro) -> { + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF_PREP)); + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF)); + robot.coralSim.attemptHandoff(); + coro.await( + robot.superstructure.setPosition(Superstructure.Positions.HANDOFF_PREP)); + }) + .named("Handoff Coral")); + } + + private NeedsNameBuilderStage createScoreCommand(List poses) { + return Command.noRequirements( + (coro) -> { + coro.fork( + new Drive.AutoAlignCommand( + robot.drive, + () -> + robot + .drive + .getPose() + .nearest(poses) + .transformBy(Poses.REEF_PREALIGN_TRANSFORM)) + .withRunningContinuously(true)); + coro.await(robot.superstructure.setPosition(Superstructure.Positions.L4_PREP)); + + Drive.AutoAlignCommand finalAlign = + new Drive.AutoAlignCommand(robot.drive, () -> robot.drive.getPose().nearest(poses)) + .withRunningContinuously(true); + coro.fork(finalAlign); + coro.waitUntil(finalAlign::atPosition); + + coro.fork(robot.superstructure.setPosition(Superstructure.Positions.L4_SCORE)); + coro.wait(Seconds.of(0.25)); + coro.await(robot.claw.setVoltage(-6)); + robot.coralSim.attemptScore(); + }); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/IntakeTest.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/IntakeTest.java new file mode 100644 index 00000000..fabac4be --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/IntakeTest.java @@ -0,0 +1,54 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Robot; +import org.wpilib.command3.Command; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.math.util.Units; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Utility; + +@Utility +public class IntakeTest extends PeriodicOpMode { + private final Robot robot; + + private final CommandGamepad controller; + + public IntakeTest(Robot robot) { + this.robot = robot; + + controller = new CommandGamepad(0); + + controller + .button(0) + .onTrue( + Command.noRequirements( + coro -> { + coro.await(robot.intake.setRollerVoltage(12)); + coro.await(robot.intake.setPivotPosition(Units.degreesToRotations(0))); + }) + .named("Deploy intake")); + controller + .button(1) + .onTrue( + Command.noRequirements( + coro -> { + coro.await(robot.intake.setRollerVoltage(0)); + coro.await(robot.intake.setPivotPosition(Units.degreesToRotations(90))); + }) + .named("Retract intake")); + controller + .button(2) + .onTrue( + Command.noRequirements( + coro -> { + coro.await(robot.intake.setRollerVoltage(0)); + coro.await(robot.intake.setPivotPosition(Units.degreesToRotations(45))); + }) + .named("Retract intake")); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/MyAuto.java b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/MyAuto.java new file mode 100644 index 00000000..c16666cc --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/opmode/MyAuto.java @@ -0,0 +1,31 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.opmode; + +import first.robot.Robot; +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.PeriodicOpMode; + +@Autonomous(name = "My Auto", group = "Group 1") +public class MyAuto extends PeriodicOpMode { + private final Robot robot; + + /** The Robot instance is passed into the opmode via the constructor. */ + public MyAuto(Robot robot) { + this.robot = robot; + } + + /* + * This method runs periodically, using the same period as the Robot instance. + * + * Additional periodic methods may be configured with addPeriodic(), + * which can have periods that differ from the main Robot instance. + */ + @Override + public void periodic() { + // Put custom auto code here + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ArmSim.java b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ArmSim.java new file mode 100644 index 00000000..d23c1414 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ArmSim.java @@ -0,0 +1,59 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.simulation; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.SingleJointedArmSim; + +public class ArmSim { + + private final TalonFX motor; + + private final TalonFXSimState motorSim; + + private final SingleJointedArmSim sim; + + private final double GEAR_RATIO = + (56.0 / 16.0) * (84.0 / 10.0); // Two stage, 56:16, 84:10 reductions + private final double J = SingleJointedArmSim.estimateMOI(Units.inchesToMeters(15.2), 3.6); + private final double ARM_LENGTH = Units.inchesToMeters(15.2); + private final double MIN_ANGLE = 0; + private final double MAX_ANGLE = 1.5 * Math.PI * 2; + + public ArmSim(TalonFX motor) { + this.motor = motor; + + motorSim = motor.getSimState(); + + motorSim.Orientation = ChassisReference.Clockwise_Positive; + + sim = + new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + GEAR_RATIO, + J, + ARM_LENGTH, + MIN_ANGLE, + MAX_ANGLE, + true, + -Math.PI / 2); + } + + public void periodic() { + sim.setInputVoltage(motorSim.getMotorVoltage()); + sim.update(0.02); + + double motorPosition = sim.getAngle() / (Math.PI * 2) * GEAR_RATIO; + motorSim.setRawRotorPosition(motorPosition); + + double motorVelocity = sim.getVelocity() / (Math.PI * 2) * GEAR_RATIO; + motorSim.setRotorVelocity(motorVelocity); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ClawSim.java b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ClawSim.java new file mode 100644 index 00000000..5265cc43 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ClawSim.java @@ -0,0 +1,46 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.simulation; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.FlywheelSim; + +public class ClawSim { + + private final TalonFX motor; + + private final TalonFXSimState motorSim; + + private final FlywheelSim sim; + + private final double GEAR_RATIO = (18.0 / 12.0) * (54.0 / 18.0) * (22.0 / 18.0) * (22.0 / 18.0); + private final double J = 0.00001; + + public ClawSim(TalonFX motor) { + this.motor = motor; + + motorSim = motor.getSimState(); + + motorSim.Orientation = ChassisReference.Clockwise_Positive; + + sim = + new FlywheelSim( + Models.flywheelFromPhysicalConstants(DCMotor.getKrakenX44(1), J, GEAR_RATIO), + DCMotor.getKrakenX44(1)); + } + + public void periodic() { + sim.setInputVoltage(motorSim.getMotorVoltage()); + sim.update(0.02); + + motorSim.setRotorVelocity(Units.radiansToRotations(sim.getAngularVelocity()) * GEAR_RATIO); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/CoralSim.java b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/CoralSim.java new file mode 100644 index 00000000..14a915a4 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/CoralSim.java @@ -0,0 +1,242 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.simulation; + +import first.robot.FieldConstants; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.wpilib.math.geometry.*; +import org.wpilib.math.shape.Rectangle2d; +import org.wpilib.math.util.Units; +import org.wpilib.simulation.DIOSim; +import org.wpilib.system.Timer; +import org.wpilib.telemetry.Telemetry; + +public class CoralSim { + + private final Supplier drivePoseSupplier; + private final DoubleSupplier elevatorHeightSupplier; + private final DoubleSupplier armAngleSupplier; + + private final DIOSim clawSensor; + private final DIOSim indexerSensor; + + private final Transform3d INTAKE_CORAL_TRANSFORM = + new Transform3d(0.5, 0, 0.15, Rotation3d.fromDegrees(0, 35, 45)); + private final Transform3d INDEXER_CORAL_TRANSFORM = + new Transform3d(0, 0, 0.201295, new Rotation3d()); + + private CoralState coralState = CoralState.GROUND; + private final Timer coralIntakeTimer = new Timer(); + + private boolean[][] redScoredCoral = new boolean[12][3]; + private boolean[][] blueScoredCoral = new boolean[12][3]; + + private Pose3d[] groundCoralPoses = + new Pose3d[] { + new Pose3d(7, 3, Units.inchesToMeters(4.5 / 2), Rotation3d.fromDegrees(0, 0, -35)), + new Pose3d(7, -3, Units.inchesToMeters(4.5 / 2), Rotation3d.fromDegrees(0, 0, 35)), + new Pose3d(-7, -3, Units.inchesToMeters(4.5 / 2), Rotation3d.fromDegrees(0, 0, -35)), + new Pose3d(-7, 3, Units.inchesToMeters(4.5 / 2), Rotation3d.fromDegrees(0, 0, 35)), + }; + private Transform2d INTAKE_DETECTION_TRANSFORM = new Transform2d(0.6, 0, Rotation2d.ZERO); + private double INTAKE_DETECTION_THRESHOLD = 0.3; + private double INTAKE_TIME_THRESHOLD = 0.5; + // private double INTAKE_TIME_THRESHOLD = 100; + + private Transform3d HANDOFF_DETECTION_TRANSFORM = + new Transform3d(0, 0, 0.201295, new Rotation3d()); + private double HANDOFF_DETECTION_THRESHOLD = 0.05; + + public CoralSim( + Supplier drivePoseSupplier, + DoubleSupplier elevatorHeightSupplier, + DoubleSupplier armAngleSupplier) { + this.drivePoseSupplier = drivePoseSupplier; + this.elevatorHeightSupplier = elevatorHeightSupplier; + this.armAngleSupplier = armAngleSupplier; + + indexerSensor = new DIOSim(0); + clawSensor = new DIOSim(1); + + indexerSensor.setIsInput(true); + clawSensor.setIsInput(true); + + // Arrays.fill(redScoredCoral, new boolean[]{true, true, true}); + // Arrays.fill(blueScoredCoral, new boolean[]{true, true, true}); + } + + public void periodic() { + List coralPoses = new ArrayList<>(); + + switch (coralState) { + case GROUND: + coralPoses.addAll(Arrays.stream(groundCoralPoses).toList()); + + indexerSensor.setValue(false); + clawSensor.setValue(false); + + var intakeRect = + new Rectangle2d( + drivePoseSupplier.get().transformBy(INTAKE_DETECTION_TRANSFORM), 0.2, 0.7); + + Telemetry.log( + "intake detection point", + new Pose3d(drivePoseSupplier.get().transformBy(INTAKE_DETECTION_TRANSFORM))); + + for (var pose : groundCoralPoses) { + if (intakeRect.contains(pose.getTranslation().toTranslation2d())) { + coralState = CoralState.INTAKE; + coralIntakeTimer.restart(); + } + } + break; + case INTAKE: + var intakePose = new Pose3d(drivePoseSupplier.get()).transformBy(INTAKE_CORAL_TRANSFORM); + var indexerPose = new Pose3d(drivePoseSupplier.get()).transformBy(INDEXER_CORAL_TRANSFORM); + coralPoses.add( + intakePose.interpolate(indexerPose, coralIntakeTimer.get() / INTAKE_TIME_THRESHOLD)); + + indexerSensor.setValue(false); + clawSensor.setValue(false); + + if (coralIntakeTimer.hasElapsed(INTAKE_TIME_THRESHOLD)) { + coralState = CoralState.INDEXER; + } + break; + case INDEXER: + coralPoses.add(new Pose3d(drivePoseSupplier.get()).transformBy(INDEXER_CORAL_TRANSFORM)); + + indexerSensor.setValue(true); + clawSensor.setValue(false); + break; + case CLAW: + coralPoses.add(getClawCoralPose()); + + indexerSensor.setValue(false); + clawSensor.setValue(true); + break; + } + + for (int branch = 0; branch < 12; branch++) { + for (int level = 2; level <= 4; level++) { + if (redScoredCoral[branch][level - 2]) { + coralPoses.add(getCoralPose(true, branch, level)); + } + if (blueScoredCoral[branch][level - 2]) { + coralPoses.add(getCoralPose(false, branch, level)); + } + } + } + + Telemetry.log("Coral", coralPoses, Pose3d.class); + } + + private double CORAL_SCORE_DISTANCE_THRESHOLD = 0.25; + + public void attemptScore() { + boolean red = drivePoseSupplier.get().getX() < 0; + topLoop: + for (int branch = 0; branch < 12; branch++) { + for (int level = 2; level <= 4; level++) { + if (getCoralPose(red, branch, level) + .getTranslation() + .getDistance(getClawCoralPose().getTranslation()) + < CORAL_SCORE_DISTANCE_THRESHOLD) { + if (red) { + redScoredCoral[branch][level - 2] = true; + } else { + blueScoredCoral[branch][level - 2] = true; + } + + coralState = CoralState.GROUND; + + break topLoop; + } + } + } + } + + public void attemptHandoff() { + if (coralState == CoralState.INDEXER) { + Telemetry.log( + "dist", + new Pose3d(drivePoseSupplier.get()) + .transformBy(HANDOFF_DETECTION_TRANSFORM) + .getTranslation() + .getDistance(getClawCoralPose().getTranslation())); + if (new Pose3d(drivePoseSupplier.get()) + .transformBy(HANDOFF_DETECTION_TRANSFORM) + .getTranslation() + .getDistance(getClawCoralPose().getTranslation()) + < HANDOFF_DETECTION_THRESHOLD) { + coralState = CoralState.CLAW; + } + } + } + + private enum CoralState { + GROUND, + INTAKE, + INDEXER, + CLAW + } + + private final double CORAL_Y_OFFSET = 0.164309; + private final Transform3d CORAL_LEFT_TRANSFORM = + new Transform3d(0, -CORAL_Y_OFFSET, 0, new Rotation3d()); + private final Transform3d CORAL_RIGHT_TRANSFORM = + new Transform3d(0, CORAL_Y_OFFSET, 0, new Rotation3d()); + private final Transform3d CORAL_L2_TRANSFORM = + new Transform3d(0.71, 0, 0.725, new Rotation3d(0, Units.degreesToRadians(-35), 0)); + private final Transform3d CORAL_L3_TRANSFORM = + new Transform3d(0.71, 0, 1.13, new Rotation3d(0, Units.degreesToRadians(-35), 0)); + private final Transform3d CORAL_L4_TRANSFORM = + new Transform3d(0.76, 0, 1.73, new Rotation3d(0, Units.degreesToRadians(90), 0)); + + private Pose3d getCoralPose(boolean red, int branch, int level) { + var reefCenter = red ? FieldConstants.RED_REEF_CENTER : FieldConstants.BLUE_REEF_CENTER; + var reefCenterPointed = + new Pose2d( + reefCenter, + Rotation2d.fromDegrees(60) + .times(branch / 2) + .plus(red ? Rotation2d.k180deg : Rotation2d.ZERO)); + var shiftedCenter = + new Pose3d(reefCenterPointed) + .transformBy((branch % 2 == 0) ? CORAL_LEFT_TRANSFORM : CORAL_RIGHT_TRANSFORM); + var branchTransform = + switch (level) { + case 2 -> CORAL_L2_TRANSFORM; + case 3 -> CORAL_L3_TRANSFORM; + case 4 -> CORAL_L4_TRANSFORM; + default -> CORAL_L2_TRANSFORM; + }; + Telemetry.log("reefCenter", new Pose2d(reefCenter, Rotation2d.ZERO)); + return shiftedCenter.transformBy(branchTransform); + } + + private Pose3d getClawCoralPose() { + var elevatorTransform = + new Transform3d(0, 0, elevatorHeightSupplier.getAsDouble() + 0.514671, new Rotation3d()); + var armTransform = + new Transform3d( + 0, + 0, + 0, + new Rotation3d(0, Units.rotationsToRadians(armAngleSupplier.getAsDouble()), 0)); + var clawCoralTransform = + new Transform3d(0.68036 - 0.02, 0, 0, new Rotation3d(0, Math.PI / 2, 0)); + + return new Pose3d(drivePoseSupplier.get()) + .transformBy(elevatorTransform) + .transformBy(armTransform) + .transformBy(clawCoralTransform); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ElevatorSim.java b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ElevatorSim.java new file mode 100644 index 00000000..a0bbd3e5 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/ElevatorSim.java @@ -0,0 +1,64 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.simulation; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import org.wpilib.math.system.DCMotor; + +public class ElevatorSim { + + private TalonFX leader; + private TalonFX follower; + + private TalonFXSimState leaderSim; + private TalonFXSimState followerSim; + + private org.wpilib.simulation.ElevatorSim sim; + + private final double GEAR_RATIO = + (44.0 / 14.0) * (42.0 / 22.0); // Two stage, 14:44, 22:44 reductions + private final double PULLEY_CIRCUMFERENCE = + 24 * 0.005; // Convert 24t pulley to circumference, in m + private final double PULLEY_RADIUS = PULLEY_CIRCUMFERENCE / (2 * Math.PI); + private final double CARRIAGE_MASS = 11.24; // kg + private final double MAX_HEIGHT = 1.347; // m + + public ElevatorSim(TalonFX leader, TalonFX follower) { + this.leader = leader; + this.follower = follower; + + leaderSim = leader.getSimState(); + followerSim = follower.getSimState(); + + leaderSim.Orientation = ChassisReference.Clockwise_Positive; + + sim = + new org.wpilib.simulation.ElevatorSim( + DCMotor.getKrakenX60(2), + GEAR_RATIO, + CARRIAGE_MASS, + PULLEY_RADIUS, + 0, + MAX_HEIGHT, + true, + 0); + } + + public void periodic() { + sim.setInputVoltage(leaderSim.getMotorVoltage()); + sim.update(0.02); + + double motorPosition = sim.getPosition() / PULLEY_CIRCUMFERENCE * GEAR_RATIO; + leaderSim.setRawRotorPosition(motorPosition); + followerSim.setRawRotorPosition(motorPosition); + + double motorVelocity = sim.getVelocity() / PULLEY_CIRCUMFERENCE * GEAR_RATIO; + leaderSim.setRotorVelocity(motorVelocity); + followerSim.setRotorVelocity(motorVelocity); + } +} diff --git a/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/RobotVisualization.java b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/RobotVisualization.java new file mode 100644 index 00000000..da7b5946 --- /dev/null +++ b/examples/stage2/templates/ctre/src/main/java/first/robot/simulation/RobotVisualization.java @@ -0,0 +1,72 @@ +/* + * Copyright 2026 FRCSoftware + * + * SPDX-License-Identifier: BSD-3-Clause + */ +package first.robot.simulation; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; +import org.wpilib.telemetry.Telemetry; + +public class RobotVisualization { + + private final Supplier drivePoseSupplier; + private final DoubleSupplier elevatorHeightSupplier; + private final DoubleSupplier armAngleSupplier; + private final DoubleSupplier intakeAngleSupplier; + + private final Pose3d ELEVATOR_STARTING_POSE = new Pose3d(); + private final double ELEVATOR_CARRIAGE_TRAVEL = 0.651510; + + private final Transform3d ARM_BASE_TRANSFORM = + new Transform3d(0, 0, 0.4191 + 0.095570802, new Rotation3d(0, -Math.PI / 2, 0)); + + private final Pose3d INTAKE_BASE_POSE = new Pose3d(0.3210, 0, 0.29837, new Rotation3d()); + + public RobotVisualization( + Supplier drivePoseSupplier, + DoubleSupplier elevatorHeightSupplier, + DoubleSupplier armAngleSupplier, + DoubleSupplier intakeAngleSupplier) { + this.drivePoseSupplier = drivePoseSupplier; + this.elevatorHeightSupplier = elevatorHeightSupplier; + this.armAngleSupplier = armAngleSupplier; + this.intakeAngleSupplier = intakeAngleSupplier; + } + + public void periodic() { + double elevatorHeight = Math.clamp(elevatorHeightSupplier.getAsDouble(), 0, 1.347); + double intakeAngle = + Math.clamp(intakeAngleSupplier.getAsDouble(), 0, Units.degreesToRadians(138.469)); + + double firstStageHeight = + elevatorHeight < ELEVATOR_CARRIAGE_TRAVEL ? 0 : elevatorHeight - ELEVATOR_CARRIAGE_TRAVEL; + Transform3d firstStageTransform = new Transform3d(0, 0, firstStageHeight, new Rotation3d()); + Pose3d firstStagePose = ELEVATOR_STARTING_POSE.transformBy(firstStageTransform); + + Transform3d carriageTransform = new Transform3d(0, 0, elevatorHeight, new Rotation3d()); + Pose3d carriagePose = ELEVATOR_STARTING_POSE.transformBy(carriageTransform); + + Transform3d armTransform = + new Transform3d( + 0, + 0, + 0, + new Rotation3d(0, Units.rotationsToRadians(armAngleSupplier.getAsDouble()), 0)); + Pose3d armPose = carriagePose.transformBy(ARM_BASE_TRANSFORM).transformBy(armTransform); + + Transform3d intakeTransform = new Transform3d(0, 0, 0, new Rotation3d(0, -intakeAngle, 0)); + Pose3d intakePose = INTAKE_BASE_POSE.transformBy(intakeTransform); + + Telemetry.log("Mechanisms", new Pose3d[] {firstStagePose, carriagePose, armPose, intakePose}); + + Telemetry.log( + "Blank pose3ds", new Pose3d[] {new Pose3d(), new Pose3d(), new Pose3d(), new Pose3d()}); + } +} diff --git a/examples/stage2/templates/ctre/vendordeps/CommandsV3.json b/examples/stage2/templates/ctre/vendordeps/CommandsV3.json new file mode 100644 index 00000000..7ce12fd7 --- /dev/null +++ b/examples/stage2/templates/ctre/vendordeps/CommandsV3.json @@ -0,0 +1,25 @@ +{ + "fileName": "CommandsV3.json", + "name": "Commands v3", + "version": "1.0.0", + "uuid": "4decdc05-a056-46cf-9561-39449bbb0130", + "wpilibYear": "2027_alpha7", + "mavenUrls": [], + "jsonUrl": "", + "conflictsWith": [ + { + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", + "offlineFileName": "CommandsV2.json" + } + ], + "javaDependencies": [ + { + "groupId": "org.wpilib", + "artifactId": "commandsv3-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/examples/stage2/templates/ctre/vendordeps/Phoenix6-26.70.0-alpha-2.json b/examples/stage2/templates/ctre/vendordeps/Phoenix6-26.70.0-alpha-2.json new file mode 100644 index 00000000..af2ed5f8 --- /dev/null +++ b/examples/stage2/templates/ctre/vendordeps/Phoenix6-26.70.0-alpha-2.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.70.0-alpha-2.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.70.0-alpha-2", + "wpilibYear": "2027_alpha7", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2027-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2027-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.70.0-alpha-2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.70.0-alpha-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.70.0-alpha-2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.70.0-alpha-2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxsystemcore" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.70.0-alpha-2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.70.0-alpha-2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.70.0-alpha-2", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file