diff --git a/.github/workflows/javadoc.yml b/.github/workflows/javadoc.yml index 443dd17b..f086b01c 100644 --- a/.github/workflows/javadoc.yml +++ b/.github/workflows/javadoc.yml @@ -17,7 +17,7 @@ jobs: - name: Generate Javadoc run: ./gradlew javadoc - name: Build Artifact - uses: actions/upload-pages-artifact@v3 + uses: actions/upload-pages-artifact@v4 with: path: ./build/docs/javadoc deploy-javadoc: diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 8d161f67..244061d4 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025", "teamNumber": 199 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec2..645e5425 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 1481bab4..53fdd890 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "maven-publish" id "eclipse" } @@ -11,7 +11,6 @@ java { } group 'org.carlmontrobotics' - def ROBOT_MAIN_CLASS = "" // Define my targets (RoboRIO) and artifacts (deployable files) @@ -37,6 +36,8 @@ deploy { 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 } } } @@ -54,6 +55,7 @@ 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() @@ -71,17 +73,24 @@ dependencies { 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' testImplementation 'junit:junit:4.13.2' implementation 'org.mockito:mockito-core:5.11.0' implementation group: 'org.apache.commons', name: 'commons-csv', version: '1.6' } +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. +// Setting up my Jar File. jar { // Note: Do NOT add all the libraries to the jar. Doing so will cause robot programs to have // 2 copies of each of the libraries classes, one from lib199 and one from the robot program. @@ -111,11 +120,6 @@ 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' -} - // Allow the generated javadocs to link to the documentation of WPILib and vendor libraries javadoc { options.with { @@ -129,3 +133,7 @@ javadoc { } } +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd491..a4b76b95 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67b..34bd9ce9 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a42..f5feea6d 100755 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# 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/. @@ -84,7 +86,8 @@ done # 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 "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +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 diff --git a/gradlew.bat b/gradlew.bat index 93e3f59f..9d21a218 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @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 ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +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 @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +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 diff --git a/settings.gradle b/settings.gradle index d94f73c6..c493958a 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } diff --git a/src/main/java/org/carlmontrobotics/lib199/CachedSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/CachedSparkMax.java deleted file mode 100644 index 34a090ae..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/CachedSparkMax.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.carlmontrobotics.lib199; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; - -@Deprecated -public class CachedSparkMax extends CANSparkMax { - - private RelativeEncoder encoder; - private SparkPIDController pidController; - - public CachedSparkMax(int deviceId, MotorType type) { - super(deviceId, type); - this.encoder = null; - this.pidController = null; - } - - @Override - public RelativeEncoder getEncoder() { - return encoder == null ? (encoder = super.getEncoder()) : encoder; - } - - @Override - public SparkPIDController getPIDController() { - return pidController == null ? (pidController = super.getPIDController()) : pidController; - } - -} diff --git a/src/main/java/org/carlmontrobotics/lib199/DummySparkMaxAnswer.java b/src/main/java/org/carlmontrobotics/lib199/DummySparkMaxAnswer.java deleted file mode 100644 index d3fd177a..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/DummySparkMaxAnswer.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.carlmontrobotics.lib199; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkLimitSwitch; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.SparkPIDController.AccelStrategy; - -import org.mockito.invocation.InvocationOnMock; - -public class DummySparkMaxAnswer extends REVLibErrorAnswer { - - private static final long serialVersionUID = 2284848703213263465L; - - public static final DummySparkMaxAnswer ANSWER = new DummySparkMaxAnswer(); - - public static final CANSparkMax DUMMY_SPARK_MAX = Mocks.mock(CANSparkMax.class, ANSWER); - - public static final RelativeEncoder DUMMY_ENCODER = Mocks.mock(RelativeEncoder.class, REVLibErrorAnswer.ANSWER); - public static final SparkAnalogSensor DUMMY_ANALOG_SENSOR = Mocks.mock(SparkAnalogSensor.class, REVLibErrorAnswer.ANSWER); - public static final SparkLimitSwitch DUMMY_LIMIT_SWITCH = Mocks.mock(SparkLimitSwitch.class, REVLibErrorAnswer.ANSWER); - public static final SparkPIDController DUMMY_PID_CONTROLLER = Mocks.mock(SparkPIDController.class, ANSWER); - public static final SparkAbsoluteEncoder DUMMY_ABSOLUTE_ENCODER = Mocks.mock(SparkAbsoluteEncoder.class, ANSWER); - - - @Override - public Object answer(InvocationOnMock invocation) throws Throwable { - Class returnType = invocation.getMethod().getReturnType(); - if(returnType == RelativeEncoder.class) { - return DUMMY_ENCODER; - } else if(returnType == SparkAnalogSensor.class) { - return DUMMY_ANALOG_SENSOR; - } else if(returnType == SparkLimitSwitch.class) { - return DUMMY_LIMIT_SWITCH; - } else if(returnType == SparkPIDController.class) { - return DUMMY_PID_CONTROLLER; - } else if(returnType == MotorType.class) { - return MotorType.kBrushless; - } else if(returnType == IdleMode.class) { - return IdleMode.kBrake; - } else if(returnType == AccelStrategy.class) { - return AccelStrategy.kTrapezoidal; - } else if(returnType == SparkAbsoluteEncoder.class) { - return DUMMY_ABSOLUTE_ENCODER; - } - return super.answer(invocation); - } - -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index 3b6ed67c..2f013360 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -13,9 +13,6 @@ public class Lib199Subsystem implements Subsystem { private static final CopyOnWriteArrayList periodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final Consumer RUN_RUNNABLE = Runnable::run; - @Deprecated - public static final long asyncSleepTime = 20; - static { ensureRegistered(); } @@ -43,37 +40,10 @@ public static void registerPeriodic(Runnable method) { periodicMethods.add(method); } - @Deprecated - /** - * @deprecated Use registerSimulationPeriodic - * @param method - */ - public static void simulationPeriodic(Runnable method) { - registerSimulationPeriodic(method); - } - public static void registerSimulationPeriodic(Runnable method) { periodicSimulationMethods.add(method); } - @Deprecated - /** - * @deprecated Use registerPeriodic - * @param method - */ - public static void registerAsyncPeriodic(Runnable method) { - registerPeriodic(method); - } - - @Deprecated - /** - * @deprecated Use registerSimulationPeriodic - * @param method - */ - public static void registerAsyncSimulationPeriodic(Runnable method) { - registerSimulationPeriodic(method); - } - @Override public void periodic() { periodicMethods.forEach(RUN_RUNNABLE); @@ -84,13 +54,6 @@ public void simulationPeriodic() { periodicSimulationMethods.forEach(RUN_RUNNABLE); } - @Deprecated - /** - * No longer does anything. - */ - public synchronized void asyncPeriodic() { - } - private Lib199Subsystem() {} } diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java index 0c12de1a..5f1dbabe 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java @@ -2,21 +2,27 @@ public class MotorConfig { - public static final MotorConfig NEO = new MotorConfig(70, 40); - public static final MotorConfig NEO_550 = new MotorConfig(40, 20); + public static final MotorConfig NEO = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); + public static final MotorConfig NEO_550 = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); + public static final MotorConfig NEO_2 = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); //TODO: find the max temp for NEO 2.0 // The temp limit of 100C for the Vortex is based on the fact that its temp sensors are mounted directly on the // windings (which is not the case for the Neo or Neo550, causing them to have very delayed temp readings) and the // fact that the winding enamel will melt at 140C. // See: https://www.chiefdelphi.com/t/rev-robotics-spark-flex-and-neo-vortex/442595/349?u=brettle // As a result I think 100C should be safe. I wouldn't increase it past 120. --Dean - public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 60); - + public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 40, MotorControllerType.SPARK_FLEX); + public static final MotorConfig NEO_SOLO_VORTEX = new MotorConfig(100, 40, MotorControllerType.SPARK_MAX); public final int temperatureLimitCelsius, currentLimitAmps; + public final MotorControllerType controllerType; - public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps) { + public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps, MotorControllerType controllerType) { this.temperatureLimitCelsius = temperatureLimitCelsius; this.currentLimitAmps = currentLimitAmps; + this.controllerType = controllerType; } + public MotorControllerType getControllerType() { + return controllerType; + } } diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index 0cd62ee7..66cdb2a2 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -7,52 +7,29 @@ package org.carlmontrobotics.lib199; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; -import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.ExternalFollower; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.SparkPIDController; - import org.carlmontrobotics.lib199.sim.MockSparkFlex; import org.carlmontrobotics.lib199.sim.MockSparkMax; +// import org.carlmontrobotics.lib199.sim.MockSparkFlex; +// import org.carlmontrobotics.lib199.sim.MockSparkMax; import org.carlmontrobotics.lib199.sim.MockTalonSRX; -import org.carlmontrobotics.lib199.sim.MockVictorSPX; -import org.carlmontrobotics.lib199.sim.MockedCANCoder; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + import edu.wpi.first.wpilibj.RobotBase; /** * Add your docs here. */ public class MotorControllerFactory { - public static WPI_VictorSPX createVictor(int port) { - WPI_VictorSPX victor; - if (RobotBase.isReal()) { - victor = new WPI_VictorSPX(port); - } else { - victor = MockVictorSPX.createMockVictorSPX(port); - } - - // Put all configurations for the victor motor controllers in here. - MotorErrors.reportError(victor.configNominalOutputForward(0, 10)); - MotorErrors.reportError(victor.configNominalOutputReverse(0, 10)); - MotorErrors.reportError(victor.configPeakOutputForward(1, 10)); - MotorErrors.reportError(victor.configPeakOutputReverse(-1, 10)); - MotorErrors.reportError(victor.configNeutralDeadband(0.001, 10)); - victor.setNeutralMode(NeutralMode.Brake); - - return victor; - } - public static WPI_TalonSRX createTalon(int id) { WPI_TalonSRX talon; if (RobotBase.isReal()) { @@ -77,130 +54,101 @@ public static WPI_TalonSRX createTalon(int id) { return talon; } - - //checks for spark max errors - - @Deprecated - public static CANSparkMax createSparkMax(int id, MotorErrors.TemperatureLimit temperatureLimit) { - return createSparkMax(id, temperatureLimit.limit); + /** + * Create a default sparkMax controller (NEO or 550). + * + * @param id the port of the motor controller + * @param motorConfig either MotorConfig.NEO or MotorConfig.NEO_550 + */ + public static SparkMax createSparkMax(int id, MotorConfig motorConfig) { + return createSparkMax(id, motorConfig, sparkConfig(motorConfig)); } - - @Deprecated - public static CANSparkMax createSparkMax(int id, int temperatureLimit) { - CANSparkMax spark; + /** + * Create a sparkMax controller (NEO or 550) with custom settings. + * + * @param id the port of the motor controller + * @param config the custom config to set + */ + public static SparkMax createSparkMax(int id, MotorConfig motorConfig, SparkBaseConfig config) { + SparkMax spark; if (RobotBase.isReal()) { - spark = new CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); + spark = new SparkMax(id, SparkLowLevel.MotorType.kBrushless); } else { - spark = MockSparkMax.createMockSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); + spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless, MockSparkMax.NEOType.NEO); } - spark.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, 1); - - MotorErrors.reportSparkMaxTemp(spark, temperatureLimit); - - MotorErrors.reportError(spark.restoreFactoryDefaults()); - MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0)); - MotorErrors.reportError(spark.setIdleMode(IdleMode.kBrake)); - MotorErrors.reportError(spark.enableVoltageCompensation(12)); - MotorErrors.reportError(spark.setSmartCurrentLimit(50)); - - MotorErrors.checkSparkMaxErrors(spark); - - SparkPIDController controller = spark.getPIDController(); - MotorErrors.reportError(controller.setOutputRange(-1, 1)); - MotorErrors.reportError(controller.setP(0)); - MotorErrors.reportError(controller.setI(0)); - MotorErrors.reportError(controller.setD(0)); - MotorErrors.reportError(controller.setFF(0)); + spark.configure( + config, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters + ); + MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius); + MotorErrors.checkSparkErrors(spark); return spark; } - - public static CANSparkMax createSparkMax(int id, MotorConfig config) { - CANSparkMax spark; - if (RobotBase.isReal()) { - spark = new CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); - } else { - spark = MockSparkMax.createMockSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); - } - - configureSpark(spark, config); - - return spark; + /** + * Create a default SparkFlex-Vortex controller. + * + * @param id the port of the motor controller + */ + public static SparkFlex createSparkFlex(int id) { + return createSparkFlex(id, MotorConfig.NEO_VORTEX, sparkConfig(MotorConfig.NEO_VORTEX)); } - - public static CANSparkFlex createSparkFlex(int id, MotorConfig config) { - CANSparkFlex spark; + /** + * Create a sparkFlex controller (VORTEX) with custom settings. + * + * @param id the port of the motor controller + * @param config the custom config to set + */ + public static SparkFlex createSparkFlex(int id, MotorConfig motorConfig, SparkBaseConfig config) { + SparkFlex spark = null; if (RobotBase.isReal()) { - spark = new CANSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless); + spark = new SparkFlex(id, SparkLowLevel.MotorType.kBrushless); } else { - spark = MockSparkFlex.createMockSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless); + spark = MockSparkFlex.createMockSparkFlex(id, SparkLowLevel.MotorType.kBrushless); } - configureSpark(spark, config); + MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius); + MotorErrors.checkSparkErrors(spark); return spark; } - private static void configureSpark(CANSparkBase spark, MotorConfig config) { - MotorErrors.reportSparkTemp(spark, config.temperatureLimitCelsius); - - MotorErrors.reportError(spark.restoreFactoryDefaults()); - //MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0)); - MotorErrors.reportError(spark.setIdleMode(IdleMode.kBrake)); - MotorErrors.reportError(spark.enableVoltageCompensation(12)); - MotorErrors.reportError(spark.setSmartCurrentLimit(config.currentLimitAmps)); - - MotorErrors.checkSparkErrors(spark); - - SparkPIDController controller = spark.getPIDController(); - MotorErrors.reportError(controller.setOutputRange(-1, 1)); - MotorErrors.reportError(controller.setP(0)); - MotorErrors.reportError(controller.setI(0)); - MotorErrors.reportError(controller.setD(0)); - MotorErrors.reportError(controller.setFF(0)); + public static SparkBaseConfig createConfig(MotorControllerType type) { + SparkBaseConfig config = null; + switch(type){ + case SPARK_MAX: + config = new SparkMaxConfig(); + break; + case SPARK_FLEX: + config = new SparkFlexConfig(); + break; + } + return config; } - /** - * @deprecated Use {@link SensorFactory#createCANCoder(int)} instead. - */ - @Deprecated - public static CANcoder createCANCoder(int port) { - CANcoder canCoder = new CANcoder(port); - if(RobotBase.isSimulation()) new MockedCANCoder(canCoder); - return canCoder; + public static MotorControllerType getControllerType(SparkBase motor){ + if(motor instanceof SparkMax){ + return MotorControllerType.SPARK_MAX; + }else if(motor instanceof SparkFlex){ + return MotorControllerType.SPARK_FLEX; + } + return null; } - /** - * Configures a USB Camera. - * See {@link CameraServer#startAutomaticCapture} for more details. - * This MUST be called AFTER AHRS initialization or the code will be unable to connect to the gyro. - * - * @return The configured camera - * - * @deprecated Use {@link SensorFactory#configureCamera()} instead. - */ - @Deprecated - public static UsbCamera configureCamera() { - UsbCamera camera = CameraServer.startAutomaticCapture(); - camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); - CameraServer.getServer().setSource(camera); - return camera; - } + public static SparkBaseConfig sparkConfig(MotorConfig motorConfig){ + SparkBaseConfig config = motorConfig.controllerType.createConfig(); + //configs that apply to all motors + config.idleMode(IdleMode.kBrake); + config.voltageCompensation(12); + config.smartCurrentLimit(motorConfig.currentLimitAmps); - /** - * This method is equivalent to calling {@link #configureCamera()} {@code numCameras} times. - * The last camera will be set as the primary Camera feed. - * To change it, call {@code CameraServer.getServer().setSource()}. - * - * @param numCameras The number of cameras to configure - * @return The configured cameras. - * - * @deprecated Use {@link SensorFactory#configureCameras(int)} instead. - */ - @Deprecated - public static UsbCamera[] configureCameras(int numCameras) { - UsbCamera[] cameras = new UsbCamera[numCameras]; - for(int i = 0; i < numCameras; i++) cameras[i] = configureCamera(); - return cameras; + config.closedLoop + .minOutput(-1) + .maxOutput(1) + .pid(0,0,0) + .velocityFF(0); + + return config; } -} +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java new file mode 100644 index 00000000..15608bf5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java @@ -0,0 +1,15 @@ +package org.carlmontrobotics.lib199; + +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.config.SparkBaseConfig; + +public enum MotorControllerType { + SPARK_MAX, + SPARK_FLEX; + public static MotorControllerType getMotorControllerType(SparkBase motor){ + return MotorControllerFactory.getControllerType(motor); + } + public SparkBaseConfig createConfig(){ + return MotorControllerFactory.createConfig(this); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index 5d5a0655..d053d72f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -6,24 +6,34 @@ import java.util.concurrent.ConcurrentSkipListMap; import com.ctre.phoenix.ErrorCode; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.FaultID; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.Faults; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.carlmontrobotics.lib199.MotorControllerFactory; + public final class MotorErrors { - private static final Map temperatureSparks = new ConcurrentSkipListMap<>(); + private static final Map temperatureSparks = new ConcurrentSkipListMap<>(); private static final Map sparkTemperatureLimits = new ConcurrentHashMap<>(); private static final Map overheatedSparks = new ConcurrentHashMap<>(); - private static final Map flags = new ConcurrentSkipListMap<>( + private static final Map flags = new ConcurrentSkipListMap<>( (spark1, spark2) -> (spark1.getDeviceId() - spark2.getDeviceId())); - private static final Map stickyFlags = new ConcurrentSkipListMap<>( + private static final Map stickyFlags = new ConcurrentSkipListMap<>( (spark1, spark2) -> (spark1.getDeviceId() - spark2.getDeviceId())); + private static final SparkBaseConfig OVERHEAT_MAX_CONFIG = new SparkMaxConfig().smartCurrentLimit(1); + private static final SparkBaseConfig OVERHEAT_FLEX_CONFIG = new SparkFlexConfig().smartCurrentLimit(1); + + public static final int kOverheatTripCount = 5; static { @@ -65,52 +75,46 @@ private static > void reportError(String vendor, T error, T ok System.err.println(Arrays.toString(stack)); } - public static void checkSparkErrors(CANSparkBase spark) { + public static void checkSparkErrors(SparkBase spark) { //Purposely obviously impersonal to differentiate from actual computer generated errors - short faults = spark.getFaults(); - short stickyFaults = spark.getStickyFaults(); - short prevFaults = flags.containsKey(spark) ? flags.get(spark) : 0; - short prevStickyFaults = stickyFlags.containsKey(spark) ? stickyFlags.get(spark) : 0; - - if (spark.getFaults() != 0 && prevFaults != faults) { - System.err.println("Whoops, big oopsie : fault error(s) with spark id : " + spark.getDeviceId() + ": [ " + formatFaults(spark) + "], ooF!"); + // short faults = spark.getFaults(); + Faults faults = spark.getFaults(); + Faults stickyFaults = spark.getStickyFaults(); + Faults prevFaults = flags.getOrDefault(spark, null); + Faults prevStickyFaults = stickyFlags.getOrDefault(spark, null); + + if (spark.hasActiveFault() && prevFaults!=null && prevFaults.rawBits != faults.rawBits) { + System.err.println("Fault Errors! (spark id " + spark.getDeviceId() + "): [" + formatFaults(spark) + "], ooF!"); } - if (spark.getStickyFaults() != 0 && prevStickyFaults != stickyFaults) { - System.err.println("Bruh, you did an Error : sticky fault(s) error with spark id : " + spark.getDeviceId() + ": " + formatStickyFaults(spark) + ", Ouch!"); + if (spark.hasStickyFault() && prevStickyFaults!=null && prevStickyFaults.rawBits != stickyFaults.rawBits) { + System.err.println("Sticky Faults! (spark id " + spark.getDeviceId() + "): [" + formatStickyFaults(spark) + "], Ouch!"); } spark.clearFaults(); flags.put(spark, faults); stickyFlags.put(spark, stickyFaults); } - @Deprecated - public static void checkSparkMaxErrors(CANSparkMax spark) { - checkSparkErrors((CANSparkBase)spark); + private static String formatFaults(Faults f) { + return "" //i hope this makes you proud of yourself, REVLib + + (f.can ? "CAN " : "") + + (f.escEeprom ? "Flash ROM " : "") + + (f.firmware ? "Firmware " : "") + + (f.gateDriver ? "Gate Driver " : "") + + (f.motorType ? "Motor Type " : "") + + (f.other ? "Other " : "") + + (f.sensor ? "Sensor " : "") + + (f.temperature ? "Temperature " : "") + ; } - private static String formatFaults(CANSparkBase spark) { - String out = ""; - for(FaultID fault: FaultID.values()) { - if(spark.getFault(fault)) { - out += (fault.name() + " "); - } - } - return out; + private static String formatFaults(SparkBase spark) { + Faults f = spark.getFaults(); + return formatFaults(f); } - private static String formatStickyFaults(CANSparkBase spark) { - String out = ""; - for(FaultID fault: FaultID.values()) { - if(spark.getStickyFault(fault)) { - out += (fault.name() + " "); - } - } - return out; - } - - @Deprecated - public static void printSparkMaxErrorMessages() { - printSparkErrorMessages(); + private static String formatStickyFaults(SparkBase spark) { + Faults f = spark.getStickyFaults(); + return formatFaults(f); } public static void printSparkErrorMessages() { @@ -125,38 +129,19 @@ static void reportNextNSparkErrors(int n) { lastSparkErrorIndexReported = (lastSparkErrorIndexReported + n) % flags.size(); } - public static CANSparkMax createDummySparkMax() { - return DummySparkMaxAnswer.DUMMY_SPARK_MAX; - } - - @Deprecated - public static void reportSparkMaxTemp(CANSparkMax spark, TemperatureLimit temperatureLimit) { - reportSparkMaxTemp(spark, temperatureLimit.limit); - } - - public static boolean isSparkMaxOverheated(CANSparkMax spark){ + public static boolean isSparkOverheated(SparkBase spark){ int id = spark.getDeviceId(); int motorMaxTemp = sparkTemperatureLimits.get(id); return ( spark.getMotorTemperature() >= motorMaxTemp ); } - @Deprecated - public static void reportSparkMaxTemp(CANSparkMax spark, int temperatureLimit) { - reportSparkTemp((CANSparkBase) spark, temperatureLimit); - } - - public static void reportSparkTemp(CANSparkBase spark, int temperatureLimit) { + public static void reportSparkTemp(SparkBase spark, int temperatureLimit) { int id = spark.getDeviceId(); temperatureSparks.put(id, spark); sparkTemperatureLimits.put(id, temperatureLimit); overheatedSparks.put(id, 0); } - @Deprecated - public static void doReportSparkMaxTemp() { - doReportSparkTemp(); - } - public static void doReportSparkTemp() { temperatureSparks.forEach(MotorErrors::reportSparkTemp); } @@ -169,14 +154,14 @@ static void reportNextNSparkTemps(int n) { lastSparkTempIndexReported = (lastSparkTempIndexReported + n) % temperatureSparks.size(); } - private static void reportSparkTemp(int port, CANSparkBase spark) { + private static void reportSparkTemp(int port, SparkBase spark) { double temp = spark.getMotorTemperature(); double limit = sparkTemperatureLimits.get(port); int numTrips = overheatedSparks.get(port); String sparkType = "of unknown type"; - if (spark instanceof CANSparkMax) { + if (spark instanceof SparkMax) { sparkType = "Max"; - } else if (spark instanceof CANSparkFlex) { + } else if (spark instanceof SparkFlex) { sparkType = "Flex"; } SmartDashboard.putNumber(String.format("Port %d Spark %s Temp", port, sparkType), temp); @@ -202,21 +187,24 @@ private static void reportSparkTemp(int port, CANSparkBase spark) { System.err.println("Port " + port + " spark is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted."); } - spark.setSmartCurrentLimit(1); + switch(MotorControllerFactory.getControllerType(spark)){ + case SPARK_MAX: + spark.configure( + OVERHEAT_MAX_CONFIG, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + break; + case SPARK_FLEX: + spark.configure( + OVERHEAT_FLEX_CONFIG, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + break; + default: + System.err.println("Unknown spark :("); + } } } private MotorErrors() {} - - @Deprecated - public static enum TemperatureLimit { - NEO(70), NEO_550(40); - - public final int limit; - - private TemperatureLimit(int limit) { - this.limit = limit; - } - } - } diff --git a/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java b/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java index f95efbed..3a3886c6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java @@ -1,9 +1,16 @@ package org.carlmontrobotics.lib199; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; @@ -12,27 +19,33 @@ public class SparkVelocityPIDController implements Sendable { @SuppressWarnings("unused") - private final CANSparkMax spark; - private final SparkPIDController pidController; + private final SparkBase spark; + private final SparkClosedLoopController pidController; private final RelativeEncoder encoder; private final String name; private double targetSpeed, tolerance; private double currentP, currentI, currentD, kS, kV; - public SparkVelocityPIDController(String name, CANSparkMax spark, double defaultP, double defaultI, double defaultD, double kS, double kV, double targetSpeed, double tolerance) { + public SparkVelocityPIDController(String name, SparkBase spark, double defaultP, double defaultI, double defaultD, double kS, double kV, double targetSpeed, double tolerance) { this.spark = spark; - this.pidController = spark.getPIDController(); + this.pidController = spark.getClosedLoopController(); this.encoder = spark.getEncoder(); this.name = name; this.targetSpeed = targetSpeed; this.tolerance = tolerance; - pidController.setP(this.currentP = defaultP); - pidController.setI(this.currentI = defaultI); - pidController.setD(this.currentD = defaultD); + + spark.configure(new SparkMaxConfig().apply( + new ClosedLoopConfig().pid( + this.currentP = defaultP, + this.currentI = defaultI, + this.currentD = defaultD + )), + SparkBase.ResetMode.kNoResetSafeParameters,//we only want to change pid params + SparkBase.PersistMode.kNoPersistParameters); this.kS = kS; this.kV = kV; - pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed)); + pidController.setReference(targetSpeed, ControlType.kVelocity, ClosedLoopSlot.kSlot0, calculateFF(targetSpeed)); SendableRegistry.addLW(this, "SparkVelocityPIDController", spark.getDeviceId()); } @@ -56,7 +69,7 @@ public double getTargetSpeed() { public void setTargetSpeed(double targetSpeed) { if(targetSpeed == this.targetSpeed) return; this.targetSpeed = targetSpeed; - pidController.setReference(targetSpeed, ControlType.kVelocity, 0, calculateFF(targetSpeed)); + pidController.setReference(targetSpeed, ControlType.kVelocity, ClosedLoopSlot.kSlot0, calculateFF(targetSpeed)); } public double getTolerance() { @@ -71,25 +84,26 @@ public double calculateFF(double velocity) { return kS * Math.signum(velocity) + kV * velocity; } + private void instantClosedLoopConfig(ClosedLoopConfig clConfig) { + spark.configure(new SparkMaxConfig().apply(clConfig), ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + @Override public void initSendable(SendableBuilder builder) { builder.setActuator(true); builder.setSmartDashboardType("SparkVelocityPIDController"); builder.addDoubleProperty("P", () -> currentP, p -> { - pidController.setP(p); - currentP = p; + instantClosedLoopConfig(new ClosedLoopConfig().p(currentP = p)); }); builder.addDoubleProperty("I", () -> currentI, i -> { - pidController.setI(i); - currentI = i; + instantClosedLoopConfig(new ClosedLoopConfig().i(currentI = i)); }); builder.addDoubleProperty("D", () -> currentD, d -> { - pidController.setD(d); - currentD = d; + instantClosedLoopConfig(new ClosedLoopConfig().d(currentD = d)); }); builder.addDoubleProperty("Target Speed", () -> targetSpeed, newSpeed -> { if(newSpeed == targetSpeed) return; - pidController.setReference(newSpeed, ControlType.kVelocity, 0, calculateFF(newSpeed)); + pidController.setReference(newSpeed, ControlType.kVelocity, ClosedLoopSlot.kSlot0, calculateFF(newSpeed)); targetSpeed = newSpeed; }); builder.addDoubleProperty("Tolerance", () -> tolerance, newTolerance -> tolerance = newTolerance); diff --git a/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java b/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java deleted file mode 100644 index 5ae5244b..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.carlmontrobotics.lib199.logging; - -/** - * Represents the type of a variable in the logging code - * @deprecated Instead use WPILib's Logging API - */ -@Deprecated -public enum VarType { - BOOLEAN, INTEGER, DOUBLE, STRING; -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/path/DifferentialDriveInterface.java b/src/main/java/org/carlmontrobotics/lib199/path/DifferentialDriveInterface.java deleted file mode 100644 index b17930f8..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/path/DifferentialDriveInterface.java +++ /dev/null @@ -1,98 +0,0 @@ -package org.carlmontrobotics.lib199.path; - -import java.util.function.Supplier; -import java.util.stream.DoubleStream; - -import edu.wpi.first.math.controller.RamseteController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RamseteCommand; - -public interface DifferentialDriveInterface extends DrivetrainInterface { - - /** - * Drives - * - * @param left power to left motor m/s - * @param right power to right motor m/s - */ - public void drive(double left, double right); - - /** - * Gets Differential Drive kinematics - * - * @return kinematics - */ - public DifferentialDriveKinematics getKinematics(); - - /** - * Gets max volts - * - * @return max volts - */ - public double getMaxVolt(); - - /** - * Gets characterization values in the form { kVolts, kVels, kAccels } - * - * @return characterization values in the form { kVolts, kVels, kAccels } - */ - public double[][] getCharacterizationValues(); - - /** - * @return The left encoder position in meters - */ - public double getLeftEncoderPosition(); - - /** - * @return The right encoder position in meters - */ - public double getRightEncoderPosition(); - - /** - * Creates Ramsete Controller - * - * @return Ramsete Controller - */ - public default RamseteController createRamsete() { - return new RamseteController(); - } - - /** - * Configures Trajectory - * - * @param path RobotPath object - */ - @Override - public default void configureAutoPath(RobotPath path) { - TrajectoryConfig config = path.getTrajectoryConfig(); - config.setKinematics(getKinematics()); - - double[][] charVals = getCharacterizationValues(); - config.addConstraint(new DifferentialDriveVoltageConstraint( - // new SimpleMotorFeedforward(Utils199.average(charVals[0]), - // Utils199.average(charVals[1]), Utils199.average(charVals[2])), - new SimpleMotorFeedforward(DoubleStream.of(charVals[0]).average().getAsDouble(), - DoubleStream.of(charVals[1]).average().getAsDouble(), - DoubleStream.of(charVals[2]).average().getAsDouble()), - getKinematics(), getMaxVolt())); - } - - /** - * Creates Ramsete Command - * - * @param trajectory Trajectory object - * @param desiredHeading Desired Heading - * @return Ramsete Command - */ - @Override - public default Command createAutoCommand(Trajectory trajectory, Supplier desiredHeading) { - return new RamseteCommand(trajectory, this::getPose, createRamsete(), getKinematics(), this::drive, this); - } - -} diff --git a/src/main/java/org/carlmontrobotics/lib199/path/DrivetrainInterface.java b/src/main/java/org/carlmontrobotics/lib199/path/DrivetrainInterface.java deleted file mode 100644 index e4023a08..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/path/DrivetrainInterface.java +++ /dev/null @@ -1,69 +0,0 @@ -package org.carlmontrobotics.lib199.path; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; - -public interface DrivetrainInterface extends Subsystem { - - /** - * Configures the constants for generating a trajectory - * - * @param path The configuration for generating a trajectory - */ - public void configureAutoPath(RobotPath path); - - /** - * Constructs a new autonomous Command that, when executed, will follow the - * provided trajectory. - * - * @param trajectory The trajectory to follow. - * @param desiredHeading A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @return Command - */ - public Command createAutoCommand(Trajectory trajectory, Supplier desiredHeading); - - /** - * Gets the max acceleration in m/s^2 - * - * @return max acceleration in m/s^2 - */ - public double getMaxAccelMps2(); - - /** - * Gets the max speed in m/s - * - * @return max speed in m/s - */ - public double getMaxSpeedMps(); - - /** - * Gets the current heading in degrees - * - * @return current heading in degrees - */ - public double getHeadingDeg(); - - /** - * @return The current position of the robot - */ - public Pose2d getPose(); - - /** - * Sets odometry to the specified pose - * - * @param initialPose The starting position of the robot on the field. - */ - public void setPose(Pose2d initialPose); - - /** - * Stops the drivetrain - */ - public void stop(); - -} diff --git a/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java b/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java deleted file mode 100644 index be65a34b..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java +++ /dev/null @@ -1,322 +0,0 @@ -package org.carlmontrobotics.lib199.path; - -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileReader; -import java.io.IOException; -import java.nio.file.Paths; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; -import java.util.concurrent.atomic.AtomicReference; -import java.util.function.Supplier; - -import org.apache.commons.csv.CSVFormat; -import org.apache.commons.csv.CSVParser; -import org.apache.commons.csv.CSVRecord; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -//Findd Me -public class RobotPath { - - private List poses; - private Trajectory trajectory; - private TrajectoryConfig config; - private DrivetrainInterface dt; - private HeadingSupplier hs; - private double maxAccelMps2; - private double maxSpeedMps; - private boolean isInverted; - - /** - * Constructs a RobotPath Object - * - * @param pathName Name of the path - * @param dt Drivetrain object - * @param isInverted Whether the path is inverted - * @param initPos Initial position - * @throws IOException If an error occured loading the path - */ - public RobotPath(String pathName, DrivetrainInterface dt, boolean isInverted, Translation2d initPos) - throws IOException { - this(getPointsFromFile(pathName, dt, isInverted, initPos), isInverted, dt); - - } - - /** - * Constructs a RobotPath Object - * - * @param poses List of points in the .path file - * @param isInverted Whether the path is inverted - * @param dt Drivetrain object - */ - public RobotPath(List poses, boolean isInverted, DrivetrainInterface dt) { - this.poses = poses; - this.dt = dt; - this.isInverted = isInverted; - this.maxAccelMps2 = dt.getMaxAccelMps2(); - this.maxSpeedMps = dt.getMaxSpeedMps(); - } - - public Rotation2d getRotation2d(int index){ - return poses.get(index).getRotation(); - } - - /** - * Gets a path command for the given path - * - * @param faceInPathDirection Only for swerve drive, unless otherwise stated. - * Determines whether robot stays facing path - * @param stopAtEnd whether the robot should stop at the end - * @return PathCommand - */ - public Command getPathCommand(boolean faceInPathDirection, boolean stopAtEnd) { - if (trajectory == null) { - generateTrajectory(); - } - // We want the robot to stay facing the same direction (in this case), so save - // the current heading (make sure to update at the start of the command) - AtomicReference headingRef = new AtomicReference<>(dt.getPose().getRotation()); - Supplier desiredHeading = (!faceInPathDirection) ? () -> headingRef.get() : () -> hs.sample(); - Command command = dt.createAutoCommand(trajectory, desiredHeading); - command = new InstantCommand(hs::reset).andThen(command, new InstantCommand(hs::stop)); - if (stopAtEnd) { - command = command.andThen(new InstantCommand(dt::stop, dt)); - } - if (!faceInPathDirection) { - command = new InstantCommand(() -> headingRef.set(dt.getPose().getRotation())).andThen(command); - SmartDashboard.putNumber("Desired Path Heading", headingRef.get().getDegrees()); - } - return command; - } - - /** - * Tells the drivetrain to assume that the robot is at the starting position of - * this path. - */ - public void initializeDrivetrainPosition() { - if (trajectory == null) { - generateTrajectory(); - } - dt.setPose(trajectory.getInitialPose()); - } - - /** - * Generates trajectory using List of poses and TrajectoryConfig objects - */ - private void generateTrajectory() { - if (config == null) { - createConfig(); - } - trajectory = TrajectoryGenerator.generateTrajectory(poses, config); - hs = new HeadingSupplier(trajectory); - } - - /** - * Retrieves the TrajectoryConfig object for this path, creating it if it does - * not already exist - * - * @return the TrajectoryConfig object - */ - public TrajectoryConfig getTrajectoryConfig() { - if (config == null) { - createConfig(); - } - return config; - } - - private void createConfig() { - config = new TrajectoryConfig(this.getMaxSpeedMps(), this.getMaxAccelMps2()); - dt.configureAutoPath(this); - if (isInverted) { - config.setReversed(true); - } - } - - /** - * Inverts the robot path - * - * @return inverted robot path - */ - public RobotPath reversed() { - List newPoses = new ArrayList<>(poses); - Collections.reverse(newPoses); - return new RobotPath(newPoses, true, dt); - } - - /** - * Get points of a path from name of a .path file - * - * @param pathName Path name - * @param dt Drivetrain object - * @param isInverted Whether the path is inverted - * @param initPos Initial position - * @return List of points in path - * @throws IOException If an error occured loading the path - */ - public static List getPointsFromFile(String pathName, DrivetrainInterface dt, boolean isInverted, - Translation2d initPos) throws IOException { - return getPointsFromFile(getPathFile(pathName), dt, isInverted, initPos); - } - - /** - * Get points of a path from a .path file - * - * @param file Filename - * @param dt Drivetrain object - * @param isInverted Whether the path is inverted - * @param initPos Initial position - * @return List of points in path - * @throws IOException If an error occured loading the path - */ - public static List getPointsFromFile(File file, DrivetrainInterface dt, boolean isInverted, - Translation2d initPos) throws IOException { - ArrayList poses = new ArrayList(); - - try { - CSVParser csvParser = CSVFormat.DEFAULT.parse(new FileReader(file)); - double x, y, tanx, tany; - Rotation2d rot; - List records = csvParser.getRecords(); - - for (int i = 1; i < records.size(); i++) { - CSVRecord record = records.get(i); - x = Double.parseDouble(record.get(0)) + initPos.getX(); - y = Double.parseDouble(record.get(1)) + initPos.getY(); - tanx = Double.parseDouble(record.get(2)); - tany = Double.parseDouble(record.get(3)); - rot = new Rotation2d(tanx, tany); - if (isInverted) { - rot = rot.rotateBy(new Rotation2d(Math.PI)); - } - poses.add(new Pose2d(x, y, rot)); - } - csvParser.close(); - } catch (FileNotFoundException e) { - System.out.println("File named: " + file.getAbsolutePath() + " not found."); - e.printStackTrace(); - } - - return poses; - } - - /** - * Gets max acceleration for path - * - * @return Max acceleration mps2 - */ - public double getMaxAccelMps2() { - return this.maxAccelMps2; - } - - /** - * Gets max speed for path - * - * @return Max speed mps - */ - public double getMaxSpeedMps() { - return this.maxSpeedMps; - } - - /** - * Sets max acceleration for path - * - * @param maxAccelMps2 New max acceleration mps2 - * @return The robot path - */ - public RobotPath setMaxAccelMps2(double maxAccelMps2) { - checkConfig("maxAccelMps2"); - this.maxAccelMps2 = maxAccelMps2; - return this; - } - - /** - * Sets max speed for path - * - * @param maxSpeedMps New max speed mps - * @return The robot path - */ - public RobotPath setMaxSpeedMps(double maxSpeedMps) { - checkConfig("maxSpeedMps"); - this.maxSpeedMps = maxSpeedMps; - return this; - } - - private void checkConfig(String varName) { - if (config != null) { - System.out.println( - "Warning: Config has already been created. The changes to " + varName + " will not affect it"); - } - } - - /** - * Gets .path file given filename - * - * @param pathName name of file - * @return .path file - */ - public static File getPathFile(String pathName) { - return Filesystem.getDeployDirectory().toPath().resolve(Paths.get("PathWeaver/Paths/" + pathName + ".path")) - .toFile(); - } - - private static class HeadingSupplier { - private Trajectory trajectory; - private Timer timer; - private boolean timerStarted; - - /** - * Constructs a HeadingSupplier object - * - * @param trajectory Represents a time-parameterized trajectory. The trajectory - * contains of various States that represent the pose, - * curvature, time elapsed, velocity, and acceleration at that - * point. - */ - public HeadingSupplier(Trajectory trajectory) { - this.trajectory = trajectory; - timer = new Timer(); - timerStarted = false; - } - - /** - * Gets the trajectory rotation at current point in time - * - * @return current trajectory rotation at current point in time - */ - public Rotation2d sample() { - if (!timerStarted) { - timerStarted = true; - timer.start(); - } - return trajectory.sample(timer.get()).poseMeters.getRotation(); - } - - /** - * Reset the timer - */ - public void reset() { - timerStarted = false; - timer.reset(); - } - - /** - * Stops the timer - */ - public void stop() { - timer.stop(); - reset(); - } - } -} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java index 3ad8f1f6..ef466638 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java @@ -6,20 +6,29 @@ import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkBase.ExternalFollower; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkRelativeEncoder; -import com.revrobotics.SparkRelativeEncoder.Type; +import com.revrobotics.sim.SparkAnalogSensorSim; +import com.revrobotics.sim.SparkMaxAlternateEncoderSim; +import com.revrobotics.sim.SparkAbsoluteEncoderSim; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkMaxAlternateEncoder; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkRelativeEncoder; +import com.revrobotics.spark.SparkSim; +import com.revrobotics.spark.SparkLowLevel; import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.motorcontrol.MotorController; /** @@ -31,15 +40,30 @@ public class MockSparkBase extends MockedMotorBase { public final MotorType type; private final MockedEncoder encoder; - private final SparkPIDController pidController; - private final MockedSparkMaxPIDController pidControllerImpl; + private final SparkBase motor; + private final SparkSim spark; + private final SparkClosedLoopController pidController; + private final MockedSparkClosedLoopController pidControllerImpl; private SparkAbsoluteEncoder absoluteEncoder = null; - private MockedEncoder absoluteEncoderImpl = null; - private MockedEncoder alternateEncoder = null; + private SparkAbsoluteEncoderSim absoluteEncoderImpl = null; + private SparkMaxAlternateEncoder alternateEncoder = null; + private SparkMaxAlternateEncoderSim alternateEncoderImpl = null; private SparkAnalogSensor analogSensor = null; - private MockedEncoder analogSensorImpl = null; + private SparkAnalogSensorSim analogSensorImpl = null; private final String name; + public enum NEOType { //is it fine if we make it public so that MotorControllerFactory can access it? + NEO(DCMotor.getNEO(1)), + NEO550(DCMotor.getNeo550(1)), + VORTEX(DCMotor.getNeoVortex(1)), + UNKNOWN(DCMotor.getNEO(1)); + + public DCMotor dcMotor; + private NEOType(DCMotor dcmotordata){ + this.dcMotor=dcmotordata; + } + } + /** * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values, and * registers this class's {@link #run()} method to be called via {@link Lib199Subsystem#registerSimulationPeriodic(Runnable)}. @@ -47,30 +71,46 @@ public class MockSparkBase extends MockedMotorBase { * @param port the port to associate this {@code MockSparkMax} with. Will be used to create the {@link SimDevice} and facilitate motor following. * @param type the type of the simulated motor. If this is set to {@link MotorType#kBrushless}, the builtin encoder simulation will be configured * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. - * @param name the name of the type of controller ("CANSparkMax" or "CANSparkFlex") + * @param name the name of the type of controller ("SparkMax" or "SparkFlex") * @param countsPerRev the number of counts per revolution of this controller's built-in encoder. + * @param neoType the type of NEO motor */ - public MockSparkBase(int port, MotorType type, String name, int countsPerRev) { + public MockSparkBase(int port, MotorType type, String name, int countsPerRev, NEOType neoType) { super(name, port); this.type = type; this.name = name; + if (neoType == NEOType.VORTEX){ //only vortex uses sparkflex + this.motor = new SparkFlex(port,type); + this.spark = new SparkSim( + this.motor, + neoType.dcMotor + ); + } else { //WARNING can't initialize a sparkbase without an actual spark... + this.motor = new SparkMax(port,type); + this.spark = new SparkSim( + this.motor, + neoType.dcMotor + ); + } + if(type == MotorType.kBrushless) { encoder = new MockedEncoder(SimDevice.create("CANEncoder:" + name, port), countsPerRev, false, false) { - @Override - public REVLibError setInverted(boolean inverted) { - System.err.println( - "(MockedEncoder) SparkRelativeEncoder cannot be inverted separately from the motor in brushless mode!"); - return REVLibError.kParamInvalid; - } + // @Override + // public REVLibError setInverted(boolean inverted) { + // System.err.println( + // "(MockedEncoder) SparkRelativeEncoder cannot be inverted separately from the motor in brushless mode!"); + // return REVLibError.kParamInvalid; + // } }; } else { encoder = new MockedEncoder(SimDevice.create("CANEncoder:" + name, port), countsPerRev, false, false); } - pidControllerImpl = new MockedSparkMaxPIDController(this); - pidController = Mocks.createMock(SparkPIDController.class, pidControllerImpl, new REVLibErrorAnswer()); - pidController.setFeedbackDevice(encoder); + pidControllerImpl = new MockedSparkClosedLoopController(this); + pidController = Mocks.createMock(SparkClosedLoopController.class, pidControllerImpl, new REVLibErrorAnswer()); + // pidController.feedbackSensor(encoder); + controllers.put(port, this); @@ -96,21 +136,30 @@ public void set(double speed) { pidControllerImpl.setDutyCycle(speed); } - public REVLibError follow(CANSparkBase leader) { + public REVLibError follow(SparkBase leader) { return follow(leader, false); } - public REVLibError follow(CANSparkBase leader, boolean invert) { + public REVLibError follow(SparkBase leader, boolean invert) { pidControllerImpl.follow(leader, invert); // No need to lookup the spark max if we already have it return REVLibError.kOk; } - public REVLibError follow(ExternalFollower leader, int deviceID) { + public REVLibError follow(SparkBase leader, int deviceID) { return follow(leader, deviceID, false); } - public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { + public REVLibError follow(SparkBase leader, int deviceID, boolean invert) { MotorController controller = null; + //ERROR: no way to check if leader is sending following frames or not + controller = getControllerWithId(deviceID); + if(controller == null) { + System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); + return REVLibError.kFollowConfigMismatch; + } + pidControllerImpl.follow(controller, invert); + return REVLibError.kOk; + /* // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, // but I think that it's unlikely and users should use the builtin definitions anyway if(leader.equals(ExternalFollower.kFollowerDisabled)) { @@ -128,6 +177,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) pidControllerImpl.follow(controller, invert); } return REVLibError.kOk; + */ } public boolean isFollower() { @@ -142,14 +192,6 @@ public RelativeEncoder getEncoder() { return encoder; } - public RelativeEncoder getEncoder(SparkRelativeEncoder.Type type, int countsPerRev) { - if(type != Type.kHallSensor) { - System.err.println("Error: MockSparkMax only supports hall effect encoders"); - return null; - } - return getEncoder(); - } - @Override public void setInverted(boolean inverted) { super.setInverted(inverted); @@ -168,7 +210,7 @@ public REVLibError disableVoltageCompensation() { return REVLibError.kOk; } - public SparkPIDController getPIDController() { + public SparkClosedLoopController getPIDController() { return pidController; } @@ -188,15 +230,9 @@ public void close() { if (encoder != null) { encoder.close(); } - if (absoluteEncoderImpl != null) { - absoluteEncoderImpl.close(); - } - if (analogSensorImpl != null) { - analogSensorImpl.close(); - } - if (alternateEncoder != null) { - alternateEncoder.close(); - } + absoluteEncoderImpl=null; + analogSensorImpl=null; + alternateEncoder=null; super.close(); } @@ -205,14 +241,15 @@ public void close() { * After this method has been called once, its output is cached for future invocations. * For this reason, the method is also {@code synchronized}. * - * @param encoderType ignored * @return the simulated encoder */ - public synchronized SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder.Type encoderType) { + public synchronized SparkAbsoluteEncoder getAbsoluteEncoder() { if(absoluteEncoder == null) { - absoluteEncoderImpl = new MockedEncoder( - SimDevice.create("CANDutyCycle:" + name, port), 0, false, - true, true); + if (motor instanceof SparkFlex){ + absoluteEncoderImpl = new SparkAbsoluteEncoderSim((SparkFlex)motor); + } else { + absoluteEncoderImpl = new SparkAbsoluteEncoderSim((SparkMax)motor); + } absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); } return absoluteEncoder; @@ -227,21 +264,15 @@ public synchronized SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder * @param countsPerRev the CPR of the absolute encoder * @return the simulated encoder */ - public RelativeEncoder getAlternateEncoder(int countsPerRev) { - return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); - } - - /** - * Creates a simulated {@link SparkMaxAlternateEncoder} linked to this simulated controller. - * After this method has been called once, its output is cached for future invocations. - * For this reason, the method is also {@code synchronized}. - * - * @param encoderType ignored - * @return the simulated encoder - */ - public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { + public synchronized RelativeEncoder getAlternateEncoder(int countsPerRev) { + // return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); if(alternateEncoder == null) { - alternateEncoder = new MockedEncoder(SimDevice.create("CANEncoder:%s[%d]-alternate".formatted(name, port)), 0, false, false); + if (motor instanceof SparkFlex){ + System.err.println("Error: Attempted to get Alternate Encoder of a SparkFlex: " + motor.getDeviceId()); + return encoder; + } + alternateEncoderImpl = new SparkMaxAlternateEncoderSim((SparkMax)motor); + alternateEncoder = Mocks.createMock(SparkMaxAlternateEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); } return alternateEncoder; } @@ -251,14 +282,15 @@ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder * After this method has been called once, its output is cached for future invocations. * For this reason, the method is also {@code synchronized}. * - * @param mode setting this to {@link SparkAnalogSensor.Mode#kAbsolute} makes the position relative to the position on startup. - * We will assume that this value is always zero, so this parameter has no effect. * @return the simulated encoder */ - public synchronized SparkAnalogSensor getAnalog(SparkAnalogSensor.Mode mode) { + public synchronized SparkAnalogSensor getAnalog() { if(analogSensor == null) { - analogSensorImpl = new MockedEncoder( - SimDevice.create("CANAIn:" + name, port), 0, true, true); + if (motor instanceof SparkFlex){ + analogSensorImpl = new SparkAnalogSensorSim((SparkFlex)motor); + } else { + analogSensorImpl = new SparkAnalogSensorSim((SparkMax)motor); + } analogSensor = Mocks.createMock(SparkAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); } return analogSensor; @@ -293,7 +325,7 @@ public double getOutputCurrent() { @Override public void disable() { - // CANSparkBase sets the motor speed to zero rather than actually disabling the motor + // SparkBase sets the motor speed to zero rather than actually disabling the motor set(0); } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java index 47fb3a11..acc97814 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java @@ -1,18 +1,18 @@ package org.carlmontrobotics.lib199.sim; -import org.carlmontrobotics.lib199.DummySparkMaxAnswer; import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkFlex; public class MockSparkFlex extends MockSparkBase { public MockSparkFlex(int port, MotorType type) { - super(port, type, "CANSparkFlex", 7168); + super(port, type, "CANSparkFlex", 7168, NEOType.VORTEX); } - public static CANSparkFlex createMockSparkFlex(int portPWM, MotorType type) { - return Mocks.createMock(CANSparkFlex.class, new MockSparkFlex(portPWM, type), new DummySparkMaxAnswer()); + public static SparkFlex createMockSparkFlex(int portPWM, MotorType type) { + return Mocks.createMock(SparkFlex.class, new MockSparkFlex(portPWM, type), new REVLibErrorAnswer()); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 653862e9..3bb57362 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -1,18 +1,18 @@ package org.carlmontrobotics.lib199.sim; -import org.carlmontrobotics.lib199.DummySparkMaxAnswer; import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; public class MockSparkMax extends MockSparkBase { - public MockSparkMax(int port, MotorType type) { - super(port, type, "CANSparkMax", 42); + public MockSparkMax(int port, MotorType type, NEOType neoType) { + super(port, type, "CANSparkMax", 42, neoType); } - public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { - return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); + public static SparkMax createMockSparkMax(int portPWM, MotorType type, NEOType neoType) { + return Mocks.createMock(SparkMax.class, new MockSparkMax(portPWM, type, neoType), new REVLibErrorAnswer()); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java deleted file mode 100644 index bb59d10d..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.carlmontrobotics.lib199.sim; - -import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; - -import org.carlmontrobotics.lib199.ErrorCodeAnswer; -import org.carlmontrobotics.lib199.Mocks; - -import edu.wpi.first.wpilibj.motorcontrol.VictorSP; - -public class MockVictorSPX extends MockPhoenixController { - public MockVictorSPX(int portPWM) { - super(portPWM); - motorPWM = new VictorSP(portPWM); - } - - public static WPI_VictorSPX createMockVictorSPX(int portPWM) { - return Mocks.createMock(WPI_VictorSPX.class, new MockVictorSPX(portPWM), new ErrorCodeAnswer(), AutoCloseable.class); - } -} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java index 02dfd603..a7847a60 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java @@ -37,5 +37,4 @@ public void callback(String name, int handle, int direction, HALValue value) { }, true); sims.put(port, this); } - } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index 0520b7da..6776a56c 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -43,8 +43,8 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl * @param countsPerRev The value that this.getCountsPerRevolution() should return * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to - * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables + * {@link #setPosition(double)}, and enables {@code setZeroOffset(double)}. * * {@link #getVelocity()} will return a value in rpm. */ @@ -58,8 +58,8 @@ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, * @param countsPerRev The value that this.getCountsPerRevolution() should return * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to - * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables + * {@link #setPosition(double)}, and enables {@code setZeroOffset(double)}. * @param useRps Whether getVelocity() should return rps instead of rpm. */ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, @@ -88,25 +88,25 @@ public REVLibError setPosition(double newPosition) { return REVLibError.kOk; } - @Override - public REVLibError setMeasurementPeriod(int period_ms) { - System.err.println("(MockedEncoder) setMeasurementPeriod not implemented"); - return REVLibError.kNotImplemented; - } + // @Override + // public REVLibError setMeasurementPeriod(int period_ms) { + // System.err.println("(MockedEncoder) setMeasurementPeriod not implemented"); + // return REVLibError.kNotImplemented; + // } - @Override - public int getMeasurementPeriod() { - System.err.println("(MockedEncoder) getMeasurementPeriod not implemented"); - return 0; - } + // @Override + // public int getMeasurementPeriod() { + // System.err.println("(MockedEncoder) getMeasurementPeriod not implemented"); + // return 0; + // } - @Override - public int getCountsPerRevolution() { - return countsPerRev; - } + // @Override + // public int getCountsPerRevolution() { + // return countsPerRev; + // } /** - * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)}) + * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@code setZeroOffset(double)}) */ public double getRawPosition() { double rotationsOrVolts = voltage != null ? voltage.get() : position.get(); @@ -128,65 +128,69 @@ public double getVelocity() { * velocityConversionFactor; } - @Override + //Let Mock to map these methods although they don't exist in the original class + public interface RemovedMethods { + REVLibError setPositionConversionFactor(double factor); + double getPositionConversionFactor(); + REVLibError setVelocityConversionFactor(double factor); + double getVelocityConversionFactor(); + REVLibError setInverted(boolean inverted); + boolean getInverted(); + } + public REVLibError setPositionConversionFactor(double factor) { positionConversionFactor = factor; return REVLibError.kOk; } - @Override public double getPositionConversionFactor() { return positionConversionFactor; } - @Override public REVLibError setVelocityConversionFactor(double factor) { velocityConversionFactor = factor; return REVLibError.kOk; } - @Override public double getVelocityConversionFactor() { return velocityConversionFactor; } - @Override public REVLibError setInverted(boolean inverted) { this.inverted = inverted; return REVLibError.kOk; } - @Override public boolean getInverted() { return inverted; } - @Override - public REVLibError setAverageDepth(int depth) { - System.err.println("(MockedEncoder) setAverageDepth not implemented"); - return REVLibError.kNotImplemented; - } - - @Override - public int getAverageDepth() { - System.err.println("(MockedEncoder) getAverageDepth not implemented"); - return 0; - } - - @Override - public REVLibError setZeroOffset(double offset) { - if (!absolute) { - System.err.println("(MockedEncoder) setZeroOffset cannot be called on a relative encoder"); - return REVLibError.kParamAccessMode; - } - positionOffset = offset; - return REVLibError.kOk; - } - - @Override - public double getZeroOffset() { - return positionOffset; - } + // @Override + // public REVLibError setAverageDepth(int depth) { + // System.err.println("(MockedEncoder) setAverageDepth not implemented"); + // return REVLibError.kNotImplemented; + // } + + // @Override + // public int getAverageDepth() { + // System.err.println("(MockedEncoder) getAverageDepth not implemented"); + // return 0; + // } + + // @Override + // public REVLibError setZeroOffset(double offset) { + // if (!absolute) { + // System.err.println("(MockedEncoder) setZeroOffset cannot be called on a relative encoder"); + // return REVLibError.kParamAccessMode; + // } + // positionOffset = offset; + // return REVLibError.kOk; + // } + + // @Override + // public double getZeroOffset() { + // return positionOffset; + // } @Override public void close() { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopController.java similarity index 68% rename from src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java rename to src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopController.java index 13629f0f..62400f22 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopController.java @@ -3,14 +3,18 @@ import java.util.Map; import java.util.concurrent.ConcurrentHashMap; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkRelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkMaxAlternateEncoder; +import com.revrobotics.spark.SparkRelativeEncoder; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.MAXMotionConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -19,12 +23,13 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; // NOT THREAD SAFE -public class MockedSparkMaxPIDController { +public class MockedSparkClosedLoopController { public final Map slots = new ConcurrentHashMap<>(); public final MockedMotorBase motor; public Slot activeSlot; - public CANSparkMax.ControlType controlType = CANSparkMax.ControlType.kDutyCycle; + public ClosedLoopSlot activeClosedLoopSlot; + public SparkMax.ControlType controlType = SparkMax.ControlType.kDutyCycle; public MotorController leader = null; public boolean invertLeader = false; public double setpoint = 0.0; @@ -34,6 +39,180 @@ public class MockedSparkMaxPIDController { public double positionPIDWrappingMinInput = 0.0; public double positionPIDWrappingMaxInput = 0.0; + public MockedSparkClosedLoopController(MockedMotorBase motor) { + this.motor = motor; + slots.put(0, activeSlot = new Slot(positionPIDWrappingMinInput, positionPIDWrappingMaxInput, positionPIDWrappingEnabled)); + } + + //Actually Real Methods + + /** Get the selected control type used when setReference(double, SparkBase.ControlType) was last called.*/ + public SparkBase.ControlType getControlType() { + return controlType; + } + + /** Get the I accumulator of the closed loop controller. */ + public double getIAccum() { + System.err.println("(MockedSparkMaxPIDController): getIAccum() is not currently implemented"); + return 0; + } + + /**Get the MAXMotion internal setpoint position. */ + public double getMAXMotionSetpointPosition() { + return setpoint; + } + + /**Get the MAXMotion internal setpoint velocity. */ + public double getMAXMotionSetpointVelocity() { + return setpoint; + } + + /** Get the selected closed loop PID slot. */ + public ClosedLoopSlot getSelectedSlot() { + return activeClosedLoopSlot; + } + + /**Get the internal setpoint of the closed loop controller. */ + public double getSetpoint() { + return setpoint; + } + + /** Determine if the setpoint has been reached.*/ + public boolean isAtSetpoint() {return false;} + + /** Set the I accumulator of the closed loop controller. */ + public REVLibError setIAccum(double iAccum) { + System.err.println("(MockedSparkMaxPIDController): setIAccum() is not currently implemented"); + return REVLibError.kNotImplemented; + } + /** Deprecated, for removal: This API element is subject to removal in a future version. + * Use {@link #setSetpoint(double, SparkBase.ControlType)} instead + */ + @Deprecated + public REVLibError setReference(double value, SparkMax.ControlType ctrl) { + return setReference(value, ctrl, ClosedLoopSlot.kSlot0); + } + /** Deprecated, for removal: This API element is subject to removal in a future version. + * Use {@link #setSetpoint(double, SparkBase.ControlType, ClosedLoopSlot)} instead + */ + @Deprecated + public REVLibError setReference(double value, SparkMax.ControlType ctrl, ClosedLoopSlot pidSlot) { + return setReference(value, ctrl, pidSlot, 0); + } + /** Deprecated, for removal: This API element is subject to removal in a future version. + * Use {@link #setSetpoint(double, SparkBase.ControlType, ClosedLoopSlot, double)} instead*/ + @Deprecated + public REVLibError setReference(double value, SparkMax.ControlType ctrl, ClosedLoopSlot pidSlot, double arbFeedforward) { + return setReference(value, ctrl, pidSlot, arbFeedforward, SparkClosedLoopController.ArbFFUnits.kVoltage); + } + /** Deprecated, for removal: This API element is subject to removal in a future version. + * Use {@link #setSetpoint(double, SparkBase.ControlType, ClosedLoopSlot, double, ArbFFUnits)} instead + */ + @Deprecated + public REVLibError setReference(double value, SparkMax.ControlType ctrl, ClosedLoopSlot pidSlot, double arbFeedforward, SparkClosedLoopController.ArbFFUnits arbFFUnits) { + if(ctrl == SparkMax.ControlType.kSmartVelocity) { + System.err.println("(MockedSparkMaxPIDController): setReference() with ControlType.kSmartVelocity is not currently implemented"); + return REVLibError.kNotImplemented; + } + + setpoint = value; + controlType = ctrl; + leader = null; + + switch(ctrl) { + case kDutyCycle: + case kVoltage: + motor.setClosedLoopControl(false); + break; + case kPosition: + case kVelocity: + case kSmartMotion: + case kMAXMotionPositionControl: + case kMAXMotionVelocityControl: + case kCurrent: + motor.setClosedLoopControl(true); + break; + case kSmartVelocity: + break; // This should never happen + } + + activeSlot = getSlot(pidSlot.value); + activeClosedLoopSlot = pidSlot; + + switch(arbFFUnits) { + case kVoltage: + break; + case kPercentOut: + arbFeedforward *= 12.0; + break; + default: + throw new IllegalArgumentException("Unsupported ArbFFUnits: " + arbFFUnits); + } + + this.arbFF = arbFeedforward; + + return REVLibError.kOk; + } + REVLibError setSetpoint(double setpoint, SparkBase.ControlType ctrl) {return null;}//Set the controller setpoint based on the selected control mode. + REVLibError setSetpoint(double setpoint, SparkBase.ControlType ctrl, ClosedLoopSlot slot) {return null;}//Set the controller setpoint based on the selected control mode. + REVLibError setSetpoint(double setpoint, SparkBase.ControlType ctrl, ClosedLoopSlot slot, double arbFeedforward) {return null;}//Set the controller setpoint based on the selected control mode. + REVLibError setSetpoint(double setpoint, SparkBase.ControlType ctrl, ClosedLoopSlot slot, double arbFeedforward, SparkClosedLoopController.ArbFFUnits arbFFUnits) {return null;}//Set the controller setpoint based on the selected control mode. + + public interface ExtraMethods { + double calculate(double currentDraw); + void setDutyCycle(double speed); + void follow(MotorController leader, boolean invert); + void stopFollowing(); + boolean isFollower(); + double getD(); + double getD(int slotID); + double getFF(); + double getFF(int slotID); + double getI(); + double getI(int slotID); + double getIMaxAccum(int slotID); + double getIZone(); + double getIZone(int slotID); + double getOutputMax(); + double getOutputMax(int slotID); + double getOutputMin(); + double getOutputMin(int slotID); + boolean getPositionPIDWrappingEnabled(); + double getPositionPIDWrappingMaxInput(); + double getPositionPIDWrappingMinInput(); + REVLibError setPositionPIDWrappingEnabled(boolean enable); + REVLibError setPositionPIDWrappingMaxInput(double max); + REVLibError setPositionPIDWrappingMinInput(double min); + double getP(); + double getP(int slotID); + MAXMotionConfig.MAXMotionPositionMode getSmartMotionAccelStrategy(int slotID); + double getSmartMotionAllowedClosedLoopError(int slotID); + double getSmartMotionMaxAccel(int slotID); + double getSmartMotionMaxVelocity(int slotID); + double getSmartMotionMinOutputVelocity(int slotID); + REVLibError setD(double gain); + REVLibError setD(double gain, int slotID); + REVLibError setFeedbackDevice(Object sensor); + REVLibError setFF(double gain); + REVLibError setFF(double gain, int slotID); + REVLibError setI(double gain); + REVLibError setI(double gain, int slotID); + REVLibError setIMaxAccum(double iMaxAccum, int slotID); + REVLibError setIZone(double IZone); + REVLibError setIZone(double IZone, int slotID); + REVLibError setOutputRange(double min, double max); + REVLibError setOutputRange(double min, double max, int slotID); + REVLibError setP(double gain); + REVLibError setP(double gain, int slotID); + REVLibError setSmartMotionAccelStrategy(MAXMotionConfig.MAXMotionPositionMode accelStrategy, int slotID); + REVLibError setSmartMotionAllowedClosedLoopError(double allowedErr, int slotId); + REVLibError setSmartMotionMaxAccel(double maxAccel, int slotID); + REVLibError setSmartMotionMaxVelocity(double maxVel, int slotID); + REVLibError setSmartMotionMinOutputVelocity(double minVel, int slotID); + Slot getSlot(int slotID); + } + + // Methods to interface with MockSparkMax public double calculate(double currentDraw) { if(leader != null) { @@ -54,16 +233,16 @@ public double calculate(double currentDraw) { case kVelocity: output = activeSlot.pidController.calculate(feedbackDevice.getVelocity(), setpoint); break; - case kSmartMotion: + case kMAXMotionPositionControl: output = activeSlot.profiledPIDController.calculate(feedbackDevice.getPosition(), setpoint); if(Math.abs(activeSlot.profiledPIDController.getGoal().velocity) < activeSlot.smartMotionMinVelocity) { - output = 0; + output = 0;//FIXME max motion doesn't have min velocity!!! } break; case kCurrent: output = activeSlot.pidController.calculate(currentDraw, setpoint); break; - case kSmartVelocity: + case kMAXMotionVelocityControl: default: throw new IllegalArgumentException("Unsupported ControlType: " + controlType); } @@ -74,7 +253,7 @@ public double calculate(double currentDraw) { public void setDutyCycle(double speed) { setpoint = speed; - controlType = CANSparkMax.ControlType.kDutyCycle; + controlType = SparkMax.ControlType.kDutyCycle; stopFollowing(); motor.setClosedLoopControl(false); } @@ -93,13 +272,7 @@ public boolean isFollower() { return leader != null; } - public MockedSparkMaxPIDController(MockedMotorBase motor) { - this.motor = motor; - slots.put(0, activeSlot = new Slot(positionPIDWrappingMinInput, positionPIDWrappingMaxInput, positionPIDWrappingEnabled)); - } - // Overrides - public double getD() { return getD(0); } @@ -126,11 +299,6 @@ public double getI(int slotID) { return slot.pidController.getI(); } - public double getIAccum() { - System.err.println("(MockedSparkMaxPIDController): getIAccum() is not currently implemented"); - return 0; - } - public double getIMaxAccum(int slotID) { return getSlot(slotID).iMaxAccum; } @@ -214,8 +382,8 @@ public double getP(int slotID) { return slot.pidController.getP(); } - public SparkPIDController.AccelStrategy getSmartMotionAccelStrategy(int slotID) { - return SparkPIDController.AccelStrategy.kTrapezoidal; + public MAXMotionConfig.MAXMotionPositionMode getSmartMotionAccelStrategy(int slotID) { + return MAXMotionConfig.MAXMotionPositionMode.kMAXMotionTrapezoidal; } public double getSmartMotionAllowedClosedLoopError(int slotID) { @@ -245,8 +413,8 @@ public REVLibError setD(double gain, int slotID) { return REVLibError.kOk; } - public REVLibError setFeedbackDevice(MotorFeedbackSensor sensor) { - if (sensor instanceof SparkRelativeEncoder + public REVLibError setFeedbackDevice(Object sensor) { + if ( sensor instanceof SparkRelativeEncoder || sensor instanceof SparkMaxAlternateEncoder || sensor instanceof SparkAnalogSensor || sensor instanceof SparkAbsoluteEncoder @@ -342,11 +510,6 @@ public REVLibError setI(double gain, int slotID) { return REVLibError.kOk; } - public REVLibError setIAccum(double iAccum) { - System.err.println("(MockedSparkMaxPIDController): setIAccum() is not currently implemented"); - return REVLibError.kNotImplemented; - } - public REVLibError setIMaxAccum(double iMaxAccum, int slotID) { Slot slot = getSlot(slotID); slot.pidController.setIntegratorRange(-iMaxAccum, iMaxAccum); @@ -386,62 +549,9 @@ public REVLibError setP(double gain, int slotID) { return REVLibError.kOk; } - public REVLibError setReference(double value, CANSparkMax.ControlType ctrl) { - return setReference(value, ctrl, 0); - } - - public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot) { - return setReference(value, ctrl, pidSlot, 0); - } - - public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward) { - return setReference(value, ctrl, pidSlot, arbFeedforward, SparkPIDController.ArbFFUnits.kVoltage); - } - - public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward, SparkPIDController.ArbFFUnits arbFFUnits) { - if(ctrl == CANSparkMax.ControlType.kSmartVelocity) { - System.err.println("(MockedSparkMaxPIDController): setReference() with ControlType.kSmartVelocity is not currently implemented"); - return REVLibError.kNotImplemented; - } - - setpoint = value; - controlType = ctrl; - leader = null; - - switch(ctrl) { - case kDutyCycle: - case kVoltage: - motor.setClosedLoopControl(false); - break; - case kPosition: - case kVelocity: - case kSmartMotion: - case kCurrent: - motor.setClosedLoopControl(true); - break; - case kSmartVelocity: - break; // This should never happen - } - - activeSlot = getSlot(pidSlot); - - switch(arbFFUnits) { - case kVoltage: - break; - case kPercentOut: - arbFeedforward *= 12.0; - break; - default: - throw new IllegalArgumentException("Unsupported ArbFFUnits: " + arbFFUnits); - } - - this.arbFF = arbFeedforward; - - return REVLibError.kOk; - } - public REVLibError setSmartMotionAccelStrategy(SparkPIDController.AccelStrategy accelStrategy, int slotID) { - if(accelStrategy != SparkPIDController.AccelStrategy.kTrapezoidal) { + public REVLibError setSmartMotionAccelStrategy(MAXMotionConfig.MAXMotionPositionMode accelStrategy, int slotID) { + if(accelStrategy != MAXMotionConfig.MAXMotionPositionMode.kMAXMotionTrapezoidal) { System.err.println("(MockedSparkMaxPIDController) Ignoring command to set accel strategy on slot " + slotID + " to " + accelStrategy + ". Only AccelStrategy.kTrapezoidal is supported."); return REVLibError.kParamNotImplementedDeprecated; } diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index d39dc473..1619a0ae 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -6,13 +6,20 @@ import java.util.function.Supplier; +import org.carlmontrobotics.lib199.MotorControllerType; import org.mockito.internal.reporting.SmartPrinter; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -23,11 +30,12 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Mass; import edu.wpi.first.units.Measure; +import edu.wpi.first.units.measure.Mass; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -41,8 +49,11 @@ public class SwerveModule implements Sendable { public enum ModuleType {FL, FR, BL, BR}; private SwerveConfig config; + private SparkBaseConfig turnConfig; + private SparkBaseConfig driveConfig; + private ModuleType type; - private CANSparkMax drive, turn; + private SparkBase drive, turn; private CANcoder turnEncoder; private PIDController drivePIDController; private ProfiledPIDController turnPIDController; @@ -57,8 +68,16 @@ public enum ModuleType {FL, FR, BL, BR}; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; private double maxTurnVelocityWithoutTippingRps; - public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANcoder turnEncoder, + + MotorControllerType driveMotorType; + MotorControllerType turnMotorType; + + public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, SparkBase turn, CANcoder turnEncoder, int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + driveMotorType = MotorControllerType.getMotorControllerType(drive); + turnMotorType = MotorControllerType.getMotorControllerType(turn); + driveConfig = driveMotorType.createConfig(); + turnConfig = turnMotorType.createConfig(); //SmartDashboard.putNumber("Target Angle (deg)", 0.0); String moduleString = type.toString(); this.timer = new Timer(); @@ -69,11 +88,16 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN this.type = type; this.drive = drive; - double positionConstant = config.wheelDiameterMeters * Math.PI / config.driveGearing; - drive.setInverted(config.driveInversion[arrIndex]); - drive.getEncoder().setPositionConversionFactor(positionConstant); - drive.getEncoder().setVelocityConversionFactor(positionConstant / 60); - turn.setInverted(config.turnInversion[arrIndex]); + driveConfig.inverted(config.driveInversion[arrIndex]); + turnConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor) + .quadratureAverageDepth(2); + maxControllableAccerlationRps2 = 0; final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; @@ -83,7 +107,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN final double neoStallCurrentAmps = 166; double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps); // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); - drive.setSmartCurrentLimit((int)Math.min(50, currentLimitAmps)); + driveConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], config.kForwardVels[arrIndex], @@ -96,8 +120,19 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN config.drivekI[arrIndex], config.drivekD[arrIndex]); - /* offset for 1 CANcoder count */ - drivetoleranceMPerS = (1.0 / (double)(drive.getEncoder().getCountsPerRevolution()) * positionConstant) / Units.millisecondsToSeconds(drive.getEncoder().getMeasurementPeriod() * drive.getEncoder().getAverageDepth()); + /* offset for 1 relative encoder count */ + switch(MotorControllerType.getMotorControllerType(drive)) { + case SPARK_MAX: + drivetoleranceMPerS = (1.0 + / (double)(((SparkMax)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(((SparkMax)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkMax)drive).configAccessor.encoder.getQuadratureAverageDepth()); + break; + case SPARK_FLEX: + drivetoleranceMPerS = (1.0 + / (double)(((SparkFlex)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(((SparkFlex)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkFlex)drive).configAccessor.encoder.getQuadratureAverageDepth()); + break; + } drivePIDController.setTolerance(drivetoleranceMPerS); //System.out.println("Velocity Constant: " + (positionConstant / 60)); @@ -120,21 +155,23 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); lastAngle = 0.0; - turnPIDController = new ProfiledPIDController(config.turnkP[arrIndex], - config.turnkI[arrIndex], - config.turnkD[arrIndex], - turnConstraints); + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); turnPIDController.enableContinuousInput(-.5, .5); turnPIDController.setTolerance(turnToleranceRot); - - CANcoderConfiguration configs = new CANcoderConfiguration(); - configs.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - this.turnEncoder = turnEncoder; - this.turnEncoder.getConfigurator().apply(configs); this.driveModifier = config.driveModifier; this.reversed = config.reversed[arrIndex]; this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); turnPIDController.reset(getModuleAngle()); @@ -159,6 +196,10 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN SendableRegistry.addLW(this, "SwerveModule", type.toString()); + //do stuff here + drive.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + turn.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } public ModuleType getType() { @@ -168,16 +209,19 @@ public ModuleType getType() { private double prevTurnVelocity = 0; public void periodic() { drivePeriodic(); - //updateSmartDashboard(); + // updateSmartDashboard(); turnPeriodic(); } public void drivePeriodic() { String moduleString = type.toString(); double actualSpeed = getCurrentSpeed(); - double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : - backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration); - + double extraAccel = calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()); + double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : backwardSimpleMotorFF) + .calculateWithVelocities( + actualSpeed, + desiredSpeed + extraAccel * TimedRobot.kDefaultPeriod //m/s + ( m/s^2 * s ) + ); // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) double pidVolts = drivePIDController.calculate(actualSpeed, desiredSpeed); @@ -223,7 +267,7 @@ public void turnPeriodic() { // SmartDashboard.putNumber("previous turn Velocity", prevTurnVelocity); // SmartDashboard.putNumber("state velocity",state.velocity); - turnFFVolts = turnSimpleMotorFeedforward.calculate(state.velocity, 0);//(state.velocity-prevTurnVelocity) / period); + turnFFVolts = turnSimpleMotorFeedforward.calculate(state.velocity);//(state.velocity-prevTurnVelocity) / period); turnVolts = turnFFVolts + turnSpeedCorrectionVolts; if (!turnPIDController.atGoal()) { turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); @@ -303,7 +347,7 @@ private void setAngle(double angle) { * @return module angle in degrees */ public double getModuleAngle() { - return MathUtil.inputModulus(Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValue()) - turnZeroDeg, -180, 180); + return MathUtil.inputModulus(Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble()) - turnZeroDeg, -180, 180); } /** @@ -339,9 +383,9 @@ public double getCurrentSpeed() { public void updateSmartDashboard() { String moduleString = type.toString(); // Display the position of the quadrature encoder. - SmartDashboard.putNumber(moduleString + " Incremental Position", turnEncoder.getPosition().getValue()); + SmartDashboard.putNumber(moduleString + " Incremental Position", turnEncoder.getPosition().getValueAsDouble()); // Display the position of the analog encoder. - SmartDashboard.putNumber(moduleString + " Absolute Angle (deg)", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValue())); + SmartDashboard.putNumber(moduleString + " Absolute Angle (deg)", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); // Display the module angle as calculated using the absolute encoder. SmartDashboard.putNumber(moduleString + " Turn Measured Pos (deg)", getModuleAngle()); SmartDashboard.putNumber(moduleString + " Encoder Position", drive.getEncoder().getPosition()); @@ -370,14 +414,14 @@ public void updateSmartDashboard() { if (drivePIDController.getD() != drivekD) { drivePIDController.setD(drivekD); } - double driveTolerance = SmartDashboard.getNumber(moduleString + " Drive Tolerance", drivePIDController.getPositionTolerance()); - if (drivePIDController.getPositionTolerance() != driveTolerance) { + double driveTolerance = SmartDashboard.getNumber(moduleString + " Drive Tolerance", drivePIDController.getErrorTolerance()); + if (drivePIDController.getErrorTolerance() != driveTolerance) { drivePIDController.setTolerance(driveTolerance); } - double drivekS = SmartDashboard.getNumber(moduleString + " Drive kS", forwardSimpleMotorFF.ks); - double drivekV = SmartDashboard.getNumber(moduleString + " Drive kV", forwardSimpleMotorFF.kv); - double drivekA = SmartDashboard.getNumber(moduleString + " Drive kA", forwardSimpleMotorFF.ka); - if (forwardSimpleMotorFF.ks != drivekS || forwardSimpleMotorFF.kv != drivekV || forwardSimpleMotorFF.ka != drivekA) { + double drivekS = SmartDashboard.getNumber(moduleString + " Drive kS", forwardSimpleMotorFF.getKs()); + double drivekV = SmartDashboard.getNumber(moduleString + " Drive kV", forwardSimpleMotorFF.getKv()); + double drivekA = SmartDashboard.getNumber(moduleString + " Drive kA", forwardSimpleMotorFF.getKa()); + if (forwardSimpleMotorFF.getKs() != drivekS || forwardSimpleMotorFF.getKv() != drivekV || forwardSimpleMotorFF.getKa() != drivekA) { forwardSimpleMotorFF = new SimpleMotorFeedforward(drivekS, drivekV, drivekA); backwardSimpleMotorFF = new SimpleMotorFeedforward(drivekS, drivekV, drivekA); } @@ -393,10 +437,10 @@ public void updateSmartDashboard() { if (turnPIDController.getPositionTolerance() != turnTolerance) { turnPIDController.setTolerance(turnTolerance); } - double kS = SmartDashboard.getNumber(moduleString + " Swerve kS", turnSimpleMotorFeedforward.ks); - double kV = SmartDashboard.getNumber(moduleString + " Swerve kV", turnSimpleMotorFeedforward.kv); - double kA = SmartDashboard.getNumber(moduleString + " Swerve kA", turnSimpleMotorFeedforward.ka); - if (turnSimpleMotorFeedforward.ks != kS || turnSimpleMotorFeedforward.kv != kV || turnSimpleMotorFeedforward.ka != kA) { + double kS = SmartDashboard.getNumber(moduleString + " Swerve kS", turnSimpleMotorFeedforward.getKs()); + double kV = SmartDashboard.getNumber(moduleString + " Swerve kV", turnSimpleMotorFeedforward.getKv()); + double kA = SmartDashboard.getNumber(moduleString + " Swerve kA", turnSimpleMotorFeedforward.getKa()); + if (turnSimpleMotorFeedforward.getKs() != kS || turnSimpleMotorFeedforward.getKv() != kV || turnSimpleMotorFeedforward.getKa() != kA) { turnSimpleMotorFeedforward = new SimpleMotorFeedforward(kS, kV, kA); maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); @@ -404,18 +448,46 @@ public void updateSmartDashboard() { } public void toggleMode() { - if (drive.getIdleMode() == IdleMode.kBrake && turn.getIdleMode() == IdleMode.kCoast) coast(); - else brake(); + switch(MotorControllerType.getMotorControllerType(drive)) { + case SPARK_MAX: + switch (MotorControllerType.getMotorControllerType(turn)) { + case SPARK_MAX: + if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + else brake(); + break; + case SPARK_FLEX: + if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + else brake(); + break; + } + case SPARK_FLEX: + switch (MotorControllerType.getMotorControllerType(turn)) { + case SPARK_MAX: + if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + else brake(); + break; + case SPARK_FLEX: + if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + else brake(); + break; + } + break; + } } public void brake() { - drive.setIdleMode(IdleMode.kBrake); - turn.setIdleMode(IdleMode.kBrake); + SparkBaseConfig config = driveMotorType.createConfig().idleMode(IdleMode.kBrake); + drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + config = turnMotorType.createConfig().idleMode(IdleMode.kBrake); + turn.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } public void coast() { - drive.setIdleMode(IdleMode.kCoast); - turn.setIdleMode(IdleMode.kCoast); + SparkBaseConfig config = driveMotorType.createConfig().idleMode(IdleMode.kCoast); + + drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + config = turnMotorType.createConfig().idleMode(IdleMode.kCoast); + turn.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } /** @@ -434,8 +506,8 @@ public void initSendable(SendableBuilder builder) { builder.setActuator(true); builder.setSafeState(() -> setSpeed(0)); builder.setSmartDashboardType("SwerveModule"); - builder.addDoubleProperty("Incremental Position", () -> turnEncoder.getPosition().getValue(), null); - builder.addDoubleProperty("Absolute Angle (deg)", () -> Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValue()), null); + builder.addDoubleProperty("Incremental Position", () -> turnEncoder.getPosition().getValueAsDouble(), null); + builder.addDoubleProperty("Absolute Angle (deg)", () -> Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble()), null); builder.addDoubleProperty("Turn Measured Pos (deg)", this::getModuleAngle, null); builder.addDoubleProperty("Encoder Position", drive.getEncoder()::getPosition, null); // Display the speed that the robot thinks it is travelling at. @@ -463,7 +535,7 @@ public void initSendable(SendableBuilder builder) { * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2) * @return a SwerveModuleSim that simulates the physics of this swerve module. */ - public SwerveModuleSim createSim(Measure massOnWheel, double turnGearing, double turnMoiKgM2) { + public SwerveModuleSim createSim(Mass massOnWheel, double turnGearing, double turnMoiKgM2) { double driveMoiKgM2 = massOnWheel.in(Kilogram) * Math.pow(config.wheelDiameterMeters/2, 2); return new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, driveMoiKgM2, turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 14c52890..4987540d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -4,11 +4,13 @@ import org.carlmontrobotics.lib199.sim.MockedEncoder; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Mass; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Mult; +import edu.wpi.first.units.measure.Mult; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -35,12 +37,15 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2) { driveMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", drivePortNum); driveEncoderSim = new SimDeviceSim("CANEncoder:CANSparkMax", drivePortNum); - drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2); + DCMotor dcmotor = DCMotor.getNEO(1); + drivePhysicsSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(dcmotor, driveMoiKgM2, driveGearing), dcmotor);//FIXME WHAT DO WE WANT THE MEASUREMENT STDDEVS TO BE? (LAST ARG) this.driveGearing = driveGearing; turnMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", turnMotorPortNum); turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); - turnPhysicsSim = new DCMotorSim(DCMotor.getNEO(1), turnGearing, turnMoiKgM2); + turnPhysicsSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(dcmotor, turnMoiKgM2, turnGearing), + dcmotor); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/vendorLibs/Elastic.java b/src/main/java/org/carlmontrobotics/lib199/vendorLibs/Elastic.java new file mode 100644 index 00000000..8c8cca42 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/vendorLibs/Elastic.java @@ -0,0 +1,398 @@ +// Copyright (c) 2023-2025 Gold87 and other Elastic contributors +// This software can be modified and/or shared under the terms +// defined by the Elastic license: +// https://github.com/Gold872/elastic-dashboard/blob/main/LICENSE + +// From: https://frc-elastic.gitbook.io/docs/additional-features-and-references/robot-notifications-with-elasticlib and https://github.com/Gold872/elastic-dashboard/blob/main/elasticlib/Elastic.java + +package org.carlmontrobotics.lib199.vendorLibs; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; +/** + * A class that provides methods for interacting with the Elastic dashboard, including sending + * notifications and selecting tabs. This taken straight from the official Elastic documentation + * (see https://frc-elastic.gitbook.io/docs/additional-features-and-references/robot-notifications-with-elasticlib for documentation + * and for original code https://github.com/Gold872/elastic-dashboard/blob/main/elasticlib/Elastic.java) + */ + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); + private static final ObjectMapper objectMapper = new ObjectMapper(); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(objectMapper.writeValueAsString(notification)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + public static class Notification { + @JsonProperty("level") + private NotificationLevel level; + + @JsonProperty("title") + private String title; + + @JsonProperty("description") + private String description; + + @JsonProperty("displayTime") + private int displayTimeMillis; + + @JsonProperty("width") + private double width; + + @JsonProperty("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java new file mode 100644 index 00000000..47aefa0f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java @@ -0,0 +1,1705 @@ +/* +docs: https://docs.limelightvision.io/docs/docs-limelight/apis/limelight-lib +code: https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java +*/ + +//LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) + +package org.carlmontrobotics.lib199.vendorLibs; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers.LimelightResults; +import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Arrays; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id && + Double.compare(txnc, other.txnc) == 0 && + Double.compare(tync, other.tync) == 0 && + Double.compare(ta, other.ta) == 0 && + Double.compare(distToCamera, other.distToCamera) == 0 && + Double.compare(distToRobot, other.distToRobot) == 0 && + Double.compare(ambiguity, other.ambiguity) == 0; + } + + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping throttle frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.java b/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.java deleted file mode 100644 index 3b4888aa..00000000 --- a/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.carlmontrobotics.lib199; - -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertNotNull; - -import com.revrobotics.REVLibError; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.SparkAnalogSensor.Mode; -import com.revrobotics.SparkLimitSwitch.Type; -import com.revrobotics.SparkPIDController.AccelStrategy; - -import org.junit.Test; - -public class DummySparkMaxAnswerTest { - - public CANSparkMax createMockedSparkMax() { - return Mocks.mock(CANSparkMax.class, new DummySparkMaxAnswer()); - } - - public static void assertTestResponses(CANSparkMax spark) { - // Check device id - assertEquals(0, spark.getDeviceId()); - - // Check get and set methods - assertEquals(0, spark.get(), 0.01); - spark.set(0); - - // Check that all REV specific objects are non-null - assertNotNull(spark.getAnalog((Mode) null)); - assertNotNull(spark.getEncoder()); - assertNotNull(spark.getForwardLimitSwitch((Type) null)); - assertNotNull(spark.getPIDController()); - - // Check that all REV specific objects return "null" values - assertEquals(0, spark.getEncoder().getPosition(), 0.01); - assertEquals(0, spark.getEncoder().getVelocity(), 0.01); - assertEquals(IdleMode.kBrake, spark.getIdleMode()); - assertEquals(MotorType.kBrushless, spark.getMotorType()); - assertEquals(AccelStrategy.kTrapezoidal, spark.getPIDController().getSmartMotionAccelStrategy(0)); - assertEquals(REVLibError.kOk, spark.getLastError()); - assertEquals(REVLibError.kOk, spark.getPIDController().setP(0, 0)); - assertEquals(REVLibError.kOk, spark.getEncoder().setAverageDepth(0)); - assertEquals(REVLibError.kOk, spark.getAnalog((Mode) null).setInverted(false)); - } - - @Test - public void testResponses() { - assertTestResponses(createMockedSparkMax()); - } - -} diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java index c887d01a..87ccc3ed 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java @@ -20,7 +20,6 @@ public class MotorControllerFactoryTest extends ErrStreamTest { public void testCreateNoErrors() throws Exception { // Call close to free PWM ports ((AutoCloseable)MotorControllerFactory.createTalon(0)).close(); - ((AutoCloseable)MotorControllerFactory.createVictor(1)).close(); MotorControllerFactory.createSparkMax(2, MotorConfig.NEO); assertEquals(0, errStream.toByteArray().length); } diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java index bdb06851..e785fb9f 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java @@ -10,8 +10,9 @@ import com.ctre.phoenix.ErrorCode; import com.revrobotics.REVLibError; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.FaultID; +import com.revrobotics.sim.SparkSimFaultManager; +import com.revrobotics.spark.SparkBase.Faults; +import com.revrobotics.spark.SparkMax; import org.carlmontrobotics.lib199.testUtils.ErrStreamTest; import org.junit.Test; @@ -19,28 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MotorErrorsTest extends ErrStreamTest { - - public static class SensorFaultSparkMax { - public short getFaults() { - return (short)FaultID.kSensorFault.value; - } - - public boolean getFault(FaultID faultID) { - return faultID == FaultID.kSensorFault; - } - } - - public static class StickySensorFaultSparkMax { - public short getStickyFaults() { - return (short)FaultID.kSensorFault.value; - } - - public boolean getStickyFault(FaultID faultID) { - return faultID == FaultID.kSensorFault; - } - } - - public static interface TemperatureSparkMax { + public static interface TemperatureSparkMax { public void setTemperature(double temperature); public int getSmartCurrentLimit(); @@ -132,31 +112,32 @@ public void testOtherErrors() { @Test public void testFaultReporting() { - CANSparkMax sensorFaultSparkMax = Mocks.createMock(CANSparkMax.class, new SensorFaultSparkMax(), false); + Faults sensorFault = new Faults(4); + SparkMax sensorFaultSparkMax = MotorControllerFactory.createSparkMax(1, MotorConfig.NEO, MotorControllerFactory.sparkConfig(MotorConfig.NEO)); + SparkSimFaultManager sparkSimFaultManager = new SparkSimFaultManager(sensorFaultSparkMax); + sparkSimFaultManager.setFaults(sensorFault); errStream.reset(); - MotorErrors.checkSparkMaxErrors(sensorFaultSparkMax); + MotorErrors.checkSparkErrors(sensorFaultSparkMax); assertNotEquals(0, errStream.toByteArray().length); errStream.reset(); - MotorErrors.checkSparkMaxErrors(sensorFaultSparkMax); + MotorErrors.checkSparkErrors(sensorFaultSparkMax); assertEquals(0, errStream.toByteArray().length); } @Test public void testStickyFaultReporting() { - CANSparkMax stickySensorFaultSparkMax = Mocks.createMock(CANSparkMax.class, new StickySensorFaultSparkMax(), false); + Faults sensorFault = new Faults(4); + SparkMax stickySensorFaultSparkMax = MotorControllerFactory.createSparkMax(1, MotorConfig.NEO, MotorControllerFactory.sparkConfig(MotorConfig.NEO)); + SparkSimFaultManager sparkSimFaultManager = new SparkSimFaultManager(stickySensorFaultSparkMax); + sparkSimFaultManager.setStickyFaults(sensorFault); errStream.reset(); - MotorErrors.checkSparkMaxErrors(stickySensorFaultSparkMax); + MotorErrors.checkSparkErrors(stickySensorFaultSparkMax); assertNotEquals(0, errStream.toByteArray().length); errStream.reset(); - MotorErrors.checkSparkMaxErrors(stickySensorFaultSparkMax); + MotorErrors.checkSparkErrors(stickySensorFaultSparkMax); assertEquals(0, errStream.toByteArray().length); } - @Test - public void testDummySparkMax() { - DummySparkMaxAnswerTest.assertTestResponses(MotorErrors.createDummySparkMax()); - } - @Test public void testReportSparkMaxTemp() { assertTrue(MotorErrors.kOverheatTripCount > 0); @@ -167,32 +148,32 @@ public void testReportSparkMaxTemp() { } private void doTestReportSparkMaxTemp(int id) { - TemperatureSparkMax spark = (TemperatureSparkMax)Mocks.createMock(CANSparkMax.class, new TemperatureSparkMax.Instance(id), TemperatureSparkMax.class); + TemperatureSparkMax spark = (TemperatureSparkMax)Mocks.createMock(SparkMax.class, new TemperatureSparkMax.Instance(id), TemperatureSparkMax.class); String smartDashboardKey = "Port " + id + " Spark Max Temp"; - MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40); + MotorErrors.reportSparkTemp((SparkMax)spark, 40); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); if(MotorErrors.kOverheatTripCount > 1) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); } @@ -205,7 +186,7 @@ private void doTestReportSparkMaxTemp(int id) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); } @@ -214,13 +195,13 @@ private void doTestReportSparkMaxTemp(int id) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkMaxTemp(); + MotorErrors.doReportSparkTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); diff --git a/src/test/java/org/carlmontrobotics/lib199/REVLibErrorAnswerTest.java b/src/test/java/org/carlmontrobotics/lib199/REVLibErrorAnswerTest.java index 748098ed..92e9c1bb 100644 --- a/src/test/java/org/carlmontrobotics/lib199/REVLibErrorAnswerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/REVLibErrorAnswerTest.java @@ -18,7 +18,7 @@ public void testResponses() throws Exception { assertEquals(0, enc.getVelocity(), 0.01); // Check that REVLibError functions return REVLibError.kOk - assertEquals(REVLibError.kOk, enc.setInverted(false)); + assertEquals(REVLibError.kOk, enc.setPosition(0)); } } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java index d5fde3bb..51a08acb 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java @@ -3,8 +3,9 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotNull; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import org.carlmontrobotics.lib199.sim.MockSparkBase.NEOType; import org.carlmontrobotics.lib199.testUtils.TestRules; import org.junit.ClassRule; import org.junit.Rule; @@ -21,7 +22,7 @@ public class MockSparkMaxTest { @Test public void testHasEncoder() { - var mockSpark = new MockSparkMax(0, MotorType.kBrushless); + var mockSpark = new MockSparkMax(0, MotorType.kBrushless, NEOType.NEO); SimDeviceSim simSpark = new SimDeviceSim("CANMotor:CANSparkMax", 0); assertNotNull(simSpark); SimDeviceSim simEncoder = new SimDeviceSim("CANEncoder:CANSparkMax", 0); @@ -33,7 +34,7 @@ public void testHasEncoder() { @Test public void testCanRecreateIfClosed() { for (int i = 0; i < 2; i++) { - try (var mockSpark = new MockSparkMax(0, MotorType.kBrushless)) { + try (var mockSpark = new MockSparkMax(0, MotorType.kBrushless, NEOType.NEO)) { SimDeviceSim simSpark = new SimDeviceSim("CANMotor:CANSparkMax", 0); assertNotNull(simSpark); @@ -48,7 +49,7 @@ public void testCanRecreateIfClosed() { @Test public void testGetOutputCurrent() { - var mockSpark = new MockSparkMax(0, MotorType.kBrushless); + var mockSpark = new MockSparkMax(0, MotorType.kBrushless, NEOType.NEO); SimDeviceSim simSpark = new SimDeviceSim("CANMotor:CANSparkMax", 0); assertNotNull(simSpark); SimDouble simCurrent = simSpark.getDouble("motorCurrent"); diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java deleted file mode 100644 index b6fdd307..00000000 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.carlmontrobotics.lib199.sim; - -import com.ctre.phoenix.motorcontrol.can.BaseMotorController; - -public class MockVictorSPXTest extends MockPheonixControllerTest { - - @Override - protected BaseMotorController createController(int portPWM) { - return MockVictorSPX.createMockVictorSPX(portPWM); - } - -} diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java index 1f842f56..d69a277e 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java @@ -9,6 +9,12 @@ import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; @@ -60,8 +66,8 @@ public void testFunctionality() { } private void testFunctionalityWithPositionConversionFactor(double factor, RelativeEncoder enc, SimDouble positionSim) { - assertEquals(REVLibError.kOk, enc.setPositionConversionFactor(factor)); - assertEquals(factor, enc.getPositionConversionFactor(), 0.01); + assertEquals(REVLibError.kOk, ((MockedEncoder) enc).setPositionConversionFactor(factor)); + assertEquals(factor, ((MockedEncoder) enc).getPositionConversionFactor(), 0.01); testPosition(10, enc, factor, positionSim); testPosition(0, enc, factor, positionSim); testPosition(-10, enc, factor, positionSim); @@ -73,7 +79,7 @@ private void testPosition(double position, RelativeEncoder enc, double conversio assertEquals(position, enc.getPosition(), 0.02); assertEquals(REVLibError.kOk, enc.setPosition(0)); assertEquals(0, enc.getPosition(), 0.02); - positionSim.set(position / enc.getPositionConversionFactor() + positionSim.get()); + positionSim.set(position / ((MockedEncoder) enc).getPositionConversionFactor() + positionSim.get()); assertEquals(position, enc.getPosition(), 0.02); } @@ -87,11 +93,11 @@ private boolean simDeviceExists(String deviceName) { private SafelyClosable createEncoder(int deviceId) { SimDevice device = SimDevice.create("testDevice", deviceId); - return (SafelyClosable)Mocks.createMock( - RelativeEncoder.class, - new MockedEncoder(device, 4096, false, false), - new REVLibErrorAnswer(), - SafelyClosable.class); + return (SafelyClosable)Mocks.createMock( + RelativeEncoder.class, + new MockedEncoder(device, 4096, false, false), + new REVLibErrorAnswer(), + SafelyClosable.class, MockedEncoder.RemovedMethods.class); } private void withEncoders(EncoderTest func) { @@ -112,5 +118,5 @@ private void withEncoder(int id, EncoderTest func) { private interface EncoderTest { public void test(RelativeEncoder encoder, SimDeviceSim sim, SimDouble posSim); } - + } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopControllerTest.java similarity index 84% rename from src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java rename to src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopControllerTest.java index b958dbd3..bf3ba5fa 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkClosedLoopControllerTest.java @@ -7,19 +7,20 @@ import java.util.function.Supplier; import com.revrobotics.REVLibError; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; +import org.carlmontrobotics.lib199.sim.MockSparkBase.NEOType; import org.junit.Test; -public class MockedSparkMaxPIDControllerTest { +public class MockedSparkClosedLoopControllerTest { @Test public void testResponses() { - MockSparkMax mockSparkMax = new MockSparkMax(0, MotorType.kBrushless); - SparkPIDController mock = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(mockSparkMax), new REVLibErrorAnswer()); + MockSparkMax mockSparkMax = new MockSparkMax(0, MotorType.kBrushless, NEOType.NEO); + MockedSparkClosedLoopController mock = new MockedSparkClosedLoopController(mockSparkMax); assertSlotValueUpdate(mock::setP, mock::setP, mock::getP, mock::getP); assertSlotValueUpdate(mock::setI, mock::setI, mock::getI, mock::getI); assertSlotValueUpdate(mock::setD, mock::setD, mock::getD, mock::getD); diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index e978a5f7..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 85% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.7.json index 6dc648db..d3f84e53 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,9 +1,9 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5-5.35.1.json similarity index 75% rename from vendordeps/Phoenix5.json rename to vendordeps/Phoenix5-5.35.1.json index ff7359e1..69df8b53 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5-5.35.1.json @@ -1,43 +1,56 @@ { - "fileName": "Phoenix5.json", + "fileName": "Phoenix5-5.35.1.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, + "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-frc2024-latest.json", + "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.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + "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.33.1" + "version": "5.35.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.1" + "version": "5.35.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -45,12 +58,13 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -60,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -68,6 +82,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -75,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -83,6 +98,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -90,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -98,6 +114,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -105,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -113,6 +130,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -120,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -128,6 +146,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -143,6 +162,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 62% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-frc2025-latest.json index 2b7d1720..6f40c840 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", - "frcYear": 2024, + "version": "25.4.0", + "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-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "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": "24.2.0" + "version": "25.4.0" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.2.0", + "artifactId": "api-cpp-sim", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.2.0", + "artifactId": "tools-sim", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.2.0", + "artifactId": "simTalonSRX", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +180,55 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +246,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +262,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +278,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +294,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,81 +310,87 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.2.0", - "libName": "CTRE_SimTalonFX", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.2.0", - "libName": "CTRE_SimVictorSPX", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.2.0", - "libName": "CTRE_SimPigeonIMU", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.2.0", - "libName": "CTRE_SimCANCoder", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.2.0", - "libName": "CTRE_SimProTalonFX", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +406,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +422,55 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f85acd40..ac62be88 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,30 +1,29 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", + "version": "2025.0.3", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 00000000..5010be04 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf3898..3718e0ac 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2025.json similarity index 76% rename from vendordeps/playingwithfusion2024.json rename to vendordeps/playingwithfusion2025.json index c8c51493..61a56411 100644 --- a/vendordeps/playingwithfusion2024.json +++ b/vendordeps/playingwithfusion2025.json @@ -1,10 +1,10 @@ { - "fileName": "playingwithfusion2024.json", + "fileName": "playingwithfusion2025.json", "name": "PlayingWithFusion", - "version": "2024.03.09", + "version": "2025.01.23", "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", - "frcYear": "2024", - "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", + "frcYear": "2025", + "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2025.json", "mavenUrls": [ "https://www.playingwithfusion.com/frc/maven/" ], @@ -12,23 +12,24 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-java", - "version": "2024.03.09" + "version": "2025.01.23" } ], "jniDependencies": [ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.03.09", - "isJar": false, + "version": "2025.01.23", "skipInvalidPlatforms": true, + "isJar": false, "validPlatforms": [ - "linuxathena", "windowsx86-64", + "windowsx86", + "linuxarm64", "linuxx86-64", - "osxuniversal", + "linuxathena", "linuxarm32", - "linuxarm64" + "osxuniversal" ] } ], @@ -36,35 +37,37 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-cpp", - "version": "2024.03.09", + "version": "2025.01.23", "headerClassifier": "headers", "sharedLibrary": false, "libName": "PlayingWithFusion", "skipInvalidPlatforms": true, "binaryPlatforms": [ - "linuxathena", "windowsx86-64", + "windowsx86", + "linuxarm64", "linuxx86-64", - "osxuniversal", + "linuxathena", "linuxarm32", - "linuxarm64" + "osxuniversal" ] }, { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.03.09", + "version": "2025.01.23", "headerClassifier": "headers", "sharedLibrary": true, "libName": "PlayingWithFusionDriver", "skipInvalidPlatforms": true, "binaryPlatforms": [ - "linuxathena", "windowsx86-64", + "windowsx86", + "linuxarm64", "linuxx86-64", - "osxuniversal", + "linuxathena", "linuxarm32", - "linuxarm64" + "osxuniversal" ] } ]