Skip to content

Commit e0a37d0

Browse files
author
Emily
committedJan 14, 2023
create 2023 project
0 parents  commit e0a37d0

30 files changed

+2751
-0
lines changed
 

‎.gitignore

+162
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
# This gitignore has been specially created by the WPILib team.
2+
# If you remove items from this file, intellisense might break.
3+
4+
### C++ ###
5+
# Prerequisites
6+
*.d
7+
8+
# Compiled Object files
9+
*.slo
10+
*.lo
11+
*.o
12+
*.obj
13+
14+
# Precompiled Headers
15+
*.gch
16+
*.pch
17+
18+
# Compiled Dynamic libraries
19+
*.so
20+
*.dylib
21+
*.dll
22+
23+
# Fortran module files
24+
*.mod
25+
*.smod
26+
27+
# Compiled Static libraries
28+
*.lai
29+
*.la
30+
*.a
31+
*.lib
32+
33+
# Executables
34+
*.exe
35+
*.out
36+
*.app
37+
38+
### Java ###
39+
# Compiled class file
40+
*.class
41+
42+
# Log file
43+
*.log
44+
45+
# BlueJ files
46+
*.ctxt
47+
48+
# Mobile Tools for Java (J2ME)
49+
.mtj.tmp/
50+
51+
# Package Files #
52+
*.jar
53+
*.war
54+
*.nar
55+
*.ear
56+
*.zip
57+
*.tar.gz
58+
*.rar
59+
60+
# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
61+
hs_err_pid*
62+
63+
### Linux ###
64+
*~
65+
66+
# temporary files which can be created if a process still has a handle open of a deleted file
67+
.fuse_hidden*
68+
69+
# KDE directory preferences
70+
.directory
71+
72+
# Linux trash folder which might appear on any partition or disk
73+
.Trash-*
74+
75+
# .nfs files are created when an open file is removed but is still being accessed
76+
.nfs*
77+
78+
### macOS ###
79+
# General
80+
.DS_Store
81+
.AppleDouble
82+
.LSOverride
83+
84+
# Icon must end with two \r
85+
Icon
86+
87+
# Thumbnails
88+
._*
89+
90+
# Files that might appear in the root of a volume
91+
.DocumentRevisions-V100
92+
.fseventsd
93+
.Spotlight-V100
94+
.TemporaryItems
95+
.Trashes
96+
.VolumeIcon.icns
97+
.com.apple.timemachine.donotpresent
98+
99+
# Directories potentially created on remote AFP share
100+
.AppleDB
101+
.AppleDesktop
102+
Network Trash Folder
103+
Temporary Items
104+
.apdisk
105+
106+
### VisualStudioCode ###
107+
.vscode/*
108+
!.vscode/settings.json
109+
!.vscode/tasks.json
110+
!.vscode/launch.json
111+
!.vscode/extensions.json
112+
113+
### Windows ###
114+
# Windows thumbnail cache files
115+
Thumbs.db
116+
ehthumbs.db
117+
ehthumbs_vista.db
118+
119+
# Dump file
120+
*.stackdump
121+
122+
# Folder config file
123+
[Dd]esktop.ini
124+
125+
# Recycle Bin used on file shares
126+
$RECYCLE.BIN/
127+
128+
# Windows Installer files
129+
*.cab
130+
*.msi
131+
*.msix
132+
*.msm
133+
*.msp
134+
135+
# Windows shortcuts
136+
*.lnk
137+
138+
### Gradle ###
139+
.gradle
140+
/build/
141+
142+
# Ignore Gradle GUI config
143+
gradle-app.setting
144+
145+
# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
146+
!gradle-wrapper.jar
147+
148+
# Cache of project
149+
.gradletasknamecache
150+
151+
# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
152+
# gradle/wrapper/gradle-wrapper.properties
153+
154+
# # VS Code Specific Java Settings
155+
# DO NOT REMOVE .classpath and .project
156+
.classpath
157+
.project
158+
.settings/
159+
bin/
160+
161+
# Simulation GUI and other tools window save file
162+
*-window.json

‎.vscode/launch.json

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
{
2+
// Use IntelliSense to learn about possible attributes.
3+
// Hover to view descriptions of existing attributes.
4+
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
5+
"version": "0.2.0",
6+
"configurations": [
7+
8+
{
9+
"type": "wpilib",
10+
"name": "WPILib Desktop Debug",
11+
"request": "launch",
12+
"desktop": true,
13+
},
14+
{
15+
"type": "wpilib",
16+
"name": "WPILib roboRIO Debug",
17+
"request": "launch",
18+
"desktop": false,
19+
}
20+
]
21+
}

‎.vscode/settings.json

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
{
2+
"java.configuration.updateBuildConfiguration": "automatic",
3+
"java.server.launchMode": "Standard",
4+
"files.exclude": {
5+
"**/.git": true,
6+
"**/.svn": true,
7+
"**/.hg": true,
8+
"**/CVS": true,
9+
"**/.DS_Store": true,
10+
"bin/": true,
11+
"**/.classpath": true,
12+
"**/.project": true,
13+
"**/.settings": true,
14+
"**/.factorypath": true,
15+
"**/*~": true
16+
},
17+
"java.test.config": [
18+
{
19+
"name": "WPIlibUnitTests",
20+
"workingDirectory": "${workspaceFolder}/build/jni/release",
21+
"vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
22+
"env": {
23+
"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
24+
"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
25+
}
26+
},
27+
],
28+
"java.test.defaultConfig": "WPIlibUnitTests"
29+
}

‎.wpilib/wpilib_preferences.json

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
{
2+
"enableCppIntellisense": false,
3+
"currentLanguage": "java",
4+
"projectYear": "2023",
5+
"teamNumber": 1306
6+
}

‎WPILib-License.md

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
Copyright (c) 2009-2021 FIRST and other WPILib contributors
2+
All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
* Redistributions of source code must retain the above copyright
7+
notice, this list of conditions and the following disclaimer.
8+
* Redistributions in binary form must reproduce the above copyright
9+
notice, this list of conditions and the following disclaimer in the
10+
documentation and/or other materials provided with the distribution.
11+
* Neither the name of FIRST, WPILib, nor the names of other WPILib
12+
contributors may be used to endorse or promote products derived from
13+
this software without specific prior written permission.
14+
15+
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
16+
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17+
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
18+
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
19+
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20+
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21+
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24+
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

‎build.gradle

+99
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
plugins {
2+
id "java"
3+
id "edu.wpi.first.GradleRIO" version "2023.1.1"
4+
}
5+
6+
sourceCompatibility = JavaVersion.VERSION_11
7+
targetCompatibility = JavaVersion.VERSION_11
8+
9+
def ROBOT_MAIN_CLASS = "frc.robot.Main"
10+
11+
// Define my targets (RoboRIO) and artifacts (deployable files)
12+
// This is added by GradleRIO's backing project DeployUtils.
13+
deploy {
14+
targets {
15+
roborio(getTargetTypeClass('RoboRIO')) {
16+
// Team number is loaded either from the .wpilib/wpilib_preferences.json
17+
// or from command line. If not found an exception will be thrown.
18+
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
19+
// want to store a team number in this file.
20+
team = project.frc.getTeamNumber()
21+
debug = project.frc.getDebugOrDefault(false)
22+
23+
artifacts {
24+
// First part is artifact name, 2nd is artifact type
25+
// getTargetTypeClass is a shortcut to get the class type using a string
26+
27+
frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
28+
}
29+
30+
// Static files artifact
31+
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
32+
files = project.fileTree('src/main/deploy')
33+
directory = '/home/lvuser/deploy'
34+
}
35+
}
36+
}
37+
}
38+
}
39+
40+
def deployArtifact = deploy.targets.roborio.artifacts.frcJava
41+
42+
// Set to true to use debug for JNI.
43+
wpi.java.debugJni = false
44+
45+
// Set this to true to enable desktop support.
46+
def includeDesktopSupport = true
47+
48+
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
49+
// Also defines JUnit 5.
50+
dependencies {
51+
implementation wpi.java.deps.wpilib()
52+
implementation wpi.java.vendor.java()
53+
54+
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
55+
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
56+
57+
roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
58+
roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
59+
60+
nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
61+
nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
62+
simulationDebug wpi.sim.enableDebug()
63+
64+
nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
65+
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
66+
simulationRelease wpi.sim.enableRelease()
67+
68+
testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2'
69+
testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2'
70+
testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2'
71+
}
72+
73+
test {
74+
useJUnitPlatform()
75+
systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
76+
}
77+
78+
// Simulation configuration (e.g. environment variables).
79+
wpi.sim.addGui().defaultEnabled = true
80+
wpi.sim.addDriverstation()
81+
82+
// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
83+
// in order to make them all available at runtime. Also adding the manifest so WPILib
84+
// knows where to look for our Robot Class.
85+
jar {
86+
from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
87+
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
88+
duplicatesStrategy = DuplicatesStrategy.INCLUDE
89+
}
90+
91+
// Configure jar and deploy tasks
92+
deployArtifact.jarTask = jar
93+
wpi.java.configureExecutableTasks(jar)
94+
wpi.java.configureTestTasks(test)
95+
96+
// Configure string concat to always inline compile
97+
tasks.withType(JavaCompile) {
98+
options.compilerArgs.add '-XDstringConcat=inline'
99+
}

‎gradle/wrapper/gradle-wrapper.jar

59.3 KB
Binary file not shown.

‎gradle/wrapper/gradle-wrapper.properties

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
distributionBase=GRADLE_USER_HOME
2+
distributionPath=permwrapper/dists
3+
distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
4+
zipStoreBase=GRADLE_USER_HOME
5+
zipStorePath=permwrapper/dists

‎gradlew

