diff --git a/lib/src/main/java/org/team100/lib/config/Camera.java b/lib/src/main/java/org/team100/lib/config/Camera.java index 21a206fd9..a22c752b5 100644 --- a/lib/src/main/java/org/team100/lib/config/Camera.java +++ b/lib/src/main/java/org/team100/lib/config/Camera.java @@ -121,6 +121,7 @@ public enum Camera { UNKNOWN(null, new Transform3d()); + private static final boolean DEBUG = false; private static Map cameras = new HashMap<>(); static { for (Camera i : Camera.values()) { @@ -138,7 +139,8 @@ private Camera(String serialNumber, Transform3d offset) { public static Camera get(String serialNumber) { if (cameras.containsKey(serialNumber)) return cameras.get(serialNumber); - System.out.printf("*** Using Camera UNKNOWN for serial number %s\n", serialNumber); + if (DEBUG) + System.out.printf("*** Using Camera UNKNOWN for serial number %s\n", serialNumber); return UNKNOWN; } diff --git a/lib/src/main/java/org/team100/lib/config/PIDConstants.java b/lib/src/main/java/org/team100/lib/config/PIDConstants.java index 65d68f704..efa6844fa 100644 --- a/lib/src/main/java/org/team100/lib/config/PIDConstants.java +++ b/lib/src/main/java/org/team100/lib/config/PIDConstants.java @@ -32,6 +32,11 @@ public static PIDConstants zero(LoggerFactory log) { return new PIDConstants(log, 0, 0); } + /** + * PID units for REV motor outboard velocity control are duty cycle per RPM, so + * if you want to control to a few hundred RPM, P should be something like + * 0.0002. + */ public static PIDConstants makeVelocityPID(LoggerFactory log, double p) { return new PIDConstants(log, 0, p); } diff --git a/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java b/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java index a1f51e4ad..a034ebd94 100644 --- a/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java +++ b/lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java @@ -31,7 +31,12 @@ public NeoVortexCANSparkMotor( neutral, motorPhase, statorCurrentLimit, ff, pid); } - /** Real or simulated depending on identity */ + /** + * Real or simulated depending on identity. + * + * PID units for outboard velocity control are duty cycle per RPM, so if you + * want to control to a few hundred RPM, P should be something like 0.0002. + */ public static BareMotor get( LoggerFactory log, CanId can, MotorPhase phase, int statorLimit, Feedforward100 ff, PIDConstants pid) { diff --git a/lib/src/main/java/org/team100/lib/network/CameraReader.java b/lib/src/main/java/org/team100/lib/network/CameraReader.java index 75961c8bd..16a32d479 100644 --- a/lib/src/main/java/org/team100/lib/network/CameraReader.java +++ b/lib/src/main/java/org/team100/lib/network/CameraReader.java @@ -16,6 +16,7 @@ import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.ValueEventData; import edu.wpi.first.util.struct.StructBuffer; +import edu.wpi.first.wpilibj.Timer; /** * Reads camera input from network tables, which is always a StructArray. @@ -31,6 +32,7 @@ public abstract class CameraReader { private static final int QUEUE_DEPTH = 10; private final DoubleLogger m_log_timestamp; + private final DoubleLogger m_log_age; /** e.g. "blips" or "Rotation3d" */ private final String m_ntValueName; /** Manages the queue of incoming messages. */ @@ -44,7 +46,8 @@ public CameraReader( String ntValueName, StructBuffer buf) { LoggerFactory log = parent.type(this); - m_log_timestamp = log.doubleLogger(Level.TRACE, "timestamp"); + m_log_timestamp = log.doubleLogger(Level.TRACE, "timestamp (s)"); + m_log_age = log.doubleLogger(Level.TRACE, "age (s)"); m_ntValueName = ntValueName; NetworkTableInstance inst = NetworkTableInstance.getDefault(); m_poller = new NetworkTableListenerPoller(inst); @@ -113,9 +116,12 @@ public void update() { // time is in microseconds // https://docs.wpilib.org/en/stable/docs/software/networktables/networktables-intro.html#timestamps - // NT provides a local time comparable to FPGATime, which is what the history uses. + // NT provides a local time comparable to FPGATime, which is what the history + // uses. double valueTimestamp = ((double) ntValue.getTime()) / 1000000.0; + double age = Timer.getFPGATimestamp() - valueTimestamp; m_log_timestamp.log(() -> valueTimestamp); + m_log_age.log(() -> age); if (DEBUG) { System.out.printf("reader timestamp %f\n", valueTimestamp); } diff --git a/lib/src/main/java/org/team100/lib/network/RawTags.java b/lib/src/main/java/org/team100/lib/network/RawTags.java new file mode 100644 index 000000000..1fa724632 --- /dev/null +++ b/lib/src/main/java/org/team100/lib/network/RawTags.java @@ -0,0 +1,30 @@ +package org.team100.lib.network; + +import java.util.function.ObjDoubleConsumer; + +import org.team100.lib.localization.Blip24; +import org.team100.lib.logging.LoggerFactory; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.struct.StructBuffer; + +/** Listen to raw tag input from the cameras, for testing. */ +public class RawTags extends CameraReader { + private final ObjDoubleConsumer m_sink; + + public RawTags(LoggerFactory parent, ObjDoubleConsumer sink) { + super(parent, "vision", "blips", StructBuffer.create(Blip24.struct)); + m_sink = sink; + } + + @Override + protected void perValue( + Transform3d cameraOffset, + double valueTimestamp, + Blip24[] value) { + for (Blip24 b : value) { + m_sink.accept(b.blipToTransform(), valueTimestamp); + } + } + +} diff --git a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AS5048RotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AS5048RotaryPositionSensor.java index 133ee7202..bbe35388c 100644 --- a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AS5048RotaryPositionSensor.java +++ b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AS5048RotaryPositionSensor.java @@ -29,6 +29,10 @@ */ public class AS5048RotaryPositionSensor extends DutyCycleRotaryPositionSensor { + // package private so simulator can see it + static final double SENSOR_MIN = 0.003888; + static final double SENSOR_MAX = 0.998058; + public AS5048RotaryPositionSensor( LoggerFactory parent, RoboRioChannel channel, @@ -37,11 +41,13 @@ public AS5048RotaryPositionSensor( super(parent, channel, inputOffset, drive); } - protected double m_sensorMin() { - return 0.003888; + @Override + protected double sensorMin() { + return SENSOR_MIN; } - protected double m_sensorMax() { - return 0.998058; + @Override + protected double sensorMax() { + return SENSOR_MAX; } } diff --git a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensor.java index 92076f495..5d6e54c42 100644 --- a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensor.java +++ b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensor.java @@ -46,12 +46,12 @@ public AnalogRotaryPositionSensor( } @Override - protected double m_sensorMin() { + protected double sensorMin() { return 0.0; } @Override - protected double m_sensorMax() { + protected double sensorMax() { return 1.0; } diff --git a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/RoboRioRotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/RoboRioRotaryPositionSensor.java index b31a9ff97..ab4e7a61a 100644 --- a/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/RoboRioRotaryPositionSensor.java +++ b/lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/RoboRioRotaryPositionSensor.java @@ -51,9 +51,9 @@ protected RoboRioRotaryPositionSensor( */ protected abstract double getRatio(); - protected abstract double m_sensorMin(); + protected abstract double sensorMin(); - protected abstract double m_sensorMax(); + protected abstract double sensorMax(); private int wrap() { double current = getWrappedPositionRad(); @@ -88,16 +88,17 @@ public double getWrappedPositionRad() { return positionRad; } - /** map to full [0,1] */ - protected double mapSensorRange(double ratio) { - // map sensor range - if (ratio < m_sensorMin()) { - ratio = m_sensorMin(); + /** + * map to full [0,1] + */ + static double mapSensorRange(double ratio, double sensorMin, double sensorMax) { + if (ratio < sensorMin) { + ratio = sensorMin; } - if (ratio > m_sensorMax()) { - ratio = m_sensorMax(); + if (ratio > sensorMax) { + ratio = sensorMax; } - return (ratio - m_sensorMin()) / (m_sensorMax() - m_sensorMin()); + return (ratio - sensorMin) / (sensorMax - sensorMin); } /** @@ -105,10 +106,10 @@ protected double mapSensorRange(double ratio) { * * @return radians, [-pi, pi] */ - protected double getRad() { + private double getRad() { double ratio = getRatio(); - double posTurns = mapSensorRange(ratio); + double posTurns = mapSensorRange(ratio, sensorMin(), sensorMax()); m_log_position_turns.log(() -> posTurns); double turnsMinusOffset = posTurns - m_positionOffset; @@ -124,6 +125,8 @@ protected double getRad() { } } + + /** * Always returns zero. * diff --git a/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensorTest.java b/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensorTest.java index e39135c48..2e60f4030 100644 --- a/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensorTest.java +++ b/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensorTest.java @@ -64,12 +64,12 @@ void testWrapping() { try { AnalogInputSim sim = new AnalogInputSim(0); sim.setInitialized(true); - final double range = sensor.m_sensorMax() - sensor.m_sensorMin(); + final double range = sensor.sensorMax() - sensor.sensorMin(); for (double unwrappedRad = -6 * Math.PI; unwrappedRad < 6 * Math.PI; unwrappedRad += 0.1) { double wrapped = MathUtil.inputModulus(unwrappedRad, 0, 2 * Math.PI); double piwrapped = MathUtil.angleModulus(unwrappedRad); double sensorTurns = wrapped / (2 * Math.PI); - double ratio = sensorTurns * range + sensor.m_sensorMin(); + double ratio = sensorTurns * range + sensor.sensorMin(); sim.setVoltage(ratio * 5); Cache.refresh(); double sensorWrapped = sensor.getWrappedPositionRad(); diff --git a/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensorTest.java b/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensorTest.java index ebfe62f25..ac6ba6569 100644 --- a/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensorTest.java +++ b/lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensorTest.java @@ -69,12 +69,12 @@ void testWrapping() { sim.setFrequency(1000); sim.setInitialized(true); - final double range = sensor.m_sensorMax() - sensor.m_sensorMin(); + final double range = sensor.sensorMax() - sensor.sensorMin(); for (double unwrappedRad = -6 * Math.PI; unwrappedRad < 6 * Math.PI; unwrappedRad += 0.1) { double wrapped = MathUtil.inputModulus(unwrappedRad, 0, 2 * Math.PI); double piwrapped = MathUtil.angleModulus(unwrappedRad); double sensorTurns = wrapped / (2 * Math.PI); - double ratio = sensorTurns * range + sensor.m_sensorMin(); + double ratio = sensorTurns * range + sensor.sensorMin(); sim.setOutput(ratio); Cache.refresh(); double sensorWrapped = sensor.getWrappedPositionRad(); diff --git a/studies/camera_delay/.gitignore b/studies/camera_delay/.gitignore new file mode 100644 index 000000000..9a9ca7be7 --- /dev/null +++ b/studies/camera_delay/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/studies/camera_delay/.vscode/launch.json b/studies/camera_delay/.vscode/launch.json new file mode 100644 index 000000000..5b804e844 --- /dev/null +++ b/studies/camera_delay/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/studies/camera_delay/.vscode/settings.json b/studies/camera_delay/.vscode/settings.json new file mode 100644 index 000000000..dccbc7c39 --- /dev/null +++ b/studies/camera_delay/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] +} diff --git a/studies/camera_delay/.wpilib/wpilib_preferences.json b/studies/camera_delay/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..e90d9cfd0 --- /dev/null +++ b/studies/camera_delay/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 100 +} \ No newline at end of file diff --git a/studies/camera_delay/README.md b/studies/camera_delay/README.md new file mode 100644 index 000000000..a67a9cb14 --- /dev/null +++ b/studies/camera_delay/README.md @@ -0,0 +1,99 @@ +# Camera Delay + +This project supports direct measurement of end-to-end camera delay, using a test rig. + +## Test Rig + +The test rig comprises a rotating disc, with a small Apriltag on it, +driven at a low constant speed by a motor. + +There are four measurements of the disc angle: + +* The real world. This defines the "zero" of rotation, relative to the test board (e.g. "level"). +* The motor. The motor has an estimate of position, which we can largely ignore. +* One of the absolute encoders we commonly use, connected to the Rio. +* A camera running the usual Apriltag detection code. + +The real-world standard is used to set all the measurements the same, +for zero speed and zero position. + +The reason we're ignoring the motor measurement is that its timing isn't well documented. + +## Position Sensor Delay + +The delay of the position sensor itself is quoted as 0.1 ms: + +https://look.ams-osram.com/m/287d7ad97d1ca22e/original/AS5048-DS000298.pdf + +The delay due to the protocol is uniformly distributed between zero and one ms, +because the sensor output is repeats every ms, and the Rio provides the +most-recently-read value at reading time, with very little delay. + +So the total position sensor delay is uniformly distributed between 0.1 and 1.1 ms. + +I'm not sure how to *measure* this delay. I think we should just assume a +fixed 0.6 ms delay for the position sensor. + +## Camera Delay + +The camera pipeline works as follows: + +1. The sensor is made active, usually for a very short time, currently 1.5 ms. +2. The sensor records a blurred average of the real world during that time. +3. The sensor is made inactive, keeping the captured pixels. +4. The pixels are streamed out of the sensor, row by row. +5. When the first pixel arrives at the Raspberry Pi's image signal processor, it records the system time. +6. Quite a bit of time passes, maybe 30 ms. +7. Finally the last pixel arrives, making a complete frame. +8. The frame is used by our analysis code, which takes awhile, maybe another 30 ms. +9. The total delay is computed (see below). +10. The NetworkTables timestamp is computed (see below). +11. The results are sent over NetworkTables. +12. The NetworkTables receiver adjusts the timestamp for clock offset. +13. Our code polls for NetworkTables input. +14. A small amount of work happens to decode the input. + +The description above has no adjustable fudge factors, and yet we have felt the need +to add them: there is some source of delay not accounted for here. + +### Total Delay + +The delay internal to the camera/pi system is computed as follows: + +1. The image timestamp is rewound by half the shutter open time. +2. The result is subtracted from the current system time, producing the *delay*. + +### Network Tables Timestamp + +The delay calculated above is subtracted from the Network Tables time (a different timebase +than the system time), to produce the Network Tables timestamp. + +## Comparing Measurements + +To compare measurements, we'll keep a timestamped history of all the measurements received. +We then log the history at a point further in the past than any of the delays: +one second ago. The samples should be the same, so we also log the difference, which +should be zero. + +## Taking Action + +The difference will certainly not be zero, so what should we do? + +Previously we added extra delay on the RoboRIO end, but I think that's the wrong +thing to do (among other reasons, it means that the simulator needs to duplicate +the "extra" delay). + +It would be better to modify the sender, so that it tells the truth. + +If the *variance* of the difference turns out to be large, then we'll need to look more closely. + +## Appendix: Camera Delay Background + +Sources of camera delay are discussed extensively elsewhere: + +* https://docs.google.com/spreadsheets/d/1eJXAVCPSDcnFvezaWv0vLu5IyZi7KkjZglR7nizP6fc +* https://docs.google.com/document/d/1JPJC3cn6eorBrsogOWRN7W_bMo1DgAfIKKjA5wP1av4 +* https://docs.google.com/document/d/1Wmumdv1L7AmdqdHs8FIjNInB5be5Tu6RLEa4f_on6Pk + +The goal of this work is not to understand the sources of delay, +we just want to measure it empirically. diff --git a/studies/camera_delay/WPILib-License.md b/studies/camera_delay/WPILib-License.md new file mode 100644 index 000000000..e7cd597b3 --- /dev/null +++ b/studies/camera_delay/WPILib-License.md @@ -0,0 +1,24 @@ +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/studies/camera_delay/build.gradle b/studies/camera_delay/build.gradle new file mode 100644 index 000000000..167a002d4 --- /dev/null +++ b/studies/camera_delay/build.gradle @@ -0,0 +1,110 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.3.2" +} +sourceSets { + main { + java { + srcDir "../../lib/src/main/java" + } + } +} +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "org.team100.frc2025.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/studies/camera_delay/camera_delay.code-workspace b/studies/camera_delay/camera_delay.code-workspace new file mode 100644 index 000000000..d3d4fbf1a --- /dev/null +++ b/studies/camera_delay/camera_delay.code-workspace @@ -0,0 +1,73 @@ +{ + "folders": [ + { + "path": "." + }, + { + "path": "../../lib" + } + ], + "settings": { + "chat.disableAIFeatures": true, + "debug.console.collapseIdenticalLines": false, + "editor.acceptSuggestionOnEnter": "off", + "editor.autoClosingBrackets": "never", + "editor.autoClosingComments": "never", + "editor.autoClosingDelete": "never", + "editor.autoClosingOvertype": "never", + "editor.autoClosingQuotes": "never", + "editor.codeLens": false, + "editor.detectIndentation": false, + "editor.hover.above": true, + "editor.inlayHints.enabled": "off", + "editor.inlineSuggest.enabled": false, + "editor.minimap.enabled": false, + "editor.parameterHints.cycle": false, + "editor.snippetSuggestions": "none", + "editor.stickyScroll.enabled": false, + "editor.stickyScroll.scrollWithEditor": false, + "editor.suggest.showSnippets": false, + "files.autoSave": "afterDelay", + "java.completion.favoriteStaticMembers": [ + "edu.wpi.first.wpilibj2.command.Commands.*", + "org.junit.jupiter.api.Assertions.*", + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*" + ], + "java.completion.guessMethodArguments": "insertBestGuessedArguments", + "java.completion.postfix.enabled": false, + "java.configuration.updateBuildConfiguration": "automatic", + "java.import.gradle.annotationProcessing.enabled": false, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx64G -Xms100m -Xlog:disable", + "java.saveActions.organizeImports": false, + "java.server.launchMode": "Standard", + "java.signatureHelp.description.enabled": true, + "java.signatureHelp.enabled": true, + "markdown.preview.scrollEditorWithPreview": false, + "markdown.preview.scrollPreviewWithEditor": false, + "terminal.integrated.scrollback": 100000, + "vsintellicode.java.completionsEnabled": true, + "workbench.colorCustomizations": { + "debugConsole.infoForeground": "#000000" + }, + "workbench.colorTheme": "Default Light Modern", + "workbench.editor.revealIfOpen": true, + "workbench.tree.enableStickyScroll": false + } +} diff --git a/studies/camera_delay/gradle/wrapper/gradle-wrapper.jar b/studies/camera_delay/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 000000000..a4b76b953 Binary files /dev/null and b/studies/camera_delay/gradle/wrapper/gradle-wrapper.jar differ diff --git a/studies/camera_delay/gradle/wrapper/gradle-wrapper.properties b/studies/camera_delay/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 000000000..8e975a5f2 --- /dev/null +++ b/studies/camera_delay/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +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/studies/camera_delay/gradlew b/studies/camera_delay/gradlew new file mode 100755 index 000000000..f5feea6d6 --- /dev/null +++ b/studies/camera_delay/gradlew @@ -0,0 +1,252 @@ +#!/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/studies/camera_delay/gradlew.bat b/studies/camera_delay/gradlew.bat new file mode 100644 index 000000000..9b42019c7 --- /dev/null +++ b/studies/camera_delay/gradlew.bat @@ -0,0 +1,94 @@ +@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/studies/camera_delay/settings.gradle b/studies/camera_delay/settings.gradle new file mode 100644 index 000000000..3bc070a1d --- /dev/null +++ b/studies/camera_delay/settings.gradle @@ -0,0 +1,30 @@ +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/studies/camera_delay/simgui-ds.json b/studies/camera_delay/simgui-ds.json new file mode 100644 index 000000000..4919a24bf --- /dev/null +++ b/studies/camera_delay/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "Joysticks": { + "window": { + "visible": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/studies/camera_delay/src/main/deploy/example.txt b/studies/camera_delay/src/main/deploy/example.txt new file mode 100644 index 000000000..bb82515da --- /dev/null +++ b/studies/camera_delay/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +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/studies/camera_delay/src/main/java/org/team100/frc2025/Main.java b/studies/camera_delay/src/main/java/org/team100/frc2025/Main.java new file mode 100644 index 000000000..a40805755 --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/Main.java @@ -0,0 +1,12 @@ + +package org.team100.frc2025; + +import edu.wpi.first.wpilibj.RobotBase; + +public final class Main { + private Main() {} + + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2025/Robot.java b/studies/camera_delay/src/main/java/org/team100/frc2025/Robot.java new file mode 100644 index 000000000..916f4a4be --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/Robot.java @@ -0,0 +1,125 @@ +package org.team100.frc2025; + +import org.team100.lib.coherence.Cache; +import org.team100.lib.coherence.Takt; +import org.team100.lib.config.Feedforward100; +import org.team100.lib.config.PIDConstants; +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.logging.Level; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.LoggerFactory.DoubleLogger; +import org.team100.lib.logging.Logging; +import org.team100.lib.motor.BareMotor; +import org.team100.lib.motor.MotorPhase; +import org.team100.lib.motor.rev.NeoVortexCANSparkMotor; +import org.team100.lib.network.RawTags; +import org.team100.lib.sensor.position.absolute.EncoderDrive; +import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; +import org.team100.lib.util.CanId; +import org.team100.lib.util.RoboRioChannel; +import org.team100.lib.util.TimeInterpolatableBuffer100; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.RobotBase; + +public class Robot extends TimedRobot100 { + /** + * The position sensor is assumed to have a fixed delay of 600 us. + */ + private static final double SENSOR_DELAY_S = 0.0006; + private final BareMotor m_motor; + private final AS5048RotaryPositionSensor m_sensor; + private final RawTags m_rawTags; + + private final TimeInterpolatableBuffer100 m_sensorBuffer; + private final TimeInterpolatableBuffer100 m_cameraBuffer; + + private final DoubleLogger m_logSensor; + private final DoubleLogger m_logCamera; + private final DoubleLogger m_logDiff; + + private final Simulator m_sim; + + public Robot() { + Logging logging = Logging.instance(); + LoggerFactory log = logging.rootLogger; + + m_motor = NeoVortexCANSparkMotor.get( + log, + new CanId(1), + MotorPhase.FORWARD, + 1, // current limit + Feedforward100.makeNeoVortex(log), + PIDConstants.makeVelocityPID(log, 0.0002)); + + RoboRioChannel sensorChannel = new RoboRioChannel(1); + m_sensor = new AS5048RotaryPositionSensor( + log, + sensorChannel, + 0.0, // offset + EncoderDrive.DIRECT); + + m_sensorBuffer = new TimeInterpolatableBuffer100<>(2, 0, Rotation2d.kZero); + m_cameraBuffer = new TimeInterpolatableBuffer100<>(2, 0, Rotation2d.kZero); + + // Update the buffer with the roll component, and accept the supplied timestamp + // as true. + m_rawTags = new RawTags(log, new Roll((r, t) -> m_cameraBuffer.put(t, r))); + + m_logSensor = log.doubleLogger(Level.TRACE, "lagged sensor"); + m_logCamera = log.doubleLogger(Level.TRACE, "lagged camera"); + m_logDiff = log.doubleLogger(Level.TRACE, "lagged difference"); + + if (RobotBase.isSimulation()) { + m_sim = new Simulator(log, m_sensor); + } else { + m_sim = null; + } + } + + @Override + public void robotPeriodic() { + // Update the clock. + Takt.update(); + + // Sim runs first so that it sees the Takt time. + if (m_sim != null) { + m_sim.run(); + } + + // Updates the sensor. + Cache.refresh(); + + // Read the sensor and update the sensor buffer. + m_sensorBuffer.put(Takt.actual() - SENSOR_DELAY_S, new Rotation2d( + m_sensor.getWrappedPositionRad())); + + // Read the camera and update the camera buffer. + m_rawTags.update(); + + // Sample the buffers from 1 sec ago and log. + double past = Takt.actual() - 1.0; + double laggedSensor = m_sensorBuffer.get(past).getRadians(); + m_logSensor.log(() -> laggedSensor); + double laggedCamera = m_cameraBuffer.get(past).getRadians(); + m_logCamera.log(() -> laggedCamera); + m_logDiff.log(() -> laggedSensor - laggedCamera); + } + + @Override + public void teleopInit() { + m_motor.setVelocity(1, 0, 0); + if (m_sim != null) { + m_sim.start(); + } + } + + @Override + public void teleopExit() { + m_motor.stop(); + if (m_sim != null) { + m_sim.stop(); + } + } + +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2025/Roll.java b/studies/camera_delay/src/main/java/org/team100/frc2025/Roll.java new file mode 100644 index 000000000..4201744af --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/Roll.java @@ -0,0 +1,21 @@ +package org.team100.frc2025; + +import java.util.function.ObjDoubleConsumer; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; + +/** Extract the roll component. */ +public class Roll implements ObjDoubleConsumer { + private final ObjDoubleConsumer m_delegate; + + public Roll(ObjDoubleConsumer delegate) { + m_delegate = delegate; + } + + @Override + public void accept(Transform3d t, double value) { + m_delegate.accept(new Rotation2d(t.getRotation().getX()), value); + } + +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedCamera.java b/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedCamera.java new file mode 100644 index 000000000..3cf365d25 --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedCamera.java @@ -0,0 +1,54 @@ +package org.team100.frc2025; + +import java.util.function.DoubleFunction; + +import org.team100.lib.coherence.Takt; +import org.team100.lib.localization.Blip24; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StructArrayPublisher; + +/** Publish a tag */ +public class SimulatedCamera implements Runnable { + // TODO: use a real number; real delay is something like 85 ms + private static final double DELAY_S = 0.085; + + private final DoubleFunction m_truth; + + /** client instance, not the default */ + private final NetworkTableInstance m_inst; + private final StructArrayPublisher m_pub; + + public SimulatedCamera(DoubleFunction truth) { + m_truth = truth; + m_inst = NetworkTableInstance.create(); + // This is a client just like the camera is a client. + m_inst.setServer("localhost"); + m_inst.startClient4("SimulatedTagDetector"); + m_pub = m_inst.getStructArrayTopic("vision/0/0/blips", Blip24.struct) + .publish(PubSubOption.keepDuplicates(true)); + } + + @Override + public void run() { + // Sample the ground-truth value in the past. + double past = Takt.actual() - DELAY_S; + Rotation2d pastValue = m_truth.apply(past); + + // Camera coordinates are z-forward so roll axis appears there. + Transform3d t = new Transform3d( + Translation3d.kZero, + new Rotation3d(0, 0, pastValue.getRadians())); + Blip24 b = new Blip24(1, t); + + // Past time in microseconds. + long pastUs = (long) (past * 1000000.0); + m_pub.set(new Blip24[] { b }, pastUs); + } + +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedGroundTruth.java b/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedGroundTruth.java new file mode 100644 index 000000000..683f90c22 --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/SimulatedGroundTruth.java @@ -0,0 +1,67 @@ +package org.team100.frc2025; + +import java.util.function.DoubleFunction; + +import org.team100.lib.coherence.Takt; +import org.team100.lib.logging.Level; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.LoggerFactory.DoubleLogger; + +import edu.wpi.first.math.geometry.Rotation2d; + +/** The actual position for simulation */ +public class SimulatedGroundTruth implements Runnable, DoubleFunction { + private static final double SPEED_RAD_S = 1.0; + /** When the disc started moving, or null if stopped */ + + private final DoubleLogger m_log_truth; + private Double t0; + /** Position at t0 */ + private Rotation2d x0; + + public SimulatedGroundTruth(LoggerFactory parent) { + LoggerFactory log = parent.type(this); + m_log_truth = log.doubleLogger(Level.TRACE, "lagged truth"); + t0 = null; + x0 = Rotation2d.kZero; + } + + /** Start spinning at the current position at time t. */ + public void start(double t) { + if (t0 != null) { + // already running + return; + } + // current x0 is the starting x0 + t0 = t; + } + + /** Stop spinning at the position at time t. */ + public void stop(double t) { + if (t0 == null) { + // already stopped + return; + } + // x0 is where it is right now + x0 = apply(t); + t0 = null; + } + + @Override + public void run() { + m_log_truth.log(() -> apply(Takt.actual() - 1.0).getRadians()); + } + + @Override + public Rotation2d apply(double t) { + if (t0 == null) { + // disc is stopped + return x0; + } + double dt = t - t0; + double dx = SPEED_RAD_S * dt; + Rotation2d dr = new Rotation2d(dx); + return x0.plus(dr); + } + +} diff --git a/studies/camera_delay/src/main/java/org/team100/frc2025/Simulator.java b/studies/camera_delay/src/main/java/org/team100/frc2025/Simulator.java new file mode 100644 index 000000000..91f7f549c --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/frc2025/Simulator.java @@ -0,0 +1,36 @@ +package org.team100.frc2025; + +import org.team100.lib.coherence.Takt; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor; +import org.team100.lib.sensor.position.absolute.wpi.SimulatedAS5048; + +/** Container for simulation stuff */ +public class Simulator implements Runnable { + + private final SimulatedGroundTruth m_truth; + private final SimulatedAS5048 m_sensor; + private final SimulatedCamera m_camera; + + public Simulator(LoggerFactory parent, AS5048RotaryPositionSensor sensor) { + m_truth = new SimulatedGroundTruth(parent); + m_sensor = new SimulatedAS5048(m_truth, sensor); + m_camera = new SimulatedCamera(m_truth); + } + + public void start() { + m_truth.start(Takt.actual()); + } + + public void stop() { + m_truth.stop(Takt.actual()); + } + + @Override + public void run() { + m_truth.run(); + m_sensor.run(); + m_camera.run(); + } + +} diff --git a/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java b/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java new file mode 100644 index 000000000..30b2fc160 --- /dev/null +++ b/studies/camera_delay/src/main/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048.java @@ -0,0 +1,65 @@ +package org.team100.lib.sensor.position.absolute.wpi; + +import static org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor.SENSOR_MAX; +import static org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor.SENSOR_MIN; + +import java.util.function.DoubleFunction; + +import org.team100.lib.coherence.Takt; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.simulation.DutyCycleSim; + +/** + * For simulation of position through the real AS5048 code. + */ +public class SimulatedAS5048 implements Runnable { + /** + * The position sensor is assumed to have a fixed delay of 600 us. + */ + private static final double DELAY_S = 0.0006; + /** + * Offset in rotations + * TODO: maybe remove this + */ + private static final double OFFSET_ROT = 0; + private static final double SENSOR_RANGE = SENSOR_MAX - SENSOR_MIN; + + private final DoubleFunction m_truth; + private final DutyCycleSim m_sim; + + public SimulatedAS5048(DoubleFunction truth, AS5048RotaryPositionSensor sensor) { + m_truth = truth; + m_sim = new DutyCycleSim(sensor.getDutyCycle()); + m_sim.setInitialized(true); + m_sim.setFrequency(1000); + m_sim.setOutput(0.5); + } + + @Override + public void run() { + // Sample the ground-truth value in the past. + Rotation2d pastValue = m_truth.apply(Takt.actual() - DELAY_S); + + // Set the HAL to the duty cycle for this rotation. + m_sim.setOutput(getDutyCycle(pastValue)); + } + + /** Match the real sensor squashing. */ + static double getDutyCycle(Rotation2d r) { + // This value can be negative. + double rotations = r.getRotations() + OFFSET_ROT; + + // This value is between 0 and 1. + double rot01 = (rotations + 1) % 1; + + // Squash the value into the real sensor range. + return rot01 * SENSOR_RANGE + SENSOR_MIN; + } + + /** For testing. */ + double output() { + return m_sim.getOutput(); + } + +} diff --git a/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedCameraTest.java b/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedCameraTest.java new file mode 100644 index 000000000..86bf5ed70 --- /dev/null +++ b/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedCameraTest.java @@ -0,0 +1,57 @@ +package org.team100.frc2025; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.network.RawTags; +import org.team100.lib.testing.Timeless2025; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; + +public class SimulatedCameraTest implements Timeless2025 { + private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + Rotation2d testR = null; + Rotation2d cameraRotation = null; + + private void acceptTag(Transform3d t, double timestamp) { + cameraRotation = new Rotation2d(t.getRotation().getX()); + } + + @Test + void test0() throws InterruptedException { + // Use a real camera. + RawTags camera = new RawTags(log, this::acceptTag); + + // This provides input to the real camera + SimulatedCamera sim = new SimulatedCamera((x) -> testR); + + // Input = 0. + testR = new Rotation2d(0); + sim.run(); + + // wait for NT rate-limiting + Thread.sleep(100); + + camera.update(); + // Output = 0. + assertEquals(0.000000, cameraRotation.getRadians(), 0.000001); + + // Input = pi/2. + testR = new Rotation2d(Math.PI / 2); + sim.run(); + + // wait for NT rate-limiting + Thread.sleep(100); + + camera.update(); + // Output = pi/2. + assertEquals(Math.PI / 2, cameraRotation.getRadians(), 0.000001); + + } + +} diff --git a/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedGroundTruthTest.java b/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedGroundTruthTest.java new file mode 100644 index 000000000..f3fcdebc0 --- /dev/null +++ b/studies/camera_delay/src/test/java/org/team100/frc2025/SimulatedGroundTruthTest.java @@ -0,0 +1,37 @@ +package org.team100.frc2025; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; + +public class SimulatedGroundTruthTest { + private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + @Test + void testStartStop() { + SimulatedGroundTruth t = new SimulatedGroundTruth(log); + + // Initial position is zero. + assertEquals(0.00, t.apply(0.00).getRadians(), 0.001); + + // Position does not change with time. + assertEquals(0.00, t.apply(0.02).getRadians(), 0.001); + + // Start movement at t=0.02. + t.start(0.02); + + // Now position changes with time. + assertEquals(0.02, t.apply(0.04).getRadians(), 0.001); + assertEquals(0.04, t.apply(0.06).getRadians(), 0.001); + + // Stop movement at t=0.06 (position as above, 0.04). + t.stop(0.06); + + // Position does not change with time. + assertEquals(0.04, t.apply(0.08).getRadians(), 0.001); + } + +} diff --git a/studies/camera_delay/src/test/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048Test.java b/studies/camera_delay/src/test/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048Test.java new file mode 100644 index 000000000..4075c37a9 --- /dev/null +++ b/studies/camera_delay/src/test/java/org/team100/lib/sensor/position/absolute/wpi/SimulatedAS5048Test.java @@ -0,0 +1,61 @@ +package org.team100.lib.sensor.position.absolute.wpi; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.logging.TestLoggerFactory; +import org.team100.lib.logging.primitive.TestPrimitiveLogger; +import org.team100.lib.sensor.position.absolute.EncoderDrive; +import org.team100.lib.testing.Timeless2025; +import org.team100.lib.util.RoboRioChannel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; + +public class SimulatedAS5048Test implements Timeless2025 { + private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger()); + + @Test + void testSquash() { + Rotation2d r = Rotation2d.kZero; + double dutyCycle = SimulatedAS5048.getDutyCycle(r); + assertEquals(0.003888, dutyCycle, 0.000001); + double turns = AS5048RotaryPositionSensor.mapSensorRange( + dutyCycle, + AS5048RotaryPositionSensor.SENSOR_MIN, + AS5048RotaryPositionSensor.SENSOR_MAX); + double angle = MathUtil.angleModulus(turns * Math.PI * 2); + // Round-trip results in zero. + assertEquals(0.000000, angle, 0.000001); + } + + Rotation2d testR = null; + + @Test + void testRoundTrip() { + // Use a real sensor. + AS5048RotaryPositionSensor sensor = new AS5048RotaryPositionSensor( + log, new RoboRioChannel(1), 0, EncoderDrive.DIRECT); + // This modifies the real sensor input. + SimulatedAS5048 sim = new SimulatedAS5048((x) -> testR, sensor); + + // Input = 0. + testR = new Rotation2d(0); + sim.run(); + assertEquals(0.003888, sim.output(), 0.000001); + // Sensor is only updated by the cache. + stepTime(); + // Output = 0. + assertEquals(0.000000, sensor.getWrappedPositionRad(), 0.000001); + + // Input = pi/2. + testR = new Rotation2d(Math.PI / 2); + sim.run(); + assertEquals(0.252430, sim.output(), 0.000001); + // Sensor is only updated by the cache. + stepTime(); + // Output = pi/2. + assertEquals(Math.PI / 2, sensor.getWrappedPositionRad(), 0.001); + } +} diff --git a/studies/camera_delay/src/test/java/org/team100/lib/testing/Timeless2025.java b/studies/camera_delay/src/test/java/org/team100/lib/testing/Timeless2025.java new file mode 100644 index 000000000..bbdcdfbda --- /dev/null +++ b/studies/camera_delay/src/test/java/org/team100/lib/testing/Timeless2025.java @@ -0,0 +1,67 @@ +package org.team100.lib.testing; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.team100.lib.coherence.Cache; +import org.team100.lib.coherence.Takt; +import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.tuning.Mutable; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; + +/** + * Sets up FPGA time stepping for unit tests. + * + * Pausing the timer makes the tests deterministic. + * + * use stepTime() to simulate the passage of time. + * + * You'll also need to reset whatever caches you use, perhaps in their periodic. + */ +public interface Timeless2025 { + + /** One big method because Junit */ + @BeforeEach + default void setupSim() { + // Do any time-related setup *in your test method* ! + HAL.initialize(500, 0); + SimHooks.pauseTiming(); + Takt.update(); + + // Make sure the cache doesn't try to update stale things. + Cache.clear(); + + // Simulated motors don't move unless enabled, so enable them. + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + // Avoid mixing mutable values between tests. + Mutable.unpublishAll(); + try { + // wait for CTRE threads + Thread.sleep(10); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + + @AfterEach + default void resumeTiming() { + SimHooks.resumeTiming(); + HAL.shutdown(); + } + + /** + * Increments the clock and resets all the memoized quantities. + * This used to allow the time step to be specified, but it's not a realistic to + * require correctness in that case, so I took it out. + */ + default void stepTime() { + SimHooks.stepTiming(TimedRobot100.LOOP_PERIOD_S); + Takt.update(); + Cache.refresh(); + } + +} diff --git a/studies/camera_delay/vendordeps/Phoenix5-frc2025-latest.json b/studies/camera_delay/vendordeps/Phoenix5-frc2025-latest.json new file mode 100644 index 000000000..c1098dcce --- /dev/null +++ b/studies/camera_delay/vendordeps/Phoenix5-frc2025-latest.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-frc2025-latest.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/studies/camera_delay/vendordeps/Phoenix6-frc2025-latest.json b/studies/camera_delay/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 000000000..cf873578b --- /dev/null +++ b/studies/camera_delay/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,419 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.1", + "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-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-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "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.2.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "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.2.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/studies/camera_delay/vendordeps/REVLib.json b/studies/camera_delay/vendordeps/REVLib.json new file mode 100644 index 000000000..86ad287c8 --- /dev/null +++ b/studies/camera_delay/vendordeps/REVLib.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/studies/camera_delay/vendordeps/ReduxLib-2025.0.0.json b/studies/camera_delay/vendordeps/ReduxLib-2025.0.0.json new file mode 100644 index 000000000..f15ff901b --- /dev/null +++ b/studies/camera_delay/vendordeps/ReduxLib-2025.0.0.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.0.json", + "name": "ReduxLib", + "version": "2025.0.0", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.0", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/studies/camera_delay/vendordeps/WPILibNewCommands.json b/studies/camera_delay/vendordeps/WPILibNewCommands.json new file mode 100644 index 000000000..3718e0acd --- /dev/null +++ b/studies/camera_delay/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "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/studies/camera_delay/vendordeps/libgrapplefrc2025.json b/studies/camera_delay/vendordeps/libgrapplefrc2025.json new file mode 100644 index 000000000..a49af315c --- /dev/null +++ b/studies/camera_delay/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.3", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.3", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file