diff --git a/.github/workflows/build-all-parallel.yml b/.github/workflows/build-all-parallel.yml index f821cb7f..781cef8f 100644 --- a/.github/workflows/build-all-parallel.yml +++ b/.github/workflows/build-all-parallel.yml @@ -45,8 +45,6 @@ jobs: directory: 'cpp/PositionClosedLoop' - project-name: 'Simulation' directory: 'cpp/Simulation' - - project-name: 'SwerveWithChoreo' - directory: 'cpp/SwerveWithChoreo' - project-name: 'SwerveWithPathPlanner' directory: 'cpp/SwerveWithPathPlanner' - project-name: 'VelocityClosedLoop' diff --git a/cpp/SwerveWithChoreo/.gitignore b/cpp/SwerveWithChoreo/.gitignore deleted file mode 100644 index 375359c2..00000000 --- a/cpp/SwerveWithChoreo/.gitignore +++ /dev/null @@ -1,184 +0,0 @@ -# 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 diff --git a/cpp/SwerveWithChoreo/.vscode/launch.json b/cpp/SwerveWithChoreo/.vscode/launch.json deleted file mode 100644 index c9c9713d..00000000 --- a/cpp/SwerveWithChoreo/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/cpp/SwerveWithChoreo/.vscode/settings.json b/cpp/SwerveWithChoreo/.vscode/settings.json deleted file mode 100644 index 5e795ffd..00000000 --- a/cpp/SwerveWithChoreo/.vscode/settings.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "disabled", - "java.import.gradle.enabled": false, - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "C_Cpp.default.configurationProvider": "vscode-wpilib" -} diff --git a/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json b/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json deleted file mode 100644 index 4b0bb1c1..00000000 --- a/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": true, - "currentLanguage": "cpp", - "projectYear": "2025beta", - "teamNumber": 7762 -} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/WPILib-License.md b/cpp/SwerveWithChoreo/WPILib-License.md deleted file mode 100644 index 645e5425..00000000 --- a/cpp/SwerveWithChoreo/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2024 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/cpp/SwerveWithChoreo/build.gradle b/cpp/SwerveWithChoreo/build.gradle deleted file mode 100644 index 8e31da3f..00000000 --- a/cpp/SwerveWithChoreo/build.gradle +++ /dev/null @@ -1,101 +0,0 @@ -plugins { - id "cpp" - id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" -} - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no - // longer exist in deploy directory on roboRIO - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Set to true to run simulation in debug mode -wpi.cpp.debugSimulation = false - -// Default enable simgui -wpi.sim.addGui().defaultEnabled = true -// Enable DS but not by default -wpi.sim.addDriverstation() - -model { - components { - frcUserProgram(NativeExecutableSpec) { - targetPlatform wpi.platforms.roborio - if (includeDesktopSupport) { - targetPlatform wpi.platforms.desktop - } - - sources.cpp { - source { - srcDir 'src/main/cpp' - include '**/*.cpp', '**/*.cc' - } - exportedHeaders { - srcDir 'src/main/include' - } - } - - // Set deploy task to deploy this component - deployArtifact.component = it - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - // Enable simulation for this component - wpi.sim.enable(it) - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - } - } - testSuites { - frcUserProgramTest(GoogleTestTestSuiteSpec) { - testing $.components.frcUserProgram - - sources.cpp { - source { - srcDir 'src/test/cpp' - include '**/*.cpp' - } - } - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - wpi.cpp.deps.googleTest(it) - } - } -} diff --git a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar b/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar deleted file mode 100644 index a4b76b95..00000000 Binary files a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar and /dev/null differ diff --git a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties b/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties deleted file mode 100644 index 34bd9ce9..00000000 --- a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties +++ /dev/null @@ -1,7 +0,0 @@ -distributionBase=GRADLE_USER_HOME -distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip -networkTimeout=10000 -validateDistributionUrl=true -zipStoreBase=GRADLE_USER_HOME -zipStorePath=permwrapper/dists diff --git a/cpp/SwerveWithChoreo/gradlew b/cpp/SwerveWithChoreo/gradlew deleted file mode 100644 index f5feea6d..00000000 --- a/cpp/SwerveWithChoreo/gradlew +++ /dev/null @@ -1,252 +0,0 @@ -#!/bin/sh - -# -# Copyright © 2015-2021 the original authors. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# 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/HEAD/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 -' "$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 - -CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar - - -# Determine the Java command to use to start the JVM. -if [ -n "$JAVA_HOME" ] ; then - if [ -x "$JAVA_HOME/jre/sh/java" ] ; then - # IBM's JDK on AIX uses strange locations for the executables - JAVACMD=$JAVA_HOME/jre/sh/java - else - JAVACMD=$JAVA_HOME/bin/java - fi - if [ ! -x "$JAVACMD" ] ; then - die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME - -Please set the JAVA_HOME variable in your environment to match the -location of your Java installation." - fi -else - JAVACMD=java - 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" ) - CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) - - JAVACMD=$( cygpath --unix "$JAVACMD" ) - - # Now convert the arguments - kludge to limit ourselves to /bin/sh - for arg do - if - case $arg in #( - -*) false ;; # don't mess with options #( - /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath - [ -e "$t" ] ;; #( - *) false ;; - esac - then - arg=$( cygpath --path --ignore --mixed "$arg" ) - fi - # Roll the args list around exactly as many times as the number of - # args, so each arg winds up back in the position where it started, but - # possibly modified. - # - # NB: a `for` loop captures its iteration list before it begins, so - # changing the positional parameters here affects neither the number of - # iterations, nor the values presented in `arg`. - shift # remove old arg - set -- "$@" "$arg" # push replacement arg - done -fi - - -# 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, 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" \ - -classpath "$CLASSPATH" \ - org.gradle.wrapper.GradleWrapperMain \ - "$@" - -# 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/cpp/SwerveWithChoreo/gradlew.bat b/cpp/SwerveWithChoreo/gradlew.bat deleted file mode 100644 index 9d21a218..00000000 --- a/cpp/SwerveWithChoreo/gradlew.bat +++ /dev/null @@ -1,94 +0,0 @@ -@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 - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* - -:end -@rem End local scope for the variables with windows NT shell -if %ERRORLEVEL% 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/cpp/SwerveWithChoreo/settings.gradle b/cpp/SwerveWithChoreo/settings.gradle deleted file mode 100644 index 969c7b09..00000000 --- a/cpp/SwerveWithChoreo/settings.gradle +++ /dev/null @@ -1,30 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2025' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} - -Properties props = System.getProperties(); -props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp b/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp deleted file mode 100644 index a045699f..00000000 --- a/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "AutoRoutines.h" - -frc2::CommandPtr AutoRoutines::SimplePathAuto(choreo::AutoFactory &factory) -{ - simplePathRoutine = factory.NewLoop("SimplePath Auto"); - simplePathTraj = factory.Trajectory("SimplePath", *simplePathRoutine); - - simplePathRoutine->Enabled().OnTrue( - m_drivetrain.RunOnce([this] { - auto const pose = simplePathTraj.GetInitialPose(); - if (pose) { - m_drivetrain.ResetPose(*pose); - } else { - simplePathRoutine->Kill(); - } - }) - .AndThen(simplePathTraj.Cmd()) - ); - return simplePathRoutine->Cmd(); -} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp b/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp deleted file mode 100644 index 84d22a06..00000000 --- a/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "Robot.h" -#include "LimelightHelpers.h" - -#include - -Robot::Robot() {} - -void Robot::RobotPeriodic() { - frc2::CommandScheduler::GetInstance().Run(); - - /* update auto routine selection */ - m_container.autoChooser.Update(); - - /* - * This example of adding Limelight is very simple and may not be sufficient for on-field use. - * Users typically need to provide a standard deviation that scales with the distance to target - * and changes with number of tags available. - * - * This example is sufficient to show that vision integration is possible, though exact implementation - * of how to use vision should be tuned per-robot and to the team's specification. - */ - if (kUseLimelight) { - auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue("limelight"); - if (llMeasurement) { - m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, utils::FPGAToCurrentTime(llMeasurement->timestampSeconds)); - } - } -} - -void Robot::DisabledInit() {} - -void Robot::DisabledPeriodic() {} - -void Robot::DisabledExit() {} - -void Robot::AutonomousInit() { - m_autonomousCommand = m_container.GetAutonomousCommand(); - - if (m_autonomousCommand) { - m_autonomousCommand->Schedule(); - } -} - -void Robot::AutonomousPeriodic() {} - -void Robot::AutonomousExit() {} - -void Robot::TeleopInit() { - if (m_autonomousCommand) { - m_autonomousCommand->Cancel(); - } -} - -void Robot::TeleopPeriodic() {} - -void Robot::TeleopExit() {} - -void Robot::TestInit() { - frc2::CommandScheduler::GetInstance().CancelAll(); -} - -void Robot::TestPeriodic() {} - -void Robot::TestExit() {} - -#ifndef RUNNING_FRC_TESTS -int main() { - return frc::StartRobot(); -} -#endif diff --git a/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp b/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp deleted file mode 100644 index 2dffade8..00000000 --- a/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "RobotContainer.h" - -#include -#include - -RobotContainer::RobotContainer() : - autoRoutines{drivetrain}, - autoFactory{ - [this] { return drivetrain.GetState().Pose; }, - [this](auto &&pose, auto &&sample) { drivetrain.FollowPath(pose, sample); }, - [] { - auto const alliance = frc::DriverStation::GetAlliance().value_or(frc::DriverStation::Alliance::kBlue); - return alliance == frc::DriverStation::Alliance::kRed; - }, - {&drivetrain}, - std::nullopt - }, - autoChooser{autoFactory, ""} -{ - autoChooser.AddAutoRoutine("SimplePath", [this] { return autoRoutines.SimplePathAuto(autoFactory); }); - - ConfigureBindings(); -} - -void RobotContainer::ConfigureBindings() -{ - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.SetDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.ApplyRequest([this]() -> auto&& { - return drive.WithVelocityX(-joystick.GetLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .WithVelocityY(-joystick.GetLeftX() * MaxSpeed) // Drive left with negative X (left) - .WithRotationalRate(-joystick.GetRightX() * MaxAngularRate); // Drive counterclockwise with negative X (left) - }) - ); - - joystick.A().WhileTrue(drivetrain.ApplyRequest([this]() -> auto&& { return brake; })); - joystick.B().WhileTrue(drivetrain.ApplyRequest([this]() -> auto&& { - return point.WithModuleDirection(frc::Rotation2d{-joystick.GetLeftY(), -joystick.GetLeftX()}); - })); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - (joystick.Back() && joystick.Y()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kForward)); - (joystick.Back() && joystick.X()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kReverse)); - (joystick.Start() && joystick.Y()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kForward)); - (joystick.Start() && joystick.X()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); - - // reset the field-centric heading on left bumper press - joystick.LeftBumper().OnTrue(drivetrain.RunOnce([this] { drivetrain.SeedFieldCentric(); })); - - drivetrain.RegisterTelemetry([this](auto const &state) { logger.Telemeterize(state); }); -} - -frc2::CommandPtr RobotContainer::GetAutonomousCommand() -{ - return autoChooser.GetSelectedAutoRoutine(); -} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp b/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp deleted file mode 100644 index ccc2678e..00000000 --- a/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "Telemetry.h" -#include - -using namespace ctre::phoenix6; - -void Telemetry::Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state) -{ - /* Telemeterize the swerve drive state */ - drivePose.Set(state.Pose); - driveSpeeds.Set(state.Speeds); - driveModuleStates.Set(state.ModuleStates); - driveModuleTargets.Set(state.ModuleTargets); - driveModulePositions.Set(state.ModulePositions); - driveTimestamp.Set(state.Timestamp.value()); - driveOdometryFrequency.Set(1.0 / state.OdometryPeriod.value()); - - /* Also write to log file */ - std::array moduleStatesArray{}; - std::array moduleTargetsArray{}; - for (int i = 0; i < 4; ++i) { - moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.Radians().value(); - moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speed.value(); - moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.Radians().value(); - moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speed.value(); - } - SignalLogger::WriteDoubleArray("DriveState/Pose", {state.Pose.X().value(), state.Pose.Y().value(), state.Pose.Rotation().Degrees().value()}); - SignalLogger::WriteDoubleArray("DriveState/ModuleStates", moduleStatesArray); - SignalLogger::WriteDoubleArray("DriveState/ModuleTargets", moduleTargetsArray); - SignalLogger::WriteValue("DriveState/OdometryPeriod", state.OdometryPeriod); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.Set("Field2d"); - fieldPub.Set(std::array{ - state.Pose.X().value(), - state.Pose.Y().value(), - state.Pose.Rotation().Degrees().value() - }); - - /* Telemeterize the module states to a Mechanism2d */ - for (size_t i = 0; i < m_moduleSpeeds.size(); ++i) { - m_moduleDirections[i]->SetAngle(state.ModuleStates[i].angle.Degrees()); - m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees()); - m_moduleSpeeds[i]->SetLength(state.ModuleStates[i].speed / (2 * MaxSpeed)); - - frc::SmartDashboard::PutData("Module " + std::to_string(i), &m_moduleMechanisms[i]); - } -} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp b/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp deleted file mode 100644 index 46339670..00000000 --- a/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp +++ /dev/null @@ -1,68 +0,0 @@ -#include "subsystems/CommandSwerveDrivetrain.h" -#include - -using namespace subsystems; - -void CommandSwerveDrivetrain::FollowPath(frc::Pose2d const &pose, choreo::SwerveSample const &sample) -{ - m_pathThetaController.EnableContinuousInput( - units::radian_t{-0.5_tr}.value(), - units::radian_t{0.5_tr}.value() - ); - - auto targetSpeeds = sample.GetChassisSpeeds(); - targetSpeeds.vx += m_pathXController.Calculate( - pose.X().value(), sample.x.value() - ) * 1_mps; - targetSpeeds.vy += m_pathYController.Calculate( - pose.Y().value(), sample.y.value() - ) * 1_mps; - targetSpeeds.omega += m_pathThetaController.Calculate( - pose.Rotation().Radians().value(), sample.heading.value() - ) * 1_rad_per_s; - - std::vector moduleForcesX(sample.moduleForcesX.begin(), sample.moduleForcesX.end()); - std::vector moduleForcesY(sample.moduleForcesY.begin(), sample.moduleForcesY.end()); - - SetControl( - m_pathApplyFieldSpeeds.WithSpeeds(targetSpeeds) - .WithWheelForceFeedforwardsX(std::move(moduleForcesX)) - .WithWheelForceFeedforwardsY(std::move(moduleForcesY)) - ); -} - -void CommandSwerveDrivetrain::Periodic() -{ - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || frc::DriverStation::IsDisabled()) { - auto const allianceColor = frc::DriverStation::GetAlliance(); - if (allianceColor) { - SetOperatorPerspectiveForward( - *allianceColor == frc::DriverStation::Alliance::kRed - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - } - } -} - -void CommandSwerveDrivetrain::StartSimThread() -{ - m_lastSimTime = utils::GetCurrentTime(); - m_simNotifier = std::make_unique([this] { - units::second_t const currentTime = utils::GetCurrentTime(); - auto const deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - UpdateSimState(deltaTime, frc::RobotController::GetBatteryVoltage()); - }); - m_simNotifier->StartPeriodic(kSimLoopPeriod); -} diff --git a/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj b/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj deleted file mode 100644 index b81e28af..00000000 --- a/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj +++ /dev/null @@ -1,67 +0,0 @@ -{ - "name":"SimplePath", - "version":"v2025.0.0", - "snapshot":{ - "waypoints":[ - {"x":4.062, "y":5.039, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.293, "y":4.495, "heading":1.5707963267948966, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.152, "y":5.296, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}, - {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"4.062 m", "val":4.062}, "y":{"exp":"5.039 m", "val":5.039}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.293 m", "val":5.293}, "y":{"exp":"4.495 m", "val":4.495}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.152 m", "val":6.152}, "y":{"exp":"5.296 m", "val":5.296}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}, - {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "waypoints":[0.0,0.5697684177504896,1.0926420849095964], - "samples":[ - {"t":0.0, "x":4.062, "y":5.039, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":29.226458822189535, "ay":-12.424796441743036, "alpha":25.02487346105948, "fx":[308.66375142931423,118.2281283217828,256.3850513508591,310.9904731878708], "fy":[32.17344906648464,-287.6695818306081,-176.35209560720307,9.16376351299228]}, - {"t":0.03798456118336597, "x":4.08308436031912, "y":5.030036584806144, "heading":0.0, "vx":1.1101542133045843, "vy":-0.4719504406322561, "omega":0.9505588370876052, "ax":29.021758238876433, "ay":-12.07119821332368, "alpha":25.78921823332505, "fx":[306.2166540936873,114.45097851791286,256.04716744650557,310.58880752731835], "fy":[42.657858451708805,-288.738405766401,-176.3894323043004,11.81472414489855]}, - {"t":0.07596912236673194, "x":4.146189767523652, "y":5.003401429740383, "heading":0.03610656030574334, "vx":2.212532964778042, "vy":-0.9304696077227876, "omega":1.9301509749425176, "ax":-5.315878660499058, "ay":5.733966718633051, "alpha":57.24869949145691, "fx":[-206.30006834011385,-257.3487409635288,190.7307809589154,92.07487832611284], "fy":[198.6755951461055,-105.55381254440157,-161.88633874149653,263.8308226452342]}, - {"t":0.11395368355009793, "x":4.226396914961343, "y":4.972194510675685, "heading":0.10942249810658088, "vx":2.0106116465549655, "vy":-0.7126673980754862, "omega":4.104717703443895, "ax":-14.85421527076752, "ay":1.7900763501026262, "alpha":35.169356623635565, "fx":[-221.64642597565933,-217.8085179657918,-19.047496376066427,-46.829462869304706], "fy":[103.27062466367552,-95.72339988244455,-139.34234880126178,192.6924970793308]}, - {"t":0.15193824473346387, "x":4.292053085467321, "y":4.946415537435109, "heading":0.265338398853491, "vx":1.4463807977716068, "vy":-0.6446721334321163, "omega":5.440610281894, "ax":1.87865415324475, "ay":-12.069381361863892, "alpha":-17.57272954129737, "fx":[84.37697252110135,70.1020588457297,-66.58460191226365,-23.983690221020176], "fy":[-137.69418022294582,-36.91127952292192,-71.99502473592722,-163.99296274533037]}, - {"t":0.18992280591682983, "x":4.3483485117377745, "y":4.913220935362567, "heading":0.4719975929809436, "vx":1.5177406513979177, "vy":-1.103122288217213, "omega":4.773117861473843, "ax":6.729116420714396, "ay":-9.426907219822809, "alpha":-22.954990510598343, "fx":[147.43351150590618,110.99549997339378,-54.43224924954307,24.92392766607512], "fy":[-105.79432002739296,22.954753871071947,-63.820368974565866,-174.03805393982847]}, - {"t":0.2279073671001958, "x":4.41085369942441, "y":4.864518621707944, "heading":0.6533023804255137, "vx":1.7733431857905364, "vy":-1.4611992222784869, "omega":3.901182619960436, "ax":6.217212503653662, "ay":-5.363273704941779, "alpha":-17.017852030853437, "fx":[125.1957022037703,70.8271420047267,-27.14760101229933,42.63076837824471], "fy":[-54.043516257935,34.310332066852396,-41.500362262106535,-121.22195585555252]}, - {"t":0.2658919282835618, "x":4.482698542849427, "y":4.8051464726928375, "heading":0.8014870903408847, "vx":2.0095012745255563, "vy":-1.6649208204669856, "omega":3.2547669782850157, "ax":7.452297109521514, "ay":-0.7638505668761703, "alpha":-16.002906071348082, "fx":[130.73340422281083,66.00340850569457,-10.17432983130126,66.96040019169689], "fy":[-5.13399005852738,69.58721024287266,-8.93518948065377,-81.50378987533193]}, - {"t":0.30387648946692775, "x":4.564404754284277, "y":4.741354133854086, "heading":0.925117985765151, "vx":2.292573510038798, "vy":-1.6939353490594422, "omega":2.6469036135062356, "ax":7.1884593920664095, "ay":3.818034260590328, "alpha":-11.910038807587478, "fx":[111.42368834322436,53.46840279853804,7.055691780048093,72.59949200040163], "fy":[38.599253433555575,87.31169861138011,27.53575831360324,-23.559369683361584]}, - {"t":0.3418610506502937, "x":4.656673004291847, "y":4.6797651241928735, "heading":1.0256594580188512, "vx":2.565623985630886, "vy":-1.5489089930878617, "omega":2.194506015723166, "ax":2.723181317765863, "ay":5.779809499044349, "alpha":1.3149162094791331, "fx":[17.237452910502054,24.76903049316307,29.049576865635597,21.58500982058484], "fy":[47.90557286513214,43.397061253469744,50.43156616299465,54.891611379906394]}, - {"t":0.37984561183365967, "x":4.756091645161041, "y":4.625100128054678, "heading":1.1090168060403525, "vx":2.6690628330089625, "vy":-1.3293654655432117, "omega":2.244452530933126, "ax":-1.6577212706357212, "ay":7.714780667545829, "alpha":14.673305524362782, "fx":[-81.57053672741239,8.297740564223327,50.85551212702628,-33.97744495986733], "fy":[46.588403532014816,2.458568899281396,89.5164898000497,123.88896129532614]}, - {"t":0.4178301730170256, "x":4.856278923232433, "y":4.580170310685905, "heading":1.1942713505247422, "vx":2.606095017979533, "vy":-1.0363229072605682, "omega":2.8018116023855058, "ax":-5.0005666993769635, "ay":9.36496492674863, "alpha":23.194455968434497, "fx":[-147.17469622055927,-2.2379332041393987,57.49367627088672,-78.19746438469859], "fy":[41.90057052998772,-20.495938115090336,133.76600311948602,163.4201121724257]}, - {"t":0.4558147342003916, "x":4.95166282284726, "y":4.547562051411459, "heading":1.3006969347598192, "vx":2.4161506862355457, "vy":-0.6805988240204084, "omega":3.682842834233394, "ax":6.8738129624669035, "ay":-3.904023420766954, "alpha":-29.79573890174241, "fx":[174.06517618077348,-0.9561061006684474,-59.05323057541544,119.78734393896669], "fy":[27.23921591658561,99.80438837463473,-119.21491896314672,-140.64132802516286]}, - {"t":0.4937992953837575, "x":5.0483981075004785, "y":4.518893388757126, "heading":1.4405881037254784, "vx":2.677249455271384, "vy":-0.8288914405078245, "omega":2.5510647669165616, "ax":16.88750155862789, "ay":7.461571076732664, "alpha":-44.07592406956584, "fx":[243.3503743643859,41.43767722148993,4.0201436014902105,285.69494396438773], "fy":[169.89802561155176,286.9921470883066,-143.50308199455745,-59.54871255890434]}, - {"t":0.5317838565671236, "x":5.162275123899805, "y":4.492791188809997, "heading":1.5374891794471497, "vx":3.318713791459273, "vy":-0.5454669374196388, "omega":0.8768601323827456, "ax":6.466457031376719, "ay":31.782237542095473, "alpha":-23.08464557876047, "fx":[131.34583520578482,-38.73569427885679,-101.76576644307411,229.1407932935459], "fy":[280.1132982326035,306.79025429889623,288.9853074972303,205.32467376792448]}, - {"t":0.5697684177504896, "x":5.293, "y":4.495, "heading":1.5707963267948966, "vx":3.564339324207209, "vy":0.6617674090423575, "omega":0.0, "ax":-17.46995262929559, "ay":29.018547812206343, "alpha":-17.231876555711285, "fx":[-39.50542738838971,-184.48359090699591,-273.6713547280688,-96.65741824478924], "fy":[306.4819959225835,248.4794912616831,142.4475943346916,289.7853091883159]}, - {"t":0.6071165368332829, "x":5.413937108308784, "y":4.539954492988536, "heading":1.5707963267948966, "vx":2.911869453037518, "vy":1.7455555883423737, "omega":-0.6435781776227005, "ax":-23.364641119392896, "ay":5.7807117477879935, "alpha":-39.67090658706738, "fx":[-67.34423076259405,-252.6730740798265,-297.2001208453638,-177.6342947780813], "fy":[289.77837510305454,168.01639761931054,-53.64736080012576,-207.49090627478944]}, - {"t":0.6444646559160763, "x":5.506394496738847, "y":4.609179416346846, "heading":1.5467598923779569, "vx":2.039244054183701, "vy":1.9614542990820625, "omega":-2.125211920958866, "ax":-17.08062391895474, "ay":-6.113696994733446, "alpha":-43.98571665319571, "fx":[-45.13948058631435,-279.97389696639857,-232.99854159165108,-22.961132191438647], "fy":[187.3372315018108,55.16288998573377,-173.33039167205808,-277.154203013213]}, - {"t":0.6818127749988696, "x":5.570643699095066, "y":4.678172102141704, "heading":1.467387224477813, "vx":1.4013148780501703, "vy":1.7331192156866413, "omega":-3.767995704464427, "ax":-13.783674765700583, "ay":-2.060787922443541, "alpha":-33.13955160972574, "fx":[-13.955054195494917,-206.469783817391,-209.47327070653645,-39.014619101826874], "fy":[120.73464042758,88.84614026474486,-101.22862305402826,-178.45898347393725]}, - {"t":0.719160894081663, "x":5.613366874127446, "y":4.741463567005436, "heading":1.3266596722040216, "vx":0.8865205515022906, "vy":1.6561526629548375, "omega":-5.005695624334843, "ax":2.672856932809343, "ay":9.54645245402934, "alpha":16.0222098923521, "fx":[-47.38196837882718,70.11939865428032,83.23389464940595,-15.042261613065044], "fy":[49.59058204687828,20.06847309949008,115.03442906944572,140.0713653128472]}, - {"t":0.7565090131644563, "x":5.648340909265066, "y":4.8099758412220845, "heading":1.139706355934146, "vx":0.9863467305201242, "vy":2.01269470602615, "omega":-4.4072962213057645, "ax":12.859139992620332, "ay":3.667765887073824, "alpha":26.60201862217061, "fx":[-4.295504320648231,164.00355143520014,201.0275711835912,76.72496560793975], "fy":[-16.39144275639421,-87.29775417519376,66.37748920237186,162.08700432843872]}, - {"t":0.7938571322472496, "x":5.694147595862764, "y":4.88770425308631, "heading":0.9751021318276732, "vx":1.4666114222668192, "vy":2.1496788631443913, "omega":-3.4137608619622495, "ax":9.05159801140143, "ay":-2.8712116541580657, "alpha":9.840448242313665, "fx":[33.74996676276294,84.39228031979921,117.89061174702113,71.89732574133126], "fy":[-36.34012022303755,-69.3187976732754,-14.902156768284591,22.88409724100924]}, - {"t":0.831205251330043, "x":5.755235729474016, "y":4.965988214530906, "heading":0.8476045846349277, "vx":1.8046715826862156, "vy":2.042444508372992, "omega":-3.046238629180254, "ax":3.140941449362886, "ay":-8.186298171812115, "alpha":-10.876582378244004, "fx":[77.7135813106321,26.038748456400604,-25.48154243504908,28.582243371597357], "fy":[-66.39100380634368,-20.537552718511357,-76.11454639420802,-115.45007627685624]}, - {"t":0.8685533704128363, "x":5.8248274399933635, "y":5.036560215270374, "heading":0.7338333015576982, "vx":1.9219798379691024, "vy":1.7367016694048991, "omega":-3.4524585230567233, "ax":-2.1136936679481573, "ay":-12.199823559824065, "alpha":-26.5652580412972, "fx":[113.85459795703362,-18.463577787989276,-145.61248556710063,-21.685183624338517], "fy":[-125.93798736716592,20.908405277057756,-106.13010292994028,-203.87133113613396]}, - {"t":0.9059014894956297, "x":5.8951355952321265, "y":5.09291409889317, "heading":0.6048904695101708, "vx":1.843037355154028, "vy":1.2810612063035216, "omega":-4.444620943848226, "ax":-4.975894252152143, "ay":-12.842141108806985, "alpha":-28.569652725571643, "fx":[98.33440398761331,-32.790105159095255,-176.94929691499476,-57.87207691625341], "fy":[-156.51287788540583,25.786971604579417,-93.35174350373175,-212.8046418218059]}, - {"t":0.943249608578423, "x":5.960499181185772, "y":5.131802689647749, "heading":0.4388922372214496, "vx":1.6571970640812623, "vy":0.801431390893762, "omega":-5.511643735996928, "ax":1.5837604531226042, "ay":16.193366722649536, "alpha":43.88839575402028, "fx":[-149.68061371021807,-121.31934856991572,246.5150473068684,78.36353928157708], "fy":[232.0159727719713,-66.98157990187129,112.687353810509,273.16732256982107]}, - {"t":0.9805977276612164, "x":6.023496953952147, "y":5.173028562544035, "heading":0.23304271062750417, "vx":1.7163475380831044, "vy":1.4062231796026206, "omega":-3.8724947050230143, "ax":0.4044973447465699, "ay":8.038246855365111, "alpha":60.94919355877796, "fx":[-190.75104638704465,-208.20122535075024,289.0879228400815,123.6251170923862], "fy":[226.59281633314,-191.0430903735248,-33.154933301743746,271.0617654748866]}, - {"t":1.0179458467440097, "x":6.087881419224354, "y":5.231154556233942, "heading":0.08841231723681776, "vx":1.731454753083373, "vy":1.706436580373686, "omega":-1.5961569659895516, "ax":-23.57280881417639, "ay":-22.046919504329445, "alpha":22.09119026985219, "fx":[-306.5951390537582,-209.05062942645768,-23.56351817578218,-262.7241796624389], "fy":[-50.58280431138501,-230.15481580587175,-309.79414240855715,-159.49182266178744]}, - {"t":1.055293965826803, "x":6.136107354188334, "y":5.279510327269729, "heading":0.028798856796209797, "vx":0.8510546823755913, "vy":0.88302560531723, "omega":-0.7710925611104655, "ax":-22.787082811023808, "ay":-23.643107792382715, "alpha":20.646088211326145, "fx":[-306.95613371172647,-214.45640732387,-35.299148510694714,-218.49182777660005], "fy":[-50.46330441881009,-225.4797127562022,-309.064962685133,-219.3170174682804]}, - {"t":1.0926420849095964, "x":6.152, "y":5.296, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor b/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor deleted file mode 100644 index f424b0c1..00000000 --- a/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor +++ /dev/null @@ -1,74 +0,0 @@ -{ - "name":"Tests", - "version":"v2025.0.0", - "type":"Swerve", - "variables":{ - "expressions":{}, - "poses":{} - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"10.5 in", - "val":0.2667 - }, - "y":{ - "exp":"10.5 in", - "val":0.2667 - } - }, - "backLeft":{ - "x":{ - "exp":"-10.5 in", - "val":-0.2667 - }, - "y":{ - "exp":"10.5 in", - "val":0.2667 - } - }, - "mass":{ - "exp":"75 lbs", - "val":34.01942775 - }, - "inertia":{ - "exp":"6.883 kg m ^ 2", - "val":6.883 - }, - "gearing":{ - "exp":"7.363636364", - "val":7.363636364 - }, - "radius":{ - "exp":"2.167 in", - "val":0.055041799999999995 - }, - "vmax":{ - "exp":"485.89966375522135 rad / s", - "val":485.8996637552213 - }, - "tmax":{ - "exp":"2.327950310559006 N m", - "val":2.327950310559006 - }, - "bumper":{ - "front":{ - "exp":"16 in", - "val":0.4064 - }, - "side":{ - "exp":"16 in", - "val":0.4064 - }, - "back":{ - "exp":"16 in", - "val":0.4064 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - } - }, - "generationFeatures":[] -} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/deploy/example.txt b/cpp/SwerveWithChoreo/src/main/deploy/example.txt deleted file mode 100644 index bb82515d..00000000 --- a/cpp/SwerveWithChoreo/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h b/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h deleted file mode 100644 index 3e36efc5..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include - -#include "subsystems/CommandSwerveDrivetrain.h" - -class AutoRoutines { -private: - subsystems::CommandSwerveDrivetrain &m_drivetrain; - - /* cache AutoTrajectory and AutoRoutine instances due to lifetime issues */ - std::optional> simplePathRoutine; - choreo::AutoTrajectory simplePathTraj; - -public: - AutoRoutines(subsystems::CommandSwerveDrivetrain &drivetrain) : - m_drivetrain{drivetrain} - {} - - frc2::CommandPtr SimplePathAuto(choreo::AutoFactory &factory); -}; diff --git a/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h b/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h deleted file mode 100644 index 35a53040..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h +++ /dev/null @@ -1,877 +0,0 @@ -#pragma once - -/// -//https://github.com/LimelightVision/limelightlib-wpicpp -/// - -#include "networktables/NetworkTable.h" -#include "networktables/NetworkTableInstance.h" -#include "networktables/NetworkTableEntry.h" -#include "networktables/NetworkTableValue.h" -#include -#include "wpi/json.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -//#include -//#include -// #include -// #include -// #include -#include -#include - -namespace LimelightHelpers -{ - inline std::string sanitizeName(const std::string &name) - { - if (name == "") - { - return "limelight"; - } - return name; - } - - inline frc::Pose3d toPose3D(const std::vector& inData) - { - if(inData.size() < 6) - { - return frc::Pose3d(); - } - return frc::Pose3d( - frc::Translation3d(units::length::meter_t(inData[0]), units::length::meter_t(inData[1]), units::length::meter_t(inData[2])), - frc::Rotation3d(units::angle::degree_t(inData[3]), units::angle::degree_t(inData[4]), - units::angle::degree_t(inData[5]))); - } - - inline frc::Pose2d toPose2D(const std::vector& inData) - { - if(inData.size() < 6) - { - return frc::Pose2d(); - } - return frc::Pose2d( - frc::Translation2d(units::length::meter_t(inData[0]), units::length::meter_t(inData[1])), - frc::Rotation2d(units::angle::degree_t(inData[5]))); - } - - inline std::shared_ptr getLimelightNTTable(const std::string &tableName) - { - return nt::NetworkTableInstance::GetDefault().GetTable(sanitizeName(tableName)); - } - - inline nt::NetworkTableEntry getLimelightNTTableEntry(const std::string &tableName, const std::string &entryName) - { - return getLimelightNTTable(tableName)->GetEntry(entryName); - } - - inline double getLimelightNTDouble(const std::string &tableName, const std::string &entryName) - { - return getLimelightNTTableEntry(tableName, entryName).GetDouble(0.0); - } - - inline std::vector getLimelightNTDoubleArray(const std::string &tableName, const std::string &entryName) - { - return getLimelightNTTableEntry(tableName, entryName).GetDoubleArray(std::span{}); - } - - inline std::string getLimelightNTString(const std::string &tableName, const std::string &entryName) - { - return getLimelightNTTableEntry(tableName, entryName).GetString(""); - } - - inline void setLimelightNTDouble(const std::string &tableName, const std::string entryName, double val) - { - getLimelightNTTableEntry(tableName, entryName).SetDouble(val); - } - - inline void setLimelightNTDoubleArray(const std::string &tableName, const std::string &entryName, const std::span &vals) - { - getLimelightNTTableEntry(tableName, entryName).SetDoubleArray(vals); - } - - inline double getTX(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "tx"); - } - - inline double getTV(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "tv"); - } - - inline double getTY(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "ty"); - } - - inline double getTA(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "ta"); - } - - inline double getLatency_Pipeline(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "tl"); - } - - inline double getLatency_Capture(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "cl"); - } - - inline std::string getJSONDump(const std::string &limelightName = "") - { - return getLimelightNTString(limelightName, "json"); - } - - inline std::vector getBotpose(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - inline std::vector getBotpose_wpiRed(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - inline std::vector getBotpose_wpiBlue(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - - - inline std::vector getBotpose_TargetSpace(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - inline std::vector getCameraPose_TargetSpace(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - inline std::vector getCameraPose_RobotSpace(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - } - - inline std::vector getTargetPose_CameraSpace(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - inline std::vector getTargetPose_RobotSpace(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - inline std::vector getTargetColor(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - inline double getFiducialID(const std::string &limelightName = "") - { - return getLimelightNTDouble(limelightName, "tid"); - } - - inline std::string getNeuralClassID(const std::string &limelightName = "") - { - return getLimelightNTString(limelightName, "tclass"); - } - - ///// - ///// - - inline void setPipelineIndex(const std::string &limelightName, int index) - { - setLimelightNTDouble(limelightName, "pipeline", index); - } - - inline void setPriorityTagID(const std::string &limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - inline void setLEDMode_PipelineControl(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - inline void setLEDMode_ForceOff(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - inline void setLEDMode_ForceBlink(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - inline void setLEDMode_ForceOn(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - inline void setStreamMode_Standard(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "stream", 0); - } - - inline void setStreamMode_PiPMain(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "stream", 1); - } - - inline void setStreamMode_PiPSecondary(const std::string &limelightName = "") - { - setLimelightNTDouble(limelightName, "stream", 2); - } - - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. - */ - inline void setCropWindow(const std::string &limelightName, double cropXMin, - double cropXMax, double cropYMin, double cropYMax) - { - double cropWindow[4]{cropXMin, cropXMax, cropYMin, cropYMax}; - setLimelightNTDoubleArray(limelightName, "crop", cropWindow); - } - - /** - * Sets the robot orientation for mt2. - */ - inline void SetRobotOrientation(const std::string& limelightName, - double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) - { - std::vector entries = {yaw, yawRate, pitch, pitchRate, roll, rollRate}; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } - - inline void SetFiducialDownscaling(const std::string& limelightName, float downscale) - { - int d = 0; // pipeline - if (downscale == 1.0) - { - d = 1; - } - if (downscale == 1.5) - { - d = 2; - } - if (downscale == 2) - { - d = 3; - } - if (downscale == 3) - { - d = 4; - } - if (downscale == 4) - { - d = 5; - } - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - inline void SetFiducialIDFiltersOverride(const std::string& limelightName, const std::vector& validIDs) - { - std::vector validIDsDouble(validIDs.begin(), validIDs.end()); - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - ///// - ///// - - /** - * Sets the camera pose in robotspace. The UI camera pose must be set to zeros - */ - inline void setCameraPose_RobotSpace(const std::string &limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double entries[6] ={forward, side, up, roll, pitch, yaw}; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - inline void setPythonScriptData(const std::string &limelightName, const std::vector &outgoingPythonData) - { - setLimelightNTDoubleArray(limelightName, "llrobot", std::span{outgoingPythonData.begin(), outgoingPythonData.size()}); - } - - inline std::vector getPythonScriptData(const std::string &limelightName = "") - { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - - inline double extractArrayEntry(const std::vector& inData, int position) { - if (inData.size() < static_cast(position + 1)) { - return 0.0; - } - return inData[position]; - } - - class RawFiducial - { - public: - int id{0}; - double txnc{0.0}; - double tync{0.0}; - double ta{0.0}; - double distToCamera{0.0}; - double distToRobot{0.0}; - double ambiguity{0.0}; - - RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) - : id(id), txnc(txnc), tync(tync), ta(ta), distToCamera(distToCamera), distToRobot(distToRobot), ambiguity(ambiguity) {} - }; - - inline std::vector getRawFiducials(const std::string& limelightName) - { - nt::NetworkTableEntry entry = LimelightHelpers::getLimelightNTTableEntry(limelightName, "rawfiducials"); - std::vector rawFiducialArray = entry.GetDoubleArray({}); - int valsPerEntry = 7; - if (rawFiducialArray.size() % valsPerEntry != 0) { - return {}; - } - - int numFiducials = rawFiducialArray.size() / valsPerEntry; - std::vector rawFiducials; - - for (int i = 0; i < numFiducials; ++i) { - int baseIndex = i * valsPerEntry; - int id = static_cast(extractArrayEntry(rawFiducialArray, baseIndex)); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials.emplace_back(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - - return rawFiducials; - } - - - class RawDetection - { - public: - int classId{-1}; - double txnc{0.0}; - double tync{9.0}; // It seems like you intentionally set this to 9.0, so I kept it as is. - double ta{0.0}; - double corner0_X{0.0}; - double corner0_Y{0.0}; - double corner1_X{0.0}; - double corner1_Y{0.0}; - double corner2_X{0.0}; - double corner2_Y{0.0}; - double corner3_X{0.0}; - double corner3_Y{0.0}; - - RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, - double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, - double corner3_X, double corner3_Y) - : classId(classId), txnc(txnc), tync(tync), ta(ta), - corner0_X(corner0_X), corner0_Y(corner0_Y), - corner1_X(corner1_X), corner1_Y(corner1_Y), - corner2_X(corner2_X), corner2_Y(corner2_Y), - corner3_X(corner3_X), corner3_Y(corner3_Y) {} - }; - - inline std::vector getRawDetections(const std::string& limelightName) - { - nt::NetworkTableEntry entry = LimelightHelpers::getLimelightNTTableEntry(limelightName, "rawdetections"); - std::vector rawDetectionArray = entry.GetDoubleArray({}); - int valsPerEntry = 11; - - if (rawDetectionArray.size() % valsPerEntry != 0) { - return {}; - } - - int numDetections = rawDetectionArray.size() / valsPerEntry; - std::vector rawDetections; - - for (int i = 0; i < numDetections; ++i) { - int baseIndex = i * valsPerEntry; - int classId = static_cast(extractArrayEntry(rawDetectionArray, baseIndex)); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections.emplace_back(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); - } - - return rawDetections; - } - - class PoseEstimate - { - public: - frc::Pose2d pose; - units::time::second_t timestampSeconds{0.0}; - double latency{0.0}; - int tagCount{0}; - double tagSpan{0.0}; - double avgTagDist{0.0}; - double avgTagArea{0.0}; - std::vector rawFiducials; - - PoseEstimate() = default; - - PoseEstimate(const frc::Pose2d& pose, units::time::second_t timestampSeconds, - double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea, - const std::vector& rawFiducials) - : pose(pose), timestampSeconds(timestampSeconds), - latency(latency), tagCount(tagCount), tagSpan(tagSpan), - avgTagDist(avgTagDist), avgTagArea(avgTagArea), rawFiducials(rawFiducials) - { - } - }; - - inline std::optional getBotPoseEstimate(const std::string& limelightName, const std::string& entryName) { - nt::NetworkTableEntry poseEntry = getLimelightNTTableEntry(limelightName, entryName); - std::vector poseArray = poseEntry.GetDoubleArray(std::span{}); - frc::Pose2d pose = toPose2D(poseArray); - - if (poseArray.size() == 0) { - // Handle the case where no data is available - return std::nullopt; // or some default PoseEstimate - } - - double latency = extractArrayEntry(poseArray, 6); - int tagCount = static_cast(extractArrayEntry(poseArray, 7)); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // getLastChange: microseconds; latency: milliseconds - units::time::second_t timestamp = units::time::second_t((poseEntry.GetLastChange() / 1000000.0) - (latency / 1000.0)); - - std::vector rawFiducials; - constexpr int valsPerFiducial = 7; - size_t expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.size() == expectedTotalVals) - { - for (int i = 0; i < tagCount; i++) - { - int baseIndex = 11 + (i * valsPerFiducial); - int id = static_cast(extractArrayEntry(poseArray, baseIndex)); - double txnc = extractArrayEntry(poseArray, baseIndex + 1); - double tync = extractArrayEntry(poseArray, baseIndex + 2); - double ta = extractArrayEntry(poseArray, baseIndex + 3); - double distToCamera = extractArrayEntry(poseArray, baseIndex + 4); - double distToRobot = extractArrayEntry(poseArray, baseIndex + 5); - double ambiguity = extractArrayEntry(poseArray, baseIndex + 6); - rawFiducials.emplace_back(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); - } - - inline std::optional getBotPoseEstimate_wpiBlue(const std::string &limelightName = "") { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); - } - - inline std::optional getBotPoseEstimate_wpiRed(const std::string &limelightName = "") { - return getBotPoseEstimate(limelightName, "botpose_wpired"); - } - - inline std::optional getBotPoseEstimate_wpiBlue_MegaTag2(const std::string &limelightName = "") { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); - } - - inline std::optional getBotPoseEstimate_wpiRed_MegaTag2(const std::string &limelightName = "") { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); - } - - inline const double INVALID_TARGET = 0.0; - class SingleTargetingResultClass - { - public: - SingleTargetingResultClass(){}; - ~SingleTargetingResultClass(){}; - double m_TargetXPixels{INVALID_TARGET}; - double m_TargetYPixels{INVALID_TARGET}; - - double m_TargetXNormalized{INVALID_TARGET}; - double m_TargetYNormalized{INVALID_TARGET}; - - double m_TargetXNormalizedCrosshairAdjusted{INVALID_TARGET}; - double m_TargetYNormalizedCrosshairAdjusted{INVALID_TARGET}; - - double m_TargetXDegreesCrosshairAdjusted{INVALID_TARGET}; - double m_TargetYDegreesCrosshairAdjusted{INVALID_TARGET}; - - double m_TargetAreaPixels{INVALID_TARGET}; - double m_TargetAreaNormalized{INVALID_TARGET}; - double m_TargetAreaNormalizedPercentage{INVALID_TARGET}; - - // not included in json// - double m_timeStamp{-1.0}; - double m_latency{0}; - double m_pipelineIndex{-1.0}; - std::vector> m_TargetCorners; - - std::vector m_CAMERATransform6DTARGETSPACE; - std::vector m_TargetTransform6DCAMERASPACE; - std::vector m_TargetTransform6DROBOTSPACE; - std::vector m_ROBOTTransform6DTARGETSPACE; - std::vector m_ROBOTTransform6DFIELDSPACE; - std::vector m_CAMERATransform6DROBOTSPACE; - - }; - - class RetroreflectiveResultClass : public SingleTargetingResultClass - { - public: - RetroreflectiveResultClass() {} - ~RetroreflectiveResultClass() {} - }; - - class FiducialResultClass : public SingleTargetingResultClass - { - public: - FiducialResultClass() {} - ~FiducialResultClass() {} - int m_fiducialID{0}; - std::string m_family{""}; - }; - - class DetectionResultClass : public SingleTargetingResultClass - { - public: - DetectionResultClass() {} - ~DetectionResultClass() {} - - int m_classID{-1}; - std::string m_className{""}; - double m_confidence{0}; - }; - - class ClassificationResultClass : public SingleTargetingResultClass - { - public: - ClassificationResultClass() {} - ~ClassificationResultClass() {} - - int m_classID{-1}; - std::string m_className{""}; - double m_confidence{0}; - }; - - class VisionResultsClass - { - public: - VisionResultsClass() {} - ~VisionResultsClass() {} - std::vector RetroResults; - std::vector FiducialResults; - std::vector DetectionResults; - std::vector ClassificationResults; - double m_timeStamp{-1.0}; - double m_latencyPipeline{0}; - double m_latencyCapture{0}; - double m_latencyJSON{0}; - double m_pipelineIndex{-1.0}; - int valid{0}; - std::vector botPose{6,0.0}; - std::vector botPose_wpired{6,0.0}; - std::vector botPose_wpiblue{6,0.0}; - void Clear() - { - RetroResults.clear(); - FiducialResults.clear(); - DetectionResults.clear(); - ClassificationResults.clear(); - botPose.clear(); - botPose_wpired.clear(); - botPose_wpiblue.clear(); - m_timeStamp = -1.0; - m_latencyPipeline = 0; - - m_pipelineIndex = -1.0; - } - }; - - class LimelightResultsClass - { - public: - LimelightResultsClass() {} - ~LimelightResultsClass() {} - VisionResultsClass targetingResults; - }; - - namespace internal - { - inline const std::string _key_timestamp{"ts"}; - inline const std::string _key_latency_pipeline{"tl"}; - inline const std::string _key_latency_capture{"cl"}; - - inline const std::string _key_pipelineIndex{"pID"}; - inline const std::string _key_TargetXDegrees{"txdr"}; - inline const std::string _key_TargetYDegrees{"tydr"}; - inline const std::string _key_TargetXNormalized{"txnr"}; - inline const std::string _key_TargetYNormalized{"tynr"}; - inline const std::string _key_TargetXPixels{"txp"}; - inline const std::string _key_TargetYPixels{"typ"}; - - inline const std::string _key_TargetXDegreesCrosshair{"tx"}; - inline const std::string _key_TargetYDegreesCrosshair{"ty"}; - inline const std::string _key_TargetXNormalizedCrosshair{"txn"}; - inline const std::string _key_TargetYNormalizedCrosshair{"tyn"}; - inline const std::string _key_TargetAreaNormalized{"ta"}; - inline const std::string _key_TargetAreaPixels{"tap"}; - inline const std::string _key_className{"class"}; - inline const std::string _key_classID{"classID"}; - inline const std::string _key_confidence{"conf"}; - inline const std::string _key_fiducialID{"fID"}; - inline const std::string _key_corners{"pts"}; - inline const std::string _key_transformCAMERAPOSE_TARGETSPACE{"t6c_ts"}; - inline const std::string _key_transformCAMERAPOSE_ROBOTSPACE{"t6c_rs"}; - - inline const std::string _key_transformTARGETPOSE_CAMERASPACE{"t6t_cs"}; - inline const std::string _key_transformROBOTPOSE_TARGETSPACE{"t6r_ts"}; - inline const std::string _key_transformTARGETPOSE_ROBOTSPACE{"t6t_rs"}; - - inline const std::string _key_botpose{"botpose"}; - inline const std::string _key_botpose_wpiblue{"botpose_wpiblue"}; - inline const std::string _key_botpose_wpired{"botpose_wpired"}; - - inline const std::string _key_transformROBOTPOSE_FIELDSPACE{"t6r_fs"}; - inline const std::string _key_skew{"skew"}; - inline const std::string _key_ffamily{"fam"}; - inline const std::string _key_colorRGB{"cRGB"}; - inline const std::string _key_colorHSV{"cHSV"}; - } - - // inline void PhoneHome() - // { - // static int sockfd = -1; - // static struct sockaddr_in servaddr, cliaddr; - - // if (sockfd == -1) { - // sockfd = socket(AF_INET, SOCK_DGRAM, 0); - // if (sockfd < 0) { - // std::cerr << "Socket creation failed" << std::endl; - // return; - // } - - // memset(&servaddr, 0, sizeof(servaddr)); - // servaddr.sin_family = AF_INET; - // servaddr.sin_addr.s_addr = inet_addr("255.255.255.255"); - // servaddr.sin_port = htons(5809); - - // // Set socket for broadcast - // int broadcast = 1; - // if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { - // std::cerr << "Error in setting Broadcast option" << std::endl; - // close(sockfd); - // sockfd = -1; - // return; - // } - - // // Set socket to non-blocking - // if (fcntl(sockfd, F_SETFL, O_NONBLOCK) < 0) { - // std::cerr << "Error setting socket to non-blocking" << std::endl; - // close(sockfd); - // sockfd = -1; - // return; - // } - - // const char *msg = "LLPhoneHome"; - // sendto(sockfd, msg, strlen(msg), 0, (const struct sockaddr *) &servaddr, sizeof(servaddr)); - // } - - // char receiveData[1024]; - // socklen_t len = sizeof(cliaddr); - - // ssize_t n = recvfrom(sockfd, (char *)receiveData, 1024, 0, (struct sockaddr *) &cliaddr, &len); - // if (n > 0) { - // receiveData[n] = '\0'; // Null-terminate the received string - // std::string received(receiveData, n); - // std::cout << "Received response: " << received << std::endl; - // } else if (n < 0 && errno != EWOULDBLOCK && errno != EAGAIN) { - // std::cerr << "Error receiving data" << std::endl; - // close(sockfd); - // sockfd = -1; - // } - // } - - inline void SetupPortForwarding(const std::string& limelightName) - { - auto& portForwarder = wpi::PortForwarder::GetInstance(); - portForwarder.Add(5800, sanitizeName(limelightName), 5800); - portForwarder.Add(5801, sanitizeName(limelightName), 5801); - portForwarder.Add(5802, sanitizeName(limelightName), 5802); - portForwarder.Add(5803, sanitizeName(limelightName), 5803); - portForwarder.Add(5804, sanitizeName(limelightName), 5804); - portForwarder.Add(5805, sanitizeName(limelightName), 5805); - portForwarder.Add(5806, sanitizeName(limelightName), 5806); - portForwarder.Add(5807, sanitizeName(limelightName), 5807); - portForwarder.Add(5808, sanitizeName(limelightName), 5808); - portForwarder.Add(5809, sanitizeName(limelightName), 5809); - } - - template - T SafeJSONAccess(const wpi::json& jsonData, const KeyType& key, const T& defaultValue) - { - try - { - return jsonData.at(key).template get(); - } - catch (wpi::json::exception&) - { - return defaultValue; - } - catch (...) - { - return defaultValue; - } - } - inline void from_json(const wpi::json &j, RetroreflectiveResultClass &t) - { - std::vector defaultValueVector(6, 0.0); - t.m_CAMERATransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_TARGETSPACE, defaultValueVector); - t.m_CAMERATransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_ROBOTSPACE, defaultValueVector); - - t.m_TargetTransform6DCAMERASPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_CAMERASPACE, defaultValueVector); - t.m_TargetTransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_ROBOTSPACE, defaultValueVector); - t.m_ROBOTTransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_TARGETSPACE, defaultValueVector); - t.m_ROBOTTransform6DFIELDSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_FIELDSPACE, defaultValueVector); - - t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); - t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); - t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); - t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); - t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); - t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); - } - - inline void from_json(const wpi::json &j, FiducialResultClass &t) - { - std::vector defaultValueVector(6, 0.0); - t.m_family = SafeJSONAccess(j, internal::_key_ffamily, ""); - t.m_fiducialID = SafeJSONAccess(j, internal::_key_fiducialID, 0.0); - t.m_CAMERATransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_TARGETSPACE, defaultValueVector); - t.m_CAMERATransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_ROBOTSPACE, defaultValueVector); - t.m_TargetTransform6DCAMERASPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_CAMERASPACE, defaultValueVector); - t.m_TargetTransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_ROBOTSPACE, defaultValueVector); - t.m_ROBOTTransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_TARGETSPACE, defaultValueVector); - t.m_ROBOTTransform6DFIELDSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_FIELDSPACE, defaultValueVector); - - t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); - t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); - t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); - t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); - t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); - t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); - } - - inline void from_json(const wpi::json &j, DetectionResultClass &t) - { - t.m_confidence = SafeJSONAccess(j, internal::_key_confidence, 0.0); - t.m_classID = SafeJSONAccess(j, internal::_key_classID, 0.0); - t.m_className = SafeJSONAccess(j, internal::_key_className, ""); - t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); - t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); - t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); - t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); - t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); - t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); - } - - inline void from_json(const wpi::json &j, ClassificationResultClass &t) - { - t.m_confidence = SafeJSONAccess(j, internal::_key_confidence, 0.0); - t.m_classID = SafeJSONAccess(j, internal::_key_classID, 0.0); - t.m_className = SafeJSONAccess(j, internal::_key_className, ""); - t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); - t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); - t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); - t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); - t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); - t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); - } - - inline void from_json(const wpi::json &j, VisionResultsClass &t) - { - t.m_timeStamp = SafeJSONAccess(j, internal::_key_timestamp, 0.0); - t.m_latencyPipeline = SafeJSONAccess(j, internal::_key_latency_pipeline, 0.0); - t.m_latencyCapture = SafeJSONAccess(j, internal::_key_latency_capture, 0.0); - t.m_pipelineIndex = SafeJSONAccess(j, internal::_key_pipelineIndex, 0.0); - t.valid = SafeJSONAccess(j, "v", 0.0); - - std::vector defaultVector{}; - t.botPose = SafeJSONAccess>(j, internal::_key_botpose, defaultVector); - t.botPose_wpired = SafeJSONAccess>(j, internal::_key_botpose_wpired, defaultVector); - t.botPose_wpiblue = SafeJSONAccess>(j, internal::_key_botpose_wpiblue, defaultVector); - - t.RetroResults = SafeJSONAccess>(j, "Retro", std::vector< RetroreflectiveResultClass>{}); - t.FiducialResults = SafeJSONAccess>(j, "Fiducial", std::vector< FiducialResultClass>{}); - t.DetectionResults = SafeJSONAccess>(j, "Detector", std::vector< DetectionResultClass>{}); - t.ClassificationResults = SafeJSONAccess>(j, "Detector", std::vector< ClassificationResultClass>{}); - } - - inline void from_json(const wpi::json &j, LimelightResultsClass &t) - { - t.targetingResults = SafeJSONAccess(j, "Results", LimelightHelpers::VisionResultsClass{}); - } - - inline LimelightResultsClass getLatestResults(const std::string &limelightName = "", bool profile = false) - { - auto start = std::chrono::high_resolution_clock::now(); - std::string jsonString = getJSONDump(limelightName); - wpi::json data; - try - { - data = wpi::json::parse(jsonString); - } - catch(const std::exception&) - { - return LimelightResultsClass(); - } - - auto end = std::chrono::high_resolution_clock::now(); - double nanos = std::chrono::duration_cast(end - start).count(); - double millis = (nanos * 0.000001); - try - { - LimelightResultsClass out = data.get(); - out.targetingResults.m_latencyJSON = millis; - if (profile) - { - std::cout << "lljson: " << millis << std::endl; - } - return out; - } - catch(...) - { - return LimelightResultsClass(); - } - } -} diff --git a/cpp/SwerveWithChoreo/src/main/include/Robot.h b/cpp/SwerveWithChoreo/src/main/include/Robot.h deleted file mode 100644 index dd003efc..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/Robot.h +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include - -#include -#include - -#include "RobotContainer.h" - -class Robot : public frc::TimedRobot { - public: - Robot(); - void RobotPeriodic() override; - void DisabledInit() override; - void DisabledPeriodic() override; - void DisabledExit() override; - void AutonomousInit() override; - void AutonomousPeriodic() override; - void AutonomousExit() override; - void TeleopInit() override; - void TeleopPeriodic() override; - void TeleopExit() override; - void TestInit() override; - void TestPeriodic() override; - void TestExit() override; - - private: - std::optional m_autonomousCommand; - - RobotContainer m_container; - - static constexpr bool kUseLimelight = false; -}; diff --git a/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h b/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h deleted file mode 100644 index bac1fb6d..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include -#include -#include - -#include "generated/TunerConstants.h" -#include "AutoRoutines.h" -#include "Telemetry.h" - -class RobotContainer { -private: - units::meters_per_second_t MaxSpeed = TunerConstants::kSpeedAt12Volts; // kSpeedAt12Volts desired top speed - units::radians_per_second_t MaxAngularRate = 0.75_tps; // 3/4 of a rotation per second max angular velocity - - /* Setting up bindings for necessary control of the swerve drive platform */ - swerve::requests::FieldCentric drive = swerve::requests::FieldCentric{} - .WithDeadband(MaxSpeed * 0.1).WithRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage); // Use open-loop control for drive motors - swerve::requests::SwerveDriveBrake brake{}; - swerve::requests::PointWheelsAt point{}; - swerve::requests::RobotCentric forwardStraight = swerve::requests::RobotCentric{} - .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage); - - /* Note: This must be constructed before the drivetrain, otherwise we need to - * define a destructor to un-register the telemetry from the drivetrain */ - Telemetry logger{MaxSpeed}; - - frc2::CommandXboxController joystick{0}; - -public: - subsystems::CommandSwerveDrivetrain drivetrain{TunerConstants::CreateDrivetrain()}; - -private: - /* Path follower */ - AutoRoutines autoRoutines; - choreo::AutoFactory autoFactory; - -public: - choreo::AutoChooser autoChooser; - - RobotContainer(); - - frc2::CommandPtr GetAutonomousCommand(); - -private: - void ConfigureBindings(); -}; diff --git a/cpp/SwerveWithChoreo/src/main/include/Telemetry.h b/cpp/SwerveWithChoreo/src/main/include/Telemetry.h deleted file mode 100644 index 9d3a88e9..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/Telemetry.h +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include "ctre/phoenix6/SignalLogger.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -#include "subsystems/CommandSwerveDrivetrain.h" - -class Telemetry { -private: - units::meters_per_second_t MaxSpeed; - - /* What to publish over networktables for telemetry */ - nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); - - /* Robot swerve drive state */ - std::shared_ptr driveStateTable = inst.GetTable("DriveState"); - nt::StructPublisher drivePose = driveStateTable->GetStructTopic("Pose").Publish(); - nt::StructPublisher driveSpeeds = driveStateTable->GetStructTopic("Speeds").Publish(); - nt::StructArrayPublisher driveModuleStates = driveStateTable->GetStructArrayTopic("ModuleStates").Publish(); - nt::StructArrayPublisher driveModuleTargets = driveStateTable->GetStructArrayTopic("ModuleTargets").Publish(); - nt::StructArrayPublisher driveModulePositions = driveStateTable->GetStructArrayTopic("ModulePositions").Publish(); - nt::DoublePublisher driveTimestamp = driveStateTable->GetDoubleTopic("Timestamp").Publish(); - nt::DoublePublisher driveOdometryFrequency = driveStateTable->GetDoubleTopic("OdometryFrequency").Publish(); - - /* Robot pose for field positioning */ - std::shared_ptr table = inst.GetTable("Pose"); - nt::DoubleArrayPublisher fieldPub = table->GetDoubleArrayTopic("robotPose").Publish(); - nt::StringPublisher fieldTypePub = table->GetStringTopic(".type").Publish(); - - /* Mechanisms to represent the swerve module states */ - std::array m_moduleMechanisms{ - frc::Mechanism2d{1, 1}, - frc::Mechanism2d{1, 1}, - frc::Mechanism2d{1, 1}, - frc::Mechanism2d{1, 1}, - }; - /* A direction and length changing ligament for speed representation */ - std::array m_moduleSpeeds{ - m_moduleMechanisms[0].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), - m_moduleMechanisms[1].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), - m_moduleMechanisms[2].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), - m_moduleMechanisms[3].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), - }; - /* A direction changing and length constant ligament for module direction */ - std::array m_moduleDirections{ - m_moduleMechanisms[0].GetRoot("RootDirection", 0.5, 0.5) - ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), - m_moduleMechanisms[1].GetRoot("RootDirection", 0.5, 0.5) - ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), - m_moduleMechanisms[2].GetRoot("RootDirection", 0.5, 0.5) - ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), - m_moduleMechanisms[3].GetRoot("RootDirection", 0.5, 0.5) - ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), - }; - -public: - /** - * Construct a telemetry object with the specified max speed of the robot. - * - * \param maxSpeed Maximum speed - */ - Telemetry(units::meters_per_second_t maxSpeed) : MaxSpeed{maxSpeed} - { - ctre::phoenix6::SignalLogger::Start(); - } - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - void Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state); -}; diff --git a/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h b/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h deleted file mode 100644 index d9cbbb18..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h +++ /dev/null @@ -1,181 +0,0 @@ -#pragma once - -#include "subsystems/CommandSwerveDrivetrain.h" - -using namespace ctre::phoenix6; - -// Generated by the Tuner X Swerve Project Generator -// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants::SteerMotorClosedLoopOutput - static constexpr configs::Slot0Configs steerGains = configs::Slot0Configs{} - .WithKP(100).WithKI(0).WithKD(0.5) - .WithKS(0.2).WithKV(1.59).WithKA(0) - .WithStaticFeedforwardSign(signals::StaticFeedforwardSignValue::UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants::DriveMotorClosedLoopOutput - static constexpr configs::Slot0Configs driveGains = configs::Slot0Configs{} - .WithKP(0.1).WithKI(0).WithKD(0) - .WithKS(0).WithKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - static constexpr swerve::ClosedLoopOutputType kSteerClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - static constexpr swerve::ClosedLoopOutputType kDriveClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - static constexpr swerve::SteerFeedbackType kSteerFeedbackType = swerve::SteerFeedbackType::FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - static constexpr units::ampere_t kSlipCurrent = 120_A; - - // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. - // Some configs will be overwritten; check the `With*InitialConfigs()` API documentation. - static constexpr configs::TalonFXConfiguration driveInitialConfigs{}; - static constexpr configs::TalonFXConfiguration steerInitialConfigs = configs::TalonFXConfiguration{} - .WithCurrentLimits( - configs::CurrentLimitsConfigs{} - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .WithStatorCurrentLimit(60_A) - .WithStatorCurrentLimitEnable(true) - ); - static constexpr configs::CANcoderConfiguration cancoderInitialConfigs{}; - // Configs for the Pigeon 2; leave this nullopt to skip applying Pigeon 2 configs - static constexpr std::optional pigeonConfigs = std::nullopt; - - static constexpr std::string_view kCANBusName = "rio"; - -public: - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - static inline const CANBus kCANBus{kCANBusName, "./logs/example.hoot"}; - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - static constexpr units::meters_per_second_t kSpeedAt12Volts = 4.55_mps; - -private: - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - static constexpr units::scalar_t kCoupleRatio = 3.5; - - static constexpr units::scalar_t kDriveGearRatio = 7.363636364; - static constexpr units::scalar_t kSteerGearRatio = 12.8; - static constexpr units::inch_t kWheelRadius = 2.167_in; - - static constexpr bool kInvertLeftSide = false; - static constexpr bool kInvertRightSide = true; - - static constexpr int kPigeonId = 1; - - // These are only used for simulation - static constexpr units::kilogram_square_meter_t kSteerInertia = 0.01_kg_sq_m; - static constexpr units::kilogram_square_meter_t kDriveInertia = 0.01_kg_sq_m; - // Simulated voltage necessary to overcome friction - static constexpr units::volt_t kSteerFrictionVoltage = 0.25_V; - static constexpr units::volt_t kDriveFrictionVoltage = 0.25_V; - -public: - static constexpr swerve::SwerveDrivetrainConstants DrivetrainConstants = swerve::SwerveDrivetrainConstants{} - .WithCANBusName(kCANBusName) - .WithPigeon2Id(kPigeonId) - .WithPigeon2Configs(pigeonConfigs); - -private: - static constexpr swerve::SwerveModuleConstantsFactory ConstantCreator = swerve::SwerveModuleConstantsFactory{} - .WithDriveMotorGearRatio(kDriveGearRatio) - .WithSteerMotorGearRatio(kSteerGearRatio) - .WithCouplingGearRatio(kCoupleRatio) - .WithWheelRadius(kWheelRadius) - .WithSteerMotorGains(steerGains) - .WithDriveMotorGains(driveGains) - .WithSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .WithDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .WithSlipCurrent(kSlipCurrent) - .WithSpeedAt12Volts(kSpeedAt12Volts) - .WithFeedbackSource(kSteerFeedbackType) - .WithDriveMotorInitialConfigs(driveInitialConfigs) - .WithSteerMotorInitialConfigs(steerInitialConfigs) - .WithCANcoderInitialConfigs(cancoderInitialConfigs) - .WithSteerInertia(kSteerInertia) - .WithDriveInertia(kDriveInertia) - .WithSteerFrictionVoltage(kSteerFrictionVoltage) - .WithDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - static constexpr int kFrontLeftDriveMotorId = 5; - static constexpr int kFrontLeftSteerMotorId = 4; - static constexpr int kFrontLeftEncoderId = 2; - static constexpr units::turn_t kFrontLeftEncoderOffset = -0.83544921875_tr; - static constexpr bool kFrontLeftSteerMotorInverted = true; - static constexpr bool kFrontLeftCANcoderInverted = false; - - static constexpr units::inch_t kFrontLeftXPos = 10.5_in; - static constexpr units::inch_t kFrontLeftYPos = 10.5_in; - - // Front Right - static constexpr int kFrontRightDriveMotorId = 7; - static constexpr int kFrontRightSteerMotorId = 6; - static constexpr int kFrontRightEncoderId = 3; - static constexpr units::turn_t kFrontRightEncoderOffset = -0.15234375_tr; - static constexpr bool kFrontRightSteerMotorInverted = true; - static constexpr bool kFrontRightCANcoderInverted = false; - - static constexpr units::inch_t kFrontRightXPos = 10.5_in; - static constexpr units::inch_t kFrontRightYPos = -10.5_in; - - // Back Left - static constexpr int kBackLeftDriveMotorId = 1; - static constexpr int kBackLeftSteerMotorId = 0; - static constexpr int kBackLeftEncoderId = 0; - static constexpr units::turn_t kBackLeftEncoderOffset = -0.4794921875_tr; - static constexpr bool kBackLeftSteerMotorInverted = true; - static constexpr bool kBackLeftCANcoderInverted = false; - - static constexpr units::inch_t kBackLeftXPos = -10.5_in; - static constexpr units::inch_t kBackLeftYPos = 10.5_in; - - // Back Right - static constexpr int kBackRightDriveMotorId = 3; - static constexpr int kBackRightSteerMotorId = 2; - static constexpr int kBackRightEncoderId = 1; - static constexpr units::turn_t kBackRightEncoderOffset = -0.84130859375_tr; - static constexpr bool kBackRightSteerMotorInverted = true; - static constexpr bool kBackRightCANcoderInverted = false; - - static constexpr units::inch_t kBackRightXPos = -10.5_in; - static constexpr units::inch_t kBackRightYPos = -10.5_in; - - -public: - static constexpr swerve::SwerveModuleConstants FrontLeft = ConstantCreator.CreateModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftCANcoderInverted); - static constexpr swerve::SwerveModuleConstants FrontRight = ConstantCreator.CreateModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightCANcoderInverted); - static constexpr swerve::SwerveModuleConstants BackLeft = ConstantCreator.CreateModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftCANcoderInverted); - static constexpr swerve::SwerveModuleConstants BackRight = ConstantCreator.CreateModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightCANcoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program. - */ - static subsystems::CommandSwerveDrivetrain CreateDrivetrain() - { - return {DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight}; - } -}; diff --git a/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h b/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h deleted file mode 100644 index 3c4a9151..00000000 --- a/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h +++ /dev/null @@ -1,261 +0,0 @@ -#pragma once - -#include "ctre/phoenix6/swerve/SwerveDrivetrain.hpp" -#include "ctre/phoenix6/SignalLogger.hpp" - -#include -#include -#include -#include -#include -#include -#include - -using namespace ctre::phoenix6; - -namespace subsystems { - -/** - * \brief Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -class CommandSwerveDrivetrain : public frc2::SubsystemBase, public swerve::SwerveDrivetrain { - static constexpr units::second_t kSimLoopPeriod = 5_ms; - std::unique_ptr m_simNotifier; - units::second_t m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - static constexpr frc::Rotation2d kBlueAlliancePerspectiveRotation{0_deg}; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - static constexpr frc::Rotation2d kRedAlliancePerspectiveRotation{180_deg}; - /* Keep track if we've ever applied the operator perspective before or not */ - bool m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during field-centric path following */ - swerve::requests::ApplyFieldSpeeds m_pathApplyFieldSpeeds; - frc::PIDController m_pathXController{10, 0, 0}; - frc::PIDController m_pathYController{10, 0, 0}; - frc::PIDController m_pathThetaController{7, 0, 0}; - - /* Swerve requests to apply during SysId characterization */ - swerve::requests::SysIdSwerveTranslation m_translationCharacterization; - swerve::requests::SysIdSwerveSteerGains m_steerCharacterization; - swerve::requests::SysIdSwerveRotation m_rotationCharacterization; - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - frc2::sysid::SysIdRoutine m_sysIdRoutineTranslation{ - frc2::sysid::Config{ - std::nullopt, // Use default ramp rate (1 V/s) - 4_V, // Reduce dynamic step voltage to 4 V to prevent brownout - std::nullopt, // Use default timeout (10 s) - // Log state with SignalLogger class - [](frc::sysid::State state) - { - SignalLogger::WriteString("SysIdTranslation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); - } - }, - frc2::sysid::Mechanism{ - [this](units::volt_t output) { SetControl(m_translationCharacterization.WithVolts(output)); }, - {}, - this - } - }; - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - frc2::sysid::SysIdRoutine m_sysIdRoutineSteer{ - frc2::sysid::Config{ - std::nullopt, // Use default ramp rate (1 V/s) - 7_V, // Use dynamic voltage of 7 V - std::nullopt, // Use default timeout (10 s) - // Log state with SignalLogger class - [](frc::sysid::State state) - { - SignalLogger::WriteString("SysIdSteer_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); - } - }, - frc2::sysid::Mechanism{ - [this](units::volt_t output) { SetControl(m_steerCharacterization.WithVolts(output)); }, - {}, - this - } - }; - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of swerve::requests::SysIdSwerveRotation for info on importing the log to SysId. - */ - frc2::sysid::SysIdRoutine m_sysIdRoutineRotation{ - frc2::sysid::Config{ - /* This is in radians per second², but SysId only supports "volts per second" */ - units::constants::detail::PI_VAL / 6 * (1_V / 1_s), - /* This is in radians per second, but SysId only supports "volts" */ - units::constants::detail::PI_VAL * 1_V, - std::nullopt, // Use default timeout (10 s) - // Log state with SignalLogger class - [](frc::sysid::State state) - { - SignalLogger::WriteString("SysIdRotation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); - } - }, - frc2::sysid::Mechanism{ - [this](units::volt_t output) - { - /* output is actually radians per second, but SysId only supports "volts" */ - SetControl(m_rotationCharacterization.WithRotationalRate(output * (1_rad_per_s / 1_V))); - /* also log the requested output for SysId */ - SignalLogger::WriteValue("Rotational_Rate", output * (1_rad_per_s / 1_V)); - }, - {}, - this - } - }; - - /* The SysId routine to test */ - frc2::sysid::SysIdRoutine *m_sysIdRoutineToApply = &m_sysIdRoutineTranslation; - -public: - /** - * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. - * - * \param drivetrainConstants Drivetrain-wide constants for the swerve drive - * \param modules Constants for each specific module - */ - template ... ModuleConstants> - CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, ModuleConstants const &... modules) : - SwerveDrivetrain{driveTrainConstants, modules...} - { - if (utils::IsSimulation()) { - StartSimThread(); - } - } - - /** - * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. - * - * \param driveTrainConstants Drivetrain-wide constants for the swerve drive - * \param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * \param modules Constants for each specific module - */ - template ... ModuleConstants> - CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency, - ModuleConstants const &... modules) : - SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency, modules...} - { - if (utils::IsSimulation()) { - StartSimThread(); - } - } - - /** - * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. - * - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. - * - * \param driveTrainConstants Drivetrain-wide constants for the swerve drive - * \param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * \param odometryStandardDeviation The standard deviation for odometry calculation - * \param visionStandardDeviation The standard deviation for vision calculation - * \param modules Constants for each specific module - */ - template ... ModuleConstants> - CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency, - std::array const &odometryStandardDeviation, std::array const &visionStandardDeviation, - ModuleConstants const &... modules) : - SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules...} - { - if (utils::IsSimulation()) { - StartSimThread(); - } - } - - /** - * \brief Returns a command that applies the specified control request to this swerve drivetrain. - * - * This captures the returned swerve request by reference, so it must live - * for at least as long as the drivetrain. This can be done by storing the - * request as a member variable of your drivetrain subsystem or robot. - * - * \param request Function returning the request to apply - * \returns Command to run - */ - template - requires std::is_lvalue_reference_v> && - requires(RequestSupplier req, swerve::SwerveDrivetrain &drive) { drive.SetControl(req()); } - frc2::CommandPtr ApplyRequest(RequestSupplier request) - { - return Run([this, request=std::move(request)] { - return SetControl(request()); - }); - } - - /** - * \brief Returns a command that applies the specified control request to this swerve drivetrain. - * - * \param request Function returning the request to apply - * \returns Command to run - */ - template - requires std::is_rvalue_reference_v> && - requires(RequestSupplier req, swerve::SwerveDrivetrain &drive) { drive.SetControl(req()); } - frc2::CommandPtr ApplyRequest(RequestSupplier request) - { - return Run([this, request=std::move(request)] { - return SetControl(request()); - }); - } - - /** - * \brief Follows the given field-centric path sample with PID. - * - * \param pose Current pose of the robot - * \param sample Sample along the path to follow - */ - void FollowPath(frc::Pose2d const &pose, choreo::SwerveSample const &sample); - - void Periodic() override; - - /** - * \brief Runs the SysId Quasistatic test in the given direction for the routine - * specified by m_sysIdRoutineToApply. - * - * \param direction Direction of the SysId Quasistatic test - * \returns Command to run - */ - frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction) - { - return m_sysIdRoutineToApply->Quasistatic(direction); - } - - /** - * \brief Runs the SysId Dynamic test in the given direction for the routine - * specified by m_sysIdRoutineToApply. - * - * \param direction Direction of the SysId Dynamic test - * \returns Command to run - */ - frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction) - { - return m_sysIdRoutineToApply->Dynamic(direction); - } - -private: - void StartSimThread(); -}; - -} diff --git a/cpp/SwerveWithChoreo/src/test/cpp/main.cpp b/cpp/SwerveWithChoreo/src/test/cpp/main.cpp deleted file mode 100644 index b8b23d23..00000000 --- a/cpp/SwerveWithChoreo/src/test/cpp/main.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -#include "gtest/gtest.h" - -int main(int argc, char** argv) { - HAL_Initialize(500, 0); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/cpp/SwerveWithChoreo/tuner-project.json b/cpp/SwerveWithChoreo/tuner-project.json deleted file mode 100644 index 2e525562..00000000 --- a/cpp/SwerveWithChoreo/tuner-project.json +++ /dev/null @@ -1 +0,0 @@ -{"Version":"1.0.0.0","LastState":11,"Modules":[{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Left","ModuleId":0,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Right","ModuleId":1,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Left","ModuleId":2,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Right","ModuleId":3,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false}],"SwerveOptions":{"IsValidConfiguration":false,"Gyro":{"Id":1,"Name":"Pigeon 2 (Device ID 1)","Model":"Pigeon 2","CANbus":"","CANbusFriendly":""},"IsValidGyroCANbus":false,"VerticalTrackSizeInches":0.0,"HorizontalTrackSizeInches":0.0,"WheelRadiusInches":0.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":5,"SwerveModuleConfiguration":{"_CouplingRatio":0.0,"_CustomName":null,"SteerRatio":15.42857142857143,"Flipped":0,"DriveRatio":0.0,"CouplingRatio":0.0,"CustomName":null},"HasVerifiedSteer":false,"HasVerifiedDrive":false}} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json b/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json deleted file mode 100644 index 1f6b2b3f..00000000 --- a/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "fileName": "ChoreoLib2025Beta.json", - "name": "ChoreoLib", - "version": "2025.0.0-beta-6", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://SleipnirGroup.github.io/ChoreoLib/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.0-beta-6" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0-beta-6", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json b/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json deleted file mode 100644 index ff0b8c9e..00000000 --- a/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json +++ /dev/null @@ -1,359 +0,0 @@ -{ - "fileName": "Phoenix6-frc2025-beta-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-3", - "frcYear": "2025", - "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-frc2025-beta-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.0.0-beta-3" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.0.0-beta-3", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-3", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.0.0-beta-3", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-3", - "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": "25.0.0-beta-3", - "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": "25.0.0-beta-3", - "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": "25.0.0-beta-3", - "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": "25.0.0-beta-3", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", - "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": "25.0.0-beta-3", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json b/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json deleted file mode 100644 index 3718e0ac..00000000 --- a/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} diff --git a/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h b/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h index d9cbbb18..10f99f5d 100644 --- a/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h +++ b/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h @@ -13,7 +13,7 @@ class TunerConstants { // output type specified by SwerveModuleConstants::SteerMotorClosedLoopOutput static constexpr configs::Slot0Configs steerGains = configs::Slot0Configs{} .WithKP(100).WithKI(0).WithKD(0.5) - .WithKS(0.2).WithKV(1.59).WithKA(0) + .WithKS(0.1).WithKV(1.59).WithKA(0) .WithStaticFeedforwardSign(signals::StaticFeedforwardSignValue::UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants::DriveMotorClosedLoopOutput @@ -80,8 +80,8 @@ class TunerConstants { static constexpr units::kilogram_square_meter_t kSteerInertia = 0.01_kg_sq_m; static constexpr units::kilogram_square_meter_t kDriveInertia = 0.01_kg_sq_m; // Simulated voltage necessary to overcome friction - static constexpr units::volt_t kSteerFrictionVoltage = 0.25_V; - static constexpr units::volt_t kDriveFrictionVoltage = 0.25_V; + static constexpr units::volt_t kSteerFrictionVoltage = 0.2_V; + static constexpr units::volt_t kDriveFrictionVoltage = 0.2_V; public: static constexpr swerve::SwerveDrivetrainConstants DrivetrainConstants = swerve::SwerveDrivetrainConstants{} diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/generated/TunerConstants.java b/java/SwerveWithChoreo/src/main/java/frc/robot/generated/TunerConstants.java index 3c02fe0f..9c841f8f 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/generated/TunerConstants.java +++ b/java/SwerveWithChoreo/src/main/java/frc/robot/generated/TunerConstants.java @@ -27,7 +27,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.2).withKV(1.59).withKA(0) + .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -90,8 +90,8 @@ public class TunerConstants { private static final double kSteerInertia = 0.01; private static final double kDriveInertia = 0.01; // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.25); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANBusName(kCANBus.getName()) diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java index 3c02fe0f..9c841f8f 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java @@ -27,7 +27,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.2).withKV(1.59).withKA(0) + .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -90,8 +90,8 @@ public class TunerConstants { private static final double kSteerInertia = 0.01; private static final double kDriveInertia = 0.01; // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.25); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANBusName(kCANBus.getName()) diff --git a/python/SwerveWithPathPlanner/generated/tuner_constants.py b/python/SwerveWithPathPlanner/generated/tuner_constants.py index 9b7fe532..4d7b782b 100644 --- a/python/SwerveWithPathPlanner/generated/tuner_constants.py +++ b/python/SwerveWithPathPlanner/generated/tuner_constants.py @@ -18,7 +18,7 @@ class TunerConstants: .with_k_p(100) .with_k_i(0) .with_k_d(0.5) - .with_k_s(0.2) + .with_k_s(0.1) .with_k_v(1.59) .with_k_a(0) .with_static_feedforward_sign(signals.StaticFeedforwardSignValue.USE_CLOSED_LOOP_SIGN) @@ -87,8 +87,8 @@ class TunerConstants: _steer_inertia: units.kilogram_square_meter = 0.01 _drive_inertia: units.kilogram_square_meter = 0.01 # Simulated voltage necessary to overcome friction - _steer_friction_voltage: units.volt = 0.25 - _drive_friction_voltage: units.volt = 0.25 + _steer_friction_voltage: units.volt = 0.2 + _drive_friction_voltage: units.volt = 0.2 drivetrain_constants = ( swerve.SwerveDrivetrainConstants()