+240
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,240 @@
1+
#!/bin/sh
2+
3+
#
4+
# Copyright © 2015-2021 the original authors.
5+
#
6+
# Licensed under the Apache License, Version 2.0 (the "License");
7+
# you may not use this file except in compliance with the License.
8+
# You may obtain a copy of the License at
9+
#
10+
# https://www.apache.org/licenses/LICENSE-2.0
11+
#
12+
# Unless required by applicable law or agreed to in writing, software
13+
# distributed under the License is distributed on an "AS IS" BASIS,
14+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
# See the License for the specific language governing permissions and
16+
# limitations under the License.
17+
#
18+
19+
##############################################################################
20+
#
21+
# Gradle start up script for POSIX generated by Gradle.
22+
#
23+
# Important for running:
24+
#
25+
# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
26+
# noncompliant, but you have some other compliant shell such as ksh or
27+
# bash, then to run this script, type that shell name before the whole
28+
# command line, like:
29+
#
30+
# ksh Gradle
31+
#
32+
# Busybox and similar reduced shells will NOT work, because this script
33+
# requires all of these POSIX shell features:
34+
# * functions;
35+
# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
36+
# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
37+
# * compound commands having a testable exit status, especially «case»;
38+
# * various built-in commands including «command», «set», and «ulimit».
39+
#
40+
# Important for patching:
41+
#
42+
# (2) This script targets any POSIX shell, so it avoids extensions provided
43+
# by Bash, Ksh, etc; in particular arrays are avoided.
44+
#
45+
# The "traditional" practice of packing multiple parameters into a
46+
# space-separated string is a well documented source of bugs and security
47+
# problems, so this is (mostly) avoided, by progressively accumulating
48+
# options in "$@", and eventually passing that to Java.
49+
#
50+
# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
51+
# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
52+
# see the in-line comments for details.
53+
#
54+
# There are tweaks for specific operating systems such as AIX, CygWin,
55+
# Darwin, MinGW, and NonStop.
56+
#
57+
# (3) This script is generated from the Groovy template
58+
# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
59+
# within the Gradle project.
60+
#
61+
# You can find Gradle at https://github.com/gradle/gradle/.
62+
#
63+
##############################################################################
64+
65+
# Attempt to set APP_HOME
66+
67+
# Resolve links: $0 may be a link
68+
app_path=$0
69+
70+
# Need this for daisy-chained symlinks.
71+
while
72+
APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
73+
[ -h "$app_path" ]
74+
do
75+
ls=$( ls -ld "$app_path" )
76+
link=${ls#*' -> '}
77+
case $link in #(
78+
/*) app_path=$link ;; #(
79+
*) app_path=$APP_HOME$link ;;
80+
esac
81+
done
82+
83+
APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
84+
85+
APP_NAME="Gradle"
86+
APP_BASE_NAME=${0##*/}
87+
88+
# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
89+
DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
90+
91+
# Use the maximum available, or set MAX_FD != -1 to use that value.
92+
MAX_FD=maximum
93+
94+
warn () {
95+
echo "$*"
96+
} >&2
97+
98+
die () {
99+
echo
100+
echo "$*"
101+
echo
102+
exit 1
103+
} >&2
104+
105+
# OS specific support (must be 'true' or 'false').
106+
cygwin=false
107+
msys=false
108+
darwin=false
109+
nonstop=false
110+
case "$( uname )" in #(
111+
CYGWIN* ) cygwin=true ;; #(
112+
Darwin* ) darwin=true ;; #(
113+
MSYS* | MINGW* ) msys=true ;; #(
114+
NONSTOP* ) nonstop=true ;;
115+
esac
116+
117+
CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
118+
119+
120+
# Determine the Java command to use to start the JVM.
121+
if [ -n "$JAVA_HOME" ] ; then
122+
if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
123+
# IBM's JDK on AIX uses strange locations for the executables
124+
JAVACMD=$JAVA_HOME/jre/sh/java
125+
else
126+
JAVACMD=$JAVA_HOME/bin/java
127+
fi
128+
if [ ! -x "$JAVACMD" ] ; then
129+
die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
130+
131+
Please set the JAVA_HOME variable in your environment to match the
132+
location of your Java installation."
133+
fi
134+
else
135+
JAVACMD=java
136+
which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
137+
138+
Please set the JAVA_HOME variable in your environment to match the
139+
location of your Java installation."
140+
fi
141+
142+
# Increase the maximum file descriptors if we can.
143+
if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
144+
case $MAX_FD in #(
145+
max*)
146+
MAX_FD=$( ulimit -H -n ) ||
147+
warn "Could not query maximum file descriptor limit"
148+
esac
149+
case $MAX_FD in #(
150+
'' | soft) :;; #(
151+
*)
152+
ulimit -n "$MAX_FD" ||
153+
warn "Could not set maximum file descriptor limit to $MAX_FD"
154+
esac
155+
fi
156+
157+
# Collect all arguments for the java command, stacking in reverse order:
158+
# * args from the command line
159+
# * the main class name
160+
# * -classpath
161+
# * -D...appname settings
162+
# * --module-path (only if needed)
163+
# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
164+
165+
# For Cygwin or MSYS, switch paths to Windows format before running java
166+
if "$cygwin" || "$msys" ; then
167+
APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
168+
CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
169+
170+
JAVACMD=$( cygpath --unix "$JAVACMD" )
171+
172+
# Now convert the arguments - kludge to limit ourselves to /bin/sh
173+
for arg do
174+
if
175+
case $arg in #(
176+
-*) false ;; # don't mess with options #(
177+
/?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
178+
[ -e "$t" ] ;; #(
179+
*) false ;;
180+
esac
181+
then
182+
arg=$( cygpath --path --ignore --mixed "$arg" )
183+
fi
184+
# Roll the args list around exactly as many times as the number of
185+
# args, so each arg winds up back in the position where it started, but
186+
# possibly modified.
187+
#
188+
# NB: a `for` loop captures its iteration list before it begins, so
189+
# changing the positional parameters here affects neither the number of
190+
# iterations, nor the values presented in `arg`.
191+
shift # remove old arg
192+
set -- "$@" "$arg" # push replacement arg
193+
done
194+
fi
195+
196+
# Collect all arguments for the java command;
197+
# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
198+
# shell script including quotes and variable substitutions, so put them in
199+
# double quotes to make sure that they get re-expanded; and
200+
# * put everything else in single quotes, so that it's not re-expanded.
201+
202+
set -- \
203+
"-Dorg.gradle.appname=$APP_BASE_NAME" \
204+
-classpath "$CLASSPATH" \
205+
org.gradle.wrapper.GradleWrapperMain \
206+
"$@"
207+
208+
# Stop when "xargs" is not available.
209+
if ! command -v xargs >/dev/null 2>&1
210+
then
211+
die "xargs is not available"
212+
fi
213+
214+
# Use "xargs" to parse quoted args.
215+
#
216+
# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
217+
#
218+
# In Bash we could simply go:
219+
#
220+
# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
221+
# set -- "${ARGS[@]}" "$@"
222+
#
223+
# but POSIX shell has neither arrays nor command substitution, so instead we
224+
# post-process each arg (as a line of input to sed) to backslash-escape any
225+
# character that might be a shell metacharacter, then use eval to reverse
226+
# that process (while maintaining the separation between arguments), and wrap
227+
# the whole thing up as a single "set" statement.
228+
#
229+
# This will of course break if any of these variables contains a newline or
230+
# an unmatched quote.
231+
#
232+
233+
eval "set -- $(
234+
printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
235+
xargs -n1 |
236+
sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
237+
tr '\n' ' '
238+
)" '"$@"'
239+
240+
exec "$JAVACMD" "$@"

‎gradlew.bat

+91
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
@rem
2+
@rem Copyright 2015 the original author or authors.
3+
@rem
4+
@rem Licensed under the Apache License, Version 2.0 (the "License");
5+
@rem you may not use this file except in compliance with the License.
6+
@rem You may obtain a copy of the License at
7+
@rem
8+
@rem https://www.apache.org/licenses/LICENSE-2.0
9+
@rem
10+
@rem Unless required by applicable law or agreed to in writing, software
11+
@rem distributed under the License is distributed on an "AS IS" BASIS,
12+
@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
@rem See the License for the specific language governing permissions and
14+
@rem limitations under the License.
15+
@rem
16+
17+
@if "%DEBUG%"=="" @echo off
18+
@rem ##########################################################################
19+
@rem
20+
@rem Gradle startup script for Windows
21+
@rem
22+
@rem ##########################################################################
23+
24+
@rem Set local scope for the variables with windows NT shell
25+
if "%OS%"=="Windows_NT" setlocal
26+
27+
set DIRNAME=%~dp0
28+
if "%DIRNAME%"=="" set DIRNAME=.
29+
set APP_BASE_NAME=%~n0
30+
set APP_HOME=%DIRNAME%
31+
32+
@rem Resolve any "." and ".." in APP_HOME to make it shorter.
33+
for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
34+
35+
@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
36+
set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
37+
38+
@rem Find java.exe
39+
if defined JAVA_HOME goto findJavaFromJavaHome
40+
41+
set JAVA_EXE=java.exe
42+
%JAVA_EXE% -version >NUL 2>&1
43+
if %ERRORLEVEL% equ 0 goto execute
44+
45+
echo.
46+
echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
47+
echo.
48+
echo Please set the JAVA_HOME variable in your environment to match the
49+
echo location of your Java installation.
50+
51+
goto fail
52+
53+
:findJavaFromJavaHome
54+
set JAVA_HOME=%JAVA_HOME:"=%
55+
set JAVA_EXE=%JAVA_HOME%/bin/java.exe
56+
57+
if exist "%JAVA_EXE%" goto execute
58+
59+
echo.
60+
echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
61+
echo.
62+
echo Please set the JAVA_HOME variable in your environment to match the
63+
echo location of your Java installation.
64+
65+
goto fail
66+
67+
:execute
68+
@rem Setup the command line
69+
70+
set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
71+
72+
73+
@rem Execute Gradle
74+
"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
75+
76+
:end
77+
@rem End local scope for the variables with windows NT shell
78+
if %ERRORLEVEL% equ 0 goto mainEnd
79+
80+
:fail
81+
rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
82+
rem the _cmd.exe /c_ return code!
83+
set EXIT_CODE=%ERRORLEVEL%
84+
if %EXIT_CODE% equ 0 set EXIT_CODE=1
85+
if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
86+
exit /b %EXIT_CODE%
87+
88+
:mainEnd
89+
if "%OS%"=="Windows_NT" endlocal
90+
91+
:omega

‎settings.gradle

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
import org.gradle.internal.os.OperatingSystem
2+
3+
pluginManagement {
4+
repositories {
5+
mavenLocal()
6+
gradlePluginPortal()
7+
String frcYear = '2023'
8+
File frcHome
9+
if (OperatingSystem.current().isWindows()) {
10+
String publicFolder = System.getenv('PUBLIC')
11+
if (publicFolder == null) {
12+
publicFolder = "C:\\Users\\Public"
13+
}
14+
def homeRoot = new File(publicFolder, "wpilib")
15+
frcHome = new File(homeRoot, frcYear)
16+
} else {
17+
def userFolder = System.getProperty("user.home")
18+
def homeRoot = new File(userFolder, "wpilib")
19+
frcHome = new File(homeRoot, frcYear)
20+
}
21+
def frcHomeMaven = new File(frcHome, 'maven')
22+
maven {
23+
name 'frcHome'
24+
url frcHomeMaven
25+
}
26+
}
27+
}

‎src/main/java/frc/robot/Constants.java

+59
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
package frc.robot;
2+
3+
import java.util.List;
4+
5+
import edu.wpi.first.math.geometry.Pose2d;
6+
import edu.wpi.first.math.geometry.Rotation2d;
7+
import edu.wpi.first.math.trajectory.Trajectory;
8+
import edu.wpi.first.math.trajectory.TrajectoryConfig;
9+
import edu.wpi.first.math.util.Units;
10+
11+
/**
12+
* A class for holding constant values in a single editable spot. What goes in this file: -Robot Motorcontroller ports
13+
* and IDS -Robot phsyical attributes -Field attributes
14+
*
15+
* Things that don't go in this file: -Subsystem-specific PID values -Subsystem-specific sensor thresholds
16+
*
17+
* Constants is interface as interface fields are public, static, and final by default
18+
*
19+
*/
20+
@SuppressWarnings("unused")
21+
public interface Constants {
22+
int FRONT_SHOOTER_ID = 2;
23+
int BACK_SHOOTER_ID = 1;
24+
int SHOOTER_KICKER_ID = 7;
25+
int FRONT_INDEX_ID = 4;
26+
int BACK_INDEX_ID = 3;
27+
int EXTERNAL_INTAKE_ID = 10;
28+
29+
30+
int DRIVE_RIGHT_LEADER_ID = 5;
31+
int DRIVE_RIGHT_FOLLOWER_ID = 6;
32+
int DRIVE_LEFT_LEADER_ID = 8;
33+
int DRIVE_LEFT_FOLLOWER_ID = 9;
34+
35+
// TODO set actual climber id
36+
int CLIMBER_MOTOR_ID = 0;
37+
38+
double CLIMBER_VELOCITY = 0.5;
39+
40+
double K_S = .61784;
41+
double K_V = 2.0067;
42+
double K_A = .28138;
43+
44+
double K_P = 2.4821;
45+
double K_I = 0;
46+
double K_D = 0;
47+
double K_RAMSETE_B = 2;
48+
double K_RAMSETE_ZETA = .7;
49+
50+
double TRACK_WIDTH_METERS = .60;
51+
double MAX_SPEED_MPS = 4.67;
52+
double MAX_ACCELERATION_MPSS = 3;
53+
double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(4) * Math.PI;
54+
55+
double MAX_DIFF_ACCELERATION = 0;
56+
57+
double TIME_TO_FULL_SPEED = .25;
58+
double TIME_PER_LOOP = .02;
59+
}

‎src/main/java/frc/robot/Main.java

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot;
9+
10+
import edu.wpi.first.wpilibj.RobotBase;
11+
12+
/**
13+
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
14+
* you are doing, do not modify this file except to change the parameter class to the startRobot
15+
* call.
16+
*/
17+
public final class Main {
18+
private Main() {}
19+
20+
/**
21+
* Main initialization function. Do not perform any initialization here.
22+
*
23+
* <p>
24+
* If you change your main robot class, change the parameter type.
25+
*
26+
* @param args arguments
27+
*/
28+
public static void main(String... args) {
29+
System.out.println("Robot is Running");
30+
RobotBase.startRobot(Robot::new);
31+
System.out.println("ROBOT HAS STARTED");
32+
}
33+
}

‎src/main/java/frc/robot/Robot.java

+194
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,194 @@
1+
2+
/*----------------------------------------------------------------------------*/
3+
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
4+
/* Open Source Software - may be modified and shared by FRC teams. The code */
5+
/* must be accompanied by the FIRST BSD license file in the root directory of */
6+
/* the project. */
7+
/*----------------------------------------------------------------------------*/
8+
9+
package frc.robot;
10+
11+
import org.opencv.core.Mat;
12+
import org.opencv.core.Point;
13+
import org.opencv.core.Scalar;
14+
import org.opencv.imgproc.Imgproc;
15+
16+
import com.revrobotics.REVPhysicsSim;
17+
18+
import edu.wpi.first.apriltag.AprilTagDetector;
19+
import edu.wpi.first.cameraserver.CameraServer;
20+
import edu.wpi.first.cscore.CvSource;
21+
import edu.wpi.first.vision.VisionThread;
22+
import edu.wpi.first.wpilibj.TimedRobot;
23+
import edu.wpi.first.wpilibj2.command.CommandScheduler;
24+
import edu.wpi.first.wpilibj2.command.RamseteCommand;
25+
// import frc.robot.Constants;
26+
// import frc.robot.subsystems.SwerveDrive;
27+
import frc.robot.subsystems.DriveTrain;
28+
import edu.wpi.first.apriltag.AprilTagDetector.Config;
29+
30+
/**
31+
* The VM is configured to automatically run this class, and to call the
32+
* functions corresponding to each mode, as
33+
* described in the TimedRobot documentation. If you change the name of this
34+
* class or the package after creating this
35+
* project, you must also update the build.gradle file in the project.
36+
*/
37+
public class Robot extends TimedRobot {
38+
39+
public static DriveTrain driveTrain;
40+
public static RamseteCommand autocmd;
41+
// private static Command testCommand = null;
42+
43+
// private RobotContainer m_robotContainer;
44+
45+
// messing around w/ apriltag detector & video stuff
46+
private AprilTagDetector detector = new AprilTagDetector();
47+
48+
private CvSource out, out2;
49+
private VisionThread m_VisionThread;
50+
51+
// public static Command driveCommand;
52+
53+
// public static Intake intake = null;
54+
55+
/**
56+
* This function is run when the robot is first started up and should be used
57+
* for any initialization code.
58+
*/
59+
@Override
60+
public void robotInit() {
61+
// configure apriltag detector
62+
detector.addFamily("tag16h5");
63+
var config = new Config();
64+
config.numThreads = 4;
65+
config.quadDecimate = 1;
66+
config.quadSigma = 0;
67+
config.refineEdges = true;
68+
detector.setConfig(config);
69+
70+
// get camera input
71+
var cam = CameraServer.startAutomaticCapture();
72+
cam.setResolution(480, 270);
73+
74+
// output video stream, one for color & markings, one for black & white
75+
out = CameraServer.putVideo("out", 480, 270);
76+
out2 = CameraServer.putVideo("bw", 480, 270);
77+
78+
m_VisionThread = new VisionThread(cam, mat -> {
79+
// create matrix for modifications, and copy input from camera
80+
Mat tempMat = Mat.zeros(mat.size(), mat.type());
81+
mat.copyTo(tempMat);
82+
// convert to grayscale and filter brightness => 120 -> white and < 120 -> black
83+
Imgproc.cvtColor(tempMat, tempMat, Imgproc.COLOR_BGR2GRAY);
84+
Imgproc.threshold(tempMat, tempMat, 120, 255, Imgproc.THRESH_BINARY);
85+
// detect tags
86+
var dets = detector.detect(tempMat);
87+
if (dets.length > 0) {
88+
// only valid tags from 1 to 8
89+
for (var det : dets) {
90+
if (det.getId() > 8 || det.getId() < 1)
91+
continue;
92+
// draw center and corners on colored output matrix
93+
Imgproc.drawMarker(mat, new Point(det.getCenterX(), det.getCenterY()), new Scalar(0, 255, 0));
94+
for (int i = 0; i < det.getCorners().length / 2; i++) {
95+
Imgproc.drawMarker(
96+
mat, new Point(det.getCornerX(i), det.getCornerY(i)),
97+
new Scalar(0, 0, 255));
98+
}
99+
}
100+
}
101+
// output frames to camera server in shuffleboard
102+
out.putFrame(mat);
103+
out2.putFrame(tempMat);
104+
}, a -> {
105+
// do nothing
106+
});
107+
// run thread
108+
m_VisionThread.setDaemon(true);
109+
m_VisionThread.start();
110+
111+
}
112+
113+
/**
114+
* This function is called every robot packet, no matter the mode. Use this for
115+
* items like diagnostics that you want
116+
* ran during disabled, autonomous, teleoperated and test.
117+
*
118+
* <p>
119+
* This runs after the mode specific periodic functions, but before LiveWindow
120+
* and SmartDashboard integrated
121+
* updating.
122+
*/
123+
@Override
124+
public void robotPeriodic() {
125+
// Runs the Scheduler. This is responsible for polling buttons, adding
126+
// newly-scheduled
127+
// commands, running already- scheduled commands, removing finished or
128+
// interrupted commands,
129+
// and running subsystem periodic() methods. This must be called from the
130+
// robot's periodic
131+
// block in order for anything in the Command-based framework to work.
132+
CommandScheduler.getInstance().run();
133+
}
134+
135+
/**
136+
* This function is called once each time the robot enters Disabled mode.
137+
*/
138+
@Override
139+
public void disabledInit() {
140+
}
141+
142+
@Override
143+
public void disabledPeriodic() {
144+
}
145+
146+
/**
147+
* This autonomous runs the autonomous command selected by your
148+
* {@link RobotContainer} class.
149+
*/
150+
@Override
151+
public void autonomousInit() {
152+
// m_robotContainer.startAuto();
153+
}
154+
155+
/**
156+
* This function is called periodically during autonomous.
157+
*/
158+
@Override
159+
public void autonomousPeriodic() {
160+
161+
}
162+
163+
@Override
164+
public void teleopInit() {
165+
// m_robotContainer.startTeleop();
166+
}
167+
168+
/**
169+
* This function is called periodically during operator control.
170+
*/
171+
@Override
172+
public void teleopPeriodic() {
173+
174+
}
175+
176+
@Override
177+
public void testInit() {
178+
// Cancels all running commands at the start of test mode.
179+
CommandScheduler.getInstance().cancelAll();
180+
}
181+
182+
/**
183+
* This function is called periodically during test mode.
184+
*/
185+
@Override
186+
public void testPeriodic() {
187+
}
188+
189+
@Override
190+
public void simulationPeriodic() {
191+
CommandScheduler.getInstance().run();
192+
REVPhysicsSim.getInstance().run();
193+
}
194+
}

‎src/main/java/frc/robot/RobotContainer.java

+121
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot;
9+
10+
import java.util.ResourceBundle.Control;
11+
import java.util.concurrent.atomic.AtomicBoolean;
12+
13+
import edu.wpi.first.wpilibj.GenericHID;
14+
import edu.wpi.first.wpilibj.XboxController;
15+
import edu.wpi.first.wpilibj2.command.Command;
16+
import edu.wpi.first.wpilibj2.command.RamseteCommand;
17+
import edu.wpi.first.wpilibj2.command.WaitCommand;
18+
import frc.robot.commands.DriveCommand;
19+
import frc.robot.subsystems.DriveTrain;
20+
import frc.robot.utils.Controller;
21+
import frc.robot.utils.REVDigitBoard;
22+
import frc.robot.utils.UserAnalog;
23+
import frc.robot.utils.UserDigital;
24+
25+
/**
26+
* This class is where the bulk of the robot should be declared. Since
27+
* Command-based is a "declarative" paradigm, very
28+
* little robot logic should actually be handled in the {@link Robot} periodic
29+
* methods (other than the scheduler calls).
30+
* Instead, the structure of the robot (including subsystems, commands, and
31+
* button mappings) should be declared here.
32+
*/
33+
@SuppressWarnings("unused")
34+
public class RobotContainer {
35+
// The robot's subsystems and commands are defined here...
36+
private Command autoCommand;
37+
private Command driveCommand;
38+
private DriveTrain driveTrain;
39+
40+
private final boolean RUN_AUTO = true;
41+
42+
// inputs for drive train
43+
private UserAnalog speedDriveTrain;
44+
private UserAnalog backwardsTurbo;
45+
private UserAnalog forwardTurbo;
46+
private UserAnalog joystickRotationDriveTrain;
47+
48+
public static double RC_MAX_SPEED;
49+
50+
// The robot's inputs that it recieves from the controller are defined here
51+
52+
/**
53+
* The container for the robot. Contains subsystems, OI devices.
54+
*/
55+
public RobotContainer() {
56+
// initialize controller using Controller init method to ensure Controller is
57+
// properly initialized when the
58+
// Controllern is required to be initialized at the start of the game.
59+
// Controller represents an xbox controller
60+
// which controls the robot using standard controller inputs such as joysticks,
61+
// buttons, and triggers.
62+
Controller.init();
63+
configureButtonBindings();
64+
65+
driveTrain = new DriveTrain();
66+
driveCommand = new DriveCommand(
67+
driveTrain,
68+
speedDriveTrain,
69+
backwardsTurbo,
70+
forwardTurbo,
71+
joystickRotationDriveTrain);
72+
}
73+
74+
/**
75+
* Use this method to define your button->command mappings. Buttons can be
76+
* created by instantiating a
77+
* {@link GenericHID} or one of its subclasses
78+
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}),
79+
* and then passing it to a
80+
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
81+
*/
82+
private void configureButtonBindings() {
83+
speedDriveTrain = Controller.simpleAxis(Controller.PRIMARY, Controller.AXIS_LY);
84+
backwardsTurbo = Controller.simpleAxis(Controller.PRIMARY, Controller.AXIS_LTRIGGER);
85+
forwardTurbo = Controller.simpleAxis(Controller.PRIMARY, Controller.AXIS_RTRIGGER);
86+
joystickRotationDriveTrain = Controller.simpleAxis(Controller.PRIMARY, Controller.AXIS_LX);
87+
}
88+
89+
/**
90+
* called when autonomous is started should create all commands that are used in
91+
* auto
92+
*/
93+
public void startAuto() {
94+
autoCommand = getAutonomousCommand();
95+
if (RUN_AUTO) {
96+
driveCommand.cancel();
97+
autoCommand.schedule();
98+
}
99+
// System.out.println("startAuto() IS RUNNING.");
100+
}
101+
102+
/**
103+
* start off teleop period by cancelling autonomous command and switching the
104+
* drivetrain command to the user driving
105+
* command
106+
*
107+
*/
108+
public void startTeleop() {
109+
// This makes sure that the autonomous stops running when teleop starts running.
110+
// If you want the autonomous to
111+
// continue until interrupted by another command, remove this line or comment it
112+
// out.
113+
if (RUN_AUTO)
114+
autoCommand.cancel();
115+
}
116+
117+
public Command getAutonomousCommand() {
118+
return null;
119+
}
120+
121+
}

‎src/main/java/frc/robot/commands/DriveCommand.java

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.wpilibj2.command.CommandBase;
4+
import frc.robot.utils.UserAnalog;
5+
import frc.robot.RobotContainer;
6+
import frc.robot.subsystems.DriveTrain;
7+
8+
/**
9+
* Command to drive the robot based on controller input
10+
*/
11+
public class DriveCommand extends CommandBase {
12+
13+
private UserAnalog speed;
14+
// private UserAnalog rightSpeed;
15+
private DriveTrain driveTrain;
16+
private UserAnalog backwardsTurbo;
17+
private UserAnalog forwardTurbo;
18+
private UserAnalog joystickRotation;
19+
private double max_spd = .65;
20+
21+
/**
22+
* initalizes drive command from given drivetrain, speed, and rotation
23+
*
24+
* @param driveTrain drivetrain to bind
25+
* @param speed initial speed
26+
* @param leftRotation initial left rotation
27+
* @param rightRotation initial right rotation
28+
*/
29+
public DriveCommand(
30+
DriveTrain driveTrain,
31+
UserAnalog speed,
32+
UserAnalog backwardsTurbo,
33+
UserAnalog forwardTurbo,
34+
UserAnalog joystickRotation) {
35+
this.speed = speed;
36+
this.driveTrain = driveTrain;
37+
this.addRequirements(driveTrain);
38+
this.backwardsTurbo = backwardsTurbo;
39+
this.forwardTurbo = forwardTurbo;
40+
this.joystickRotation = joystickRotation;
41+
}
42+
43+
/**
44+
* called repeatedly when command is schedules to run
45+
*/
46+
47+
@Override
48+
public void execute() {
49+
double spd = speed.get();
50+
double rotation = -joystickRotation.get();
51+
52+
if (RobotContainer.RC_MAX_SPEED < .65 || RobotContainer.RC_MAX_SPEED > 1) {
53+
max_spd = .75;
54+
} else {
55+
max_spd = RobotContainer.RC_MAX_SPEED;
56+
}
57+
58+
if (Math.abs(forwardTurbo.get() - backwardsTurbo.get()) > .05) {
59+
spd = max_spd * (backwardsTurbo.get() - forwardTurbo.get());
60+
} else {
61+
spd = 0;
62+
}
63+
64+
if (Math.abs(rotation) < .05) {
65+
rotation = 0;
66+
}
67+
68+
// SmartDashboard.putNumber("Target Speed Drive Command", spd);
69+
// SmartDashboard.putNumber("max speed mult", max_spd);
70+
driveTrain.arcadeDrive(spd, .8 * rotation);
71+
72+
}
73+
74+
// the current speed, the current stick position
75+
}

‎src/main/java/frc/robot/commands/TimedCommand.java

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.wpilibj.Timer;
4+
import edu.wpi.first.wpilibj2.command.CommandBase;
5+
6+
public class TimedCommand extends CommandBase {
7+
private Timer timer;
8+
private double timeoutS;
9+
10+
public TimedCommand(double timeoutS) {
11+
this.timeoutS = timeoutS;
12+
}
13+
14+
@Override
15+
public void initialize() {
16+
timer = new Timer();
17+
timer.start();
18+
}
19+
20+
@Override
21+
public boolean isFinished() {
22+
return timer.hasElapsed(timeoutS);
23+
}
24+
}

‎src/main/java/frc/robot/subsystems/DriveTrain.java

+335
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,335 @@
1+
package frc.robot.subsystems;
2+
3+
import static frc.robot.Constants.*;
4+
import static frc.robot.utils.MotorUtils.*;
5+
6+
import java.util.concurrent.locks.LockSupport;
7+
8+
import com.ctre.phoenix.motorcontrol.ControlMode;
9+
import com.ctre.phoenix.motorcontrol.can.TalonFX;
10+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
11+
import com.kauailabs.navx.frc.AHRS;
12+
13+
import edu.wpi.first.math.controller.PIDController;
14+
import edu.wpi.first.math.geometry.Pose2d;
15+
import edu.wpi.first.math.geometry.Rotation2d;
16+
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
17+
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
18+
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
19+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
20+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
21+
import frc.robot.Constants;
22+
23+
/**
24+
* Used by DriveTrain command to move robot Calculates output for each side of
25+
* the drivetrain
26+
*/
27+
@SuppressWarnings("unused")
28+
public class DriveTrain extends SubsystemBase implements AutoCloseable {
29+
private WPI_TalonFX leftLeader;
30+
private WPI_TalonFX leftFollower;
31+
32+
private WPI_TalonFX rightLeader;
33+
private WPI_TalonFX rightFollower;
34+
// private AHRS gyro = new AHRS();
35+
private final DifferentialDriveOdometry m_odometry;
36+
private PIDController controller;
37+
38+
private BuiltInAccelerometer accelerometer;
39+
private double previousRightPercentOutput;
40+
41+
private double previousLeftPercentOutput;
42+
43+
/**
44+
* Initializing drive train and talonmFX settings
45+
*/
46+
public DriveTrain() {
47+
leftLeader = initWPITalonFX(DRIVE_LEFT_LEADER_ID);
48+
rightLeader = initWPITalonFX(DRIVE_RIGHT_LEADER_ID);
49+
leftFollower = initWPITalonFX(DRIVE_LEFT_FOLLOWER_ID);
50+
rightFollower = initWPITalonFX(DRIVE_RIGHT_FOLLOWER_ID);
51+
52+
accelerometer = new BuiltInAccelerometer();
53+
// leftLeader.config_kP(0, .5);
54+
// rightLeader.config_kP(0, .5);
55+
56+
leftFollower.follow(leftLeader);
57+
rightFollower.follow(rightLeader);
58+
59+
m_odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
60+
61+
// leftLeader.config_kP(0, .25);
62+
// leftLeader.config_kI(0, 0);
63+
// leftLeader.config_kD(0, 0);
64+
// leftLeader.setControlFramePeriod(0, 20);
65+
// leftLeader.configClosedloopRamp(1);
66+
67+
// rightLeader.config_kP(0, .25);
68+
// rightLeader.config_kI(0, 0);
69+
// rightLeader.config_kD(0, 0);
70+
// rightLeader.setControlFramePeriod(0, 20);
71+
// rightLeader.configClosedloopRamp(1);
72+
// gyro.reset();
73+
previousRightPercentOutput = 0;
74+
previousLeftPercentOutput = 0;
75+
}
76+
77+
/**
78+
* Sets the talonmFX speeds for the given speed and rotation
79+
*
80+
* @param speed speed from a joystick input
81+
* @param rotation rotation from joystick triggers
82+
*/
83+
public void arcadeDrive(double speed, double rotation) {
84+
double maxInput = Math.copySign(Math.max(Math.abs(speed), Math.abs(rotation)), speed);
85+
double leftMotorOutput;
86+
double rightMotorOutput;
87+
88+
// speed is -1 to 1, rotation is also -1 to 1
89+
if (speed >= 0) {
90+
if (rotation >= 0) {
91+
leftMotorOutput = maxInput;
92+
rightMotorOutput = speed - rotation;
93+
} else {
94+
leftMotorOutput = speed + rotation;
95+
rightMotorOutput = maxInput;
96+
}
97+
} else {
98+
if (rotation >= 0) {
99+
leftMotorOutput = speed + rotation;
100+
rightMotorOutput = maxInput;
101+
} else {
102+
leftMotorOutput = maxInput;
103+
rightMotorOutput = speed - rotation;
104+
}
105+
}
106+
107+
leftMotorOutput = limitAcceleration(leftMotorOutput, previousLeftPercentOutput);
108+
rightMotorOutput = limitAcceleration(rightMotorOutput, previousRightPercentOutput);
109+
110+
System.out.printf("left: %.02f, right : %.02f\n", leftMotorOutput, rightMotorOutput);
111+
112+
leftLeader.set(-leftMotorOutput);
113+
rightLeader.set(rightMotorOutput);
114+
115+
previousLeftPercentOutput = leftMotorOutput;
116+
previousRightPercentOutput = rightMotorOutput;
117+
}
118+
119+
/**
120+
* limits acceleration for the robot, should prevent rocking does not prevent
121+
* rapid decceleration
122+
*
123+
* @param currentTargetPercentOutput target output for motor that user inputed
124+
* @param previousPercentOutput previous outputed target for motor
125+
* @return a target motor value
126+
*/
127+
public double limitAcceleration(
128+
double currentTargetPercentOutput,
129+
double previousPercentOutput) {
130+
// increment
131+
final double INCR = Constants.TIME_PER_LOOP / Constants.TIME_TO_FULL_SPEED;
132+
// change that the user wants
133+
double error = currentTargetPercentOutput - previousPercentOutput;
134+
135+
// target is going towards 0
136+
boolean isDecel = Math.abs(currentTargetPercentOutput) < .05;
137+
138+
if (isDecel)
139+
return currentTargetPercentOutput;
140+
141+
// divide that change over a period of time
142+
// if the change in acceleration is too large positively, accelerate slower
143+
if (error > INCR)
144+
return previousPercentOutput + INCR;
145+
146+
// the change in acceleration is too large negatively, accelerate to the
147+
// negative direction slower
148+
if (error < -INCR)
149+
return previousPercentOutput - INCR;
150+
151+
return currentTargetPercentOutput;
152+
}
153+
154+
/**
155+
* Test the lead motors and folowing motors test to see if initialization
156+
* process for setting 'following' is correct
157+
*
158+
* @param left left talonmFX output
159+
* @param right right talonmFX output
160+
*/
161+
public void testDrive(double left, double right) {
162+
leftLeader.set(ControlMode.PercentOutput, left);
163+
rightLeader.set(ControlMode.PercentOutput, right);
164+
}
165+
166+
/**
167+
* testing method for eaching individual talonmFX only works if constructor does
168+
* not set follow
169+
*
170+
* @param leftFront left front talonmFX output
171+
* @param rightFront right front talonmFX output
172+
* @param leftFollow left follow talonmFX output
173+
* @param rightFollow right followe talonmFX output
174+
*/
175+
public void testMotors(
176+
double leftFront,
177+
double rightFront,
178+
double leftFollow,
179+
double rightFollow) {
180+
leftLeader.set(ControlMode.PercentOutput, leftFront);
181+
rightLeader.set(ControlMode.PercentOutput, rightFront);
182+
183+
// experimental
184+
leftFollower.follow(leftFollower);
185+
rightFollower.follow(rightFollower);
186+
187+
leftFollower.set(ControlMode.PercentOutput, leftFollow);
188+
rightFollower.set(ControlMode.PercentOutput, rightFollow);
189+
190+
// experimental
191+
leftFollower.follow(leftLeader);
192+
rightFollower.follow(rightLeader);
193+
}
194+
195+
/**
196+
* take encoder ticks displacement of left wheels and convert into meters
197+
*
198+
* @return displacement of left wheels in meters
199+
*/
200+
public double getLeftDistance() {
201+
double encoderTicks = leftLeader.getSelectedSensorPosition();
202+
double distance = encoderTicks / 2048 / 5.88 * Constants.WHEEL_CIRCUMFERENCE;
203+
return distance;
204+
}
205+
206+
/**
207+
* Take encoder ticks displacement of right wheels and convert into meters
208+
*
209+
* @return displacement of right wheels in meters
210+
*/
211+
public double getRightDistance() {
212+
double encoderTicks = rightLeader.getSelectedSensorPosition();
213+
double distance = encoderTicks / 2048 / 5.88 * Constants.WHEEL_CIRCUMFERENCE;
214+
return distance;
215+
}
216+
217+
int disp, vel;
218+
219+
@Override
220+
public void periodic() {
221+
// SmartDashboard.putNumber("Acceleration in x", accelerometer.getX());
222+
223+
// sketchy distance
224+
vel += accelerometer.getX();
225+
disp += vel;
226+
227+
m_odometry.update(new Rotation2d(), getLeftDistance(), getRightDistance());
228+
229+
// SmartDashboard.putNumber("Movement in x", m_odometry.getPoseMeters().getX());
230+
}
231+
232+
public Pose2d getPose() {
233+
return m_odometry.getPoseMeters();
234+
}
235+
236+
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
237+
// encoder ticks per 100 ms
238+
// rotations per second
239+
// meters per second
240+
double leftSpeed = leftLeader.getSelectedSensorVelocity()
241+
* (10.0 / 2048 / 5.88) // dm -> m, et -> rot, gear ratio
242+
* Constants.WHEEL_CIRCUMFERENCE;
243+
// SmartDashboard.putNumber("leftSpeed", leftSpeed);
244+
double rightSpeed = rightLeader.getSelectedSensorVelocity()
245+
* (10.0 / 2048 / 5.88)
246+
* Constants.WHEEL_CIRCUMFERENCE;
247+
// SmartDashboard.putNumber("rightSpeed", rightSpeed);
248+
249+
return new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
250+
}
251+
252+
public void resetOdometry(Pose2d pose) {
253+
resetEncoders();
254+
m_odometry.resetPosition(new Rotation2d(), 0, 0, pose);
255+
}
256+
257+
public void resetEncoders() {
258+
leftLeader.setSelectedSensorPosition(0);
259+
rightLeader.setSelectedSensorPosition(0);
260+
}
261+
262+
public void tankDriveVolts(double lVolts, double rVolts) {
263+
leftLeader.set(ControlMode.PercentOutput, lVolts / 12.0 * (46.0 / 48.0));
264+
rightLeader.set(ControlMode.PercentOutput, rVolts / 12.0);
265+
// SmartDashboard.putNumber("lVolts", lVolts);
266+
// SmartDashboard.putNumber("rVolts", rVolts);
267+
}
268+
269+
/**
270+
* two params : speed, rotation use those two params to generate target state of
271+
* robot, which means we need to know
272+
* the speed forward and rotational speed. Then compare those two speeds to the
273+
* current speed of the robot, and find
274+
* the difference between the current speed between the target speed Adjust the
275+
* target speed based on the current
276+
* speed and the difference. Basically arcadeDrive.
277+
*/
278+
public void adjustedArcadeDrive(double speed, double rotation) {
279+
double maxInput = Math.copySign(Math.max(Math.abs(speed), Math.abs(rotation)), speed);
280+
double leftMotorOutput;
281+
double rightMotorOutput;
282+
if (speed >= 0) {
283+
if (rotation >= 0) {
284+
leftMotorOutput = speed;
285+
rightMotorOutput = speed - rotation;
286+
} else {
287+
leftMotorOutput = speed + rotation;
288+
rightMotorOutput = maxInput;
289+
}
290+
} else {
291+
if (rotation >= 0) {
292+
leftMotorOutput = speed + rotation;
293+
rightMotorOutput = maxInput;
294+
} else {
295+
leftMotorOutput = maxInput;
296+
rightMotorOutput = speed - rotation;
297+
}
298+
}
299+
DifferentialDriveWheelSpeeds currentSpeed = getWheelSpeeds();
300+
301+
// Somehow the flipped output for the right motor is assined to the left motor.
302+
// Somehow it woirks in the original arcadeDrive.
303+
double desiredLeftOutput = rightMotorOutput;
304+
double desiredRightOutput = -leftMotorOutput;
305+
// System.out.println("current: " + lspeed + " " + rspeed);
306+
// System.out.println("desired: " + desiredLeftOutput + " " +
307+
// desiredRightOutput);
308+
// P(ID?) Constants
309+
final double P = .5;
310+
// normalize speeds to [-1,1]
311+
double curLSpeed = currentSpeed.leftMetersPerSecond / Constants.MAX_SPEED_MPS;
312+
double curRSpeed = currentSpeed.rightMetersPerSecond / Constants.MAX_SPEED_MPS;
313+
// get error
314+
double leftErr = desiredLeftOutput - curLSpeed;// curLSpeed - desiredLeftOutput;
315+
double rightErr = desiredRightOutput - curRSpeed;// curRSpeed - desiredRightOutput;
316+
// got proportional offset
317+
double leftDiff = leftErr * P;
318+
double rightDiff = rightErr * P;
319+
320+
// get final speeds;
321+
double leftSpeed = curLSpeed + leftDiff;
322+
double rightSpeed = curRSpeed + rightDiff;
323+
System.out.printf("left: %f , right : %f\n", leftSpeed, rightSpeed);
324+
leftLeader.set(leftSpeed);
325+
rightLeader.set(rightSpeed);
326+
}
327+
328+
@Override
329+
public void close() throws Exception {
330+
leftLeader.close();
331+
rightLeader.close();
332+
leftFollower.close();
333+
rightFollower.close();
334+
}
335+
}

‎src/main/java/frc/robot/utils/Controller.java

+150
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
package frc.robot.utils;
2+
3+
import edu.wpi.first.wpilibj.Joystick;
4+
import edu.wpi.first.wpilibj2.command.Command;
5+
import edu.wpi.first.wpilibj2.command.Commands;
6+
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
7+
8+
/**
9+
* contains controller mappings and related methods
10+
*/
11+
public class Controller {
12+
// Controller Map:
13+
public static final int AXIS_RTRIGGER = 3;
14+
public static final int AXIS_LTRIGGER = 2;
15+
public static final int BUTTON_LTRIGGER = 5;
16+
public static final int BUTTON_RTRIGGER = 6;
17+
public static final int BUTTON_RBUMPER = BUTTON_RTRIGGER;
18+
public static final int BUTTON_LBUMPER = BUTTON_LTRIGGER;
19+
public static final int AXIS_LY = 1;
20+
public static final int AXIS_LX = 0;
21+
public static final int AXIS_RY = 5;
22+
public static final int AXIS_RX = 4;
23+
public static final int BUTTON_START = 8;
24+
public static final int BUTTON_BACK = 7;
25+
public static final int BUTTON_X = 3;
26+
public static final int BUTTON_Y = 4;
27+
public static final int BUTTON_A = 1;
28+
public static final int BUTTON_B = 2;
29+
30+
public static final int PRIMARY = 0;
31+
public static final int SECONDARY = 1;
32+
33+
public static Joystick primaryJoystick = null;
34+
public static Joystick secondaryJoystick = null;
35+
36+
/**
37+
* intializes primary and secondary joysticks
38+
*/
39+
public static void init() {
40+
primaryJoystick = new Joystick(PRIMARY);
41+
secondaryJoystick = new Joystick(SECONDARY);
42+
}
43+
44+
/**
45+
* Constructs a UserAnalog that precisely mirrors the axis, with no
46+
* tranformation.
47+
*
48+
* @param player - one of either PRIMARY or SECONDARY. This value is validated,
49+
* and an invalid parameter will return
50+
* a UserAnalog that always gets 0
51+
* @param axis - The axis on the Xbox controller to grab values from. This
52+
* parameter is not validated, so make
53+
* sure you have a valid axis!
54+
* @return the UserAnalog instance
55+
*/
56+
public static UserAnalog simpleAxis(int player, int axis) {
57+
Joystick joystick;
58+
if (player == PRIMARY) {
59+
joystick = primaryJoystick;
60+
} else if (player == SECONDARY) {
61+
joystick = secondaryJoystick;
62+
} else {
63+
System.err.println("ERROR: Invalid Player Controller requested");
64+
return () -> 0;
65+
}
66+
return () -> {
67+
// deadbanding
68+
double raw = joystick.getRawAxis(axis);
69+
double sign = Math.signum(raw);
70+
final double deadband = 0.1;
71+
final double multiplier = 1 / (1 - deadband);
72+
return sign * Math.max(0, Math.abs(raw) - deadband) * multiplier;
73+
};
74+
}
75+
76+
/**
77+
* Constructs a UserDigital that precisely mirrors the button value, with no
78+
* tranformation.
79+
*
80+
* @param player - one of either PRIMARY or SECONDARY. This value is validated,
81+
* and an invalid parameter will return
82+
* a UserDigital that always gets false.
83+
* @param button - The button on the Xbox controller to grab values from. This
84+
* parameter is not validated, so make
85+
* sure you have a valid button!
86+
* @return the UserDigital instance
87+
*/
88+
public static UserDigital simpleButton(int player, int button) {
89+
Joystick joystick;
90+
if (player == PRIMARY) {
91+
joystick = primaryJoystick;
92+
} else if (player == SECONDARY) {
93+
joystick = secondaryJoystick;
94+
} else {
95+
System.err.println("ERROR: Invalid Player Controller requested");
96+
return () -> false;
97+
}
98+
return () -> joystick.getRawButton(button);
99+
}
100+
101+
/**
102+
* binds command to given button and retunr it
103+
*
104+
* @param player player id
105+
* @param button button id
106+
* @param cmd command to bind to said button
107+
* @return the bound button in case other operations need to be done and to
108+
* protect against trash collection
109+
*/
110+
public static JoystickButton bindCommand(int player, int button, Command cmd) {
111+
JoystickButton b = getJoystickButton(player, button);
112+
b.onTrue(cmd);
113+
return b;
114+
}
115+
116+
/**
117+
* binds callback to given button and retunr it
118+
*
119+
* @param player player id
120+
* @param button button id
121+
* @param callback callback to bind to specified button
122+
* @return the bound button in case other operations need to be done and to
123+
* protect against trash collection
124+
*/
125+
public static JoystickButton bindCallback(int player, int button, Runnable callback) {
126+
// bind to one time command i guess
127+
return bindCommand(player, button, Commands.runOnce(callback));
128+
}
129+
130+
/**
131+
* get joystick button from player and button ids, secondary if invalid player
132+
* is given
133+
*
134+
* @param player player id (secondary by default)
135+
* @param button button id
136+
* @return the corresponding joystick button
137+
*/
138+
public static JoystickButton getJoystickButton(int player, int button) {
139+
Joystick joystick;
140+
if (player == PRIMARY) {
141+
joystick = primaryJoystick;
142+
} else if (player == SECONDARY) {
143+
joystick = secondaryJoystick;
144+
} else {
145+
System.err.println("ERROR: Invalid Player Controller requested");
146+
joystick = secondaryJoystick;
147+
}
148+
return new JoystickButton(joystick, button);
149+
}
150+
}

‎src/main/java/frc/robot/utils/MotorUtils.java

+48
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
package frc.robot.utils;
2+
3+
import com.revrobotics.CANSparkMax;
4+
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
5+
import com.ctre.phoenix.motorcontrol.NeutralMode;
6+
import com.ctre.phoenix.motorcontrol.can.TalonFX;
7+
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
8+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
9+
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
10+
11+
public class MotorUtils {
12+
public static TalonSRX initTalonSRX(int motorID) {
13+
var motor = new TalonSRX(motorID);
14+
motor.configFactoryDefault();
15+
motor.setNeutralMode(NeutralMode.Brake);
16+
return motor;
17+
}
18+
19+
public static WPI_TalonSRX initWPITalonSRX(int motorID) {
20+
var motor = new WPI_TalonSRX(motorID);
21+
motor.configFactoryDefault();
22+
motor.setNeutralMode(NeutralMode.Coast);
23+
return motor;
24+
}
25+
26+
public static TalonFX initTalonFX(int motorID) {
27+
var motor = new TalonFX(motorID);
28+
motor.configFactoryDefault();
29+
motor.setNeutralMode(NeutralMode.Brake);
30+
return motor;
31+
}
32+
33+
public static WPI_TalonFX initWPITalonFX(int motorID) {
34+
var motor = new WPI_TalonFX(motorID);
35+
motor.configFactoryDefault();
36+
motor.setNeutralMode(NeutralMode.Coast);
37+
motor.configClosedloopRamp(2);
38+
return motor;
39+
}
40+
41+
// for use with NEO motor
42+
public static CANSparkMax initSparkMax(int motorID) {
43+
var motor = new CANSparkMax(motorID, MotorType.kBrushless);
44+
motor.restoreFactoryDefaults();
45+
motor.setIdleMode(CANSparkMax.IdleMode.kCoast);
46+
return motor;
47+
}
48+
}

‎src/main/java/frc/robot/utils/REVDigitBoard.java

+243
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,243 @@
1+
package frc.robot.utils;
2+
3+
import java.util.Arrays;
4+
import java.util.HashMap;
5+
import java.util.Map;
6+
7+
import edu.wpi.first.wpilibj.AnalogInput;
8+
import edu.wpi.first.wpilibj.DigitalInput;
9+
import edu.wpi.first.wpilibj.I2C;
10+
import edu.wpi.first.wpilibj.I2C.Port;
11+
import edu.wpi.first.wpilibj.Timer;
12+
13+
public class REVDigitBoard {
14+
/*
15+
* DOCUMENTATION::
16+
*
17+
* REVDigitBoard() : constructor void display(String str) : displays the first
18+
* four characters of the string (only alpha (converted to uppercase), numbers,
19+
* and spaces) void display(double batt) : displays a decimal number (like
20+
* battery voltage) in the form of 12.34 (ten-one-decimal-tenth-hundredth) void
21+
* clear() : clears the display boolean getButtonA() : button A on the board
22+
* boolean getButtonB() : button B on the board double getPot() : potentiometer
23+
* value
24+
*/
25+
26+
I2C i2c;
27+
DigitalInput buttonA, buttonB;
28+
AnalogInput pot;
29+
30+
byte[][] charreg;
31+
Map<Character, Integer> charmap;
32+
33+
public REVDigitBoard() {
34+
i2c = new I2C(Port.kMXP, 0x70);
35+
buttonA = new DigitalInput(19);
36+
buttonB = new DigitalInput(20);
37+
pot = new AnalogInput(3);
38+
39+
byte[] osc = new byte[1];
40+
byte[] blink = new byte[1];
41+
byte[] bright = new byte[1];
42+
osc[0] = (byte) 0x21;
43+
blink[0] = (byte) 0x81;
44+
bright[0] = (byte) 0xEF;
45+
46+
i2c.writeBulk(osc);
47+
Timer.delay(.01);
48+
i2c.writeBulk(bright);
49+
Timer.delay(.01);
50+
i2c.writeBulk(blink);
51+
Timer.delay(.01);
52+
53+
charreg = new byte[37][2]; // charreg is short for character registry
54+
charmap = new HashMap<Character, Integer>();
55+
56+
charreg[0][0] = (byte) 0b00111111;
57+
charreg[9][1] = (byte) 0b00000000; // 0
58+
charmap.put('0', 0);
59+
charreg[1][0] = (byte) 0b00000110;
60+
charreg[0][1] = (byte) 0b00000000; // 1
61+
charmap.put('1', 1);
62+
charreg[2][0] = (byte) 0b11011011;
63+
charreg[1][1] = (byte) 0b00000000; // 2
64+
charmap.put('2', 2);
65+
charreg[3][0] = (byte) 0b11001111;
66+
charreg[2][1] = (byte) 0b00000000; // 3
67+
charmap.put('3', 3);
68+
charreg[4][0] = (byte) 0b11100110;
69+
charreg[3][1] = (byte) 0b00000000; // 4
70+
charmap.put('4', 4);
71+
charreg[5][0] = (byte) 0b11101101;
72+
charreg[4][1] = (byte) 0b00000000; // 5
73+
charmap.put('5', 5);
74+
charreg[6][0] = (byte) 0b11111101;
75+
charreg[5][1] = (byte) 0b00000000; // 6
76+
charmap.put('6', 6);
77+
charreg[7][0] = (byte) 0b00000111;
78+
charreg[6][1] = (byte) 0b00000000; // 7
79+
charmap.put('7', 7);
80+
charreg[8][0] = (byte) 0b11111111;
81+
charreg[7][1] = (byte) 0b00000000; // 8
82+
charmap.put('8', 8);
83+
charreg[9][0] = (byte) 0b11101111;
84+
charreg[8][1] = (byte) 0b00000000; // 9
85+
charmap.put('9', 9);
86+
87+
charreg[10][0] = (byte) 0b11110111;
88+
charreg[10][1] = (byte) 0b00000000; // A
89+
charmap.put('A', 10);
90+
charreg[11][0] = (byte) 0b10001111;
91+
charreg[11][1] = (byte) 0b00010010; // B
92+
charmap.put('B', 11);
93+
charreg[12][0] = (byte) 0b00111001;
94+
charreg[12][1] = (byte) 0b00000000; // C
95+
charmap.put('C', 12);
96+
charreg[13][0] = (byte) 0b00001111;
97+
charreg[13][1] = (byte) 0b00010010; // D
98+
charmap.put('D', 13);
99+
charreg[14][0] = (byte) 0b11111001;
100+
charreg[14][1] = (byte) 0b00000000; // E
101+
charmap.put('E', 14);
102+
charreg[15][0] = (byte) 0b11110001;
103+
charreg[15][1] = (byte) 0b00000000; // F
104+
charmap.put('F', 15);
105+
charreg[16][0] = (byte) 0b10111101;
106+
charreg[16][1] = (byte) 0b00000000; // G
107+
charmap.put('G', 16);
108+
charreg[17][0] = (byte) 0b11110110;
109+
charreg[17][1] = (byte) 0b00000000; // H
110+
charmap.put('H', 17);
111+
charreg[18][0] = (byte) 0b00001001;
112+
charreg[18][1] = (byte) 0b00010010; // I
113+
charmap.put('I', 18);
114+
charreg[19][0] = (byte) 0b00011110;
115+
charreg[19][1] = (byte) 0b00000000; // J
116+
charmap.put('J', 19);
117+
charreg[20][0] = (byte) 0b01110000;
118+
charreg[20][1] = (byte) 0b00001100; // K
119+
charmap.put('K', 20);
120+
charreg[21][0] = (byte) 0b00111000;
121+
charreg[21][1] = (byte) 0b00000000; // L
122+
charmap.put('L', 21);
123+
charreg[22][0] = (byte) 0b00110110;
124+
charreg[22][1] = (byte) 0b00000101; // M
125+
charmap.put('M', 22);
126+
charreg[23][0] = (byte) 0b00110110;
127+
charreg[23][1] = (byte) 0b00001001; // N
128+
charmap.put('N', 23);
129+
charreg[24][0] = (byte) 0b00111111;
130+
charreg[24][1] = (byte) 0b00000000; // O
131+
charmap.put('O', 24);
132+
charreg[25][0] = (byte) 0b11110011;
133+
charreg[25][1] = (byte) 0b00000000; // P
134+
charmap.put('P', 25);
135+
charreg[26][0] = (byte) 0b00111111;
136+
charreg[26][1] = (byte) 0b00001000; // Q
137+
charmap.put('Q', 26);
138+
charreg[27][0] = (byte) 0b11110011;
139+
charreg[27][1] = (byte) 0b00001000; // R
140+
charmap.put('R', 27);
141+
charreg[28][0] = (byte) 0b10001101;
142+
charreg[28][1] = (byte) 0b00000001; // S
143+
charmap.put('S', 28);
144+
charreg[29][0] = (byte) 0b00000001;
145+
charreg[29][1] = (byte) 0b00010010; // T
146+
charmap.put('T', 29);
147+
charreg[30][0] = (byte) 0b00111110;
148+
charreg[30][1] = (byte) 0b00000000; // U
149+
charmap.put('U', 30);
150+
charreg[31][0] = (byte) 0b00110000;
151+
charreg[31][1] = (byte) 0b00100100; // V
152+
charmap.put('V', 31);
153+
charreg[32][0] = (byte) 0b00110110;
154+
charreg[32][1] = (byte) 0b00101000; // W
155+
charmap.put('W', 32);
156+
charreg[33][0] = (byte) 0b00000000;
157+
charreg[33][1] = (byte) 0b00101101; // X
158+
charmap.put('X', 33);
159+
charreg[34][0] = (byte) 0b00000000;
160+
charreg[34][1] = (byte) 0b00010101; // Y
161+
charmap.put('Y', 34);
162+
charreg[35][0] = (byte) 0b00001001;
163+
charreg[35][1] = (byte) 0b00100100; // Z
164+
charmap.put('Z', 35);
165+
charreg[36][0] = (byte) 0b00000000;
166+
charreg[36][1] = (byte) 0b00000000; // space
167+
charmap.put(' ', 36);
168+
}
169+
170+
public void display(String str) { // only displays first 4 chars
171+
int[] charz = new int[4];
172+
// uppercase and map it
173+
str = repeat(' ', Math.max(0, 4 - str.length())) + str.toUpperCase(); // pad it to 4 chars
174+
175+
for (int i = 0; i < 4; i++) {
176+
charz[i] = charmap.getOrDefault(str.charAt(i), 36);;
177+
}
178+
this._display(charz);
179+
}
180+
181+
public void display(double batt) { // optimized for battery voltage, needs a double like 12.34
182+
int[] charz = {
183+
36, 36, 36, 36
184+
};
185+
// idk how to decimal point
186+
int ten = (int) (batt / 10);
187+
int one = (int) (batt % 10);
188+
int tenth = (int) ((batt * 10) % 10);
189+
int hundredth = (int) ((batt * 100) % 10);
190+
191+
if (ten != 0)
192+
charz[0] = ten;
193+
charz[1] = one;
194+
charz[2] = tenth;
195+
charz[3] = hundredth;
196+
197+
this._display(charz);
198+
}
199+
200+
void clear() {
201+
int[] charz = {
202+
36, 36, 36, 36
203+
}; // whyy java
204+
this._display(charz);
205+
}
206+
207+
public boolean getButtonA() {
208+
return buttonA.get();
209+
}
210+
211+
public boolean getButtonB() {
212+
return buttonB.get();
213+
}
214+
215+
double getPot() {
216+
return pot.getVoltage();
217+
}
218+
219+
////// not supposed to be publicly used..
220+
221+
void _display(int[] charz) {
222+
byte[] byte1 = new byte[10];
223+
byte1[0] = (byte) (0b0000111100001111);
224+
byte1[2] = charreg[charz[3]][0];
225+
byte1[3] = charreg[charz[3]][1];
226+
byte1[4] = charreg[charz[2]][0];
227+
byte1[5] = charreg[charz[2]][1];
228+
byte1[6] = charreg[charz[1]][0];
229+
byte1[7] = charreg[charz[1]][1];
230+
byte1[8] = charreg[charz[0]][0];
231+
byte1[9] = charreg[charz[0]][1];
232+
// send the array to the board
233+
i2c.writeBulk(byte1);
234+
Timer.delay(0.01);
235+
}
236+
237+
String repeat(char c, int n) {
238+
char[] arr = new char[n];
239+
Arrays.fill(arr, c);
240+
return new String(arr);
241+
}
242+
243+
}

‎src/main/java/frc/robot/utils/UserAnalog.java

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package frc.robot.utils;
2+
3+
/**
4+
* interface for analong inputs to the robot
5+
*/
6+
public interface UserAnalog {
7+
8+
/**
9+
* Creates a UserAnalog
10+
*
11+
* @param digital - the digital input to construct from
12+
* @param trueVal - the output when the digitalInput is true
13+
* @param falseVal - the output when teh digitalinput is false
14+
* @return the constructed Analog
15+
*/
16+
public static UserAnalog fromDigital(
17+
UserDigital digital,
18+
double trueVal,
19+
double falseVal
20+
) {
21+
return () -> digital.get() ? trueVal : falseVal;
22+
}
23+
24+
/**
25+
* @return value - a [-1,1] value based on the input from the user or controller
26+
*/
27+
public double get();
28+
}

‎src/main/java/frc/robot/utils/UserDigital.java

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package frc.robot.utils;
2+
3+
/**
4+
* interface for digital (boolean) inputs to the robot
5+
*/
6+
public interface UserDigital {
7+
8+
/**
9+
* Constructs a UserDigital from a UserAnalog using thresholded values
10+
*
11+
* @param analog - getter for the analog value
12+
* @param threshold - if analog val above or equal threshold = true, below=false
13+
* @param flip - if true, flips the threshold from above = true to above = false
14+
* @return the constructed object
15+
*/
16+
public static UserDigital fromAnalog(
17+
UserAnalog analog,
18+
double threshold,
19+
boolean flip
20+
) {
21+
return () -> (analog.get() >= threshold) ^ flip;
22+
}
23+
24+
/**
25+
* @return value - a boolean value based on the input from the user or controller.
26+
*/
27+
public boolean get();
28+
}

‎src/test/java/SimpleMotorFeedforward.java

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
2+
3+
public class SimpleMotorFeedforward {
4+
5+
}

‎src/test/java/SomeTest.java

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
2+
import frc.robot.subsystems.DriveTrain;
3+
4+
import org.junit.jupiter.api.AfterEach;
5+
import org.junit.jupiter.api.BeforeEach;
6+
import org.junit.jupiter.api.Test;
7+
8+
import edu.wpi.first.hal.HAL;
9+
// import edu.wpi.first.wpilibj.simulation.PWMSim;
10+
// import edu.wpi.first.wpilibj2.command.CommandScheduler;
11+
12+
/**
13+
* testers probably
14+
*/
15+
public class SomeTest {
16+
private DriveTrain dtrain;
17+
18+
@BeforeEach
19+
public void setup() {
20+
assert HAL.initialize(500, 0);
21+
// System.out.println("Setup ");
22+
dtrain = new DriveTrain();
23+
}
24+
25+
@AfterEach
26+
public void shutdown() throws Exception {
27+
// System.out.println("done");
28+
HAL.shutdown();
29+
dtrain.close();
30+
}
31+
32+
@Test
33+
public void test1() {
34+
}
35+
}

‎src/test/java/test.java

+101
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
2+
import edu.wpi.first.math.trajectory.Trajectory;
3+
import edu.wpi.first.math.trajectory.TrajectoryConfig;
4+
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
5+
import edu.wpi.first.math.trajectory.Trajectory.State;
6+
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
7+
import edu.wpi.first.math.util.Units;
8+
import edu.wpi.first.wpilibj.GenericHID;
9+
import edu.wpi.first.wpilibj.MotorSafety;
10+
import edu.wpi.first.wpilibj.XboxController;
11+
import edu.wpi.first.wpilibj2.command.Command;
12+
import edu.wpi.first.wpilibj2.command.RamseteCommand;
13+
import frc.robot.commands.DriveCommand;
14+
import frc.robot.subsystems.DriveTrain;
15+
import frc.robot.utils.Controller;
16+
import frc.robot.utils.UserAnalog;
17+
import frc.robot.utils.UserDigital;
18+
19+
import java.util.stream.Collectors;
20+
21+
import org.opencv.core.Mat;
22+
23+
import java.util.List;
24+
25+
import edu.wpi.first.math.controller.PIDController;
26+
import edu.wpi.first.math.controller.RamseteController;
27+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
28+
import edu.wpi.first.math.geometry.Pose2d;
29+
import edu.wpi.first.math.geometry.Rotation2d;
30+
import edu.wpi.first.math.geometry.Translation2d;
31+
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
32+
import frc.robot.Constants;
33+
34+
/**
35+
* test
36+
*/
37+
@SuppressWarnings("unused")
38+
public class test {
39+
40+
private static DifferentialDriveKinematics driveKinematics = new DifferentialDriveKinematics(
41+
Constants.TRACK_WIDTH_METERS
42+
);
43+
private static DifferentialDriveVoltageConstraint speedConstraint = new DifferentialDriveVoltageConstraint(
44+
new SimpleMotorFeedforward(Constants.K_S, Constants.K_V, Constants.K_A),
45+
driveKinematics,
46+
5
47+
);
48+
49+
static Pose2d pose = new Pose2d(0, 0, new Rotation2d(0));
50+
static RamseteController m_follower = new RamseteController(2, .7);
51+
static TrajectoryConfig config = new TrajectoryConfig(
52+
Constants.MAX_SPEED_MPS + 1,
53+
Constants.MAX_ACCELERATION_MPSS + 2
54+
).setKinematics(driveKinematics).addConstraint(speedConstraint);
55+
static Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
56+
// List.of(
57+
// new Pose2d(0, 0, new Rotation2d(0)),
58+
// new Pose2d(
59+
// Units.inchesToMeters(-89.95),
60+
// Units.inchesToMeters(-17.38),
61+
// new Rotation2d(Math.PI * -120.0 / 180)
62+
// ),
63+
// new Pose2d(Units.inchesToMeters(-0.15), Units.inchesToMeters(-72.24), new Rotation2d(Math.PI * -60 / 180)),
64+
// new Pose2d(0, 0, new Rotation2d(0))
65+
// ),
66+
List.of(
67+
new Pose2d(0, 0, new Rotation2d(0)),
68+
new Pose2d(4, 0, new Rotation2d(0)),
69+
new Pose2d(4, 4, new Rotation2d(Math.PI / 2)),
70+
new Pose2d(0, 4, new Rotation2d(Math.PI / 2)),
71+
new Pose2d(0, 0, new Rotation2d(Math.PI / 2))
72+
),
73+
config
74+
);
75+
76+
public static void printSpeeds() {
77+
Pose2d prevpose = pose;
78+
for (double time = 0; time < 10; time += .05) {
79+
var state = exampleTrajectory.sample(time);
80+
// System.out.print(state.poseMeters.toString().replaceAll("[A-za-z]+2d", "") + " ");
81+
var x = m_follower.calculate(prevpose, state);
82+
System.out.printf(
83+
"%.2gs: %s\n",
84+
time,
85+
driveKinematics.toWheelSpeeds(x)
86+
.toString()
87+
.replace("DifferentialDriveWheelSpeeds", "")
88+
);
89+
System.out.print(state.poseMeters);
90+
prevpose = state.poseMeters;
91+
}
92+
}
93+
94+
public static void main(String[] args) {
95+
try (var drive = new DriveTrain()) {
96+
drive.adjustedArcadeDrive(1, 1);
97+
} catch (Exception e) {
98+
e.printStackTrace();
99+
}
100+
}
101+
}

‎vendordeps/Phoenix.json

+423
Large diffs are not rendered by default.

‎vendordeps/REVLib.json

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
{
2+
"fileName": "REVLib.json",
3+
"name": "REVLib",
4+
"version": "2023.1.2",
5+
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
6+
"mavenUrls": [
7+
"https://maven.revrobotics.com/"
8+
],
9+
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
10+
"javaDependencies": [
11+
{
12+
"groupId": "com.revrobotics.frc",
13+
"artifactId": "REVLib-java",
14+
"version": "2023.1.2"
15+
}
16+
],
17+
"jniDependencies": [
18+
{
19+
"groupId": "com.revrobotics.frc",
20+
"artifactId": "REVLib-driver",
21+
"version": "2023.1.2",
22+
"skipInvalidPlatforms": true,
23+
"isJar": false,
24+
"validPlatforms": [
25+
"windowsx86-64",
26+
"windowsx86",
27+
"linuxarm64",
28+
"linuxx86-64",
29+
"linuxathena",
30+
"linuxarm32",
31+
"osxuniversal"
32+
]
33+
}
34+
],
35+
"cppDependencies": [
36+
{
37+
"groupId": "com.revrobotics.frc",
38+
"artifactId": "REVLib-cpp",
39+
"version": "2023.1.2",
40+
"libName": "REVLib",
41+
"headerClassifier": "headers",
42+
"sharedLibrary": false,
43+
"skipInvalidPlatforms": true,
44+
"binaryPlatforms": [
45+
"windowsx86-64",
46+
"windowsx86",
47+
"linuxarm64",
48+
"linuxx86-64",
49+
"linuxathena",
50+
"linuxarm32",
51+
"osxuniversal"
52+
]
53+
},
54+
{
55+
"groupId": "com.revrobotics.frc",
56+
"artifactId": "REVLib-driver",
57+
"version": "2023.1.2",
58+
"libName": "REVLibDriver",
59+
"headerClassifier": "headers",
60+
"sharedLibrary": false,
61+
"skipInvalidPlatforms": true,
62+
"binaryPlatforms": [
63+
"windowsx86-64",
64+
"windowsx86",
65+
"linuxarm64",
66+
"linuxx86-64",
67+
"linuxathena",
68+
"linuxarm32",
69+
"osxuniversal"
70+
]
71+
}
72+
]
73+
}

‎vendordeps/WPILibNewCommands.json

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
{
2+
"fileName": "WPILibNewCommands.json",
3+
"name": "WPILib-New-Commands",
4+
"version": "1.0.0",
5+
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
6+
"mavenUrls": [],
7+
"jsonUrl": "",
8+
"javaDependencies": [
9+
{
10+
"groupId": "edu.wpi.first.wpilibNewCommands",
11+
"artifactId": "wpilibNewCommands-java",
12+
"version": "wpilib"
13+
}
14+
],
15+
"jniDependencies": [],
16+
"cppDependencies": [
17+
{
18+
"groupId": "edu.wpi.first.wpilibNewCommands",
19+
"artifactId": "wpilibNewCommands-cpp",
20+
"version": "wpilib",
21+
"libName": "wpilibNewCommands",
22+
"headerClassifier": "headers",
23+
"sourcesClassifier": "sources",
24+
"sharedLibrary": true,
25+
"skipInvalidPlatforms": true,
26+
"binaryPlatforms": [
27+
"linuxathena",
28+
"linuxarm32",
29+
"linuxarm64",
30+
"windowsx86-64",
31+
"windowsx86",
32+
"linuxx86-64",
33+
"osxuniversal"
34+
]
35+
}
36+
]
37+
}

‎vendordeps/navx_frc.json

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
{
2+
"fileName": "navx_frc.json",
3+
"name": "KauaiLabs_navX_FRC",
4+
"version": "4.0.447",
5+
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
6+
"mavenUrls": [
7+
"https://repo1.maven.org/maven2/"
8+
],
9+
"jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
10+
"javaDependencies": [
11+
{
12+
"groupId": "com.kauailabs.navx.frc",
13+
"artifactId": "navx-java",
14+
"version": "4.0.447"
15+
}
16+
],
17+
"jniDependencies": [],
18+
"cppDependencies": [
19+
{
20+
"groupId": "com.kauailabs.navx.frc",
21+
"artifactId": "navx-cpp",
22+
"version": "4.0.447",
23+
"headerClassifier": "headers",
24+
"sourcesClassifier": "sources",
25+
"sharedLibrary": false,
26+
"libName": "navx_frc",
27+
"skipInvalidPlatforms": true,
28+
"binaryPlatforms": [
29+
"linuxathena",
30+
"linuxraspbian",
31+
"windowsx86-64"
32+
]
33+
}
34+
]
35+
}

0 commit comments

Comments
 (0)
Please sign in to comment.