├── gradle.properties ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── src └── main │ └── kotlin │ └── org │ └── team2471 │ └── frc │ └── lib │ ├── ctre │ ├── loggedTalonFX │ │ ├── LoggedMotor.kt │ │ ├── MasterMotor.kt │ │ └── LoggedTalonFX.kt │ ├── CANCoderUtil.kt │ ├── PhoenixUtil.kt │ └── TalonFXUtil.kt │ ├── motion_profiling │ ├── BooleanPair.kt │ ├── CubicCoefficients1D.kt │ └── MotionKey.kt │ ├── vision │ ├── PipelineVisionPacket.kt │ ├── QuixVisionCamera.kt │ ├── PipelineConfig.kt │ ├── QuixVisionSim.kt │ ├── Fiducial.kt │ ├── limelight │ │ ├── LimelightCamera.kt │ │ └── VisionIOLimelight.kt │ └── photonVision │ │ └── PhotonVisionCamera.kt │ ├── control │ ├── commands │ │ ├── WaitUntilOrTimeCommand.kt │ │ └── CommandUtils.kt │ ├── LoopLogger.kt │ ├── PDVelocityController.kt │ └── XboxControllerUtils.kt │ ├── util │ ├── RobotMode.kt │ ├── WpilibUtils.kt │ └── DynamicInterpolatingTreeMap.kt │ ├── localization │ ├── InterpolatableChassisSpeeds.kt │ ├── PoseEstimate.kt │ ├── Measurement.kt │ ├── NTManager.kt │ └── CameraInfo.kt │ ├── math │ └── MathUtils.kt │ ├── units │ ├── UTranslation2d.kt │ └── Units.kt │ └── swerve │ └── SwerveSetpointGenerator.kt ├── vendordeps ├── Autopilot.json ├── questnavlib.json ├── AdvantageKit.json ├── WPILibNewCommands.json ├── ChoreoLib.json ├── Studica.json ├── photonlib.json └── Phoenix6.json ├── README.md ├── settings.gradle ├── LICENSE ├── .gitignore ├── gradlew.bat └── gradlew /gradle.properties: -------------------------------------------------------------------------------- 1 | kotlin.code.style=official 2 | org.gradle.warning.mode=all -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TeamMeanMachine/meanlib/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/loggedTalonFX/LoggedMotor.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.ctre.loggedTalonFX 2 | 3 | interface LoggedMotor { 4 | fun simPeriodic() 5 | } -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/loggedTalonFX/MasterMotor.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.ctre.loggedTalonFX 2 | 3 | import org.team2471.frc.lib.util.isSim 4 | 5 | object MasterMotor { 6 | private val motors = mutableListOf() 7 | 8 | fun simPeriodic() { 9 | if (isSim) { 10 | motors.forEach { 11 | it.simPeriodic() 12 | } 13 | } 14 | } 15 | 16 | fun addMotor(motor: LoggedMotor) = motors.add(motor) 17 | } -------------------------------------------------------------------------------- /vendordeps/Autopilot.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Autopilot", 3 | "uuid": "4e922d47-9954-4db5-929f-c708cc62e152", 4 | "fileName": "Autopilot.json", 5 | "version": "1.3.0", 6 | "frcYear": "2025", 7 | "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json", 8 | "mavenUrls": [ "https://jitpack.io" ], 9 | "javaDependencies": [ 10 | { 11 | "groupId": "com.github.therekrab", 12 | "artifactId": "autopilot", 13 | "version": "1.3.0" 14 | } 15 | ], 16 | "cppDependencies": [], 17 | "jniDependencies": [] 18 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/motion_profiling/BooleanPair.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.motion_profiling 2 | 3 | data class BooleanPair(var x: Double, var y: Double) { 4 | operator fun plus(b: BooleanPair) = BooleanPair(x + b.x, y + b.y) 5 | 6 | operator fun minus(b: BooleanPair) = BooleanPair(x - b.x, y - b.y) 7 | 8 | operator fun times(scalar: Double) = BooleanPair(x * scalar, y * scalar) 9 | 10 | operator fun div(scalar: Double) = BooleanPair(x / scalar, y / scalar) 11 | 12 | fun set(X: Double, Y: Double) { 13 | x = X 14 | y = Y 15 | } 16 | } -------------------------------------------------------------------------------- /vendordeps/questnavlib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "questnavlib.json", 3 | "name": "questnavlib", 4 | "version": "2025-1.1.1-beta", 5 | "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://maven.questnav.gg/releases", 9 | "https://maven.questnav.gg/snapshots" 10 | ], 11 | "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.1.1-beta/questnavlib-json-2025-1.1.1-beta.json", 12 | "javaDependencies": [ 13 | { 14 | "groupId": "gg.questnav", 15 | "artifactId": "questnavlib-java", 16 | "version": "2025-1.1.1-beta" 17 | } 18 | ], 19 | "cppDependencies": [], 20 | "jniDependencies": [] 21 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/PipelineVisionPacket.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision 2 | 3 | import org.photonvision.targeting.PhotonTrackedTarget 4 | 5 | /** A data class for a pipeline packet. */ 6 | class PipelineVisionPacket( 7 | /** 8 | * If the vision packet has valid targets. 9 | * 10 | * @return if targets are found. 11 | */ 12 | val hasTargets: Boolean, 13 | /** 14 | * Gets best target. 15 | * 16 | * @return the best target. 17 | */ 18 | val bestTarget: PhotonTrackedTarget?, 19 | /** 20 | * Gets targets. 21 | * 22 | * @return the targets. 23 | */ 24 | val targets: MutableList?, 25 | /** 26 | * Gets the capture timestamp in seconds. -1 if the result has no timestamp set. 27 | * 28 | * @return the timestamp in seconds. 29 | */ 30 | val captureTimestamp: Double 31 | ) 32 | -------------------------------------------------------------------------------- /vendordeps/AdvantageKit.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "AdvantageKit.json", 3 | "name": "AdvantageKit", 4 | "version": "4.1.2", 5 | "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" 9 | ], 10 | "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "org.littletonrobotics.akit", 14 | "artifactId": "akit-java", 15 | "version": "4.1.2" 16 | } 17 | ], 18 | "jniDependencies": [ 19 | { 20 | "groupId": "org.littletonrobotics.akit", 21 | "artifactId": "akit-wpilibio", 22 | "version": "4.1.2", 23 | "skipInvalidPlatforms": false, 24 | "isJar": false, 25 | "validPlatforms": [ 26 | "linuxathena", 27 | "linuxx86-64", 28 | "linuxarm64", 29 | "osxuniversal", 30 | "windowsx86-64" 31 | ] 32 | } 33 | ], 34 | "cppDependencies": [] 35 | } 36 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # Meanlib 4 | 5 | Meanlib is a Kotlin FRC Robot utility library made by Team 2471 Mean Machine. 6 | 7 | 8 | ## Installation 9 | 10 | Add meanlib as a submodule: 11 | ```shell 12 | git submodule add https://github.com/TeamMeanMachine/meanlib/ 13 | ``` 14 | 15 | Checkout the current year's branch: 16 | ```shell 17 | git checkout frc[CURRENT_YEAR] # ex: git checkout frc2026 18 | ``` 19 | 20 | Add meanlib to your build.gradle dependencies: 21 | ```groovy 22 | dependencies { 23 | implementation(project(":meanlib")) 24 | .. 25 | } 26 | ``` 27 | 28 | Add meanlib to the jar task in your build.gradle: 29 | ```groovy 30 | jar { 31 | dependsOn(':meanlib:jar') // <- add me 32 | 33 | from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } 34 | from sourceSets.main.allSource 35 | manifest GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) 36 | duplicatesStrategy = DuplicatesStrategy.INCLUDE 37 | } 38 | ``` 39 | 40 | -------------------------------------------------------------------------------- /vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "1.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "frcYear": "2025", 7 | "mavenUrls": [], 8 | "jsonUrl": "", 9 | "javaDependencies": [ 10 | { 11 | "groupId": "edu.wpi.first.wpilibNewCommands", 12 | "artifactId": "wpilibNewCommands-java", 13 | "version": "wpilib" 14 | } 15 | ], 16 | "jniDependencies": [], 17 | "cppDependencies": [ 18 | { 19 | "groupId": "edu.wpi.first.wpilibNewCommands", 20 | "artifactId": "wpilibNewCommands-cpp", 21 | "version": "wpilib", 22 | "libName": "wpilibNewCommands", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": true, 26 | "skipInvalidPlatforms": true, 27 | "binaryPlatforms": [ 28 | "linuxathena", 29 | "linuxarm32", 30 | "linuxarm64", 31 | "windowsx86-64", 32 | "windowsx86", 33 | "linuxx86-64", 34 | "osxuniversal" 35 | ] 36 | } 37 | ] 38 | } 39 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/control/commands/WaitUntilOrTimeCommand.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.control.commands 2 | 3 | import edu.wpi.first.util.sendable.SendableBuilder 4 | import edu.wpi.first.util.sendable.SendableRegistry 5 | import edu.wpi.first.wpilibj.Timer 6 | import edu.wpi.first.wpilibj2.command.Command 7 | 8 | class WaitUntilOrTimeCommand(val seconds: Double = Double.MAX_VALUE, val condition: () -> Boolean): Command() { 9 | private val timer = Timer() 10 | 11 | init { 12 | SendableRegistry.setName(this, getName() + ": " + seconds + " seconds") 13 | } 14 | 15 | override fun initialize() { 16 | timer.restart() 17 | } 18 | 19 | override fun end(interrupted: Boolean) { 20 | timer.stop() 21 | } 22 | 23 | override fun isFinished(): Boolean { 24 | return timer.hasElapsed(seconds) || condition() 25 | } 26 | 27 | override fun runsWhenDisabled(): Boolean { 28 | return true 29 | } 30 | 31 | override fun initSendable(builder: SendableBuilder) { 32 | super.initSendable(builder) 33 | builder.addDoubleProperty("duration", { seconds }, null) 34 | } 35 | } -------------------------------------------------------------------------------- /vendordeps/ChoreoLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "ChoreoLib2025.json", 3 | "name": "ChoreoLib", 4 | "version": "2025.0.1", 5 | "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://lib.choreo.autos/dep", 9 | "https://repo1.maven.org/maven2" 10 | ], 11 | "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", 12 | "javaDependencies": [ 13 | { 14 | "groupId": "choreo", 15 | "artifactId": "ChoreoLib-java", 16 | "version": "2025.0.1" 17 | }, 18 | { 19 | "groupId": "com.google.code.gson", 20 | "artifactId": "gson", 21 | "version": "2.11.0" 22 | } 23 | ], 24 | "jniDependencies": [], 25 | "cppDependencies": [ 26 | { 27 | "groupId": "choreo", 28 | "artifactId": "ChoreoLib-cpp", 29 | "version": "2025.0.1", 30 | "libName": "ChoreoLib", 31 | "headerClassifier": "headers", 32 | "sharedLibrary": false, 33 | "skipInvalidPlatforms": true, 34 | "binaryPlatforms": [ 35 | "windowsx86-64", 36 | "linuxx86-64", 37 | "osxuniversal", 38 | "linuxathena", 39 | "linuxarm32", 40 | "linuxarm64" 41 | ] 42 | } 43 | ] 44 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/QuixVisionCamera.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision 2 | 3 | import edu.wpi.first.math.Matrix 4 | import edu.wpi.first.math.geometry.Transform3d 5 | import edu.wpi.first.math.numbers.N1 6 | import edu.wpi.first.math.numbers.N3 7 | import edu.wpi.first.math.numbers.N8 8 | import org.photonvision.simulation.PhotonCameraSim 9 | import java.util.* 10 | 11 | interface QuixVisionCamera { 12 | /** Returns the simulated camera object. */ 13 | val cameraSim: PhotonCameraSim 14 | 15 | fun updateInputs() 16 | 17 | /** Returns the latest measurement. */ 18 | val latestMeasurement: PipelineVisionPacket 19 | 20 | /** Select the active pipeline index. */ 21 | fun setPipelineIndex(index: Int) 22 | 23 | /** Get the active pipeline config. */ 24 | val pipelineConfig: PipelineConfig 25 | 26 | /** Returns the robot-to-camera transform. */ 27 | val transform: Transform3d 28 | 29 | val cameraMatrix: Optional> 30 | 31 | val distCoeffs: Optional> 32 | 33 | /** Returns the type of fiducials this camera is tracking. */ 34 | val fiducialType: Fiducial.Type 35 | 36 | var allDataPopulated: Boolean 37 | } 38 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/PipelineConfig.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision 2 | 3 | import edu.wpi.first.math.Matrix 4 | import edu.wpi.first.math.numbers.N1 5 | import edu.wpi.first.math.numbers.N3 6 | import edu.wpi.first.math.numbers.N8 7 | import org.team2471.frc.lib.units.asRotation2d 8 | import org.team2471.frc.lib.units.degrees 9 | import org.photonvision.simulation.SimCameraProperties 10 | 11 | /** Describes a given vision camera pipeline configuration. */ 12 | class PipelineConfig( 13 | val fiducialType: Fiducial.Type = Fiducial.Type.APRILTAG, 14 | // pixels 15 | val imageWidth: Int = 1280, 16 | // pixels 17 | val imageHeight: Int = 800, 18 | 19 | //used for sim only 20 | val simCameraProp: SimCameraProperties = SimCameraProperties().apply { 21 | setCalibration(resWidth, resHeight, 70.2.degrees.asRotation2d) 22 | setCalibError(0.001, 0.005) // Values from docs. Should change 23 | fps = 20.0 24 | avgLatencyMs = 20.0 25 | latencyStdDevMs = 3.0 26 | }, 27 | // used for sim only 28 | val camIntrinsics: Matrix = simCameraProp.intrinsics, 29 | // used for sim only 30 | val distCoeffs: Matrix = simCameraProp.distCoeffs 31 | ) -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2025' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name = 'frcHome' 24 | url = frcHomeMaven 25 | } 26 | } 27 | } 28 | 29 | 30 | plugins { 31 | id("org.gradle.toolchains.foojay-resolver-convention") version "0.5.0" 32 | } 33 | 34 | Properties props = System.getProperties() 35 | props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true") 36 | 37 | rootProject.name = 'meanlib' -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This is free and unencumbered software released into the public domain. 2 | 3 | Anyone is free to copy, modify, publish, use, compile, sell, or 4 | distribute this software, either in source code form or as a compiled 5 | binary, for any purpose, commercial or non-commercial, and by any 6 | means. 7 | 8 | In jurisdictions that recognize copyright laws, the author or authors 9 | of this software dedicate any and all copyright interest in the 10 | software to the public domain. We make this dedication for the benefit 11 | of the public at large and to the detriment of our heirs and 12 | successors. We intend this dedication to be an overt act of 13 | relinquishment in perpetuity of all present and future rights to this 14 | software under copyright law. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 | IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR 20 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 21 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | For more information, please refer to 25 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/util/RobotMode.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.util 2 | 3 | import edu.wpi.first.hal.HALUtil 4 | import edu.wpi.first.wpilibj.DriverStation 5 | import edu.wpi.first.wpilibj.RuntimeType 6 | 7 | /** Stores basic robot information like Alliance color and isReal/Sim/Replay */ 8 | 9 | val doReplay: Boolean = false 10 | val robotMode: RobotMode = when (RuntimeType.getValue(HALUtil.getHALRuntimeType())) { 11 | RuntimeType.kRoboRIO2, RuntimeType.kRoboRIO -> RobotMode.REAL 12 | RuntimeType.kSimulation -> if (doReplay) RobotMode.REPLAY else RobotMode.SIM 13 | else -> RobotMode.REAL 14 | }.also { println("robotMode = $it") } 15 | 16 | val isReal = robotMode == RobotMode.REAL 17 | val isSim = !isReal 18 | val isReplay = robotMode == RobotMode.REPLAY 19 | 20 | enum class RobotMode { 21 | REAL, 22 | SIM, 23 | REPLAY 24 | } 25 | 26 | 27 | //Alliance bool 28 | val isRedAlliance: Boolean 29 | get() = if (DriverStation.getAlliance().isEmpty) { 30 | prevIsRedAlliance ?: true // If no alliance, return the last known alliance or default to red 31 | } else { 32 | (DriverStation.getAlliance().get() == DriverStation.Alliance.Red).also { prevIsRedAlliance = it } 33 | } 34 | val isBlueAlliance: Boolean get() = !isRedAlliance 35 | private var prevIsRedAlliance: Boolean? = null -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/QuixVisionSim.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.wpilibj.smartdashboard.Field2d 5 | import org.team2471.frc.lib.util.isReal 6 | import org.team2471.frc.lib.vision.photonVision.PhotonVisionCamera 7 | import org.photonvision.estimation.TargetModel 8 | import org.photonvision.simulation.VisionSystemSim 9 | import org.photonvision.simulation.VisionTargetSim 10 | 11 | object QuixVisionSim { 12 | private val m_visionSim = VisionSystemSim("main") 13 | 14 | fun addCamera(camera: PhotonVisionCamera) { 15 | m_visionSim.addCamera(camera.cameraSim, camera.transform) 16 | println("vision sim has ${m_visionSim.cameraSims.size} cameras") 17 | } 18 | 19 | fun resetSimPose(pose: Pose2d) { 20 | m_visionSim.resetRobotPose(pose) 21 | } 22 | 23 | fun updatePose(pose: Pose2d) { 24 | m_visionSim.update(pose) 25 | } 26 | 27 | fun setTargets(tags: Array) { 28 | if (!isReal) { 29 | tags.forEach { 30 | m_visionSim.addVisionTargets("apriltag", VisionTargetSim(it.pose, TargetModel.kAprilTag36h11, it.id)) 31 | } 32 | } 33 | } 34 | 35 | val simField: Field2d 36 | get() = m_visionSim.debugField 37 | } 38 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/localization/InterpolatableChassisSpeeds.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.localization 2 | 3 | import edu.wpi.first.math.interpolation.Interpolatable 4 | import edu.wpi.first.math.kinematics.ChassisSpeeds 5 | 6 | class InterpolatableChassisSpeeds( 7 | vxMetersPerSecond: Double, 8 | vyMetersPerSecond: Double, 9 | omegaRadiansPerSecond: Double 10 | ): ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond), 11 | Interpolatable { 12 | 13 | override fun interpolate(endValue: InterpolatableChassisSpeeds, t: Double): InterpolatableChassisSpeeds { 14 | if (t < 0) { 15 | return this 16 | } 17 | if (t >= 1) { 18 | return endValue 19 | } 20 | return InterpolatableChassisSpeeds( 21 | (1.0 - t) * vxMetersPerSecond + t * endValue.vxMetersPerSecond, 22 | (1.0 - t) * vyMetersPerSecond + t * endValue.vyMetersPerSecond, 23 | (1.0 - t) * omegaRadiansPerSecond + t * endValue.omegaRadiansPerSecond 24 | ) 25 | } 26 | 27 | companion object { 28 | fun fromChassisSpeeds(cs: ChassisSpeeds): InterpolatableChassisSpeeds { 29 | return InterpolatableChassisSpeeds( 30 | cs.vxMetersPerSecond, cs.vyMetersPerSecond, cs.omegaRadiansPerSecond 31 | ) 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/control/LoopLogger.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.control 2 | 3 | import edu.wpi.first.wpilibj.Timer 4 | import org.littletonrobotics.junction.Logger 5 | 6 | /** 7 | * Publishes loop times to NetworkTables. 8 | * 9 | * Period is the time between the named loop 10 | * SinceReset is the time between reset was called and the loop got recorded. Usually reset should be called first thing in main periodic. 11 | */ 12 | object LoopLogger { 13 | private val prevTimes = mutableMapOf() 14 | private var startTime = Timer.getFPGATimestamp() 15 | private val loopsAndIndex = mutableMapOf() 16 | 17 | fun reset() { 18 | startTime = Timer.getFPGATimestamp() 19 | } 20 | 21 | /** Log the period and the time of the named loop at the current moment. */ 22 | fun record(loopName: String): Pair { 23 | val loopIndex: Int = loopsAndIndex.getOrPut(loopName) { loopsAndIndex.size } 24 | val now = Timer.getFPGATimestamp() 25 | val prevTime = prevTimes.put(loopName, now) ?: now //put returns previous value 26 | val period = now - prevTime 27 | val sinceReset = now - startTime 28 | 29 | Logger.recordOutput("LoopLogger/Period/$loopIndex $loopName", period) 30 | Logger.recordOutput("LoopLogger/SinceReset/$loopIndex $loopName", sinceReset) 31 | return Pair(period, sinceReset) 32 | } 33 | } -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/intellij,gradle,java 3 | 4 | ### Intellij ### 5 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm 6 | .idea 7 | 8 | ## File-based project format: 9 | *.iws 10 | 11 | *.iml 12 | 13 | ## Plugin-specific files: 14 | 15 | # IntelliJ 16 | /out/ 17 | 18 | # mpeltonen/sbt-idea plugin 19 | .idea_modules/ 20 | 21 | 22 | # Crashlytics plugin (for Android Studio and IntelliJ) 23 | com_crashlytics_export_strings.xml 24 | crashlytics.properties 25 | crashlytics-build.properties 26 | fabric.properties 27 | 28 | ### Intellij Patch ### 29 | # Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 30 | 31 | # *.iml 32 | # modules.xml 33 | # .idea/misc.xml 34 | # *.ipr 35 | 36 | 37 | ### Java ### 38 | *.class 39 | 40 | # Mobile Tools for Java (J2ME) 41 | .mtj.tmp/ 42 | 43 | # Package Files # 44 | *.jar 45 | *.war 46 | *.ear 47 | 48 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 49 | hs_err_pid* 50 | 51 | 52 | ### Gradle ### 53 | .gradle 54 | /build/ 55 | 56 | ### Kotlin ### 57 | .kotlin 58 | 59 | # Ignore Gradle GUI config 60 | gradle-app.setting 61 | 62 | # Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) 63 | !gradle-wrapper.jar 64 | 65 | # Cache of project 66 | .gradletasknamecache 67 | 68 | # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 69 | # gradle/wrapper/gradle-wrapper.properties 70 | 71 | # Vim 72 | *.swp 73 | 74 | # Allow jar files in libs folder 75 | !libs/*.jar 76 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/util/WpilibUtils.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.util 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.math.geometry.Rotation2d 5 | import edu.wpi.first.math.geometry.Transform2d 6 | import edu.wpi.first.math.geometry.Translation2d 7 | import edu.wpi.first.math.kinematics.ChassisSpeeds 8 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics 9 | import edu.wpi.first.math.kinematics.SwerveModuleState 10 | import edu.wpi.first.units.measure.Angle 11 | 12 | inline val ChassisSpeeds.translation: Translation2d get() = Translation2d(this.vxMetersPerSecond, this.vyMetersPerSecond) 13 | fun ChassisSpeeds.fieldToRobotCentric(heading: Rotation2d): ChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(this, heading) 14 | fun ChassisSpeeds.robotToFieldCentric(heading: Rotation2d): ChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(this, heading) 15 | fun ChassisSpeeds.toTransform2d(deltaSeconds: Double): Transform2d = Transform2d(this.translation, Rotation2d(this.omegaRadiansPerSecond)) * deltaSeconds 16 | fun SwerveDriveKinematics.toChassisSpeedsK(speeds: Array): ChassisSpeeds { 17 | return this.toChassisSpeeds(*speeds) //Intellij sometimes thinks this is an error. which is lame... 18 | } 19 | 20 | fun Pose2d.changeRotation(newRotation: Rotation2d): Pose2d { 21 | return Pose2d(this.translation, newRotation) 22 | } 23 | fun Pose2d.addRotation(rotation: Rotation2d): Pose2d { 24 | return Pose2d(this.translation, this.rotation.plus(rotation)) 25 | } 26 | 27 | fun Translation2d.angleTo(other: Translation2d): Angle { 28 | return (other - this).angle.measure 29 | } 30 | -------------------------------------------------------------------------------- /vendordeps/Studica.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "Studica-2025.0.1.json", 3 | "name": "Studica", 4 | "version": "2025.0.1", 5 | "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://dev.studica.com/maven/release/2025/" 9 | ], 10 | "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", 11 | "cppDependencies": [ 12 | { 13 | "artifactId": "Studica-cpp", 14 | "binaryPlatforms": [ 15 | "linuxathena", 16 | "linuxarm32", 17 | "linuxarm64", 18 | "linuxx86-64", 19 | "osxuniversal", 20 | "windowsx86-64" 21 | ], 22 | "groupId": "com.studica.frc", 23 | "headerClassifier": "headers", 24 | "libName": "Studica", 25 | "sharedLibrary": false, 26 | "skipInvalidPlatforms": true, 27 | "version": "2025.0.1" 28 | }, 29 | { 30 | "artifactId": "Studica-driver", 31 | "binaryPlatforms": [ 32 | "linuxathena", 33 | "linuxarm32", 34 | "linuxarm64", 35 | "linuxx86-64", 36 | "osxuniversal", 37 | "windowsx86-64" 38 | ], 39 | "groupId": "com.studica.frc", 40 | "headerClassifier": "headers", 41 | "libName": "StudicaDriver", 42 | "sharedLibrary": false, 43 | "skipInvalidPlatforms": true, 44 | "version": "2025.0.1" 45 | } 46 | ], 47 | "javaDependencies": [ 48 | { 49 | "artifactId": "Studica-java", 50 | "groupId": "com.studica.frc", 51 | "version": "2025.0.1" 52 | } 53 | ], 54 | "jniDependencies": [ 55 | { 56 | "artifactId": "Studica-driver", 57 | "groupId": "com.studica.frc", 58 | "isJar": false, 59 | "skipInvalidPlatforms": true, 60 | "validPlatforms": [ 61 | "linuxathena", 62 | "linuxarm32", 63 | "linuxarm64", 64 | "linuxx86-64", 65 | "osxuniversal", 66 | "windowsx86-64" 67 | ], 68 | "version": "2025.0.1" 69 | } 70 | ] 71 | } 72 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/control/PDVelocityController.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.control 2 | 3 | /** 4 | * A PD controller for closed loop velocity control. 5 | * 6 | * @param p the proportional gain of the system 7 | * @param d the differential gain of the system 8 | * @param ff the velocity feedforward gain of the system (gets multiplied by velocity setpoint) 9 | * @param coastToStop disable the PD controller when the setpoint is zero. 10 | */ 11 | 12 | class PDVelocityController(var p: Double, var d: Double, var ff: Double, val coastToStop: Boolean = false ) { 13 | var lastError: Double = 0.0 14 | var pdPower: Double = 0.0 15 | 16 | fun updatePDF(newP: Double = p, newD: Double = d, newFF: Double = ff) { 17 | if (p != newP || d != newD || ff != newFF) { 18 | p = newP 19 | d = newD 20 | ff = newFF 21 | println("New newP: $p ; New newD: $d") 22 | } 23 | } 24 | 25 | fun updatePercentage(velocitySetpoint: Double, currVelocity: Double): Double { 26 | val error = velocitySetpoint - currVelocity 27 | val ffPower = velocitySetpoint * ff 28 | 29 | val deltaError = error - lastError 30 | lastError = error 31 | 32 | pdPower += error * p + deltaError * d 33 | 34 | var power = pdPower + ffPower 35 | 36 | if (coastToStop && velocitySetpoint == 0.0) { 37 | power = 0.0 38 | pdPower = 0.0 39 | } 40 | 41 | if (pdPower + ffPower > 1.0) pdPower = 1.0 - ffPower 42 | 43 | return power 44 | } 45 | 46 | fun updateVoltage(velocitySetpoint: Double, currVelocity: Double): Double { 47 | val error = velocitySetpoint - currVelocity 48 | val ffVoltage = velocitySetpoint * ff 49 | 50 | val deltaError = error - lastError 51 | lastError = error 52 | 53 | pdPower += error * p + deltaError * d 54 | 55 | var voltage = pdPower + ffVoltage 56 | 57 | if (coastToStop && velocitySetpoint == 0.0) { 58 | voltage = 0.0 59 | pdPower = 0.0 60 | } 61 | 62 | if (pdPower + ffVoltage > 12.0) pdPower = 12.0 - ffVoltage 63 | 64 | return voltage 65 | } 66 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/motion_profiling/CubicCoefficients1D.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.motion_profiling 2 | 3 | // CUBIC COEFFICIENTS f(t) = a*t^3 + b*t^2 + c*t + d 4 | class CubicCoefficients1D internal constructor(private val d: Double, p4: Double, private val c: Double, r4: Double) { 5 | // construct from two values and two tangents (slope) 6 | // a 2 -2 1 1 p1 7 | // b = -3 3 -2 -1 * p4 8 | // c 0 0 1 0 r1 9 | // d 1 0 0 0 r4 10 | private val a: Double = 2 * d + -2 * p4 + c + r4 11 | private val b: Double = -3 * d + 3 * p4 + -2 * c + -r4 12 | var fDValue: Double = 0.0 13 | private set 14 | private var fdb = 0.0 15 | private var fdc = 0.0 16 | private var fdd = 0.0 // BUMP FD COEFFICIENTS 17 | var fdSteps: Int = 0 // BUMP FD memory 18 | private set 19 | var fdPrevValue: Double = 0.0 20 | private set 21 | 22 | fun evaluate(t: Double): Double { 23 | return t * (t * (a * t + b) + c) + d 24 | } 25 | 26 | fun derivative(t: Double): Double { 27 | return t * (3 * a * t + 2 * b) + c 28 | } 29 | 30 | fun secondDerivative(t: Double): Double { 31 | return 3 * a * t + 2 * b 32 | } 33 | 34 | fun initFD(steps: Int): Double { 35 | fdSteps = steps 36 | // fda 0 0 0 1 a 37 | // fdb = delta**3 delta**2 delta 0 * b 38 | // fdc 6*delta**3 2*delta**2 0 0 c 39 | // fdd 6*delta**3 0 0 0 d 40 | val fd12 = 1.0 / steps 41 | val fd11 = fd12 * fd12 42 | val fd10 = fd12 * fd11 43 | val fd20 = 6.0f * fd10 44 | val fd21 = 2.0f * fd11 45 | this.fDValue = d 46 | fdb = a * fd10 + b * fd11 + c * fd12 47 | fdc = a * fd20 + b * fd21 48 | fdd = a * fd20 49 | return this.fDValue 50 | } 51 | 52 | fun bumpFD(): Double { 53 | fdPrevValue = this.fDValue 54 | this.fDValue += fdb 55 | fdb += fdc 56 | fdc += fdd 57 | return this.fDValue 58 | } 59 | 60 | fun bumpFDFaster(): Double { 61 | this.fDValue += fdb 62 | fdb += fdc 63 | fdc += fdd 64 | return this.fDValue 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/control/XboxControllerUtils.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.control 2 | 3 | import edu.wpi.first.wpilibj2.command.button.CommandXboxController 4 | import org.team2471.frc.lib.util.isSim 5 | 6 | /** Sometimes the sim GUI doesn't detect an Xbox controller as a gamepad and does not bind it as such. [simBeingDumb] attempts to rebind the joystick as if the "map gamepad" button was pressed. */ 7 | class MeanCommandXboxController(port: Int, val simBeingDumb: Boolean = false): CommandXboxController(port) { 8 | override fun getRightX(): Double { 9 | if (simBeingDumb && isSim) { 10 | return super.leftTriggerAxis 11 | } 12 | return super.getRightX() 13 | } 14 | 15 | override fun getLeftTriggerAxis(): Double { 16 | if (simBeingDumb && isSim) { 17 | return super.rightX 18 | } 19 | return super.leftTriggerAxis 20 | } 21 | } 22 | 23 | inline val CommandXboxController.a: Boolean get() = this.hid.aButton 24 | inline val CommandXboxController.b: Boolean get() = this.hid.bButton 25 | inline val CommandXboxController.x: Boolean get() = this.hid.xButton 26 | inline val CommandXboxController.y: Boolean get() = this.hid.yButton 27 | 28 | inline val CommandXboxController.rightBumper: Boolean get() = this.hid.rightBumperButton 29 | inline val CommandXboxController.leftBumper: Boolean get() = this.hid.leftBumperButton 30 | 31 | inline val CommandXboxController.start: Boolean get() = this.hid.startButton 32 | inline val CommandXboxController.back: Boolean get() = this.hid.backButton 33 | 34 | inline val CommandXboxController.rightStickButton: Boolean get() = this.hid.rightStickButton 35 | inline val CommandXboxController.leftStickButton: Boolean get() = this.hid.leftStickButton 36 | 37 | inline val CommandXboxController.dPad: Direction get() = when (this.hid.pov) { 38 | -1 -> Direction.IDLE 39 | 0 -> Direction.UP 40 | 45 -> Direction.UP_RIGHT 41 | 90 -> Direction.RIGHT 42 | 135 -> Direction.DOWN_RIGHT 43 | 180 -> Direction.DOWN 44 | 225 -> Direction.DOWN_LEFT 45 | 270 -> Direction.LEFT 46 | 315 -> Direction.UP_LEFT 47 | else -> throw IllegalStateException("Invalid DPAD value ${this.hid.pov}") 48 | } 49 | 50 | enum class Direction { IDLE, UP, UP_RIGHT, RIGHT, DOWN_RIGHT, DOWN, DOWN_LEFT, LEFT, UP_LEFT } 51 | 52 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/localization/PoseEstimate.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.localization 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.util.struct.Struct 5 | import java.nio.ByteBuffer 6 | 7 | /** 8 | * The PoseEstimate class represents an estimate of a robot's pose (position and orientation) at a 9 | * given point in time. It includes an identifier, the pose itself, and a flag indicating whether 10 | * vision data was used in the estimation. 11 | */ 12 | class PoseEstimate @JvmOverloads constructor( 13 | val id: Int = 0, 14 | val pose: Pose2d? = null, 15 | val hasVision: Boolean = false 16 | ) { 17 | override fun toString(): String = String.format("PoseEstimate(%s, %s, %s)", this.id, this.pose, hasVision) 18 | 19 | companion object { 20 | // Struct for serialization. 21 | @JvmField 22 | val struct: PoseEstimateStruct = PoseEstimateStruct() 23 | } 24 | } 25 | /** 26 | * PoseEstimateStruct is a class that implements the Struct interface for the PoseEstimate type. It 27 | * provides methods to get the type class, type name, size, schema, and nested structures. It also 28 | * provides methods to pack and unpack PoseEstimate objects to and from ByteBuffers. 29 | * 30 | * 31 | * The PoseEstimateStruct class is immutable and ensures that the PoseEstimate objects are 32 | * correctly serialized and deserialized with the appropriate schema. 33 | */ 34 | class PoseEstimateStruct : Struct { 35 | override fun getTypeClass(): Class = PoseEstimate::class.java 36 | 37 | override fun getTypeName(): String = "PoseEstimate" 38 | 39 | override fun getSize(): Int = Pose2d.struct.size + Struct.kSizeInt32 * 2 40 | 41 | override fun getSchema(): String = "int32 id;" + "Pose2d pose;" + "int32 hasVision;" 42 | 43 | override fun getNested(): Array> = arrayOf(Pose2d.struct) 44 | 45 | override fun unpack(bb: ByteBuffer): PoseEstimate { 46 | val id = bb.getInt() 47 | val pose = Pose2d.struct.unpack(bb) 48 | val hasVision = bb.getInt() != 0 49 | return PoseEstimate(id, pose, hasVision) 50 | } 51 | 52 | override fun pack(bb: ByteBuffer, value: PoseEstimate) { 53 | bb.putInt(value.id) 54 | Pose2d.struct.pack(bb, value.pose) 55 | bb.putInt(if (value.hasVision) 1 else 0) 56 | } 57 | 58 | override fun isImmutable(): Boolean = true 59 | } -------------------------------------------------------------------------------- /vendordeps/photonlib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "photonlib.json", 3 | "name": "photonlib", 4 | "version": "v2025.1.1", 5 | "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://maven.photonvision.org/repository/internal", 9 | "https://maven.photonvision.org/repository/snapshots" 10 | ], 11 | "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", 12 | "jniDependencies": [ 13 | { 14 | "groupId": "org.photonvision", 15 | "artifactId": "photontargeting-cpp", 16 | "version": "v2025.1.1", 17 | "skipInvalidPlatforms": true, 18 | "isJar": false, 19 | "validPlatforms": [ 20 | "windowsx86-64", 21 | "linuxathena", 22 | "linuxx86-64", 23 | "osxuniversal" 24 | ] 25 | } 26 | ], 27 | "cppDependencies": [ 28 | { 29 | "groupId": "org.photonvision", 30 | "artifactId": "photonlib-cpp", 31 | "version": "v2025.1.1", 32 | "libName": "photonlib", 33 | "headerClassifier": "headers", 34 | "sharedLibrary": true, 35 | "skipInvalidPlatforms": true, 36 | "binaryPlatforms": [ 37 | "windowsx86-64", 38 | "linuxathena", 39 | "linuxx86-64", 40 | "osxuniversal" 41 | ] 42 | }, 43 | { 44 | "groupId": "org.photonvision", 45 | "artifactId": "photontargeting-cpp", 46 | "version": "v2025.1.1", 47 | "libName": "photontargeting", 48 | "headerClassifier": "headers", 49 | "sharedLibrary": true, 50 | "skipInvalidPlatforms": true, 51 | "binaryPlatforms": [ 52 | "windowsx86-64", 53 | "linuxathena", 54 | "linuxx86-64", 55 | "osxuniversal" 56 | ] 57 | } 58 | ], 59 | "javaDependencies": [ 60 | { 61 | "groupId": "org.photonvision", 62 | "artifactId": "photonlib-java", 63 | "version": "v2025.1.1" 64 | }, 65 | { 66 | "groupId": "org.photonvision", 67 | "artifactId": "photontargeting-java", 68 | "version": "v2025.1.1" 69 | } 70 | ] 71 | } -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS= 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/Fiducial.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision 2 | 3 | import edu.wpi.first.apriltag.AprilTag 4 | import edu.wpi.first.math.geometry.Pose3d 5 | import edu.wpi.first.util.struct.Struct 6 | import org.team2471.frc.lib.units.asMeters 7 | import org.team2471.frc.lib.units.inches 8 | import java.nio.ByteBuffer 9 | 10 | // An ID of -1 indicates this is an unlabeled fiducial (e.g. retroreflective tape) 11 | class Fiducial(val type: Type, val id: Int, val pose: Pose3d, val size: Double) { 12 | enum class Type(@JvmField val value: Int) { 13 | RETROREFLECTIVE(0), 14 | APRILTAG(1) 15 | } 16 | 17 | val x: Double 18 | get() = pose.x 19 | 20 | val y: Double 21 | get() = pose.y 22 | 23 | val z: Double 24 | get() = pose.z 25 | 26 | val xRot: Double 27 | get() = pose.rotation.x 28 | 29 | val yRot: Double 30 | get() = pose.rotation.y 31 | 32 | val zRot: Double 33 | get() = pose.rotation.z 34 | 35 | override fun toString(): String = "(${type.name}, ID: $id, Pose: $pose, Size: $size)" 36 | 37 | companion object { 38 | // Struct for serialization. 39 | val struct: FiducialStruct = FiducialStruct() 40 | 41 | private val aprilTagSize = 6.5.inches.asMeters // m 42 | 43 | fun constructFiducialList(aprilTags: List): Array { 44 | return Array(aprilTags.size) { 45 | val tag = aprilTags[it] 46 | Fiducial( 47 | Type.APRILTAG, 48 | tag.ID, 49 | tag.pose, 50 | aprilTagSize 51 | ) 52 | } 53 | } 54 | } 55 | } 56 | 57 | class FiducialStruct : Struct { 58 | override fun getTypeClass(): Class = Fiducial::class.java 59 | 60 | override fun getTypeName(): String = "Fiducial" 61 | 62 | override fun getSize(): Int = 63 | Pose3d.struct.size + Struct.kSizeInt32 * 2 + Struct.kSizeDouble 64 | 65 | override fun getSchema(): String = 66 | "int32 type;int32 id;Pose3d pose;double size;" 67 | 68 | override fun getNested(): Array> = arrayOf(Pose3d.struct) 69 | 70 | override fun unpack(bb: ByteBuffer): Fiducial { 71 | val type = Fiducial.Type.entries[bb.getInt()] 72 | val id = bb.getInt() 73 | val pose = Pose3d.struct.unpack(bb) 74 | val size = bb.getDouble() 75 | return Fiducial(type, id, pose, size) 76 | } 77 | 78 | override fun pack(bb: ByteBuffer, value: Fiducial) { 79 | bb.putInt(value.type.value) 80 | bb.putInt(value.id) 81 | Pose3d.struct.pack(bb, value.pose) 82 | bb.putDouble(value.size) 83 | } 84 | 85 | override fun isImmutable(): Boolean = true 86 | } 87 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/loggedTalonFX/LoggedTalonFX.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.ctre.loggedTalonFX 2 | 3 | import com.ctre.phoenix6.hardware.TalonFX 4 | import com.ctre.phoenix6.signals.NeutralModeValue 5 | import edu.wpi.first.math.system.plant.DCMotor 6 | import edu.wpi.first.math.system.plant.LinearSystemId 7 | import edu.wpi.first.wpilibj.simulation.DCMotorSim 8 | import org.team2471.frc.lib.util.isReal 9 | import org.team2471.frc.lib.units.volts 10 | import kotlinx.coroutines.DelicateCoroutinesApi 11 | import kotlinx.coroutines.GlobalScope 12 | import kotlinx.coroutines.launch 13 | 14 | /** 15 | * Wrapper for that [TalonFX] class that supports simulation when [configSim] is called. 16 | * Also supports backing safe calls when calling [brakeMode] & [coastMode] 17 | * @see TalonFX 18 | * @see DCMotorSim 19 | */ 20 | class LoggedTalonFX(id: Int, canBus: String? = ""): TalonFX(id, canBus), LoggedMotor { 21 | private val talonFXSim = this.simState 22 | private var motor: DCMotor? = null 23 | private var motorSim: DCMotorSim? = null 24 | 25 | private var addedToMaster = false 26 | 27 | 28 | init { 29 | talonFXSim.setSupplyVoltage(12.0.volts) 30 | } 31 | 32 | /** 33 | * Configure the simulation to have accurate values. 34 | * @param motor The type of [DCMotor] motor to sim. 35 | * @param jKgMetersSquared The moment of inertia of the motor. 36 | * @see DCMotor 37 | */ 38 | fun configSim(motor: DCMotor, jKgMetersSquared: Double) { 39 | this.motor = motor 40 | motorSim = DCMotorSim(LinearSystemId.createDCMotorSystem(this.motor, jKgMetersSquared, 1.0), this.motor) 41 | motorSim?.setState(0.0, 0.0) 42 | 43 | //Ensures this gets added to the MasterMotor list only once 44 | if (!addedToMaster) { 45 | addedToMaster = true 46 | MasterMotor.addMotor(this) 47 | } 48 | } 49 | 50 | /** 51 | * A backing safe call to set the brake mode of the motor. 52 | * This function will finish instantly, but the motor will take longer (>100 ms) to apply the change. 53 | * @see setNeutralMode 54 | * @see GlobalScope 55 | */ 56 | @OptIn(DelicateCoroutinesApi::class) 57 | fun brakeMode() { 58 | if (isReal) { 59 | GlobalScope.launch { 60 | setNeutralMode(NeutralModeValue.Brake) 61 | } 62 | } 63 | } 64 | 65 | /** 66 | * A backing safe call to set the coast mode of the motor. 67 | * This function will finish instantly, but the motor will take longer (>100 ms) to apply the change. 68 | * @see setNeutralMode 69 | * @see GlobalScope 70 | */ 71 | @OptIn(DelicateCoroutinesApi::class) 72 | fun coastMode() { 73 | if (isReal) { 74 | GlobalScope.launch { 75 | setNeutralMode(NeutralModeValue.Coast) 76 | } 77 | } 78 | } 79 | 80 | override fun simPeriodic() { 81 | if (motorSim != null) { 82 | val talonFXVoltage = talonFXSim.motorVoltage 83 | 84 | motorSim!!.inputVoltage = talonFXVoltage 85 | motorSim!!.update(0.02) 86 | 87 | talonFXSim.setRawRotorPosition(motorSim!!.angularPosition) 88 | talonFXSim.setRotorVelocity(motorSim!!.angularVelocity) 89 | talonFXSim.setRotorAcceleration(motorSim!!.angularAcceleration) 90 | } 91 | } 92 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/CANCoderUtil.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.ctre 2 | 3 | import com.ctre.phoenix6.configs.CANcoderConfiguration 4 | import com.ctre.phoenix6.hardware.CANcoder 5 | import com.ctre.phoenix6.signals.SensorDirectionValue 6 | import edu.wpi.first.units.measure.Angle 7 | import org.team2471.frc.lib.util.isReal 8 | import org.team2471.frc.lib.util.isSim 9 | import org.team2471.frc.lib.units.asRotations 10 | import org.team2471.frc.lib.units.degrees 11 | import org.team2471.frc.lib.units.rotations 12 | 13 | /** Grabs the MagnetOffset from the [CANcoder]. */ 14 | fun CANcoder.getMagnetSensorOffset(): Angle { 15 | if (!this.isConnected || isSim) return 0.0.degrees 16 | val initialConfigs = CANcoderConfiguration() 17 | PhoenixUtil.tryUntilOk(5) { this.configurator.refresh(initialConfigs) } 18 | 19 | return initialConfigs.MagnetSensor.MagnetOffset.rotations 20 | } 21 | 22 | /** 23 | * Applies the MagnetOffset config to the [CANcoder] while not changing other configuration values. 24 | * This is probably a backing call 25 | */ 26 | fun CANcoder.setMagnetSensorOffset(offset: Angle) { 27 | if (isReal) { 28 | val initialConfigs = CANcoderConfiguration() 29 | PhoenixUtil.tryUntilOk(5) { this.configurator.refresh(initialConfigs) } 30 | 31 | initialConfigs.MagnetSensor.MagnetOffset = offset.asRotations 32 | 33 | this.configurator.apply(initialConfigs, 0.0) 34 | } 35 | } 36 | 37 | /** 38 | * Sets the [CANcoder]s MagnetOffset config so its current position equals the specified angle. 39 | * This is probably a backing call 40 | */ 41 | fun CANcoder.setCANCoderAngle(angle: Angle): Angle { 42 | val initialPosition = this.absolutePosition.valueAsDouble.rotations 43 | val initialOffset = getMagnetSensorOffset() 44 | println("initial position $initialPosition initial offset $initialOffset") 45 | 46 | val rawPosition = initialPosition - initialOffset 47 | val newOffset = (angle - rawPosition) 48 | println("rawPosition $rawPosition new offset $newOffset") 49 | 50 | this.setMagnetSensorOffset(newOffset) 51 | return newOffset 52 | } 53 | 54 | fun CANcoderConfiguration.inverted(inverted: Boolean): CANcoderConfiguration { 55 | return this.apply { 56 | MagnetSensor.SensorDirection = if (inverted) SensorDirectionValue.Clockwise_Positive else SensorDirectionValue.CounterClockwise_Positive 57 | } 58 | } 59 | 60 | fun CANcoderConfiguration.magnetSensorOffset(magnetOffsetRotations: Double): CANcoderConfiguration { 61 | return this.apply { 62 | MagnetSensor.MagnetOffset = magnetOffsetRotations 63 | } 64 | } 65 | 66 | /** 67 | * Applies a factory default configuration to the [CANcoder]. 68 | * 69 | * @param modifications optionally provide a block to modify the configuration before it gets sent to the device. 70 | */ 71 | fun CANcoder.applyConfiguration(modifications: CANcoderConfiguration.() -> Unit = {}) { 72 | this.configurator.apply(CANcoderConfiguration().apply(modifications)) 73 | } 74 | 75 | /** 76 | * Modifies the current device configuration and applies to the [CANcoder]. 77 | * 78 | * @param overrides provide a block to modify the configuration before it gets sent to the device. 79 | */ 80 | fun CANcoder.modifyConfiguration(overrides: CANcoderConfiguration.() -> Unit) { 81 | val oldConfiguration = CANcoderConfiguration() 82 | this.configurator.refresh(oldConfiguration) 83 | this.configurator.apply(oldConfiguration.apply(overrides)) 84 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/util/DynamicInterpolatingTreeMap.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.util 2 | 3 | import edu.wpi.first.math.interpolation.Interpolator 4 | import edu.wpi.first.math.interpolation.InverseInterpolator 5 | import java.util.* 6 | 7 | /** 8 | * This is a copy of WPILib's InterpolatingTreeMap that allows for deleting of keys 9 | * 10 | * Interpolating Tree Maps are used to get values at points that are not defined by making a guess 11 | * from points that are defined. This uses linear interpolation. 12 | * 13 | *

{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be 14 | * provided. 15 | * 16 | * @param The type of keys held in this map. 17 | * @param The type of values held in this map. 18 | */ 19 | 20 | open class DynamicInterpolatingTreeMap { 21 | private val map: TreeMap 22 | 23 | private val inverseInterpolator: InverseInterpolator 24 | private val interpolator: Interpolator 25 | 26 | private val maxSize: Int 27 | 28 | /** 29 | * Constructs an InterpolatingTreeMap. 30 | * 31 | * @param inverseInterpolator Function to use for inverse interpolation of the keys. 32 | * @param interpolator Function to use for interpolation of the values. 33 | * @param maxSize The maximum size of the map. If zero, no maximum size will be enforced. Otherwise, the lowest key will be deleted if adding a key pushes the size over the specified value. 34 | */ 35 | constructor(inverseInterpolator: InverseInterpolator, interpolator: Interpolator, maxSize: Int = 0) { 36 | map = TreeMap() 37 | this.inverseInterpolator = inverseInterpolator 38 | this.interpolator = interpolator 39 | 40 | this.maxSize = maxSize 41 | } 42 | 43 | /** 44 | * Constructs an InterpolatingTreeMap using `comparator`. 45 | * 46 | * @param inverseInterpolator Function to use for inverse interpolation of the keys. 47 | * @param interpolator Function to use for interpolation of the values. 48 | * @param comparator Comparator to use on keys. 49 | * @param maxSize The maximum size of the map. If zero, no maximum size will be enforced. Otherwise, the lowest key will be deleted if adding a key pushes the size over the specified value. 50 | */ 51 | constructor( 52 | inverseInterpolator: InverseInterpolator, 53 | interpolator: Interpolator, 54 | comparator: Comparator, 55 | maxSize: Int = 0 56 | ) { 57 | this.inverseInterpolator = inverseInterpolator 58 | this.interpolator = interpolator 59 | map = TreeMap(comparator) 60 | 61 | this.maxSize = maxSize 62 | } 63 | 64 | /** 65 | * Inserts a key-value pair. 66 | * 67 | * @param key The key. 68 | * @param value The value. 69 | */ 70 | fun put(key: K, value: V) { 71 | map.put(key, value) 72 | 73 | if (maxSize > 0 && map.size > maxSize) { 74 | remove(map.firstKey()) 75 | } 76 | } 77 | 78 | /** 79 | * Removes a key-value pair for the specified key. 80 | * 81 | * @param key The key. 82 | */ 83 | fun remove(key: K) { 84 | map.remove(key) 85 | } 86 | 87 | /** 88 | * Replaces the value for the specified key only if it is currently mapped to some value. 89 | * 90 | * @param key The key. 91 | * @param newValue The new value. 92 | */ 93 | fun replace(key: K, newValue: V) { 94 | map.replace(key, newValue) 95 | } 96 | 97 | /** 98 | * Returns the value associated with a given key. 99 | * 100 | * 101 | * If there's no matching key, the value returned will be an interpolation between the keys 102 | * before and after the provided one, using the [Interpolator] and [ ] provided. 103 | * 104 | * @param key The key. 105 | * @return The value associated with the given key. 106 | */ 107 | fun get(key: K): V? { 108 | val `val` = map.get(key) 109 | if (`val` == null) { 110 | val ceilingKey = map.ceilingKey(key) 111 | val floorKey = map.floorKey(key) 112 | 113 | if (ceilingKey == null && floorKey == null) { 114 | return null 115 | } 116 | if (ceilingKey == null) { 117 | return map[floorKey] 118 | } 119 | if (floorKey == null) { 120 | return map[ceilingKey] 121 | } 122 | val floor = map[floorKey] 123 | val ceiling = map[ceilingKey] 124 | 125 | return interpolator.interpolate( 126 | floor, ceiling, inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key) 127 | ) 128 | } else { 129 | return `val` 130 | } 131 | } 132 | 133 | /** Clears the contents. */ 134 | fun clear() { 135 | map.clear() 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/localization/Measurement.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.localization 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.math.geometry.Rotation2d 5 | import org.photonvision.targeting.TargetCorner 6 | 7 | /** 8 | * The Measurement class represents a set of odometry and vision measurements. It includes the 9 | * robot's pose and associated uncertainties, as well as vision measurements from multiple cameras. 10 | */ 11 | // Odom measurement 12 | /** 13 | * Constructs a new Measurement object with the specified pose. 14 | * 15 | * @param pose The initial pose of the measurement. 16 | */ 17 | class Measurement(private val pose: Pose2d) { 18 | private val poseSigmas: Pose2d = Pose2d(0.1, 0.1, Rotation2d.fromDegrees(0.1)) // TODO: Don't hardcode // TODO: Actually use this 19 | 20 | // Vision measurements 21 | private val cameraIds: ArrayList = ArrayList() 22 | private val targetIds: ArrayList = ArrayList() 23 | 24 | // Fiducial corner ID is valid only for AprilTags. -1 denotes the center. IDs are numbered as 25 | // follows: 26 | // * -> +X 3 ----- 2 27 | // * | | | 28 | // * V | | 29 | // * +Y 0 ----- 1 30 | private val fiducialCornerIds: ArrayList = ArrayList() 31 | private val corners: ArrayList = ArrayList() 32 | 33 | // Default to a moderate uncertainty. 34 | private var pixelSigma = 50.0 35 | 36 | 37 | /** 38 | * Adds a vision measurement to the system. 39 | * 40 | * @param cameraID The ID of the camera that captured the measurement. 41 | * @param targetID The ID of the target being measured. 42 | * @param pixelXY The pixel coordinates of the target corner in the image. 43 | */ 44 | fun addVisionMeasurement(cameraID: Int, targetID: Int, pixelXY: TargetCorner) { 45 | addVisionMeasurement(cameraID, targetID, -1, pixelXY) 46 | } 47 | 48 | /** 49 | * Adds a vision measurement to the localization system. 50 | * 51 | * @param cameraID The ID of the camera that captured the measurement. 52 | * @param targetID The ID of the target being measured. 53 | * @param fiducialCornerID The ID of the fiducial corner being measured. 54 | * @param pixelXY The pixel coordinates of the target corner in the image. 55 | */ 56 | fun addVisionMeasurement(cameraID: Int, targetID: Int, fiducialCornerID: Int, pixelXY: TargetCorner) { 57 | cameraIds.add(cameraID) 58 | targetIds.add(targetID) 59 | fiducialCornerIds.add(fiducialCornerID) 60 | corners.add(pixelXY) 61 | } 62 | 63 | /** 64 | * Sets the uncertainty of the vision measurement. 65 | * 66 | * @param pixelSigma The standard deviation of the pixel measurement noise. 67 | */ 68 | fun setVisionUncertainty(pixelSigma: Double) { 69 | this.pixelSigma = pixelSigma 70 | } 71 | 72 | /** 73 | * Converts the measurement data to an array of doubles. Necessary for sending arbitrary-lengthed 74 | * data over NetworkTables. 75 | * 76 | * @param id The identifier for the measurement. 77 | * @return A double array containing the measurement data. The array consists of static data 78 | * followed by vision data for each camera. The structure of the array is as follows: - 79 | * data[0]: Measurement ID - data[1]: X coordinate of the pose - data[2]: Y coordinate of the 80 | * pose - data[3]: Rotation in radians of the pose - data[4]: X coordinate sigma of the pose - 81 | * data[5]: Y coordinate sigma of the pose - data[6]: Rotation sigma in radians of the pose - 82 | * data[7]: Pixel sigma - For each camera: - data[8 + 5 * i]: Camera ID - data[9 + 5 * i]: 83 | * Target ID - data[10 + 5 * i]: Fiducial corner ID - data[11 + 5 * i]: X coordinate of the 84 | * corner - data[12 + 5 * i]: Y coordinate of the corner 85 | */ 86 | fun toArray(id: Int): DoubleArray { 87 | val kStaticDataLength = 8 88 | val kVisionDataLength = 5 89 | val data = DoubleArray(kStaticDataLength + kVisionDataLength * cameraIds.size) 90 | data[0] = id.toDouble() 91 | data[1] = pose.x 92 | data[2] = pose.y 93 | data[3] = pose.rotation.radians 94 | data[4] = poseSigmas.x 95 | data[5] = poseSigmas.y 96 | data[6] = poseSigmas.rotation.radians 97 | data[7] = pixelSigma 98 | for (i in cameraIds.indices) { 99 | data[kStaticDataLength + kVisionDataLength * i] = cameraIds[i].toDouble() 100 | data[kStaticDataLength + kVisionDataLength * i + 1] = targetIds[i].toDouble() 101 | data[kStaticDataLength + kVisionDataLength * i + 2] = fiducialCornerIds[i].toDouble() 102 | data[kStaticDataLength + kVisionDataLength * i + 3] = corners[i].x 103 | data[kStaticDataLength + kVisionDataLength * i + 4] = corners[i].y 104 | } 105 | return data 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/math/MathUtils.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.math 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.math.geometry.Rotation2d 5 | import edu.wpi.first.math.geometry.Transform2d 6 | import edu.wpi.first.math.geometry.Translation2d 7 | import edu.wpi.first.math.geometry.Twist2d 8 | import kotlin.math.* 9 | 10 | @JvmName("squareOf") 11 | fun square(x: Double): Double = x.square() 12 | 13 | fun Double.square(): Double = this * this 14 | 15 | fun Double.squareWithSign() = (this.square()).withSign(this) 16 | 17 | fun Double.cube() = this * this * this 18 | 19 | fun Double.squareRootWithSign() = sqrt(abs(this)).withSign(this) 20 | 21 | fun Double.log10() = log10(this) 22 | 23 | fun average(vararg x: Double) = x.sum() / x.size 24 | 25 | fun lerp(min: Double, max: Double, k: Double) = min + (max - min) * k 26 | 27 | fun Double.deadband(tolerance: Double): Double = 28 | if (abs(this) < tolerance) { 29 | 0.0 30 | } else { 31 | (this - tolerance.withSign(this)) / (1.0 - tolerance) 32 | } 33 | 34 | /** Divide [Translation2d] by its magnitude, making it have a magnitude of 1 while preserving its angle. */ 35 | fun Translation2d.normalize(): Translation2d { 36 | val mag = norm 37 | return if (mag != 0.0) this.div(mag) else this 38 | } 39 | 40 | fun Translation2d.toPose2d(heading: Rotation2d = Rotation2d()): Pose2d { 41 | return Pose2d(this, heading) 42 | } 43 | 44 | fun Translation2d.mirrorXAxis() = Translation2d(-x, y) 45 | fun Translation2d.mirrorYAxis() = Translation2d(x, -y) 46 | 47 | fun Transform2d.mirrorXAxis() = Transform2d(-x, y, rotation) 48 | fun Transform2d.mirrorYAxis() = Transform2d(x, -y, rotation) 49 | 50 | fun Translation2d.coerceInDynamic(oneLimit: Translation2d, twoLimit: Translation2d): Translation2d = 51 | Translation2d(this.x.coerceInDynamic(oneLimit.x, twoLimit.x), this.y.coerceInDynamic(oneLimit.y, twoLimit.y)) 52 | 53 | /** doesn't work with negative values of n */ 54 | infix fun Double.mod(n: Double) = if (this < 0) { 55 | (this % n + n) % n 56 | } else { 57 | this % n 58 | } 59 | 60 | fun Double.round(digits: Int): Double { 61 | return round(this, digits) 62 | } 63 | @JvmName("roundToDigit") 64 | fun round(number: Double, digits: Int): Double { 65 | if (!number.isNaN()) { 66 | val modulo = 10.0.pow(digits.toDouble()) 67 | return (number * modulo).roundToInt() / modulo 68 | } 69 | else { 70 | return number 71 | } 72 | } 73 | 74 | fun linearMap(inLo: Double, inHi: Double, outLo: Double, outHi: Double, inAlpha: Double): Double { 75 | return (inAlpha - inLo) / (inHi - inLo) * (outHi - outLo) + outLo 76 | } 77 | 78 | fun cubicMap(inLo: Double, inHi: Double, outLo: Double, outHi: Double, inAlpha: Double): Double { 79 | val x = (inAlpha - inLo) / (inHi - inLo) 80 | val cubic = (3 - 2 * x) * x * x 81 | return cubic * (outHi - outLo) + outLo 82 | } 83 | 84 | fun windRelativeAngles(angle1: Double, angle2: Double): Double { 85 | val diff = angle1 - angle2 86 | val absDiff = abs(diff) 87 | return if (absDiff > 180.0) { 88 | angle2 + 360.0 * sign(diff) * floor((absDiff / 360.0) + 0.5) 89 | } else { 90 | angle2 91 | } 92 | } 93 | 94 | /** 95 | * Finds the closest point along a line defined by [linePointOne] and [linePointTwo] to the provided [referencePoint] 96 | */ 97 | fun findClosestPointOnLine(linePointOne: Translation2d, linePointTwo: Translation2d, referencePoint: Translation2d): Translation2d { 98 | val lineAngle = (linePointTwo - linePointOne).angle 99 | val rotatedReferencePoint = referencePoint.rotateBy(-lineAngle) 100 | val rotatedPointOne = linePointOne.rotateBy(-lineAngle) 101 | val rotatedPointTwo = linePointTwo.rotateBy(-lineAngle) 102 | val closestXPoint = rotatedReferencePoint.x.coerceInDynamic(rotatedPointOne.x, rotatedPointTwo.x) 103 | val coercedTranslation = Translation2d(closestXPoint, rotatedPointOne.y) 104 | val closestPointOnLine = coercedTranslation.rotateBy(lineAngle) 105 | 106 | return closestPointOnLine 107 | } 108 | 109 | fun Double.coerceInDynamic(oneLimit: Double, twoLimit: Double) = this.coerceIn(min(oneLimit, twoLimit), max(oneLimit, twoLimit)) 110 | 111 | fun interpTo(from: Double, to: Double, speed: Double, dt: Double = 0.02): Double { 112 | return from + (to - from) * dt * speed 113 | } 114 | 115 | fun epsilonEquals(a: Double, b: Double, epsilon: Double): Boolean { 116 | return (a - epsilon <= b) && (a + epsilon >= b) 117 | } 118 | 119 | fun epsilonEquals(a: Double, b: Double): Boolean { 120 | return epsilonEquals(a, b, 1e-5) 121 | } 122 | 123 | fun epsilonEquals(twist: Twist2d, other: Twist2d): Boolean { 124 | return epsilonEquals(twist.dx, other.dx) && 125 | epsilonEquals(twist.dy, other.dy) && 126 | epsilonEquals(twist.dtheta, other.dtheta) 127 | } 128 | 129 | @JvmName("epsilonEqualsOf") 130 | fun Twist2d.epsilonEquals(other: Twist2d) = epsilonEquals(this, other) 131 | 132 | fun Double.epsonEquals(other: Double) = epsilonEquals(this, other) 133 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/PhoenixUtil.kt: -------------------------------------------------------------------------------- 1 | // Copyright 2021-2025 FRC 6328 2 | // http://github.com/Mechanical-Advantage 3 | // 4 | // This program is free software; you can redistribute it and/or 5 | // modify it under the terms of the GNU General Public License 6 | // version 3 as published by the Free Software Foundation or 7 | // available in the root directory of this project. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | package org.team2471.frc.lib.ctre 14 | 15 | import com.ctre.phoenix6.StatusCode 16 | import com.ctre.phoenix6.Utils 17 | import com.ctre.phoenix6.Utils.getCurrentTimeSeconds 18 | import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC 19 | import com.ctre.phoenix6.controls.PositionVoltage 20 | import com.ctre.phoenix6.controls.VoltageOut 21 | import com.ctre.phoenix6.signals.ControlModeValue 22 | import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters 23 | import com.ctre.phoenix6.swerve.SwerveModule 24 | import com.ctre.phoenix6.swerve.SwerveModuleConstants 25 | import com.ctre.phoenix6.swerve.SwerveRequest 26 | import edu.wpi.first.math.kinematics.SwerveModuleState 27 | import edu.wpi.first.wpilibj.DriverStation 28 | import edu.wpi.first.wpilibj.Timer 29 | import org.team2471.frc.lib.util.isSim 30 | import java.util.function.Supplier 31 | 32 | object PhoenixUtil { 33 | 34 | val positionControlModes = listOf( 35 | ControlModeValue.PositionVoltage, ControlModeValue.PositionVoltageFOC, 36 | ControlModeValue.PositionTorqueCurrentFOC, ControlModeValue.MotionMagicDutyCycle, 37 | ControlModeValue.MotionMagicDutyCycleFOC, ControlModeValue.MotionMagicVoltage, 38 | ControlModeValue.MotionMagicVoltageFOC, ControlModeValue.PositionDutyCycle, 39 | ControlModeValue.PositionDutyCycleFOC, ControlModeValue.MotionMagicExpoDutyCycle, 40 | ControlModeValue.MotionMagicExpoDutyCycleFOC, ControlModeValue.MotionMagicExpoVoltage, 41 | ControlModeValue.MotionMagicExpoVoltageFOC, ControlModeValue.MotionMagicExpoTorqueCurrentFOC, 42 | ) 43 | 44 | /** Attempts to run the command until no error is produced. */ 45 | fun tryUntilOk(maxAttempts: Int, command: Supplier) { 46 | for (i in 0.. 76 | ): StatusCode { 77 | modulesToApply.forEachIndexed { index, module -> 78 | val wantedState = moduleStates.getOrNull(index) ?: SwerveModuleState(0.0, module.currentState.angle) 79 | module.apply(SwerveModule.ModuleRequest().withState(wantedState)) 80 | } 81 | 82 | return StatusCode.OK 83 | } 84 | } 85 | 86 | /** 87 | * Swerve request to set the individual module states. But, reads [SwerveModuleState.speedMetersPerSecond] as voltage NOT m/s 88 | * 89 | * If no value is passed in, the modules are set to their current angle with 0 volts 90 | */ 91 | class ApplyModuleStatesVoltage(vararg val moduleStates: SwerveModuleState? = arrayOf()): SwerveRequest { 92 | 93 | /** Local reference to a voltage request for the drive motors */ 94 | private val m_driveRequest = VoltageOut(0.0) 95 | 96 | /** Local reference to a position voltage request for the steer motors */ 97 | private val m_steerRequest_Voltage = PositionVoltage(0.0) 98 | 99 | /** Local reference to a position torque current request for the steer motors */ 100 | private val m_steerRequest_TorqueCurrent = PositionTorqueCurrentFOC(0.0) 101 | 102 | override fun apply( 103 | parameters: SwerveControlParameters?, 104 | vararg modulesToApply: SwerveModule<*, *, *>? 105 | ): StatusCode { 106 | modulesToApply.forEachIndexed { i, m -> 107 | val wantedState = moduleStates.getOrNull(i) ?: SwerveModuleState(0.0, m!!.currentState.angle) 108 | when (m!!.steerClosedLoopOutputType) { 109 | SwerveModuleConstants.ClosedLoopOutputType.Voltage -> m.apply( 110 | m_driveRequest.withOutput(wantedState.speedMetersPerSecond), 111 | m_steerRequest_Voltage.withPosition(wantedState.angle.measure) 112 | ) 113 | 114 | SwerveModuleConstants.ClosedLoopOutputType.TorqueCurrentFOC -> m.apply( 115 | m_driveRequest.withOutput(wantedState.speedMetersPerSecond), 116 | m_steerRequest_TorqueCurrent.withPosition(wantedState.angle.measure) 117 | ) 118 | } 119 | } 120 | return StatusCode.OK 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/limelight/LimelightCamera.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision.limelight 2 | 3 | import edu.wpi.first.math.Matrix 4 | import edu.wpi.first.math.geometry.Transform3d 5 | import edu.wpi.first.math.numbers.N1 6 | import edu.wpi.first.math.numbers.N3 7 | import edu.wpi.first.math.numbers.N8 8 | import edu.wpi.first.networktables.NetworkTableInstance 9 | import edu.wpi.first.wpilibj.RobotController 10 | import edu.wpi.first.wpilibj.Timer 11 | import org.team2471.frc.lib.vision.Fiducial 12 | import org.team2471.frc.lib.vision.PipelineConfig 13 | import org.team2471.frc.lib.vision.PipelineVisionPacket 14 | import org.team2471.frc.lib.vision.QuixVisionCamera 15 | import org.littletonrobotics.junction.LogTable 16 | import org.littletonrobotics.junction.Logger 17 | import org.littletonrobotics.junction.inputs.LoggableInputs 18 | import org.photonvision.PhotonCamera 19 | import org.photonvision.simulation.PhotonCameraSim 20 | import org.photonvision.targeting.PhotonPipelineResult 21 | import org.photonvision.targeting.PhotonTrackedTarget 22 | import org.photonvision.targeting.TargetCorner 23 | import java.util.Optional 24 | 25 | class LimelightCamera( 26 | val cameraName: String, 27 | override val transform: Transform3d, 28 | override val cameraMatrix: Optional>, 29 | override val distCoeffs: Optional> 30 | ): QuixVisionCamera { 31 | private val pipelineConfigs = arrayOf(PipelineConfig()) 32 | override val cameraSim: PhotonCameraSim = 33 | PhotonCameraSim(PhotonCamera(cameraName), pipelineConfigs[0].simCameraProp) 34 | 35 | private val loggingName: String = "Inputs/LimelightCamera [$cameraName]" 36 | 37 | private val inputs = LimelightCameraInputs() 38 | 39 | 40 | class LimelightCameraInputs : LoggableInputs { 41 | var latestResult: PhotonPipelineResult = PhotonPipelineResult() 42 | 43 | override fun toLog(table: LogTable) { 44 | table.put("Latest Result", latestResult) 45 | } 46 | 47 | override fun fromLog(table: LogTable) { 48 | latestResult = table.get("Latest Result", latestResult) 49 | } 50 | } 51 | 52 | override fun updateInputs() { 53 | val corners = NetworkTableInstance.getDefault().getTable(cameraName).getEntry("tcornxy").getDoubleArray(doubleArrayOf()) 54 | val fiducials = NetworkTableInstance.getDefault().getTable(cameraName).getEntry("rawfiducials").getDoubleArray(doubleArrayOf()) 55 | 56 | if (corners.size >= 8 && fiducials.size >= 6) { 57 | val targets = mutableListOf() 58 | for (i in 0..(corners.size / 8)-1) { 59 | targets.add( 60 | PhotonTrackedTarget( 61 | 0.0, //camToTag[4], 62 | 0.0, //camToTag[3], 63 | 0.0, //LimelightHelpers.getTA(cameraName), 64 | 0.0, //camToTag[5], 65 | fiducials[i * 6].toInt(), //LimelightHelpers.getFiducialID(cameraName).toInt(), 66 | 0, 67 | 0.0F, 68 | Transform3d(), //robotToTag, 69 | Transform3d(), //robotToTag, 70 | 0.0, 71 | mutableListOf( 72 | TargetCorner(corners[(i * 8)], corners[(i * 8) + 1]), 73 | TargetCorner(corners[(i * 8) + 2], corners[(i * 8) + 3]), 74 | TargetCorner(corners[(i * 8) + 4], corners[(i * 8) + 5]), 75 | TargetCorner(corners[(i * 8) + 6], corners[(i * 8) + 7]), 76 | ), 77 | mutableListOf( 78 | TargetCorner(corners[(i * 8) + 0], corners[(i * 8) + 1]), 79 | TargetCorner(corners[(i * 8) + 2], corners[(i * 8) + 3]), 80 | TargetCorner(corners[(i * 8) + 4], corners[(i * 8) + 5]), 81 | TargetCorner(corners[(i * 8) + 6], corners[(i * 8) + 7]), 82 | ) 83 | ) 84 | ) 85 | } 86 | 87 | val tl = LimelightHelpers.getLatency_Pipeline(cameraName) * 1000 88 | val cl = LimelightHelpers.getLatency_Capture(cameraName) * 1000 89 | inputs.latestResult = PhotonPipelineResult( 90 | 0.0.toLong(), 91 | (RobotController.getTime() - tl - cl).toLong(), 92 | (RobotController.getTime()), 93 | 0.0.toLong(), 94 | targets 95 | ) 96 | } else { 97 | inputs.latestResult = PhotonPipelineResult() 98 | } 99 | 100 | Logger.processInputs(loggingName, inputs) 101 | } 102 | 103 | override fun setPipelineIndex(index: Int) {} 104 | 105 | override val pipelineConfig: PipelineConfig 106 | get() = pipelineConfigs[0] 107 | 108 | override val fiducialType: Fiducial.Type 109 | get() = pipelineConfig.fiducialType 110 | 111 | override var allDataPopulated: Boolean = true 112 | 113 | override val latestMeasurement: PipelineVisionPacket 114 | get() { 115 | val startTimestamp = Timer.getFPGATimestamp() 116 | val result = inputs.latestResult 117 | val hasTargets = result.hasTargets() 118 | if (!hasTargets) { 119 | return PipelineVisionPacket(false, null, null, -1.0) 120 | } 121 | 122 | val endTimestamp = Timer.getFPGATimestamp() 123 | Logger.recordOutput("$loggingName/GetLatestMeasurementMs", (endTimestamp - startTimestamp) * 1000.0) 124 | 125 | return PipelineVisionPacket( 126 | hasTargets, 127 | result.getBestTarget(), 128 | result.getTargets(), 129 | result.timestampSeconds - 0.03 130 | ) 131 | } 132 | } -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | ############################################################################## 4 | ## 5 | ## Gradle start up script for UN*X 6 | ## 7 | ############################################################################## 8 | 9 | # Attempt to set APP_HOME 10 | # Resolve links: $0 may be a link 11 | PRG="$0" 12 | # Need this for relative symlinks. 13 | while [ -h "$PRG" ] ; do 14 | ls=`ls -ld "$PRG"` 15 | link=`expr "$ls" : '.*-> \(.*\)$'` 16 | if expr "$link" : '/.*' > /dev/null; then 17 | PRG="$link" 18 | else 19 | PRG=`dirname "$PRG"`"/$link" 20 | fi 21 | done 22 | SAVED="`pwd`" 23 | cd "`dirname \"$PRG\"`/" >/dev/null 24 | APP_HOME="`pwd -P`" 25 | cd "$SAVED" >/dev/null 26 | 27 | APP_NAME="Gradle" 28 | APP_BASE_NAME=`basename "$0"` 29 | 30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 31 | DEFAULT_JVM_OPTS="" 32 | 33 | # Use the maximum available, or set MAX_FD != -1 to use that value. 34 | MAX_FD="maximum" 35 | 36 | warn () { 37 | echo "$*" 38 | } 39 | 40 | die () { 41 | echo 42 | echo "$*" 43 | echo 44 | exit 1 45 | } 46 | 47 | # OS specific support (must be 'true' or 'false'). 48 | cygwin=false 49 | msys=false 50 | darwin=false 51 | nonstop=false 52 | case "`uname`" in 53 | CYGWIN* ) 54 | cygwin=true 55 | ;; 56 | Darwin* ) 57 | darwin=true 58 | ;; 59 | MINGW* ) 60 | msys=true 61 | ;; 62 | NONSTOP* ) 63 | nonstop=true 64 | ;; 65 | esac 66 | 67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 68 | 69 | # Determine the Java command to use to start the JVM. 70 | if [ -n "$JAVA_HOME" ] ; then 71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 72 | # IBM's JDK on AIX uses strange locations for the executables 73 | JAVACMD="$JAVA_HOME/jre/sh/java" 74 | else 75 | JAVACMD="$JAVA_HOME/bin/java" 76 | fi 77 | if [ ! -x "$JAVACMD" ] ; then 78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 79 | 80 | Please set the JAVA_HOME variable in your environment to match the 81 | location of your Java installation." 82 | fi 83 | else 84 | JAVACMD="java" 85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 86 | 87 | Please set the JAVA_HOME variable in your environment to match the 88 | location of your Java installation." 89 | fi 90 | 91 | # Increase the maximum file descriptors if we can. 92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 93 | MAX_FD_LIMIT=`ulimit -H -n` 94 | if [ $? -eq 0 ] ; then 95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 96 | MAX_FD="$MAX_FD_LIMIT" 97 | fi 98 | ulimit -n $MAX_FD 99 | if [ $? -ne 0 ] ; then 100 | warn "Could not set maximum file descriptor limit: $MAX_FD" 101 | fi 102 | else 103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 104 | fi 105 | fi 106 | 107 | # For Darwin, add options to specify how the application appears in the dock 108 | if $darwin; then 109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 110 | fi 111 | 112 | # For Cygwin, switch paths to Windows format before running java 113 | if $cygwin ; then 114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 116 | JAVACMD=`cygpath --unix "$JAVACMD"` 117 | 118 | # We build the pattern for arguments to be converted via cygpath 119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 120 | SEP="" 121 | for dir in $ROOTDIRSRAW ; do 122 | ROOTDIRS="$ROOTDIRS$SEP$dir" 123 | SEP="|" 124 | done 125 | OURCYGPATTERN="(^($ROOTDIRS))" 126 | # Add a user-defined pattern to the cygpath arguments 127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 129 | fi 130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 131 | i=0 132 | for arg in "$@" ; do 133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 135 | 136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 138 | else 139 | eval `echo args$i`="\"$arg\"" 140 | fi 141 | i=$((i+1)) 142 | done 143 | case $i in 144 | (0) set -- ;; 145 | (1) set -- "$args0" ;; 146 | (2) set -- "$args0" "$args1" ;; 147 | (3) set -- "$args0" "$args1" "$args2" ;; 148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;; 149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 154 | esac 155 | fi 156 | 157 | # Escape application args 158 | save () { 159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 160 | echo " " 161 | } 162 | APP_ARGS=$(save "$@") 163 | 164 | # Collect all arguments for the java command, following the shell quoting and substitution rules 165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 166 | 167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong 168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then 169 | cd "$(dirname "$0")" 170 | fi 171 | 172 | exec "$JAVACMD" "$@" 173 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/localization/NTManager.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.localization 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.networktables.* 5 | import edu.wpi.first.util.struct.StructBuffer 6 | import org.team2471.frc.lib.vision.Fiducial 7 | import org.team2471.frc.lib.vision.QuixVisionCamera 8 | import org.littletonrobotics.junction.AutoLog 9 | import org.littletonrobotics.junction.Logger 10 | import java.util.* 11 | import java.util.concurrent.atomic.AtomicReference 12 | 13 | /** 14 | * The NTManager class is responsible for managing network table interactions related to robot 15 | * localization. It handles publishing and subscribing to various topics such as measurements, 16 | * targets, and camera information. It also maintains the latest pose estimate and updates inputs 17 | * accordingly. 18 | */ 19 | class NTManager { 20 | private val localizerTable: NetworkTable 21 | 22 | // Combined Robot to DS odometry and vision measurements 23 | private val measurementsPub: DoubleArrayPublisher 24 | 25 | // Robot to DS targets 26 | private val targetPub: StructArrayPublisher 27 | 28 | // Robot to Camera transforms and intrinsics 29 | private val cameraInfosPublisher: StructArrayPublisher 30 | 31 | // Use an AtomicReference to make updating the value thread-safe 32 | private val latestPoseEstimateReference = AtomicReference() 33 | 34 | private val inputs = PoseEstimateInputsAutoLogged() 35 | 36 | @AutoLog 37 | open class PoseEstimateInputs { 38 | @JvmField var id: Int = 0 39 | @JvmField var pose: Pose2d? = null 40 | @JvmField var hasVision: Boolean = false 41 | } 42 | 43 | /** 44 | * Manages the NetworkTables (NT) interactions for localization. This class sets up publishers and 45 | * subscribers for various localization-related topics. 46 | * 47 | * 48 | * It initializes the following publishers: 49 | * 50 | * 51 | * * Measurements publisher for double array topic "measurements" 52 | * * Targets publisher for struct array topic "targets" with Fiducial structure 53 | * * Cameras publisher for struct array topic "cameras" with CameraInfo structure 54 | * 55 | * 56 | * 57 | * It also sets up a listener for the "estimates" topic to update the latest pose estimate. 58 | * 59 | * 60 | * Usage: 61 | * 62 | * 

`NTManager ntManager = new NTManager();
 63 |     `
* 64 | */ 65 | init { 66 | val inst = NetworkTableInstance.getDefault() 67 | localizerTable = inst.getTable("localizer") 68 | measurementsPub = localizerTable.getDoubleArrayTopic("measurements").publish(PubSubOption.sendAll(true)) 69 | targetPub = localizerTable.getStructArrayTopic("targets", Fiducial.struct) 70 | .publish(PubSubOption.sendAll(true)) 71 | cameraInfosPublisher = localizerTable.getStructArrayTopic("cameras", CameraInfo.struct) 72 | .publish(PubSubOption.sendAll(true)) 73 | 74 | // Setup listener for when the estimate is updated. 75 | val estimatesSub = localizerTable.getStructTopic("estimates", PoseEstimate.struct) 76 | .subscribe(PoseEstimate(), PubSubOption.sendAll(true)) 77 | val poseEstimateStructBuffer = StructBuffer.create(PoseEstimate.struct) 78 | inst.addListener(estimatesSub, EnumSet.of(NetworkTableEvent.Kind.kValueAll)) { event: NetworkTableEvent -> 79 | latestPoseEstimateReference.set(poseEstimateStructBuffer.read(event.valueData.value.getRaw())) 80 | } 81 | } 82 | 83 | /** 84 | * Publishes a measurement to the network table. 85 | * 86 | * @param measurement The measurement to be published. 87 | * @param id The identifier for the measurement. 88 | */ 89 | fun publishMeasurement(measurement: Measurement, id: Int) { 90 | measurementsPub.set(measurement.toArray(id)) 91 | NetworkTableInstance.getDefault().flush() 92 | } 93 | 94 | /** 95 | * Publishes an array of fiducial targets to the network table. 96 | * 97 | * @param targets An array of Fiducial objects representing the targets to be published. 98 | */ 99 | fun publishTargets(targets: Array) { 100 | targetPub.set(targets) 101 | } 102 | 103 | var camerasPublished = false 104 | /** 105 | * Publishes information about a list of cameras to the network table. 106 | * 107 | * @param cameras An ArrayList of QuixVisionCamera objects representing the cameras to be 108 | * published. Each camera's transform, camera matrix, and distortion coefficients will be used 109 | * to create a CameraInfo object, which will then be published. 110 | */ 111 | fun publishCameras(cameras: List) { 112 | if (camerasPublished) return 113 | var allCamerasPublished = true 114 | val array = cameras.map { 115 | if (!it.allDataPopulated) allCamerasPublished = false 116 | CameraInfo(it.transform, it.cameraMatrix, it.distCoeffs) 117 | }.toTypedArray() 118 | 119 | cameraInfosPublisher.set(array) 120 | camerasPublished = allCamerasPublished 121 | } 122 | 123 | /** 124 | * Updates the input values with the latest pose estimate if available. 125 | * 126 | * 127 | * This method retrieves the latest pose estimate and updates the input values with the ID, 128 | * pose, and vision status from the estimate. If the latest pose estimate is null, the input 129 | * values are not updated. The updated inputs are then processed by the Logger. 130 | */ 131 | fun updateInputs() { 132 | val latestEstimate = latestPoseEstimateReference.get() 133 | if (latestEstimate != null) { 134 | inputs.id = latestEstimate.id 135 | inputs.pose = latestEstimate.pose 136 | inputs.hasVision = latestEstimate.hasVision 137 | } 138 | Logger.processInputs("Inputs/NTManager", inputs) 139 | } 140 | 141 | /** 142 | * Retrieves the latest pose estimate from NT. 143 | * 144 | * @return A [PoseEstimate] object containing the latest pose information. 145 | */ 146 | val latestPoseEstimate: PoseEstimate 147 | get() = PoseEstimate(inputs.id, inputs.pose, inputs.hasVision) 148 | } 149 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/photonVision/PhotonVisionCamera.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision.photonVision 2 | 3 | import edu.wpi.first.math.Matrix 4 | import edu.wpi.first.math.geometry.Transform3d 5 | import edu.wpi.first.math.numbers.N1 6 | import edu.wpi.first.math.numbers.N3 7 | import edu.wpi.first.math.numbers.N8 8 | import edu.wpi.first.wpilibj.Timer 9 | import org.team2471.frc.lib.util.isReal 10 | import org.team2471.frc.lib.util.isSim 11 | import org.team2471.frc.lib.vision.Fiducial 12 | import org.team2471.frc.lib.vision.PipelineConfig 13 | import org.team2471.frc.lib.vision.PipelineVisionPacket 14 | import org.team2471.frc.lib.vision.QuixVisionCamera 15 | import org.team2471.frc.lib.vision.QuixVisionSim 16 | import org.ejml.simple.SimpleMatrix 17 | import org.littletonrobotics.junction.LogTable 18 | import org.littletonrobotics.junction.Logger 19 | import org.littletonrobotics.junction.inputs.LoggableInputs 20 | import org.photonvision.PhotonCamera 21 | import org.photonvision.simulation.PhotonCameraSim 22 | import org.photonvision.targeting.PhotonPipelineResult 23 | import java.util.Optional 24 | 25 | class PhotonVisionCamera( 26 | cameraName: String, 27 | override val transform: Transform3d, 28 | private val pipelineConfigs: Array 29 | ) : QuixVisionCamera { 30 | override val cameraSim: PhotonCameraSim = 31 | PhotonCameraSim(PhotonCamera(cameraName), pipelineConfigs[0].simCameraProp) 32 | 33 | private val loggingName: String = "Cameras/PhotonVisionCamera [$cameraName]" 34 | private val camera: PhotonCamera = if (isReal) PhotonCamera(cameraName) else cameraSim.camera 35 | 36 | private val inputs = PhotonCameraInputs() 37 | 38 | override var allDataPopulated: Boolean = false 39 | 40 | 41 | class PhotonCameraInputs : LoggableInputs { 42 | // TODO: Monitor performance and consider not logging the whole PhotonPipelineResult. 43 | var pipelineIndex: Int = 0 44 | var latestResult: PhotonPipelineResult = PhotonPipelineResult() 45 | var cameraMatrix: Optional> = Optional.empty>() 46 | var distCoeffs: Optional> = Optional.empty>() 47 | var allDataPopulated: Boolean = false 48 | 49 | override fun toLog(table: LogTable) { 50 | table.put("PipelineIndex", pipelineIndex) 51 | // table.put("LatestResult", latestResult) // Commented out bc loop cycles where large 52 | table.put("CameraMatrixIsPresent", cameraMatrix.isPresent) 53 | if (cameraMatrix.isPresent) table.put("CameraMatrixData", cameraMatrix.get().data) 54 | table.put("DistCoeffsIsPresent", distCoeffs.isPresent) 55 | if (distCoeffs.isPresent) table.put("DistCoeffsData", distCoeffs.get().data) 56 | } 57 | 58 | override fun fromLog(table: LogTable) { 59 | var allDataPop = true 60 | pipelineIndex = table.get("PipelineIndex", pipelineIndex) 61 | latestResult = table.get("LatestResult", latestResult) 62 | cameraMatrix = 63 | if (table.get("CameraMatrixIsPresent", false)) { 64 | Optional.of>( 65 | Matrix( 66 | SimpleMatrix(3, 3, true, *table.get("CameraMatrixData", DoubleArray(9))) 67 | ) 68 | ) 69 | } else { 70 | allDataPop = false 71 | Optional.empty>() 72 | } 73 | distCoeffs = 74 | if (table.get("DistCoeffsIsPresent", false)) { 75 | Optional.of>( 76 | Matrix( 77 | SimpleMatrix(8, 1, true, *table.get("DistCoeffsData", DoubleArray(8))) 78 | ) 79 | ) 80 | } else { 81 | allDataPop = false 82 | Optional.empty>() 83 | } 84 | allDataPopulated = allDataPop 85 | } 86 | } 87 | 88 | init { 89 | if (isSim) setPipelineIndex(0) 90 | QuixVisionSim.addCamera(this) 91 | } 92 | 93 | override fun updateInputs() { 94 | inputs.pipelineIndex = camera.pipelineIndex 95 | // TODO: Handle all results, not just the latest. 96 | inputs.latestResult = camera.allUnreadResults.lastOrNull() ?: PhotonPipelineResult() 97 | // Only update these once, since they shouldn't be changing. 98 | if (inputs.cameraMatrix.isEmpty) { 99 | val cameraMatrix = camera.cameraMatrix 100 | if (cameraMatrix.isPresent) inputs.cameraMatrix = cameraMatrix 101 | } 102 | if (inputs.distCoeffs.isEmpty) { 103 | val distCoeffs = camera.distCoeffs 104 | if (distCoeffs.isPresent) inputs.distCoeffs = distCoeffs 105 | } 106 | allDataPopulated = inputs.allDataPopulated 107 | // Logger.processInputs(loggingName, inputs) 108 | } 109 | 110 | override fun setPipelineIndex(index: Int) { 111 | if (index > pipelineConfigs.size) { 112 | println("Invalid pipeline index: $index") 113 | return 114 | } 115 | camera.pipelineIndex = index 116 | } 117 | 118 | override val pipelineConfig: PipelineConfig 119 | get() = pipelineConfigs[inputs.pipelineIndex] 120 | 121 | override val cameraMatrix: Optional> 122 | get() = inputs.cameraMatrix 123 | 124 | override val distCoeffs: Optional> 125 | get() = inputs.distCoeffs 126 | 127 | override val fiducialType: Fiducial.Type 128 | get() = pipelineConfig.fiducialType 129 | 130 | override val latestMeasurement: PipelineVisionPacket 131 | get() { 132 | val startTimestamp = Timer.getFPGATimestamp() 133 | val result = inputs.latestResult 134 | val hasTargets = result.hasTargets() 135 | if (!hasTargets) { 136 | return PipelineVisionPacket(false, null, null, -1.0) 137 | } 138 | 139 | val endTimestamp = Timer.getFPGATimestamp() 140 | Logger.recordOutput("$loggingName/GetLatestMeasurementMs", (endTimestamp - startTimestamp) * 1000.0) 141 | 142 | return PipelineVisionPacket( 143 | hasTargets, 144 | result.getBestTarget(), 145 | result.getTargets(), 146 | result.timestampSeconds - 0.03 147 | ) 148 | } 149 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/units/UTranslation2d.kt: -------------------------------------------------------------------------------- 1 | @file:Suppress("UNUSED") 2 | 3 | package org.team2471.frc.lib.units 4 | 5 | import edu.wpi.first.math.geometry.Rotation2d 6 | import edu.wpi.first.math.geometry.Translation2d 7 | import edu.wpi.first.math.geometry.struct.Translation2dStruct 8 | import edu.wpi.first.units.DistanceUnit 9 | import edu.wpi.first.units.LinearAccelerationUnit 10 | import edu.wpi.first.units.LinearVelocityUnit 11 | import edu.wpi.first.units.Measure 12 | import edu.wpi.first.units.Unit 13 | import edu.wpi.first.units.VelocityUnit 14 | import edu.wpi.first.util.struct.StructSerializable 15 | 16 | /** 17 | * Unit Translation2D 18 | * 19 | * Preserves the type of unit that it got constructed with. Inherits from WPILib Translation2D so can be used as a drop in replacement. 20 | * */ 21 | @Suppress("UNCHECKED_CAST") 22 | class UTranslation2d(x: Measure, y: Measure): Translation2d(x.baseUnitMagnitude(), y.baseUnitMagnitude()), StructSerializable { 23 | private val unit = x.unit().baseUnit 24 | 25 | val x: Measure 26 | get() = unit.of(super.getX()) as Measure 27 | val y: Measure 28 | get() = unit.of(super.getY()) as Measure 29 | 30 | val norm: Measure 31 | get() = unit.of(super.getNorm()) as Measure 32 | 33 | private fun fromTranslation2d(translation2d: Translation2d) = UTranslation2d(unit.of(translation2d.x) as Measure, unit.of(translation2d.y) as Measure) 34 | fun translation2d() = Translation2d(super.getX(), super.getY()) 35 | 36 | operator fun plus(other: UTranslation2d) = UTranslation2d(x + other.x, y + other.y) 37 | operator fun minus(other: UTranslation2d) = UTranslation2d(x - other.x, y - other.y) 38 | 39 | override operator fun div(by: Double) = UTranslation2d(x / by, y / by) 40 | // operator fun div(by: Measure) = Translation2d((x / by).magnitude(), (y / by).magnitude()) 41 | 42 | override operator fun times(by: Double) = UTranslation2d(x * by, y * by) 43 | // operator fun times(by: Measure) = Translation2d((x * by).magnitude(), (y * by).magnitude()) 44 | 45 | override fun rotateBy(other: Rotation2d): UTranslation2d = fromTranslation2d(super.rotateBy(other)) 46 | 47 | fun getDistance(other: UTranslation2d) = (this - other).norm 48 | fun interpolate(endValue: UTranslation2d, t: Double): UTranslation2d = fromTranslation2d(super.interpolate(endValue, t)) 49 | fun nearest(translations: List>): UTranslation2d = fromTranslation2d(super.nearest(translations)) 50 | fun rotateAround(other: UTranslation2d, rot: Rotation2d): UTranslation2d = fromTranslation2d(super.rotateAround(other, rot)) 51 | 52 | override fun unaryMinus(): UTranslation2d = fromTranslation2d(super.unaryMinus()) 53 | 54 | companion object { 55 | @JvmField 56 | val struct: Translation2dStruct = Translation2d.struct // Use the Translation2D struct 57 | } 58 | } 59 | 60 | // Distance UTranslation2d constructors 61 | inline val Translation2d.feet: UTranslation2d get() = UTranslation2d(x.feet, y.feet) 62 | inline val Translation2d.inches: UTranslation2d get() = UTranslation2d(x.inches, y.inches) 63 | inline val Translation2d.meters: UTranslation2d get() = UTranslation2d(x.meters, y.meters) 64 | inline val Translation2d.centimeters: UTranslation2d get() = UTranslation2d(x.centimeters, y.centimeters) 65 | inline val Translation2d.millimeters: UTranslation2d get() = UTranslation2d(x.centimeters, y.centimeters) 66 | 67 | // Distance UTranslation2d destructors 68 | inline val UTranslation2d.asFeet: Translation2d get() = Translation2d(x.asFeet, y.asFeet) 69 | inline val UTranslation2d.asInches: Translation2d get() = Translation2d(x.asInches, y.asInches) 70 | inline val UTranslation2d.asMeters: Translation2d get() = Translation2d(x.asMeters, y.asMeters) 71 | inline val UTranslation2d.asCentimeters: Translation2d get() = Translation2d(x.asCentimeters, y.asCentimeters) 72 | inline val UTranslation2d.asMillimeters: Translation2d get() = Translation2d(x.asMillimeters, y.asMillimeters) 73 | 74 | // Velocity UTranslation2d constructors 75 | @get:JvmName("UTranslation2dDistancePerSecond") 76 | inline val UTranslation2d.perSecond: UTranslation2d get() = UTranslation2d(x.perSecond, y.perSecond) 77 | inline val Translation2d.feetPerSecond: UTranslation2d get() = UTranslation2d(x.feetPerSecond, y.feetPerSecond) 78 | inline val Translation2d.inchesPerSecond: UTranslation2d get() = UTranslation2d(x.inchesPerSecond, y.inchesPerSecond) 79 | inline val Translation2d.metersPerSecond: UTranslation2d get() = UTranslation2d(x.metersPerSecond, y.metersPerSecond) 80 | 81 | // Velocity UTranslation2d destructors 82 | inline val UTranslation2d.asFeetPerSecond: Translation2d get() = Translation2d(this.x.asFeetPerSecond, this.y.asFeetPerSecond) 83 | inline val UTranslation2d.asInchesPerSecond: Translation2d get() = Translation2d(this.x.asInchesPerSecond, this.y.asInchesPerSecond) 84 | inline val UTranslation2d.asMetersPerSecond: Translation2d get() = Translation2d(this.x.asMetersPerSecond, this.y.asMetersPerSecond) 85 | 86 | // Acceleration UTranslation2d constructors 87 | inline val UTranslation2d.perSecondPerSecond: UTranslation2d get() = UTranslation2d(x.perSecondPerSecond, y.perSecondPerSecond) 88 | @get:JvmName("UTranslation2dLinearVelocityPerSecond") 89 | inline val UTranslation2d.perSecond: UTranslation2d get() = UTranslation2d(this.x.perSecond, this.y.perSecond) 90 | inline val Translation2d.feetPerSecondPerSecond: UTranslation2d get() = UTranslation2d(x.feetPerSecondPerSecond, y.feetPerSecondPerSecond) 91 | inline val Translation2d.inchesPerSecondPerSecond: UTranslation2d get() = UTranslation2d(x.inchesPerSecondPerSecond, y.inchesPerSecondPerSecond) 92 | inline val Translation2d.metersPerSecondPerSecond: UTranslation2d get() = UTranslation2d(x.metersPerSecondPerSecond, y.metersPerSecondPerSecond) 93 | 94 | // Acceleration UTranslation2d destructors 95 | inline val UTranslation2d.asFeetPerSecondPerSecond: Translation2d get() = Translation2d(this.x.asFeetPerSecondPerSecond, this.y.asFeetPerSecondPerSecond) 96 | inline val UTranslation2d.asInchesPerSecondPerSecond: Translation2d get() = Translation2d(this.x.asInchesPerSecondPerSecond, this.y.asInchesPerSecondPerSecond) 97 | inline val UTranslation2d.asMetersPerSecondPerSecond: Translation2d get() = Translation2d(this.x.asMetersPerSecondPerSecond, this.y.asMetersPerSecondPerSecond) 98 | 99 | // Jerk UTranslation2d constructors 100 | @get:JvmName("UTranslation2dLinearAccelerationPerSecond") 101 | inline val UTranslation2d.perSecond: UTranslation2d> get() = UTranslation2d(this.x.perSecond, this.y.perSecond) 102 | 103 | // Jerk UTranslation2d destructors 104 | inline val UTranslation2d>.asFeetJerk: Translation2d get() = Translation2d(this.x.asFeetJerk, this.y.asFeetJerk) 105 | inline val UTranslation2d>.asInchesJerk: Translation2d get() = Translation2d(this.x.asInchesJerk, this.y.asInchesJerk) 106 | inline val UTranslation2d>.asMetersJerk: Translation2d get() = Translation2d(this.x.asMetersJerk, this.y.asMetersJerk) -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/localization/CameraInfo.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.localization 2 | 3 | import edu.wpi.first.math.MatBuilder 4 | import edu.wpi.first.math.Matrix 5 | import edu.wpi.first.math.Nat 6 | import edu.wpi.first.math.geometry.Transform3d 7 | import edu.wpi.first.math.numbers.N1 8 | import edu.wpi.first.math.numbers.N3 9 | import edu.wpi.first.math.numbers.N8 10 | import edu.wpi.first.util.struct.Struct 11 | import java.nio.ByteBuffer 12 | import java.util.* 13 | 14 | /** 15 | * The CameraInfo class holds information about a camera's transform, camera matrix, and distortion 16 | * coefficients. 17 | * 18 | * 19 | * This class is used to encapsulate the camera's intrinsic and extrinsic parameters, which are 20 | * essential for camera calibration and image processing tasks in robotics. 21 | * 22 | * 23 | * It contains the following fields: 24 | * 25 | * 26 | * * `m_transform` - The 3D transform representing the camera's position and orientation. 27 | * * `m_cameraMatrix` - An optional 3x3 matrix representing the camera's intrinsic 28 | * parameters. 29 | * * `m_distCoeffs` - An optional 8x1 matrix representing the camera's distortion 30 | * coefficients. 31 | * 32 | * 33 | * 34 | * The class provides getter methods to access these fields: 35 | * 36 | * 37 | * * [.getTransform] - Returns the camera's transform. 38 | * * [.getCameraMatrix] - Returns the camera's intrinsic matrix, if available. 39 | * * [.getDistCoeffs] - Returns the camera's distortion coefficients, if available. 40 | * 41 | * 42 | * 43 | * Additionally, it includes a static nested class `CameraInfoStruct` for serialization 44 | * purposes. 45 | * 46 | * @param transform The 3D transform representing the camera's position and orientation. 47 | * @param cameraMatrix An optional 3x3 matrix representing the camera's intrinsic parameters. 48 | * @param distCoeffs An optional 8x1 matrix representing the camera's distortion coefficients. 49 | */ 50 | class CameraInfo( 51 | val transform: Transform3d, 52 | val cameraMatrix: Optional>, 53 | val distCoeffs: Optional> 54 | ) { 55 | companion object { 56 | // Struct for serialization. 57 | @JvmField 58 | val struct: CameraInfoStruct = CameraInfoStruct() 59 | } 60 | } 61 | /** 62 | * CameraInfoStruct is a structure that implements the Struct interface for the CameraInfo class. It 63 | * provides methods to get the type class, type name, size, schema, nested structures, and to pack 64 | * and unpack CameraInfo objects from a ByteBuffer. 65 | * 66 | * 67 | * The structure includes the following fields: 68 | * 69 | * 70 | * * Transform3d transform 71 | * * int32 hasCameraMatrix 72 | * * double M00, M01, M02, M10, M11, M12, M20, M21, M22 73 | * * int32 hasDistCoeffs 74 | * * double d0, d1, d2, d3, d4, d5, d6, d7 75 | * 76 | * Used by NetworkTables to efficiently transmit camera calibration data between robot and driver 77 | * station. 78 | * 79 | * @see ByteBuffer 80 | * @see CameraInfo 81 | * @see Transform3d 82 | * @see Struct 83 | */ 84 | class CameraInfoStruct : Struct { 85 | override fun getTypeClass(): Class = CameraInfo::class.java 86 | 87 | override fun getTypeName(): String = "CameraInfo" 88 | 89 | override fun getSize(): Int = Transform3d.struct.size + Struct.kSizeInt32 * 2 + Struct.kSizeDouble * 17 90 | 91 | override fun getSchema(): String = 92 | ("Transform3d transform;" 93 | + "int32 hasCameraMatrix;" 94 | + "double M00;" 95 | + "double M01;" 96 | + "double M02;" 97 | + "double M10;" 98 | + "double M11;" 99 | + "double M12;" 100 | + "double M20;" 101 | + "double M21;" 102 | + "double M22;" 103 | + "int32 hasDistCoeffs;" 104 | + "double d0;" 105 | + "double d1;" 106 | + "double d2;" 107 | + "double d3;" 108 | + "double d4;" 109 | + "double d5;" 110 | + "double d6;" 111 | + "double d7;") 112 | 113 | override fun getNested(): Array> = arrayOf(Transform3d.struct) 114 | 115 | /** 116 | * Unpacks camera information from a ByteBuffer into a CameraInfo object. 117 | * 118 | * @param bb The ByteBuffer containing the packed camera information data 119 | * @return A new CameraInfo object containing the unpacked camera transformation, optional camera 120 | * matrix, and optional distortion coefficients 121 | * 122 | * The ByteBuffer should contain, in order: - A Transform3d struct - A boolean flag for 123 | * camera matrix presence - Nine doubles representing a 3x3 camera matrix (if present) - A 124 | * boolean flag for distortion coefficients presence - Eight doubles representing distortion 125 | * coefficients (if present) 126 | */ 127 | override fun unpack(bb: ByteBuffer): CameraInfo { 128 | val transform = Transform3d.struct.unpack(bb) 129 | val hasCameraMatrix = bb.getInt() != 0 130 | val m00 = bb.getDouble() 131 | val m01 = bb.getDouble() 132 | val m02 = bb.getDouble() 133 | val m10 = bb.getDouble() 134 | val m11 = bb.getDouble() 135 | val m12 = bb.getDouble() 136 | val m20 = bb.getDouble() 137 | val m21 = bb.getDouble() 138 | val m22 = bb.getDouble() 139 | val hasDistCoeffs = bb.getInt() != 0 140 | val d0 = bb.getDouble() 141 | val d1 = bb.getDouble() 142 | val d2 = bb.getDouble() 143 | val d3 = bb.getDouble() 144 | val d4 = bb.getDouble() 145 | val d5 = bb.getDouble() 146 | val d6 = bb.getDouble() 147 | val d7 = bb.getDouble() 148 | return CameraInfo( 149 | transform, 150 | if (hasCameraMatrix) { 151 | Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), m00, m01, m02, m10, m11, m12, m20, m21, m22)) 152 | } else { 153 | Optional.empty>() 154 | }, 155 | if (hasDistCoeffs) { 156 | Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), d0, d1, d2, d3, d4, d5, d6, d7)) 157 | } else { 158 | Optional.empty>() 159 | } 160 | ) 161 | } 162 | 163 | /** 164 | * Packs camera information into a ByteBuffer. This method serializes a CameraInfo object, 165 | * including its transformation matrix, camera matrix (if present), and distortion coefficients 166 | * (if present). 167 | * 168 | * @param bb The ByteBuffer to pack the data into 169 | * @param value The CameraInfo object containing the data to be packed 170 | */ 171 | override fun pack(bb: ByteBuffer, value: CameraInfo) { 172 | Transform3d.struct.pack(bb, value.transform) 173 | val hasCameraMatrix = value.cameraMatrix.isPresent 174 | bb.putInt(if (hasCameraMatrix) 1 else 0) 175 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(0, 0) else 0.0) 176 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(0, 1) else 0.0) 177 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(0, 2) else 0.0) 178 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(1, 0) else 0.0) 179 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(1, 1) else 0.0) 180 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(1, 2) else 0.0) 181 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(2, 0) else 0.0) 182 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(2, 1) else 0.0) 183 | bb.putDouble(if (hasCameraMatrix) value.cameraMatrix.get().get(2, 2) else 0.0) 184 | val hasDistCoeffs = value.distCoeffs.isPresent 185 | bb.putInt(if (hasDistCoeffs) 1 else 0) 186 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(0, 0) else 0.0) 187 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(1, 0) else 0.0) 188 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(2, 0) else 0.0) 189 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(3, 0) else 0.0) 190 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(4, 0) else 0.0) 191 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(5, 0) else 0.0) 192 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(6, 0) else 0.0) 193 | bb.putDouble(if (hasDistCoeffs) value.distCoeffs.get().get(7, 0) else 0.0) 194 | } 195 | 196 | override fun isImmutable(): Boolean = true 197 | } 198 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/vision/limelight/VisionIOLimelight.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.vision.limelight 2 | 3 | import edu.wpi.first.math.geometry.Pose2d 4 | import edu.wpi.first.math.geometry.Translation2d 5 | import edu.wpi.first.networktables.NetworkTableInstance 6 | import edu.wpi.first.units.measure.Angle 7 | import edu.wpi.first.wpilibj.Timer 8 | import org.littletonrobotics.junction.LogTable 9 | import org.littletonrobotics.junction.Logger 10 | import org.littletonrobotics.junction.inputs.LoggableInputs 11 | import org.team2471.frc.lib.units.asDegrees 12 | import org.team2471.frc.lib.units.asSeconds 13 | import org.team2471.frc.lib.units.milliseconds 14 | import kotlin.math.sqrt 15 | 16 | 17 | class VisionIOLimelight(val name: String, val useMegatag2: Boolean = true, val headingSupplier: () -> Angle): VisionIO { 18 | 19 | /* 20 | There are 5 different limelight IMU modes. 21 | 0: Ignores internal imu, only uses external IMU through setRobotOrientation() 22 | 1: Resets internal IMU to the given angle whenever setRobotOrientation() is called 23 | 2: Solely relies on the internal IMU 24 | 3: IMU_ASSIST_MT1 - uses seen AprilTags in MegaTag1 to update heading 25 | 4: IMU_ASSIST_EXTERNALIMU - uses external IMU for gradual heading correction. 26 | */ 27 | override var imuMode = 1 28 | set(value) { 29 | println("Resetting Limelight IMU") 30 | LimelightHelpers.SetIMUMode(name, value) 31 | field = value 32 | } 33 | 34 | override var imuAssistAlpha = 0.001 35 | set(value) { 36 | LimelightHelpers.SetIMUAssistAlpha(name, value) 37 | field = value 38 | } 39 | 40 | 41 | 42 | override var mode: LimelightMode = LimelightMode.APRILTAG 43 | set(value) { 44 | when (value) { 45 | LimelightMode.APRILTAG -> LimelightHelpers.setPipelineIndex(name, 0) 46 | LimelightMode.GAMEPIECE -> LimelightHelpers.setPipelineIndex(name, 1) 47 | } 48 | 49 | field = value 50 | } 51 | 52 | val heartbeatSub = NetworkTableInstance.getDefault().getTable(name).getDoubleTopic("hb").subscribe(0.0) 53 | var prevHeartbeats = MutableList(3) { 0.0 } 54 | var beforeFirstEnable = true 55 | var isEnabled = false 56 | 57 | override fun updateInputs(inputs: VisionIO.VisionIOInputs) { 58 | 59 | val heartbeat = heartbeatSub.get() 60 | if (heartbeat != 0.0 && prevHeartbeats[2] != heartbeat) { 61 | if (!inputs.isConnected) { 62 | onConnect() 63 | } 64 | inputs.isConnected = true 65 | } else { 66 | inputs.isConnected = false 67 | } 68 | prevHeartbeats.add(0, heartbeat) 69 | prevHeartbeats.removeAt(prevHeartbeats.size - 1) 70 | 71 | inputs.mode = mode 72 | 73 | if (mode == LimelightMode.APRILTAG) { 74 | inputs.seesTag = LimelightHelpers.getTV(name) 75 | 76 | val llPoseEstimate = 77 | if (beforeFirstEnable || !useMegatag2) LimelightHelpers.getBotPoseEstimate_wpiBlue(name) else LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2( 78 | name 79 | ) 80 | // kinda satisfies the compiler? ive already filtered the nulls out 81 | val veryRawFiducials: List = LimelightHelpers.getRawFiducials(name) 82 | .filterIndexed { index, fiducial -> fiducial != null && index < 4 } as List 83 | val rawFiducials = DoubleArray(15) { 0.0 } 84 | 85 | for (i in veryRawFiducials.indices) { 86 | rawFiducials[i * 4] = veryRawFiducials[i].id.toDouble() 87 | rawFiducials[i * 4 + 1] = veryRawFiducials[i].txnc 88 | rawFiducials[i * 4 + 2] = veryRawFiducials[i].tync 89 | rawFiducials[i * 4 + 3] = veryRawFiducials[i].ta 90 | } 91 | 92 | inputs.aprilTagPoseEstimate = llPoseEstimate?.pose ?: Pose2d() 93 | inputs.aprilTagTimestamp = 94 | Timer.getFPGATimestamp() - (llPoseEstimate?.latency?.milliseconds?.asSeconds ?: 0.0) 95 | inputs.rawFiducials = rawFiducials 96 | 97 | inputs.objectCorners = DoubleArray(8) { 0.0 } 98 | inputs.objectCoords = DoubleArray(2) { 0.0 } 99 | } else { 100 | inputs.objectCoords = doubleArrayOf( 101 | LimelightHelpers.getTX(name), LimelightHelpers.getTY(name) 102 | ) 103 | 104 | inputs.objectCorners = NetworkTableInstance.getDefault().getTable(name).getEntry("tcornxy") 105 | .getDoubleArray(DoubleArray(8) { 0.0 }) 106 | 107 | inputs.aprilTagPoseEstimate = Pose2d() 108 | inputs.aprilTagTimestamp = 0.0 109 | } 110 | 111 | Logger.processInputs(name, inputs) 112 | } 113 | 114 | fun onConnect() { 115 | LimelightHelpers.SetIMUMode(name, imuMode) 116 | LimelightHelpers.SetIMUAssistAlpha(name, imuAssistAlpha) 117 | 118 | if (isEnabled) { 119 | LimelightHelpers.SetThrottle(name, 0) 120 | } else { 121 | // LimelightHelpers.SetThrottle(name, 100) 122 | } 123 | } 124 | 125 | override fun enable() { 126 | beforeFirstEnable = false 127 | isEnabled = true 128 | LimelightHelpers.SetIMUMode(name, imuMode) 129 | LimelightHelpers.SetIMUAssistAlpha(name, imuAssistAlpha) 130 | LimelightHelpers.SetThrottle(name, 0) 131 | } 132 | 133 | override fun disable() { 134 | isEnabled = false 135 | // LimelightHelpers.SetThrottle(name, 100) 136 | } 137 | 138 | 139 | 140 | override fun gyroReset() { 141 | LimelightHelpers.SetIMUMode(name, 1) 142 | LimelightHelpers.SetRobotOrientation(name, headingSupplier.invoke().asDegrees, 0.0, 0.0, 0.0, 0.0, 0.0) 143 | imuMode = imuMode 144 | } 145 | 146 | override fun disabledGyroReset() { 147 | LimelightHelpers.SetIMUMode(name, 1) 148 | LimelightHelpers.SetRobotOrientation(name, headingSupplier.invoke().asDegrees, 0.0, 0.0, 0.0, 0.0, 0.0) 149 | } 150 | 151 | override fun updateCropping(minX: Double, maxX: Double, minY: Double, maxY: Double) { 152 | LimelightHelpers.setCropWindow(name, minX, maxX, minY, maxY) 153 | } 154 | } 155 | 156 | interface VisionIO { 157 | 158 | var imuMode: Int 159 | var imuAssistAlpha: Double 160 | var mode: LimelightMode 161 | 162 | fun updateInputs(inputs: VisionIOInputs) 163 | 164 | fun enable() 165 | fun disable() 166 | 167 | fun gyroReset() 168 | fun disabledGyroReset() 169 | 170 | fun updateCropping(minX: Double, maxX: Double, minY: Double, maxY: Double) 171 | 172 | open class VisionIOInputs : LoggableInputs { 173 | 174 | var isConnected = false 175 | var mode = LimelightMode.APRILTAG 176 | 177 | // April Tag 178 | var seesTag = false 179 | 180 | var aprilTagPoseEstimate = Pose2d() 181 | 182 | // Seconds 183 | var aprilTagTimestamp = 0.0 184 | 185 | //id, x, y, area, id, x, y, area, id, x, y, area,... 186 | // if you end up seeing more than 5 tags ill be shocked 187 | var rawFiducials: DoubleArray = DoubleArray(20) { 0.0 } 188 | 189 | // (id, (x, y), area),... 190 | val trimmedFiducials: List, Double>> 191 | get() { 192 | return rawFiducials.filterIndexed { index, value -> index % 4 == 0 && value != 0.0 } 193 | .mapIndexed { index, id -> 194 | Triple( 195 | id, 196 | Pair(rawFiducials[index * 4 + 1], rawFiducials[index * 4 + 2]), 197 | rawFiducials[index * 4 + 3] 198 | ) 199 | } 200 | } 201 | 202 | 203 | // object detection 204 | var objectCorners: DoubleArray = DoubleArray(8) { 0.0 } 205 | var objectCoords: DoubleArray = DoubleArray(2) { 0.0 } 206 | 207 | val hasObjects: Boolean 208 | get() = objectCorners.isNotEmpty() && objectCoords.isNotEmpty() 209 | 210 | val objectCenter: Translation2d 211 | get() { 212 | if (hasObjects) { 213 | val targetCornersX = objectCorners.filterIndexed { index, _ -> index % 2 == 0 } 214 | val targetCornersY = objectCorners.filterIndexed { index, _ -> index % 2 == 1 } 215 | 216 | return Translation2d( 217 | (targetCornersX.max() + targetCornersX.min()) / 2, 218 | (targetCornersY.max() + targetCornersY.min()) / 2 219 | ) 220 | } else { 221 | return Translation2d() 222 | } 223 | } 224 | 225 | val objectDimensions: Pair 226 | get() { 227 | try { 228 | val targetCornersX = objectCorners.filterIndexed { index, _ -> index % 2 == 0 } 229 | val targetCornersY = objectCorners.filterIndexed { index, _ -> index % 2 == 1 } 230 | 231 | return targetCornersX.max() - targetCornersX.min() to targetCornersY.max() - targetCornersY.min() 232 | } catch (_: Exception) { 233 | return 0.0 to 0.0 234 | } 235 | } 236 | 237 | override fun toLog(table: LogTable) { 238 | table.put("Is Connected", isConnected) 239 | table.put("Mode", mode) 240 | table.put("Sees Tag", seesTag) 241 | table.put("AprilTag Pose Estimate", aprilTagPoseEstimate) 242 | table.put("AprilTag Timestamp", aprilTagTimestamp) 243 | table.put("RawFiducials", rawFiducials) 244 | table.put("Object Corners", objectCorners) 245 | table.put("Object Coordinates", objectCoords) 246 | } 247 | 248 | override fun fromLog(table: LogTable) { 249 | isConnected = table.get("Is Connected", isConnected) 250 | mode = table.get("Mode", mode) 251 | seesTag = table.get("Sees Tag", seesTag) 252 | aprilTagPoseEstimate = table.get("AprilTag Pose Estimate", aprilTagPoseEstimate).first() 253 | aprilTagTimestamp = table.get("AprilTag Timestamp", aprilTagTimestamp) 254 | rawFiducials = table.get("RawFiducials", rawFiducials) 255 | objectCorners = table.get("Object Corners", objectCorners) 256 | objectCoords = table.get("Object Coordinates", objectCoords) 257 | } 258 | } 259 | } 260 | 261 | enum class LimelightMode { 262 | APRILTAG, GAMEPIECE 263 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/units/Units.kt: -------------------------------------------------------------------------------- 1 | @file:Suppress("UNUSED") 2 | 3 | package org.team2471.frc.lib.units 4 | 5 | import edu.wpi.first.math.geometry.Rotation2d 6 | import edu.wpi.first.units.* 7 | import edu.wpi.first.units.Units.* 8 | import edu.wpi.first.units.measure.* 9 | import kotlin.math.* 10 | 11 | //Unit Conversions 12 | 13 | //Distance 14 | inline val Measure.asInches: Double get() = `in`(Inches) 15 | inline val Measure.asFeet: Double get() = `in`(Feet) 16 | inline val Measure.asMeters: Double get() = `in`(Meters) 17 | inline val Measure.asCentimeters: Double get() = `in`(Centimeters) 18 | inline val Measure.asMillimeters: Double get() = `in`(Millimeter) 19 | 20 | inline val Double.inches: Distance get() = Inches.of(this) 21 | inline val Double.feet: Distance get() = Feet.of(this) 22 | inline val Double.meters: Distance get() = Meters.of(this) 23 | inline val Double.centimeters: Distance get() = Centimeters.of(this) 24 | inline val Double.millimeters: Distance get() = Millimeters.of(this) 25 | 26 | 27 | //Angle 28 | inline val Angle.asDegrees: Double get() = `in`(Degrees) 29 | inline val Angle.asRotations: Double get() = `in`(Rotations) 30 | inline val Angle.asRadians: Double get() = `in`(Radians) 31 | inline val Angle.asRotation2d: Rotation2d get() = Rotation2d(this) 32 | 33 | inline val Double.degrees: Angle get() = Degrees.of(this) 34 | inline val Double.rotations: Angle get() = Rotations.of(this) 35 | inline val Double.radians: Angle get() = Radians.of(this) 36 | 37 | 38 | //Time 39 | inline val Time.asSeconds: Double get() = `in`(Seconds) 40 | inline val Time.asMinutes: Double get() = `in`(Minutes) 41 | inline val Time.asMilliseconds: Double get() = `in`(Milliseconds) 42 | inline val Time.asMicroseconds: Double get() = `in`(Microseconds) 43 | 44 | inline val Double.seconds: Time get() = Seconds.of(this) 45 | inline val Double.milliseconds: Time get() = Milliseconds.of(this) 46 | inline val Double.microseconds: Time get() = Microseconds.of(this) 47 | inline val Double.minutes: Time get() = Minutes.of(this) 48 | 49 | 50 | //Linear Velocity 51 | inline val Measure.asInchesPerSecond: Double get() = `in`(InchesPerSecond) 52 | inline val Measure.asFeetPerSecond: Double get() = `in`(FeetPerSecond) 53 | inline val Measure.asMetersPerSecond: Double get() = `in`(MetersPerSecond) 54 | 55 | inline val Measure.perSecond: LinearVelocity get() = InchesPerSecond.of(this.asInches) 56 | 57 | inline val Double.inchesPerSecond: LinearVelocity get() = InchesPerSecond.of(this) 58 | inline val Double.feetPerSecond: LinearVelocity get() = FeetPerSecond.of(this) 59 | inline val Double.metersPerSecond: LinearVelocity get() = MetersPerSecond.of(this) 60 | 61 | 62 | //Angular Velocity 63 | inline val AngularVelocity.asDegreesPerSecond: Double get() = `in`(DegreesPerSecond) 64 | inline val AngularVelocity.asRotationsPerSecond: Double get() = `in`(RotationsPerSecond) 65 | inline val AngularVelocity.asRPM: Double get() = `in`(RPM) 66 | inline val AngularVelocity.asRadiansPerSecond: Double get() = `in`(RadiansPerSecond) 67 | 68 | inline val Angle.perSecond: AngularVelocity get() = DegreesPerSecond.of(this.asDegrees) 69 | 70 | inline val Double.degreesPerSecond: AngularVelocity get() = DegreesPerSecond.of(this) 71 | inline val Double.rotationsPerSecond: AngularVelocity get() = RotationsPerSecond.of(this) 72 | inline val Double.rpm: AngularVelocity get() = RPM.of(this) 73 | inline val Double.radiansPerSecond: AngularVelocity get() = RadiansPerSecond.of(this) 74 | 75 | 76 | //Linear Acceleration 77 | inline val Measure.asInchesPerSecondPerSecond: Double get() = `in`(InchesPerSecond.per(Second)) 78 | inline val Measure.asFeetPerSecondPerSecond: Double get() = `in`(FeetPerSecondPerSecond) 79 | inline val Measure.asMetersPerSecondPerSecond: Double get() = `in`(MetersPerSecondPerSecond) 80 | inline val Measure.asGs: Double get() = `in`(Gs) 81 | 82 | inline val Measure.perSecondPerSecond: LinearAcceleration get() = FeetPerSecondPerSecond.of(this.asFeet) 83 | inline val Measure.perSecond: LinearAcceleration get() = FeetPerSecondPerSecond.of(this.asFeetPerSecond) 84 | 85 | inline val Double.inchesPerSecondPerSecond: LinearAcceleration get() = InchesPerSecond.per(Second).of(this) 86 | inline val Double.feetPerSecondPerSecond: LinearAcceleration get() = FeetPerSecondPerSecond.of(this) 87 | inline val Double.metersPerSecondPerSecond: LinearAcceleration get() = MetersPerSecondPerSecond.of(this) 88 | inline val Double.Gs: LinearAcceleration get() = Units.Gs.of(this) 89 | 90 | //Linear Jerk 91 | inline val Measure.perSecond: Velocity get() = MetersPerSecondPerSecond.per(Second).of(this.asMetersPerSecondPerSecond) 92 | 93 | inline val Measure>.asInchesJerk: Double get() = `in`(MetersPerSecondPerSecond.per(Second)) 94 | inline val Measure>.asFeetJerk: Double get() = `in`(InchesPerSecond.per(Second).per(Second)) 95 | inline val Measure>.asMetersJerk: Double get() = `in`(FeetPerSecondPerSecond.per(Second)) 96 | 97 | 98 | //Angular Acceleration 99 | inline val AngularAcceleration.asDegreesPerSecondPerSecond: Double get() = `in`(DegreesPerSecondPerSecond) 100 | inline val AngularAcceleration.asRotationsPerSecondPerSecond: Double get() = `in`(RotationsPerSecondPerSecond) 101 | inline val AngularAcceleration.asRadiansPerSecondPerSecond: Double get() = `in`(RadiansPerSecondPerSecond) 102 | 103 | inline val Angle.perSecondPerSecond: AngularAcceleration get() = DegreesPerSecondPerSecond.of(this.asDegrees) 104 | inline val AngularVelocity.perSecond: AngularAcceleration get() = DegreesPerSecondPerSecond.of(this.asDegreesPerSecond) 105 | 106 | inline val Double.degreesPerSecondPerSecond: AngularAcceleration get() = DegreesPerSecondPerSecond.of(this) 107 | inline val Double.rotationsPerSecondPerSecond: AngularAcceleration get() = RotationsPerSecondPerSecond.of(this) 108 | inline val Double.radiansPerSecondPerSecond: AngularAcceleration get() = RadiansPerSecondPerSecond.of(this) 109 | 110 | 111 | //Mass 112 | inline val Mass.asKilograms: Double get() = `in`(Kilograms) 113 | inline val Mass.asGrams: Double get() = `in`(Grams) 114 | inline val Mass.asPounds: Double get() = `in`(Pounds) 115 | inline val Mass.asOunces: Double get() = `in`(Ounces) 116 | 117 | inline val Double.kilograms: Mass get() = Kilograms.of(this) 118 | inline val Double.grams: Mass get() = Grams.of(this) 119 | inline val Double.pounds: Mass get() = Pounds.of(this) 120 | inline val Double.ounces: Mass get() = Ounces.of(this) 121 | 122 | 123 | //The Force 124 | inline val Force.asNewtons: Double get() = `in`(Newtons) 125 | inline val Force.asOuncesForce: Double get() = `in`(OuncesForce) 126 | inline val Force.asPoundsForce: Double get() = `in`(PoundsForce) 127 | 128 | inline val Double.newtons: Force get() = Newtons.of(this) 129 | inline val Double.ouncesForce: Force get() = OuncesForce.of(this) 130 | inline val Double.poundsForce: Force get() = PoundsForce.of(this) 131 | 132 | 133 | //Torque 134 | inline val Torque.asNewtonMeters: Double get() = `in`(NewtonMeters) 135 | inline val Torque.asPoundFeet: Double get() = `in`(PoundFeet) 136 | inline val Torque.asOunceInches: Double get() = `in`(OunceInches) 137 | 138 | inline val Double.newtonMeters: Torque get() = NewtonMeters.of(this) 139 | inline val Double.poundFeet: Torque get() = PoundFeet.of(this) 140 | inline val Double.ounceInches: Torque get() = OunceInches.of(this) 141 | 142 | 143 | //MOI 144 | inline val Double.kilogramSquareMeters: MomentOfInertia get() = KilogramSquareMeters.of(this) 145 | 146 | inline val MomentOfInertia.asKilogramSquareMeters: Double get() = `in`(KilogramSquareMeters) 147 | 148 | 149 | //Voltage 150 | inline val Double.volts: Voltage get() = Volts.of(this) 151 | 152 | inline val Voltage.asVolts: Double get() = `in`(Volts) 153 | 154 | 155 | //Current 156 | inline val Current.asAmps: Double get() = `in`(Amps) 157 | 158 | inline val Double.amps: Current get() = Amps.of(this) 159 | 160 | //Temperature 161 | inline val Double.celsius: Temperature get() = Celsius.of(this) 162 | inline val Double.fahrenheit: Temperature get() = Fahrenheit.of(this) 163 | 164 | inline val Temperature.asCelsius: Double get() = `in`(Celsius) 165 | inline val Temperature.asFahrenheit: Double get() = `in`(Fahrenheit) 166 | 167 | 168 | //Other 169 | inline val Double.voltsPerSecond: Velocity get() = Volts.per(Second).of(this) 170 | 171 | inline val Velocity.asVoltsPerSecond: Double get() = `in`(Volts.per(Second)) 172 | 173 | 174 | //Formulas 175 | fun Measure.toAngular(radius: Measure) = RadiansPerSecond.of(this.asMetersPerSecond / radius.asMeters)!! 176 | fun AngularVelocity.toLinear(radius: Measure) = MetersPerSecond.of(this.asRadiansPerSecond * radius.asMeters)!! 177 | 178 | 179 | /**Converts a [Double] in hertz into an equivalent [Time] unit.*/ 180 | fun Double.hertzToTime() = if (this == 0.0) 0.0.seconds else (1.0 / this).seconds 181 | 182 | @JvmName("sinOf") 183 | fun sin(angle: Angle) = sin(angle.asRadians) 184 | @JvmName("cosOf") 185 | fun cos(angle: Angle) = cos(angle.asRadians) 186 | @JvmName("tanOf") 187 | fun tan(angle: Angle) = tan(angle.asRadians) 188 | fun Angle.sin() = sin(this) 189 | fun Angle.cos() = cos(this) 190 | fun Angle.tan() = tan(this) 191 | 192 | fun asin(value: Double) = kotlin.math.asin(value).radians 193 | fun acos(value: Double) = kotlin.math.acos(value).radians 194 | fun atan(value: Double) = kotlin.math.atan(value).radians 195 | 196 | fun atan2(y: Measure, x: Measure) = atan2(y.asInches, x.asInches).radians 197 | 198 | fun Angle.wrap() = asDegrees.IEEErem(360.0).degrees 199 | fun Angle.unWrap(nearByAngle: Angle): Angle = nearByAngle + (this - nearByAngle).wrap() 200 | 201 | fun Rotation2d.wrap() = measure.wrap().asRotation2d 202 | fun Rotation2d.unWrap(nearByAngle: Angle) = measure.unWrap(nearByAngle).asRotation2d 203 | 204 | fun Angle.absoluteValue() = asDegrees.absoluteValue.degrees 205 | fun Rotation2d.absoluteValue() = degrees.absoluteValue.degrees.asRotation2d 206 | fun Measure.absoluteValue() = asFeet.absoluteValue.feet 207 | fun AngularVelocity.absoluteValue() = asDegreesPerSecond.absoluteValue.degreesPerSecond 208 | fun Measure.absoluteValue() = asFeetPerSecond.absoluteValue.feetPerSecond 209 | fun AngularAcceleration.absoluteValue() = asDegreesPerSecondPerSecond.absoluteValue.degreesPerSecondPerSecond 210 | fun Measure.absoluteValue() = asFeetPerSecondPerSecond.absoluteValue.feetPerSecondPerSecond 211 | 212 | //String 213 | fun Angle.toReadableString() = "$asDegrees degrees" 214 | @JvmName("DistanceToReadableString") 215 | fun Measure.toReadableString() = "$asFeet feet" 216 | fun AngularVelocity.toReadableString() = "$asDegreesPerSecond degrees/second" 217 | @JvmName("LinearVelocityToReadableString") 218 | fun Measure.toReadableString() = "$asFeetPerSecond feet/second" 219 | fun AngularAcceleration.toReadableString() = "$asDegreesPerSecondPerSecond degrees/second^2" 220 | @JvmName("LinearAccelerationToReadableString") 221 | fun Measure.toReadableString() = "$asFeetPerSecondPerSecond feet/second^2" 222 | fun Time.toReadableString() = "$asSeconds seconds" 223 | fun Voltage.toReadableString() = "$asVolts volts" 224 | 225 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/ctre/TalonFXUtil.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.ctre 2 | 3 | import com.ctre.phoenix6.configs.MotionMagicConfigs 4 | import com.ctre.phoenix6.configs.TalonFXConfiguration 5 | import com.ctre.phoenix6.controls.Follower 6 | import com.ctre.phoenix6.hardware.TalonFX 7 | import com.ctre.phoenix6.signals.FeedbackSensorSourceValue 8 | import com.ctre.phoenix6.signals.GravityTypeValue 9 | import com.ctre.phoenix6.signals.InvertedValue 10 | import com.ctre.phoenix6.signals.NeutralModeValue 11 | import com.ctre.phoenix6.signals.StaticFeedforwardSignValue 12 | import edu.wpi.first.wpilibj.DriverStation 13 | 14 | /** 15 | * Add a follower to the main motor and applies the master's configuration. 16 | * 17 | * Make sure to call this function AFTER configuring the master motor. 18 | * If the master motor's configuration changes after this function is called, the follower configuration will NOT update to match the master motor. 19 | * 20 | * @param followerID The CAN ID of a [TalonFX] follower motor. 21 | * @param opposeMasterDirection Whether to respect the master motor's invert setting or do the opposite. 22 | * 23 | * @see Follower 24 | */ 25 | fun TalonFX.addFollower(followerID: Int, opposeMasterDirection: Boolean = false) { 26 | try { 27 | val follower = TalonFX(followerID, network) 28 | val masterConfig = TalonFXConfiguration() 29 | this.configurator.refresh(masterConfig) 30 | follower.configurator.apply(masterConfig) 31 | follower.setControl(Follower(deviceID, opposeMasterDirection)) 32 | } catch (e: Exception) { 33 | DriverStation.reportError("Failed to add follower to $deviceID: ${e.message}", true) 34 | } 35 | } 36 | 37 | /** 38 | * Add a follower to the main motor and applies the master's configuration. 39 | * 40 | * Make sure to call this function AFTER configuring the master motor. 41 | * If the master motor's configuration changes after this function is called, the follower configuration will NOT update to match the master motor. 42 | * 43 | * @param follower The follower motor. 44 | * @param opposeMasterDirection Whether to respect the master motor's invert setting or do the opposite. 45 | * 46 | * @see Follower 47 | */ 48 | fun TalonFX.addFollower(follower: TalonFX, opposeMasterDirection: Boolean) = this.addFollower(follower.deviceID, opposeMasterDirection) 49 | 50 | /** 51 | * Set the supply current limits. 52 | * @param continuousLimit the continuous allowable limit 53 | * @param peakLimit the maximum possible current the motor can draw. 54 | * @param peakDuration amount of seconds the motor limits to [peakLimit], then it will limit to [continuousLimit] 55 | */ 56 | fun TalonFXConfiguration.currentLimits(continuousLimit: Double, peakLimit: Double, peakDuration: Double): TalonFXConfiguration { 57 | this.CurrentLimits.apply { 58 | SupplyCurrentLimit = peakLimit 59 | SupplyCurrentLowerLimit = continuousLimit 60 | SupplyCurrentLowerTime = peakDuration 61 | SupplyCurrentLimitEnable = true 62 | } 63 | return this 64 | } 65 | 66 | /** 67 | * Set the stator current limits. 68 | * @param peakLimit the maximum possible current the motor can draw. 69 | */ 70 | fun TalonFXConfiguration.statorCurrentLimit(peakLimit: Double): TalonFXConfiguration { 71 | this.CurrentLimits.apply { 72 | StatorCurrentLimit = peakLimit 73 | StatorCurrentLimitEnable = true 74 | } 75 | return this 76 | } 77 | 78 | /** 79 | * Motor will update its position and velocity whenever the CANcoder publishes its information on the CAN bus. 80 | * The motor's internal rotor will not be used. 81 | * 82 | * @param encoderID CAN ID of the CANcoder. 83 | * @param motorToSensorRatio number of motor rotations for 1 CANcoder rotation. 84 | * @param sensorToMechanismRatio number of sensor rotations for 1 mechanism rotation. 85 | * 86 | * @see FeedbackSensorSourceValue.RemoteCANcoder 87 | * @see fusedCANCoder 88 | * @see alternateFeedbackSensor 89 | * 90 | * @author Justin likes "Remote" better than "Fused." Test both but start with RemoteCANCoder. 91 | */ 92 | fun TalonFXConfiguration.remoteCANCoder(encoderID: Int, motorToSensorRatio: Double, sensorToMechanismRatio: Double = 1.0): TalonFXConfiguration = 93 | alternateFeedbackSensor(encoderID, FeedbackSensorSourceValue.RemoteCANcoder, motorToSensorRatio, sensorToMechanismRatio) 94 | 95 | /** 96 | * Motor will fuse its position and velocity with another CANcoder. Slow speed will use CANcoder, fast speed will use motor rotor. 97 | * 98 | * Make sure the motor and encoder move in the same direction. 99 | * 100 | * @param encoderID CAN ID of the CANcoder. 101 | * @param motorToSensorRatio number of motor rotations for 1 CANcoder rotation. 102 | * @param sensorToMechanismRatio number of sensor rotations for 1 mechanism rotation. 103 | * 104 | * @see FeedbackSensorSourceValue.FusedCANcoder 105 | * @see remoteCANCoder 106 | * @see alternateFeedbackSensor 107 | * 108 | * @author Justin likes "Remote" better than "Fused." Test both but start with RemoteCANCoder. 109 | */ 110 | fun TalonFXConfiguration.fusedCANCoder(encoderID: Int, motorToSensorRatio: Double, sensorToMechanismRatio: Double = 1.0): TalonFXConfiguration = 111 | alternateFeedbackSensor(encoderID, FeedbackSensorSourceValue.FusedCANcoder, motorToSensorRatio, sensorToMechanismRatio) 112 | 113 | /** 114 | * Sets the configs that affect the feedback of this motor. Aka: What it will think its own position/velocity is. 115 | * Useful for eliminating control error between the motor and the mechanism. 116 | * 117 | * This will automatically apply any gear ratio you put in, causing the motor to be in the "mechanism perspective" 118 | * 119 | * @param encoderID CAN ID of the feedback device. 120 | * @param feedbackSensorSource the type of feedback device. 121 | * @param motorToSensorRatio number of motor rotations for 1 feedback device rotation. 122 | * @param sensorToMechanismRatio number of sensor rotations for 1 mechanism rotation. 123 | * 124 | * @see FeedbackSensorSourceValue 125 | * @see TalonFXConfiguration.Feedback 126 | */ 127 | fun TalonFXConfiguration.alternateFeedbackSensor(encoderID: Int, feedbackSensorSource: FeedbackSensorSourceValue, motorToSensorRatio: Double, sensorToMechanismRatio: Double = 1.0): TalonFXConfiguration { 128 | this.Feedback.apply { 129 | FeedbackSensorSource = feedbackSensorSource 130 | FeedbackRemoteSensorID = encoderID 131 | RotorToSensorRatio = motorToSensorRatio 132 | SensorToMechanismRatio = sensorToMechanismRatio 133 | } 134 | return this 135 | } 136 | 137 | /** 138 | * Set whether the motor should be inverted. 139 | * 140 | * True - Clockwise_Positive 141 | * 142 | * False - CounterClockwise_Positive (Factory Default) 143 | * 144 | * @param invert whether to invert the motor 145 | * 146 | * @see InvertedValue.CounterClockwise_Positive 147 | * @see InvertedValue.Clockwise_Positive 148 | */ 149 | fun TalonFXConfiguration.inverted(invert: Boolean): TalonFXConfiguration { 150 | this.MotorOutput.Inverted = if (invert) InvertedValue.Clockwise_Positive else InvertedValue.CounterClockwise_Positive 151 | return this 152 | } 153 | 154 | /** 155 | * Set motor neutral mode to brake. 156 | */ 157 | fun TalonFXConfiguration.brakeMode(): TalonFXConfiguration { 158 | this.MotorOutput.NeutralMode = NeutralModeValue.Brake 159 | return this 160 | } 161 | 162 | /** 163 | * Set motor neutral mode to coast. 164 | */ 165 | fun TalonFXConfiguration.coastMode(): TalonFXConfiguration { 166 | this.MotorOutput.NeutralMode = NeutralModeValue.Coast 167 | return this 168 | } 169 | 170 | /** 171 | * Set the proportional gain. 172 | * 173 | * @see TalonFXConfiguration.Slot0 174 | */ 175 | fun TalonFXConfiguration.p(p: Double): TalonFXConfiguration { 176 | this.Slot0.kP = p 177 | return this 178 | } 179 | 180 | /** 181 | * Set the derivative gain. 182 | * 183 | * @see TalonFXConfiguration.Slot0 184 | */ 185 | fun TalonFXConfiguration.d(d: Double): TalonFXConfiguration { 186 | this.Slot0.kD = d 187 | return this 188 | } 189 | 190 | /** 191 | * Set the integral gain. 192 | * 193 | * @see TalonFXConfiguration.Slot0 194 | */ 195 | fun TalonFXConfiguration.i(i: Double): TalonFXConfiguration { 196 | this.Slot0.kI = i 197 | return this 198 | } 199 | 200 | /** 201 | * Set the static feedforward gain. 202 | * 203 | * @see StaticFeedforwardSignValue.UseClosedLoopSign 204 | * @see StaticFeedforwardSignValue.UseVelocitySign 205 | */ 206 | fun TalonFXConfiguration.s(s: Double, staticFeedforwardSign: StaticFeedforwardSignValue): TalonFXConfiguration { 207 | this.Slot0.apply { 208 | kS = s 209 | StaticFeedforwardSign = staticFeedforwardSign 210 | } 211 | return this 212 | } 213 | 214 | /** 215 | * Set the velocity feedforward gain. 216 | * 217 | * @see TalonFXConfiguration.Slot0 218 | */ 219 | fun TalonFXConfiguration.v(v: Double): TalonFXConfiguration { 220 | this.Slot0.kV = v 221 | return this 222 | } 223 | 224 | /** 225 | * Set the acceleration feedforward gain. 226 | * 227 | * @see TalonFXConfiguration.Slot0 228 | */ 229 | fun TalonFXConfiguration.a(a: Double): TalonFXConfiguration { 230 | this.Slot0.kA = a 231 | return this 232 | } 233 | 234 | /** 235 | * Set the gravity feedforward/feedback gain. 236 | * 237 | * @see GravityTypeValue.Elevator_Static 238 | * @see GravityTypeValue.Arm_Cosine 239 | */ 240 | fun TalonFXConfiguration.g(g: Double, gravityType: GravityTypeValue): TalonFXConfiguration { 241 | this.Slot0.apply { 242 | kG = g 243 | GravityType = gravityType 244 | } 245 | return this 246 | } 247 | 248 | /** 249 | * Configure the motion magic cruse velocity, acceleration, and optional jerk. 250 | * 251 | * @see TalonFXConfiguration.MotionMagic 252 | */ 253 | fun TalonFXConfiguration.motionMagic(cruseVelocity: Double, acceleration: Double, jerk: Double? = null): TalonFXConfiguration { 254 | this.MotionMagic.apply { 255 | MotionMagicCruiseVelocity = cruseVelocity 256 | MotionMagicAcceleration = acceleration 257 | if (jerk != null) MotionMagicJerk = jerk 258 | } 259 | return this 260 | } 261 | 262 | /** 263 | * Configure the motion magic expo configs. 264 | * 265 | * @see TalonFXConfiguration.MotionMagic 266 | * @see MotionMagicConfigs.MotionMagicExpo_kV 267 | * @see MotionMagicConfigs.MotionMagicExpo_kA 268 | * @see MotionMagicConfigs.MotionMagicCruiseVelocity 269 | */ 270 | fun TalonFXConfiguration.motionMagicExpo(expoKV: Double, expoKA: Double, maxVelocity: Double? = null): TalonFXConfiguration { 271 | this.MotionMagic.apply { 272 | MotionMagicExpo_kV = expoKV 273 | MotionMagicExpo_kA = expoKA 274 | if (maxVelocity != null) MotionMagicCruiseVelocity = maxVelocity 275 | } 276 | return this 277 | } 278 | 279 | /** 280 | * Applies a factory default configuration to the [TalonFX]. 281 | * 282 | * @param modifications optionally provide a block to modify the configuration before it gets sent to the motor. 283 | * 284 | * @see modifyConfiguration 285 | */ 286 | fun TalonFX.applyConfiguration(modifications: TalonFXConfiguration.() -> Unit = {}) { 287 | // Create a factory default configuration, apply modifications, then apply to the motor. 288 | this.configurator.apply(TalonFXConfiguration().apply { modifications() }) 289 | } 290 | 291 | /** 292 | * Modifies the configuration currently on the motor. 293 | * 294 | * @param overrides provide a block to modify the configuration before it gets sent to the device. 295 | * 296 | * @see applyConfiguration 297 | */ 298 | fun TalonFX.modifyConfiguration(overrides: TalonFXConfiguration.() -> Unit) { 299 | // Get the current motor configuration, apply modifications, then apply to the motor. 300 | val oldConfiguration = TalonFXConfiguration() 301 | this.configurator.refresh(oldConfiguration) // Get motor configuration parameters 302 | this.configurator.apply(oldConfiguration.apply(overrides)) // Apply overrides to the config and send config to motor. 303 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/control/commands/CommandUtils.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.control.commands 2 | 3 | import edu.wpi.first.units.measure.Time 4 | import edu.wpi.first.wpilibj2.command.Command 5 | import edu.wpi.first.wpilibj2.command.Commands 6 | import edu.wpi.first.wpilibj2.command.DeferredCommand 7 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup 8 | import edu.wpi.first.wpilibj2.command.Subsystem 9 | import edu.wpi.first.wpilibj2.command.WrapperCommand 10 | 11 | /** 12 | * Turns the function into a runOnce Command 13 | * 14 | * @see Commands.runOnce 15 | */ 16 | fun (() -> Unit).toCommand(vararg requirements: Subsystem): Command = runOnceCommand(*requirements) { this() } 17 | 18 | fun printlnCommand(message: String) = Commands.print(message) 19 | /** 20 | * Decorates this command with a lambda to call on interrupt or end, following the command's inherent end(boolean) method. 21 | * @param run A lambda (function) accepting a boolean parameter specifying whether the command was interrupted. 22 | * @return: the decorated command 23 | */ 24 | fun Command.finallyRun(run: (Boolean) -> Unit): WrapperCommand = 25 | this.finallyDo { interrupted -> run(interrupted)} 26 | 27 | /** 28 | * Decorates this command to schedule another command on interrupt or end, following the command's inherent end(boolean) method. 29 | * @param command A command to run after the main command has been interrupted or finished 30 | * @return: the decorated command 31 | */ 32 | fun Command.finallyRun(command: Command): Command = this.finallyRun { command.schedule() } 33 | 34 | /** 35 | * After the initial command finishes, wait [seconds] more than finish. 36 | * @param seconds The amount of time to wait after the initial command has finished 37 | * @see Command.andThen 38 | * @see Commands.waitSeconds 39 | */ 40 | fun Command.finallyWait(seconds: Double) = this.andThen(waitCommand(seconds))!! 41 | 42 | /** 43 | * After the initial command finishes, wait until a condition becomes true. 44 | * @param condition the condition 45 | * @see Command.andThen 46 | * @see Commands.waitUntil 47 | */ 48 | fun Command.finallyWaitUntil(condition: () -> Boolean) = this.andThen(waitUntilCommand(condition))!! 49 | 50 | /** 51 | * Only schedule and continue running the command if [condition] is true. 52 | * 53 | * Combines onlyIf and onlyWhile and will only start the command if the [condition] is true 54 | * and will stop the command when the [condition] returns false. 55 | * 56 | * @param condition a lambda (function) that should return true if the function should continue running. 57 | * @see Command.onlyIf 58 | * @see Command.onlyWhile 59 | * @see onlyRunWhileFalse 60 | */ 61 | fun Command.onlyRunWhileTrue(condition: () -> Boolean) = this.onlyWhile(condition).onlyIf(condition)!! 62 | 63 | /** 64 | * Only schedule and continue running the command if [condition] is false. 65 | * 66 | * Combines until and unless and will only start the command if the [condition] is false 67 | * and will stop the command when the [condition] returns true. 68 | * 69 | * @param condition a lambda (function) that should return true if the function should stop. 70 | * @see Command.unless 71 | * @see Command.until 72 | * @see onlyRunWhileTrue 73 | */ 74 | fun Command.onlyRunWhileFalse(condition: () -> Boolean) = this.until(condition).unless(condition)!! 75 | 76 | /** 77 | * Before the initial command starts, wait [seconds] more than start. 78 | * @param seconds The amount of time to wait before the initial command starts 79 | * @see Command.beforeStarting 80 | * @see Commands.waitSeconds 81 | */ 82 | fun Command.beforeWait(seconds: Double) = this.beforeStarting(waitCommand(seconds))!! 83 | 84 | /** 85 | * Before the initial command starts, wait until [condition] returns true. 86 | * @param condition A lambda (function) returning if the command should start 87 | * @see Command.beforeStarting 88 | * @see Commands.waitUntil 89 | */ 90 | fun Command.beforeWaitUntil(condition: () -> Boolean) = this.beforeStarting(waitUntilCommand(condition))!! 91 | 92 | /** 93 | * Before the command starts, run this [action] first. 94 | * @param action the action to run 95 | * @param requirements subsystems the action requires 96 | * @return the command 97 | * @see Command.beforeStarting 98 | */ 99 | fun Command.beforeRun(vararg requirements: Subsystem, action: () -> Unit): SequentialCommandGroup = 100 | this.beforeStarting(action, *requirements) 101 | 102 | 103 | /** 104 | * Constructs a command that runs an action once and finishes. 105 | * @param action the action to run 106 | * @param requirements subsystems the action requires 107 | * @return the command 108 | * @see edu.wpi.first.wpilibj2.command.InstantCommand 109 | */ 110 | fun runOnceCommand(vararg requirements: Subsystem, action: () -> Unit): Command = Commands.runOnce(action, *requirements) 111 | 112 | /** 113 | * Constructs a command that runs an action once and finishes. 114 | * @param action the action to run 115 | * @param requirements subsystems the action requires 116 | * @return the command 117 | * @see edu.wpi.first.wpilibj2.command.InstantCommand 118 | */ 119 | fun runOnce(vararg requirements: Subsystem, action: () -> Unit): Command = runOnceCommand(*requirements, action = action) 120 | 121 | /** 122 | * Constructs a command that runs an action every iteration until interrupted. 123 | * @param action the action to run 124 | * @param requirements subsystems the action requires 125 | * @return the command 126 | * @see edu.wpi.first.wpilibj2.command.RunCommand 127 | */ 128 | fun runCommand(vararg requirements: Subsystem, action: () -> Unit): Command = Commands.run(action, *requirements) 129 | 130 | /** 131 | * Runs a group of commands in series, one after the other. 132 | * @param commands the commands to include 133 | * @return the command group 134 | * @see SequentialCommandGroup 135 | */ 136 | fun sequenceCommand(vararg commands: Command): Command = Commands.sequence(*commands) 137 | 138 | /** 139 | * Runs a group of commands in series, one after the other. Once the last command ends, the group is restarted. 140 | * @param commands the commands to include 141 | * @return the command group 142 | * @see SequentialCommandGroup, 143 | * @see Command.repeatedly() 144 | */ 145 | fun repeatingSequenceCommand(vararg commands: Command): Command = Commands.repeatingSequence(*commands) 146 | 147 | /** 148 | * Runs a group of commands at the same time. Ends once all commands in the group finish. 149 | * @param commands the commands to include 150 | * @return the command 151 | * @see edu.wpi.first.wpilibj2.command.ParallelCommandGroup 152 | */ 153 | fun parallelCommand(vararg commands: Command): Command = Commands.parallel(*commands) 154 | 155 | /** 156 | * Constructs a command that does nothing, finishing after a specified duration. 157 | * @param seconds after how long the command finishes 158 | * @return the command 159 | * @see edu.wpi.first.wpilibj2.command.WaitCommand 160 | */ 161 | fun waitCommand(seconds: Double): Command = Commands.waitSeconds(seconds) 162 | 163 | /** 164 | * Constructs a command that does nothing, finishing after a specified duration. 165 | * @param time after how long the command finishes 166 | * @return the command 167 | * @see edu.wpi.first.wpilibj2.command.WaitCommand 168 | */ 169 | fun waitCommand(time: Time): Command = Commands.waitTime(time) 170 | 171 | /** 172 | * Constructs a command that does nothing, finishing once a condition becomes true. 173 | * @param condition the condition 174 | * @return the command 175 | * @see edu.wpi.first.wpilibj2.command.WaitUntilCommand 176 | */ 177 | fun waitUntilCommand(condition: () -> Boolean): Command = Commands.waitUntil(condition) 178 | 179 | /** 180 | * Constructs a command that does nothing, finishing once a condition becomes true. 181 | * @param condition the condition 182 | * @param overrideSeconds the maximum time to wait for the condition to become true 183 | * @return the command 184 | * @see WaitUntilOrTimeCommand 185 | */ 186 | fun waitUntilCommand(overrideSeconds: Double, condition: () -> Boolean): Command = 187 | WaitUntilOrTimeCommand(overrideSeconds, condition) 188 | 189 | /** 190 | * Constructs a command that does nothing until interrupted. 191 | * @param requirements Subsystems to require 192 | * @return the command 193 | */ 194 | fun idleCommand(vararg requirements: Subsystem) = Commands.idle(*requirements)!! 195 | 196 | /** 197 | * Runs a group of commands at the same time. Ends once a specific command finishes, and cancels the others. 198 | * @param deadline the deadline command 199 | * @param otherCommands the other commands to include 200 | * @return the command group 201 | * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument 202 | * @see edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup 203 | */ 204 | fun deadlineCommand(deadline: Command, vararg otherCommands: Command ): Command = Commands.deadline(deadline, *otherCommands) 205 | /** 206 | * Runs a group of commands at the same time. Ends once any command in the group finishes, and cancels the others. 207 | * @param commands the commands to include 208 | * @return the command group 209 | * @see edu.wpi.first.wpilibj2.command.ParallelRaceGroup 210 | */ 211 | fun raceCommand(vararg commands: Command): Command = Commands.race(*commands) 212 | 213 | /** 214 | * Runs one of two commands, based on the boolean selector function. 215 | * @param onTrue the command to run if the selector function returns true 216 | * @param onFalse the command to run if the selector function returns false 217 | * @param selector the selector function 218 | * @return the command 219 | * @see edu.wpi.first.wpilibj2.command.ConditionalCommand 220 | */ 221 | fun eitherCommand(onTrue: Command, onFalse: Command, selector: () -> Boolean): Command = Commands.either(onTrue, onFalse, selector) 222 | 223 | /** 224 | * Constructs a command that runs an action once, and then runs an action every iteration until interrupted. 225 | * @param start the action to run on start 226 | * @param run the action to run every iteration 227 | * @param requirements subsystems the action requires 228 | * @return the command 229 | */ 230 | fun startRunCommand(vararg requirements: Subsystem, start: () -> Unit, run: () -> Unit): Command = Commands.startRun(start, run, *requirements) 231 | 232 | /** 233 | * Constructs a command that runs an action once and another action when the command is interrupted. 234 | * @param start the action to run on start 235 | * @param end the action to run on interrupt 236 | * @param requirements subsystems the action requires 237 | * @return the command 238 | * @see edu.wpi.first.wpilibj2.command.StartEndCommand 239 | */ 240 | fun startEndCommand(vararg requirements: Subsystem, start: () -> Unit, end: () -> Unit): Command = Commands.startEnd(start, end, *requirements) 241 | 242 | /** 243 | * Constructs a command that runs an action every iteration until interrupted, and then runs a second action. 244 | * @param run the action to run every iteration 245 | * @param end the action to run on interrupt 246 | * @param requirements subsystems the action requires 247 | * @return the command 248 | */ 249 | fun runEndCommand(vararg requirements: Subsystem, run: () -> Unit, end: () -> Unit): Command = Commands.runEnd(run, end, *requirements) 250 | 251 | /** 252 | * Runs one of several commands, based on the selector function. 253 | * @param commands map of commands to select from 254 | * @param selector the selector function 255 | * @param K The type of key used to select the command 256 | * @return the command 257 | * @see edu.wpi.first.wpilibj2.command.SelectCommand 258 | */ 259 | fun selectCommand(commands: Map, selector: () -> K): Command = Commands.select(commands, selector) 260 | 261 | /** 262 | * Creates [DeferredCommand], a command that gets constructed at runtime. 263 | * 264 | * The most similar type of command to 2025 Meanlib's "use" function. 265 | * 266 | * @param supplier a function that returns the command to run 267 | * @param requirements the subsystems required by the command 268 | * 269 | * @see DeferredCommand 270 | */ 271 | fun deferCommand(vararg requirements: Subsystem, supplier: () -> Command): Command = Commands.defer(supplier, mutableSetOf(*requirements)) 272 | 273 | /** 274 | * Creates [DeferredCommand], a command that gets constructed at runtime. 275 | * 276 | * The most similar type of command to 2025 Meanlib's "use" function. 277 | * 278 | * @param supplier a function that returns the command to run 279 | * @param requirements the subsystems required by the command 280 | * 281 | * @see DeferredCommand 282 | */ 283 | fun use(vararg requirements: Subsystem, supplier: () -> Command): Command = deferCommand(*requirements, supplier = supplier) 284 | -------------------------------------------------------------------------------- /vendordeps/Phoenix6.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "Phoenix6-frc2025-latest.json", 3 | "name": "CTRE-Phoenix (v6)", 4 | "version": "25.4.0", 5 | "frcYear": "2025", 6 | "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", 7 | "mavenUrls": [ 8 | "https://maven.ctr-electronics.com/release/" 9 | ], 10 | "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", 11 | "conflictsWith": [ 12 | { 13 | "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", 14 | "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", 15 | "offlineFileName": "Phoenix6-replay-frc2025-latest.json" 16 | } 17 | ], 18 | "javaDependencies": [ 19 | { 20 | "groupId": "com.ctre.phoenix6", 21 | "artifactId": "wpiapi-java", 22 | "version": "25.4.0" 23 | } 24 | ], 25 | "jniDependencies": [ 26 | { 27 | "groupId": "com.ctre.phoenix6", 28 | "artifactId": "api-cpp", 29 | "version": "25.4.0", 30 | "isJar": false, 31 | "skipInvalidPlatforms": true, 32 | "validPlatforms": [ 33 | "windowsx86-64", 34 | "linuxx86-64", 35 | "linuxarm64", 36 | "linuxathena" 37 | ], 38 | "simMode": "hwsim" 39 | }, 40 | { 41 | "groupId": "com.ctre.phoenix6", 42 | "artifactId": "tools", 43 | "version": "25.4.0", 44 | "isJar": false, 45 | "skipInvalidPlatforms": true, 46 | "validPlatforms": [ 47 | "windowsx86-64", 48 | "linuxx86-64", 49 | "linuxarm64", 50 | "linuxathena" 51 | ], 52 | "simMode": "hwsim" 53 | }, 54 | { 55 | "groupId": "com.ctre.phoenix6.sim", 56 | "artifactId": "api-cpp-sim", 57 | "version": "25.4.0", 58 | "isJar": false, 59 | "skipInvalidPlatforms": true, 60 | "validPlatforms": [ 61 | "windowsx86-64", 62 | "linuxx86-64", 63 | "linuxarm64", 64 | "osxuniversal" 65 | ], 66 | "simMode": "swsim" 67 | }, 68 | { 69 | "groupId": "com.ctre.phoenix6.sim", 70 | "artifactId": "tools-sim", 71 | "version": "25.4.0", 72 | "isJar": false, 73 | "skipInvalidPlatforms": true, 74 | "validPlatforms": [ 75 | "windowsx86-64", 76 | "linuxx86-64", 77 | "linuxarm64", 78 | "osxuniversal" 79 | ], 80 | "simMode": "swsim" 81 | }, 82 | { 83 | "groupId": "com.ctre.phoenix6.sim", 84 | "artifactId": "simTalonSRX", 85 | "version": "25.4.0", 86 | "isJar": false, 87 | "skipInvalidPlatforms": true, 88 | "validPlatforms": [ 89 | "windowsx86-64", 90 | "linuxx86-64", 91 | "linuxarm64", 92 | "osxuniversal" 93 | ], 94 | "simMode": "swsim" 95 | }, 96 | { 97 | "groupId": "com.ctre.phoenix6.sim", 98 | "artifactId": "simVictorSPX", 99 | "version": "25.4.0", 100 | "isJar": false, 101 | "skipInvalidPlatforms": true, 102 | "validPlatforms": [ 103 | "windowsx86-64", 104 | "linuxx86-64", 105 | "linuxarm64", 106 | "osxuniversal" 107 | ], 108 | "simMode": "swsim" 109 | }, 110 | { 111 | "groupId": "com.ctre.phoenix6.sim", 112 | "artifactId": "simPigeonIMU", 113 | "version": "25.4.0", 114 | "isJar": false, 115 | "skipInvalidPlatforms": true, 116 | "validPlatforms": [ 117 | "windowsx86-64", 118 | "linuxx86-64", 119 | "linuxarm64", 120 | "osxuniversal" 121 | ], 122 | "simMode": "swsim" 123 | }, 124 | { 125 | "groupId": "com.ctre.phoenix6.sim", 126 | "artifactId": "simCANCoder", 127 | "version": "25.4.0", 128 | "isJar": false, 129 | "skipInvalidPlatforms": true, 130 | "validPlatforms": [ 131 | "windowsx86-64", 132 | "linuxx86-64", 133 | "linuxarm64", 134 | "osxuniversal" 135 | ], 136 | "simMode": "swsim" 137 | }, 138 | { 139 | "groupId": "com.ctre.phoenix6.sim", 140 | "artifactId": "simProTalonFX", 141 | "version": "25.4.0", 142 | "isJar": false, 143 | "skipInvalidPlatforms": true, 144 | "validPlatforms": [ 145 | "windowsx86-64", 146 | "linuxx86-64", 147 | "linuxarm64", 148 | "osxuniversal" 149 | ], 150 | "simMode": "swsim" 151 | }, 152 | { 153 | "groupId": "com.ctre.phoenix6.sim", 154 | "artifactId": "simProTalonFXS", 155 | "version": "25.4.0", 156 | "isJar": false, 157 | "skipInvalidPlatforms": true, 158 | "validPlatforms": [ 159 | "windowsx86-64", 160 | "linuxx86-64", 161 | "linuxarm64", 162 | "osxuniversal" 163 | ], 164 | "simMode": "swsim" 165 | }, 166 | { 167 | "groupId": "com.ctre.phoenix6.sim", 168 | "artifactId": "simProCANcoder", 169 | "version": "25.4.0", 170 | "isJar": false, 171 | "skipInvalidPlatforms": true, 172 | "validPlatforms": [ 173 | "windowsx86-64", 174 | "linuxx86-64", 175 | "linuxarm64", 176 | "osxuniversal" 177 | ], 178 | "simMode": "swsim" 179 | }, 180 | { 181 | "groupId": "com.ctre.phoenix6.sim", 182 | "artifactId": "simProPigeon2", 183 | "version": "25.4.0", 184 | "isJar": false, 185 | "skipInvalidPlatforms": true, 186 | "validPlatforms": [ 187 | "windowsx86-64", 188 | "linuxx86-64", 189 | "linuxarm64", 190 | "osxuniversal" 191 | ], 192 | "simMode": "swsim" 193 | }, 194 | { 195 | "groupId": "com.ctre.phoenix6.sim", 196 | "artifactId": "simProCANrange", 197 | "version": "25.4.0", 198 | "isJar": false, 199 | "skipInvalidPlatforms": true, 200 | "validPlatforms": [ 201 | "windowsx86-64", 202 | "linuxx86-64", 203 | "linuxarm64", 204 | "osxuniversal" 205 | ], 206 | "simMode": "swsim" 207 | }, 208 | { 209 | "groupId": "com.ctre.phoenix6.sim", 210 | "artifactId": "simProCANdi", 211 | "version": "25.4.0", 212 | "isJar": false, 213 | "skipInvalidPlatforms": true, 214 | "validPlatforms": [ 215 | "windowsx86-64", 216 | "linuxx86-64", 217 | "linuxarm64", 218 | "osxuniversal" 219 | ], 220 | "simMode": "swsim" 221 | }, 222 | { 223 | "groupId": "com.ctre.phoenix6.sim", 224 | "artifactId": "simProCANdle", 225 | "version": "25.4.0", 226 | "isJar": false, 227 | "skipInvalidPlatforms": true, 228 | "validPlatforms": [ 229 | "windowsx86-64", 230 | "linuxx86-64", 231 | "linuxarm64", 232 | "osxuniversal" 233 | ], 234 | "simMode": "swsim" 235 | } 236 | ], 237 | "cppDependencies": [ 238 | { 239 | "groupId": "com.ctre.phoenix6", 240 | "artifactId": "wpiapi-cpp", 241 | "version": "25.4.0", 242 | "libName": "CTRE_Phoenix6_WPI", 243 | "headerClassifier": "headers", 244 | "sharedLibrary": true, 245 | "skipInvalidPlatforms": true, 246 | "binaryPlatforms": [ 247 | "windowsx86-64", 248 | "linuxx86-64", 249 | "linuxarm64", 250 | "linuxathena" 251 | ], 252 | "simMode": "hwsim" 253 | }, 254 | { 255 | "groupId": "com.ctre.phoenix6", 256 | "artifactId": "tools", 257 | "version": "25.4.0", 258 | "libName": "CTRE_PhoenixTools", 259 | "headerClassifier": "headers", 260 | "sharedLibrary": true, 261 | "skipInvalidPlatforms": true, 262 | "binaryPlatforms": [ 263 | "windowsx86-64", 264 | "linuxx86-64", 265 | "linuxarm64", 266 | "linuxathena" 267 | ], 268 | "simMode": "hwsim" 269 | }, 270 | { 271 | "groupId": "com.ctre.phoenix6.sim", 272 | "artifactId": "wpiapi-cpp-sim", 273 | "version": "25.4.0", 274 | "libName": "CTRE_Phoenix6_WPISim", 275 | "headerClassifier": "headers", 276 | "sharedLibrary": true, 277 | "skipInvalidPlatforms": true, 278 | "binaryPlatforms": [ 279 | "windowsx86-64", 280 | "linuxx86-64", 281 | "linuxarm64", 282 | "osxuniversal" 283 | ], 284 | "simMode": "swsim" 285 | }, 286 | { 287 | "groupId": "com.ctre.phoenix6.sim", 288 | "artifactId": "tools-sim", 289 | "version": "25.4.0", 290 | "libName": "CTRE_PhoenixTools_Sim", 291 | "headerClassifier": "headers", 292 | "sharedLibrary": true, 293 | "skipInvalidPlatforms": true, 294 | "binaryPlatforms": [ 295 | "windowsx86-64", 296 | "linuxx86-64", 297 | "linuxarm64", 298 | "osxuniversal" 299 | ], 300 | "simMode": "swsim" 301 | }, 302 | { 303 | "groupId": "com.ctre.phoenix6.sim", 304 | "artifactId": "simTalonSRX", 305 | "version": "25.4.0", 306 | "libName": "CTRE_SimTalonSRX", 307 | "headerClassifier": "headers", 308 | "sharedLibrary": true, 309 | "skipInvalidPlatforms": true, 310 | "binaryPlatforms": [ 311 | "windowsx86-64", 312 | "linuxx86-64", 313 | "linuxarm64", 314 | "osxuniversal" 315 | ], 316 | "simMode": "swsim" 317 | }, 318 | { 319 | "groupId": "com.ctre.phoenix6.sim", 320 | "artifactId": "simVictorSPX", 321 | "version": "25.4.0", 322 | "libName": "CTRE_SimVictorSPX", 323 | "headerClassifier": "headers", 324 | "sharedLibrary": true, 325 | "skipInvalidPlatforms": true, 326 | "binaryPlatforms": [ 327 | "windowsx86-64", 328 | "linuxx86-64", 329 | "linuxarm64", 330 | "osxuniversal" 331 | ], 332 | "simMode": "swsim" 333 | }, 334 | { 335 | "groupId": "com.ctre.phoenix6.sim", 336 | "artifactId": "simPigeonIMU", 337 | "version": "25.4.0", 338 | "libName": "CTRE_SimPigeonIMU", 339 | "headerClassifier": "headers", 340 | "sharedLibrary": true, 341 | "skipInvalidPlatforms": true, 342 | "binaryPlatforms": [ 343 | "windowsx86-64", 344 | "linuxx86-64", 345 | "linuxarm64", 346 | "osxuniversal" 347 | ], 348 | "simMode": "swsim" 349 | }, 350 | { 351 | "groupId": "com.ctre.phoenix6.sim", 352 | "artifactId": "simCANCoder", 353 | "version": "25.4.0", 354 | "libName": "CTRE_SimCANCoder", 355 | "headerClassifier": "headers", 356 | "sharedLibrary": true, 357 | "skipInvalidPlatforms": true, 358 | "binaryPlatforms": [ 359 | "windowsx86-64", 360 | "linuxx86-64", 361 | "linuxarm64", 362 | "osxuniversal" 363 | ], 364 | "simMode": "swsim" 365 | }, 366 | { 367 | "groupId": "com.ctre.phoenix6.sim", 368 | "artifactId": "simProTalonFX", 369 | "version": "25.4.0", 370 | "libName": "CTRE_SimProTalonFX", 371 | "headerClassifier": "headers", 372 | "sharedLibrary": true, 373 | "skipInvalidPlatforms": true, 374 | "binaryPlatforms": [ 375 | "windowsx86-64", 376 | "linuxx86-64", 377 | "linuxarm64", 378 | "osxuniversal" 379 | ], 380 | "simMode": "swsim" 381 | }, 382 | { 383 | "groupId": "com.ctre.phoenix6.sim", 384 | "artifactId": "simProTalonFXS", 385 | "version": "25.4.0", 386 | "libName": "CTRE_SimProTalonFXS", 387 | "headerClassifier": "headers", 388 | "sharedLibrary": true, 389 | "skipInvalidPlatforms": true, 390 | "binaryPlatforms": [ 391 | "windowsx86-64", 392 | "linuxx86-64", 393 | "linuxarm64", 394 | "osxuniversal" 395 | ], 396 | "simMode": "swsim" 397 | }, 398 | { 399 | "groupId": "com.ctre.phoenix6.sim", 400 | "artifactId": "simProCANcoder", 401 | "version": "25.4.0", 402 | "libName": "CTRE_SimProCANcoder", 403 | "headerClassifier": "headers", 404 | "sharedLibrary": true, 405 | "skipInvalidPlatforms": true, 406 | "binaryPlatforms": [ 407 | "windowsx86-64", 408 | "linuxx86-64", 409 | "linuxarm64", 410 | "osxuniversal" 411 | ], 412 | "simMode": "swsim" 413 | }, 414 | { 415 | "groupId": "com.ctre.phoenix6.sim", 416 | "artifactId": "simProPigeon2", 417 | "version": "25.4.0", 418 | "libName": "CTRE_SimProPigeon2", 419 | "headerClassifier": "headers", 420 | "sharedLibrary": true, 421 | "skipInvalidPlatforms": true, 422 | "binaryPlatforms": [ 423 | "windowsx86-64", 424 | "linuxx86-64", 425 | "linuxarm64", 426 | "osxuniversal" 427 | ], 428 | "simMode": "swsim" 429 | }, 430 | { 431 | "groupId": "com.ctre.phoenix6.sim", 432 | "artifactId": "simProCANrange", 433 | "version": "25.4.0", 434 | "libName": "CTRE_SimProCANrange", 435 | "headerClassifier": "headers", 436 | "sharedLibrary": true, 437 | "skipInvalidPlatforms": true, 438 | "binaryPlatforms": [ 439 | "windowsx86-64", 440 | "linuxx86-64", 441 | "linuxarm64", 442 | "osxuniversal" 443 | ], 444 | "simMode": "swsim" 445 | }, 446 | { 447 | "groupId": "com.ctre.phoenix6.sim", 448 | "artifactId": "simProCANdi", 449 | "version": "25.4.0", 450 | "libName": "CTRE_SimProCANdi", 451 | "headerClassifier": "headers", 452 | "sharedLibrary": true, 453 | "skipInvalidPlatforms": true, 454 | "binaryPlatforms": [ 455 | "windowsx86-64", 456 | "linuxx86-64", 457 | "linuxarm64", 458 | "osxuniversal" 459 | ], 460 | "simMode": "swsim" 461 | }, 462 | { 463 | "groupId": "com.ctre.phoenix6.sim", 464 | "artifactId": "simProCANdle", 465 | "version": "25.4.0", 466 | "libName": "CTRE_SimProCANdle", 467 | "headerClassifier": "headers", 468 | "sharedLibrary": true, 469 | "skipInvalidPlatforms": true, 470 | "binaryPlatforms": [ 471 | "windowsx86-64", 472 | "linuxx86-64", 473 | "linuxarm64", 474 | "osxuniversal" 475 | ], 476 | "simMode": "swsim" 477 | } 478 | ] 479 | } -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/swerve/SwerveSetpointGenerator.kt: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 FRC 6328 2 | // http://github.com/Mechanical-Advantage 3 | // 4 | // Use of this source code is governed by an MIT-style 5 | // license that can be found in the LICENSE file at 6 | // the root directory of this project. 7 | package org.team2471.frc.lib.swerve 8 | 9 | import edu.wpi.first.math.geometry.Rotation2d 10 | import edu.wpi.first.math.geometry.Translation2d 11 | import edu.wpi.first.math.geometry.Twist2d 12 | import edu.wpi.first.math.kinematics.ChassisSpeeds 13 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics 14 | import edu.wpi.first.math.kinematics.SwerveModuleState 15 | import org.team2471.frc.lib.math.epsilonEquals 16 | import kotlin.collections.ArrayList 17 | import kotlin.collections.MutableList 18 | import kotlin.collections.indices 19 | import kotlin.math.* 20 | 21 | /** 22 | * "Inspired" by FRC team 254. See the license file in the root directory of this project. 23 | * 24 | * 25 | * Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path 26 | * follower), and outputs a new setpoint that respects all of the kinematic constraints on module 27 | * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the 28 | * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is 29 | * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). 30 | */ 31 | class SwerveSetpointGenerator(val kinematics: SwerveDriveKinematics, val moduleLocations: Array, val moduleLimits: ModuleLimits) { 32 | 33 | data class SwerveSetpoint(val chassisSpeeds: ChassisSpeeds, val moduleStates: Array) 34 | data class ModuleLimits(val maxDriveVelocity: Double, val maxDriveAcceleration: Double, val maxSteeringVelocity: Double) 35 | 36 | 37 | /** 38 | * Check if it would be faster to go to the opposite of the goal heading (and reverse drive 39 | * direction). 40 | * 41 | * @param prevToGoal The rotation from the previous state to the goal state (i.e. 42 | * prev.inverse().rotateBy(goal)). 43 | * @return True if the shortest path to achieve this rotation involves flipping the drive 44 | * direction. 45 | */ 46 | private fun flipHeading(prevToGoal: Rotation2d): Boolean { 47 | return abs(prevToGoal.radians) > Math.PI / 2.0 48 | } 49 | 50 | private fun unwrapAngle(ref: Double, angle: Double): Double { 51 | val diff = angle - ref 52 | return if (diff > Math.PI) { 53 | angle - 2.0 * Math.PI 54 | } else if (diff < -Math.PI) { 55 | angle + 2.0 * Math.PI 56 | } else { 57 | angle 58 | } 59 | } 60 | 61 | private fun interface Function2d { 62 | fun f(x: Double, y: Double): Double 63 | } 64 | 65 | /** 66 | * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. 67 | * This is a pretty naive way to do root finding, but it's usually faster than simple bisection 68 | * while being robust in ways that e.g. the Newton-Raphson method isn't. 69 | * 70 | * @param func The Function2d to take the root of. 71 | * @param x0 x value of the lower bracket. 72 | * @param y0 y value of the lower bracket. 73 | * @param f0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during 74 | * recursion) 75 | * @param x1 x value of the upper bracket. 76 | * @param y1 y value of the upper bracket. 77 | * @param f1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during 78 | * recursion) 79 | * @param iterationsLeft Number of iterations of root finding left. 80 | * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the 81 | * (approximate) root. 82 | */ 83 | private fun findRoot( 84 | func: Function2d, 85 | x0: Double, 86 | y0: Double, 87 | f0: Double, 88 | x1: Double, 89 | y1: Double, 90 | f1: Double, 91 | iterationsLeft: Int 92 | ): Double { 93 | if (iterationsLeft < 0 || epsilonEquals(f0, f1)) { 94 | return 1.0 95 | } 96 | val sGuess = max(0.0, min(1.0, -f0 / (f1 - f0))) 97 | val xGuess = (x1 - x0) * sGuess + x0 98 | val yGuess = (y1 - y0) * sGuess + y0 99 | val fGuess = func.f(xGuess, yGuess) 100 | return if (sign(f0) == sign(fGuess)) { 101 | // 0 and guess on same side of root, so use upper bracket. 102 | (sGuess + (1.0 - sGuess) * findRoot(func, xGuess, yGuess, fGuess, x1, y1, f1, iterationsLeft - 1)) 103 | } else { 104 | // Use lower bracket. 105 | (sGuess * findRoot(func, x0, y0, f0, xGuess, yGuess, fGuess, iterationsLeft - 1)) 106 | } 107 | } 108 | 109 | private fun findSteeringMaxS( 110 | x0: Double, 111 | y0: Double, 112 | f0: Double, 113 | x1: Double, 114 | y1: Double, 115 | f1: Double, 116 | maxDeviation: Double, 117 | maxIterations: Int 118 | ): Double { 119 | var f1 = f1 120 | f1 = unwrapAngle(f0, f1) 121 | val diff = f1 - f0 122 | if (abs(diff) <= maxDeviation) { 123 | // Can go all the way to s=1. 124 | return 1.0 125 | } 126 | val offset = f0 + sign(diff) * maxDeviation 127 | val func = 128 | Function2d { x: Double, y: Double -> unwrapAngle(f0, atan2(y, x)) - offset } 129 | return findRoot(func, x0, y0, f0 - offset, x1, y1, f1 - offset, maxIterations) 130 | } 131 | 132 | private fun findDriveMaxS( 133 | x0: Double, 134 | y0: Double, 135 | f0: Double, 136 | x1: Double, 137 | y1: Double, 138 | f1: Double, 139 | maxVelStep: Double, 140 | maxIterations: Int 141 | ): Double { 142 | val diff = f1 - f0 143 | if (abs(diff) <= maxVelStep) { 144 | // Can go all the way to s=1. 145 | return 1.0 146 | } 147 | val offset = f0 + sign(diff) * maxVelStep 148 | val func = 149 | Function2d { x: Double, y: Double -> hypot(x, y) - offset } 150 | return findRoot(func, x0, y0, f0 - offset, x1, y1, f1 - offset, maxIterations) 151 | } 152 | 153 | // protected double findDriveMaxS( 154 | // double x_0, double y_0, double x_1, double y_1, double max_vel_step) { 155 | // // Our drive velocity between s=0 and s=1 is quadratic in s: 156 | // // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 157 | // // = a * s^2 + b * s + c 158 | // // Where: 159 | // // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 160 | // // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) 161 | // // c = x_0^2 + y_0^2 162 | // // We want to find where this quadratic results in a velocity that is > max_vel_step from our 163 | // // velocity at s=0: 164 | // // sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... 165 | // final double dx = x_1 - x_0; 166 | // final double dy = y_1 - y_0; 167 | // final double a = dx * dx + dy * dy; 168 | // final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; 169 | // final double c = x_0 * x_0 + y_0 * y_0; 170 | // final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); 171 | // final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); 172 | // return 0.0; 173 | // } 174 | /** 175 | * Generate a new setpoint. 176 | * 177 | // * @param limits The kinematic limits to respect for this setpoint. 178 | * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous 179 | * iteration setpoint instead of the actual measured/estimated kinematic state. 180 | * @param desiredState The desired state of motion, such as from the driver sticks or a path 181 | * following algorithm. 182 | * @param dt The loop time. 183 | * @return A Setpoint object that satisfies all of the KinematicLimits while converging to 184 | * desiredState quickly. 185 | */ 186 | fun generateSetpoint(prevSetpoint: SwerveSetpoint, desiredState: ChassisSpeeds, dt: Double): SwerveSetpoint { 187 | var desiredState: ChassisSpeeds = desiredState 188 | val modules: Array = moduleLocations 189 | 190 | val desiredModuleState: Array = kinematics.toSwerveModuleStates(desiredState) 191 | // Make sure desiredState respects velocity limits. 192 | if (moduleLimits.maxDriveVelocity > 0.0) { 193 | SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, moduleLimits.maxDriveVelocity) 194 | desiredState = kinematics.toChassisSpeeds(*desiredModuleState) 195 | } 196 | 197 | // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so 198 | // just use the previous angle. 199 | var needToSteer = true 200 | if (desiredState.toTwist2d(0.02).epsilonEquals(Twist2d())) { 201 | needToSteer = false 202 | for (i in modules.indices) { 203 | desiredModuleState[i].angle = prevSetpoint.moduleStates[i].angle 204 | desiredModuleState[i].speedMetersPerSecond = 0.0 205 | } 206 | } 207 | 208 | // For each module, compute local Vx and Vy vectors. 209 | val prevVx = DoubleArray(modules.size) 210 | val prevVy = DoubleArray(modules.size) 211 | val prevHeading: Array = arrayOfNulls(modules.size) 212 | val desiredVx = DoubleArray(modules.size) 213 | val desiredVy = DoubleArray(modules.size) 214 | val desiredHeading: Array = arrayOfNulls(modules.size) 215 | var allModulesShouldFlip = true 216 | for (i in modules.indices) { 217 | prevVx[i] = (prevSetpoint.moduleStates[i].angle.cos * prevSetpoint.moduleStates[i].speedMetersPerSecond) 218 | prevVy[i] = (prevSetpoint.moduleStates[i].angle.sin * prevSetpoint.moduleStates[i].speedMetersPerSecond) 219 | prevHeading[i] = prevSetpoint.moduleStates[i].angle 220 | if (prevSetpoint.moduleStates[i].speedMetersPerSecond < 0.0) { 221 | prevHeading[i] = prevHeading[i]!!.rotateBy(Rotation2d.fromRadians(Math.PI)) 222 | } 223 | desiredVx[i] = desiredModuleState[i].angle.cos * desiredModuleState[i].speedMetersPerSecond 224 | desiredVy[i] = desiredModuleState[i].angle.sin * desiredModuleState[i].speedMetersPerSecond 225 | desiredHeading[i] = desiredModuleState[i].angle 226 | if (desiredModuleState[i].speedMetersPerSecond < 0.0) { 227 | desiredHeading[i] = desiredHeading[i]!!.rotateBy(Rotation2d.fromRadians(Math.PI)) 228 | } 229 | if (allModulesShouldFlip) { 230 | val requiredRotationRad: Double = abs(prevHeading[i]!!.unaryMinus().rotateBy(desiredHeading[i]).radians) 231 | if (requiredRotationRad < Math.PI / 2.0) { 232 | allModulesShouldFlip = false 233 | } 234 | } 235 | } 236 | if (allModulesShouldFlip 237 | && !prevSetpoint.chassisSpeeds.toTwist2d(0.02).epsilonEquals(Twist2d()) && !desiredState.toTwist2d(0.02) 238 | .epsilonEquals(Twist2d()) 239 | ) { 240 | // It will (likely) be faster to stop the robot, rotate the modules in place to the complement 241 | // of the desired 242 | // angle, and accelerate again. 243 | return generateSetpoint(prevSetpoint, ChassisSpeeds(), dt) 244 | } 245 | 246 | // Compute the deltas between start and goal. We can then interpolate from the start state to 247 | // the goal state; then 248 | // find the amount we can move from start towards goal in this cycle such that no kinematic 249 | // limit is exceeded. 250 | val dx: Double = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds.vxMetersPerSecond 251 | val dy: Double = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds.vyMetersPerSecond 252 | val dtheta: Double = 253 | desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds.omegaRadiansPerSecond 254 | 255 | // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at 256 | // desiredState. 257 | var minS = 1.0 258 | 259 | // In cases where an individual module is stopped, we want to remember the right steering angle 260 | // to command (since 261 | // inverse kinematics doesn't care about angle, we can be opportunistically lazy). 262 | val overrideSteering: MutableList = ArrayList(modules.size) 263 | // Enforce steering velocity limits. We do this by taking the derivative of steering angle at 264 | // the current angle, 265 | // and then backing out the maximum interpolant between start and goal states. We remember the 266 | // minimum across all modules, since 267 | // that is the active constraint. 268 | val maxThetaStep = dt * moduleLimits.maxSteeringVelocity 269 | for (i in modules.indices) { 270 | if (!needToSteer) { 271 | overrideSteering.add(prevSetpoint.moduleStates[i].angle) 272 | continue 273 | } 274 | overrideSteering.add(null) 275 | if (epsilonEquals(prevSetpoint.moduleStates[i].speedMetersPerSecond, 0.0)) { 276 | // If module is stopped, we know that we will need to move straight to the final steering 277 | // angle, so limit based 278 | // purely on rotation in place. 279 | if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { 280 | // Goal angle doesn't matter. Just leave module at its current angle. 281 | overrideSteering[i] = prevSetpoint.moduleStates[i].angle 282 | continue 283 | } 284 | 285 | var necessaryRotation: Rotation2d = prevSetpoint.moduleStates[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle) 286 | if (flipHeading(necessaryRotation)) { 287 | necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)) 288 | } 289 | // getRadians() bounds to +/- Pi. 290 | val numStepsNeeded: Double = abs(necessaryRotation.radians) / maxThetaStep 291 | 292 | if (numStepsNeeded <= 1.0) { 293 | // Steer directly to goal angle. 294 | overrideSteering[i] = desiredModuleState[i].angle 295 | // Don't limit the global min_s; 296 | continue 297 | } else { 298 | // Adjust steering by max_theta_step. 299 | overrideSteering[i] = prevSetpoint.moduleStates[i].angle 300 | .rotateBy(Rotation2d.fromRadians(sign(necessaryRotation.radians) * maxThetaStep)) 301 | minS = 0.0 302 | continue 303 | } 304 | } 305 | if (minS == 0.0) { 306 | // s can't get any lower. Save some CPU. 307 | continue 308 | } 309 | 310 | val kMaxIterations = 8 311 | val s = 312 | findSteeringMaxS( 313 | prevVx[i], 314 | prevVy[i], 315 | prevHeading[i]!!.radians, 316 | desiredVx[i], 317 | desiredVy[i], 318 | desiredHeading[i]!!.radians, 319 | maxThetaStep, 320 | kMaxIterations 321 | ) 322 | minS = min(minS, s) 323 | } 324 | 325 | // Enforce drive wheel acceleration limits. 326 | val maxVelStep = dt * moduleLimits.maxDriveAcceleration 327 | for (i in modules.indices) { 328 | if (minS == 0.0) { 329 | // No need to carry on. 330 | break 331 | } 332 | val vxMinS = if (minS == 1.0) desiredVx[i] else (desiredVx[i] - prevVx[i]) * minS + prevVx[i] 333 | val vyMinS = if (minS == 1.0) desiredVy[i] else (desiredVy[i] - prevVy[i]) * minS + prevVy[i] 334 | // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we 335 | // already know we can't go faster 336 | // than that. 337 | val kMaxIterations = 10 338 | val s = minS * findDriveMaxS( 339 | prevVx[i], 340 | prevVy[i], 341 | hypot(prevVx[i], prevVy[i]), 342 | vxMinS, 343 | vyMinS, 344 | hypot(vxMinS, vyMinS), 345 | maxVelStep, 346 | kMaxIterations 347 | ) 348 | 349 | minS = min(minS, s) 350 | } 351 | 352 | val retSpeeds = ChassisSpeeds( 353 | prevSetpoint.chassisSpeeds.vxMetersPerSecond + minS * dx, 354 | prevSetpoint.chassisSpeeds.vyMetersPerSecond + minS * dy, 355 | prevSetpoint.chassisSpeeds.omegaRadiansPerSecond + minS * dtheta 356 | ) 357 | val retStates: Array = kinematics.toSwerveModuleStates(retSpeeds) 358 | for (i in modules.indices) { 359 | val maybeOverride = overrideSteering[i] 360 | if (maybeOverride != null) { 361 | val override: Rotation2d = maybeOverride 362 | if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { 363 | retStates[i].speedMetersPerSecond *= -1.0 364 | } 365 | retStates[i].angle = override 366 | } 367 | val deltaRotation: Rotation2d = prevSetpoint.moduleStates[i].angle.unaryMinus().rotateBy(retStates[i].angle) 368 | if (flipHeading(deltaRotation)) { 369 | retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)) 370 | retStates[i].speedMetersPerSecond *= -1.0 371 | } 372 | } 373 | return SwerveSetpoint(retSpeeds, retStates) 374 | } 375 | } 376 | -------------------------------------------------------------------------------- /src/main/kotlin/org/team2471/frc/lib/motion_profiling/MotionKey.kt: -------------------------------------------------------------------------------- 1 | package org.team2471.frc.lib.motion_profiling 2 | 3 | import kotlin.math.abs 4 | import kotlin.math.cos 5 | import kotlin.math.sin 6 | 7 | class MotionKey { 8 | @Transient 9 | private val CLAMPTOLERANCE = 0.005 10 | private var m_timeAndValue: BooleanPair = BooleanPair(0.0, 0.0) 11 | private var m_prevAngleAndMagnitude: BooleanPair = BooleanPair(0.0, 1.0) 12 | private var m_nextAngleAndMagnitude: BooleanPair = BooleanPair(0.0, 1.0) 13 | private var m_prevTangent: BooleanPair = BooleanPair(0.0, 0.0) 14 | private var m_nextTangent: BooleanPair = BooleanPair(0.0, 0.0) 15 | var prevSlopeMethod: SlopeMethod = SlopeMethod.SLOPE_SMOOTH 16 | var nextSlopeMethod: SlopeMethod = SlopeMethod.SLOPE_SMOOTH 17 | private var m_markBeginOrEndKeysToZeroSlope: Boolean = true 18 | 19 | var nextKey: MotionKey? = null 20 | 21 | @Transient 22 | var motionCurve: MotionCurve? = null 23 | 24 | @Transient 25 | var prevKey: MotionKey? = null 26 | 27 | @Transient 28 | private var m_bTangentsDirty: Boolean = true 29 | 30 | @Transient 31 | private var m_bCoefficientsDirty: Boolean = true 32 | 33 | @Transient 34 | private var m_xCoeff: CubicCoefficients1D? = null 35 | 36 | @Transient 37 | private var m_yCoeff: CubicCoefficients1D? = null 38 | 39 | init { 40 | m_timeAndValue.set(0.0, 0.0) 41 | m_prevAngleAndMagnitude.set(0.0, 1.0) 42 | m_nextAngleAndMagnitude.set(0.0, 1.0) 43 | } 44 | 45 | fun onPositionChanged() { 46 | this.motionCurve!!.onKeyPositionChanged(this) // tell the curve too 47 | 48 | setTangentsDirty(true) 49 | setCoefficientsDirty(true) 50 | 51 | if (this.prevKey != null) { 52 | this.prevKey!!.setTangentsDirty(true) 53 | this.prevKey!!.setCoefficientsDirty(true) 54 | if (this.prevKey!!.prevKey != null && this.prevKey!!.prevKey!!.nextSlopeMethod == SlopeMethod.SLOPE_PLATEAU) { // Need to go two away if it is Plateau because they use Prev and Next Tangents 55 | this.prevKey!!.prevKey!!.setTangentsDirty(true) 56 | this.prevKey!!.prevKey!!.setCoefficientsDirty(true) 57 | } 58 | } 59 | 60 | if (this.nextKey != null) { 61 | this.nextKey!!.setTangentsDirty(true) 62 | if (this.nextKey!!.nextKey != null && this.nextKey!!.nextKey!!.prevSlopeMethod == SlopeMethod.SLOPE_PLATEAU) { // Need to go two away if it is Plateau because they use Prev and Next Tangents 63 | this.nextKey!!.nextKey!!.setTangentsDirty(true) 64 | this.nextKey!!.setCoefficientsDirty(true) 65 | } 66 | } 67 | } 68 | 69 | var time: Double 70 | get() = m_timeAndValue.x 71 | set(time) { 72 | m_timeAndValue.x = time 73 | onPositionChanged() 74 | } 75 | 76 | var value: Double 77 | get() = m_timeAndValue.y 78 | set(value) { 79 | m_timeAndValue.y = value 80 | onPositionChanged() 81 | if (value > this.motionCurve!!.maxValue) { //set curve min and max 82 | this.motionCurve!!.maxValue = value 83 | } else if (value < this.motionCurve!!.minValue) { 84 | this.motionCurve!!.minValue = value 85 | } 86 | } 87 | 88 | fun areTangentsDirty(): Boolean { 89 | return m_bTangentsDirty 90 | } 91 | 92 | fun setTangentsDirty(bTangentsDirty: Boolean) { 93 | m_bTangentsDirty = bTangentsDirty 94 | } 95 | 96 | fun areCoefficientsDirty(): Boolean { 97 | return m_bCoefficientsDirty 98 | } 99 | 100 | fun setCoefficientsDirty(bCoefficientsDirty: Boolean) { 101 | m_bCoefficientsDirty = bCoefficientsDirty 102 | } 103 | 104 | var timeAndValue: BooleanPair 105 | get() = m_timeAndValue 106 | set(m_timeAndValue) { 107 | this.m_timeAndValue = m_timeAndValue 108 | onPositionChanged() 109 | } 110 | 111 | var prevAngleAndMagnitude: BooleanPair 112 | get() = m_prevAngleAndMagnitude 113 | set(m_prevAngleAndMagnitude) { 114 | m_markBeginOrEndKeysToZeroSlope = false 115 | this.m_prevAngleAndMagnitude = m_prevAngleAndMagnitude 116 | this.prevSlopeMethod = SlopeMethod.SLOPE_MANUAL 117 | setTangentsDirty(true) 118 | onPositionChanged() 119 | } 120 | 121 | var nextAngleAndMagnitude: BooleanPair 122 | get() = m_nextAngleAndMagnitude 123 | set(m_nextAngleAndMagnitude) { 124 | m_markBeginOrEndKeysToZeroSlope = false 125 | this.m_nextAngleAndMagnitude = m_nextAngleAndMagnitude 126 | this.nextSlopeMethod = SlopeMethod.SLOPE_MANUAL 127 | setTangentsDirty(true) 128 | onPositionChanged() 129 | } 130 | 131 | var prevTangent: BooleanPair 132 | get() { 133 | if (areTangentsDirty()) calculateTangents() 134 | 135 | return m_prevTangent 136 | } 137 | set(m_PrevTangent) { 138 | this.m_prevTangent = m_PrevTangent 139 | } 140 | 141 | var nextTangent: BooleanPair 142 | get() { 143 | if (areTangentsDirty()) calculateTangents() 144 | 145 | return m_nextTangent 146 | } 147 | set(m_NextTangent) { 148 | this.m_nextTangent = m_NextTangent 149 | } 150 | 151 | var prevMagnitude: Double 152 | get() = m_prevAngleAndMagnitude.y 153 | set(magnitude) { 154 | m_prevAngleAndMagnitude.y = magnitude 155 | this.prevSlopeMethod = SlopeMethod.SLOPE_MANUAL 156 | } 157 | 158 | var nextMagnitude: Double 159 | get() = m_nextAngleAndMagnitude.y 160 | set(magnitude) { 161 | m_nextAngleAndMagnitude.y = magnitude 162 | this.nextSlopeMethod = SlopeMethod.SLOPE_MANUAL 163 | } 164 | 165 | fun insertBefore(newKey: MotionKey) { 166 | this.prevKey = newKey.prevKey 167 | if (newKey.prevKey != null) newKey.prevKey!!.nextKey = this 168 | newKey.prevKey = this 169 | this.nextKey = newKey 170 | } 171 | 172 | fun insertAfter(newKey: MotionKey) { 173 | this.nextKey = newKey.nextKey 174 | if (newKey.nextKey != null) newKey.nextKey!!.prevKey = this 175 | newKey.nextKey = this 176 | this.prevKey = newKey 177 | } 178 | 179 | private fun calculateTangents() { 180 | setTangentsDirty(false) 181 | 182 | var bCalcSmoothPrev = false 183 | var bCalcSmoothNext = false 184 | 185 | when (this.prevSlopeMethod) { 186 | SlopeMethod.SLOPE_MANUAL -> { 187 | m_prevTangent.set(cos(this.prevAngleAndMagnitude.x), sin(this.prevAngleAndMagnitude.x)) 188 | if (this.prevKey != null) // TODO: result is unused 189 | m_prevTangent.times(this.timeAndValue.x - prevKey!!.timeAndValue.x) 190 | } 191 | 192 | SlopeMethod.SLOPE_LINEAR -> if (this.prevKey != null) m_prevTangent = 193 | this.timeAndValue.minus(prevKey!!.timeAndValue) 194 | 195 | SlopeMethod.SLOPE_FLAT -> if (this.prevKey != null) m_prevTangent.set( 196 | (this.timeAndValue.x - prevKey!!.timeAndValue.x) * 0.5, 197 | 0.0 198 | ) 199 | 200 | SlopeMethod.SLOPE_SMOOTH -> bCalcSmoothPrev = true 201 | SlopeMethod.SLOPE_CLAMPED -> { 202 | val fClampTolerence = (this.motionCurve!!.maxValue - this.motionCurve!!.minValue) * CLAMPTOLERANCE 203 | if (this.prevKey != null && abs(prevKey!!.value - this.value) <= fClampTolerence) // make Flat 204 | m_prevTangent.set(this.time - prevKey!!.time, 0.0) 205 | else if (this.nextKey != null && abs(nextKey!!.value - this.value) <= fClampTolerence) // Make Flat 206 | { 207 | if (this.prevKey != null) m_prevTangent.set(this.time - prevKey!!.time, 0.0) 208 | else m_prevTangent.set(0.0, 0.0) 209 | } else bCalcSmoothPrev = true 210 | } 211 | 212 | SlopeMethod.SLOPE_PLATEAU -> if (this.prevKey == null || this.nextKey == null) { 213 | if (this.prevKey != null) m_prevTangent.set(this.time - prevKey!!.time, 0.0) // Make Flat 214 | else m_prevTangent.set(0.0, 0.0) 215 | } else // we have a prev and a next, lets see if both the prev's out tangent and the next's in tangent are both either greater or less than our value, if so lets make out tangent flat 216 | { 217 | val fPrevTangentValue: Double 218 | if (prevKey!!.nextSlopeMethod == SlopeMethod.SLOPE_PLATEAU) fPrevTangentValue = 219 | prevKey!!.value // This way we don't get an infinite recursion 220 | else { 221 | val vPrevPos = prevKey!!.timeAndValue 222 | val vPrevTangent = prevKey!!.nextTangent.times(1.0 / 3.0).plus(vPrevPos) 223 | fPrevTangentValue = vPrevTangent.y 224 | } 225 | 226 | val fNextTangentValue: Double 227 | if (nextKey!!.prevSlopeMethod == SlopeMethod.SLOPE_PLATEAU) fNextTangentValue = 228 | nextKey!!.value // This way we don't get an infinite recursion 229 | else { 230 | val vNextPos = nextKey!!.timeAndValue 231 | val vNextTangent = vNextPos.minus(nextKey!!.prevTangent.times(1.0 / 3.0)) 232 | fNextTangentValue = vNextTangent.y 233 | } 234 | 235 | val fValue = this.value 236 | if (fPrevTangentValue > fValue && fNextTangentValue > fValue) m_prevTangent.set( 237 | this.time - prevKey!!.time, 238 | 0.0 239 | ) // Make Flat 240 | else if (fPrevTangentValue < fValue && fNextTangentValue < fValue) m_prevTangent.set( 241 | this.time - prevKey!!.time, 242 | 0.0 243 | ) // Make Flat 244 | else bCalcSmoothPrev = true 245 | } 246 | 247 | SlopeMethod.SLOPE_STEPPED, SlopeMethod.SLOPE_STEPPED_NEXT -> assert(false) // Not a valid method for PREV Interp Method, it is only valid for NEXT key direction 248 | else -> {} 249 | } 250 | 251 | when (this.nextSlopeMethod) { 252 | SlopeMethod.SLOPE_MANUAL -> { 253 | m_nextTangent.set(cos(this.nextAngleAndMagnitude.x), sin(this.nextAngleAndMagnitude.x)) 254 | if (this.nextKey != null) m_nextTangent.times(nextKey!!.timeAndValue.x - this.timeAndValue.x) 255 | } 256 | 257 | SlopeMethod.SLOPE_LINEAR -> if (this.nextKey != null) m_nextTangent = 258 | nextKey!!.timeAndValue.minus(this.timeAndValue) 259 | 260 | SlopeMethod.SLOPE_FLAT -> if (this.nextKey != null) m_nextTangent.set( 261 | nextKey!!.timeAndValue.x - this.timeAndValue.x, 262 | 0.0 263 | ) 264 | 265 | SlopeMethod.SLOPE_SMOOTH -> bCalcSmoothNext = true 266 | SlopeMethod.SLOPE_CLAMPED -> { 267 | val fClampTolerence = (this.motionCurve!!.maxValue - this.motionCurve!!.minValue) * CLAMPTOLERANCE 268 | if (this.prevKey != null && abs(prevKey!!.value - this.value) <= fClampTolerence) // make Flat 269 | { 270 | if (this.nextKey != null) m_nextTangent.set(nextKey!!.time - this.time, 0.0) 271 | else m_nextTangent.set(0.0, 0.0) 272 | } else if (this.nextKey != null && abs(nextKey!!.value - this.value) <= fClampTolerence) // Make Flat 273 | m_nextTangent.set(nextKey!!.time - this.time, 0.0) 274 | else bCalcSmoothNext = true 275 | } 276 | 277 | SlopeMethod.SLOPE_PLATEAU -> if (this.prevKey == null || this.nextKey == null) { 278 | if (this.nextKey != null) m_nextTangent.set(nextKey!!.time - this.time, 0.0) // Make it flat 279 | else m_nextTangent.set(0.0, 0.0) 280 | } else // we have a prev and a next, lets see if both the prev's out tangent and the next's in tangent are both either greater or less than our value, if so lets make out tangent flat 281 | { 282 | val fPrevTangentValue: Double 283 | if (prevKey!!.nextSlopeMethod == SlopeMethod.SLOPE_PLATEAU) fPrevTangentValue = 284 | prevKey!!.value // This way we don't get an infinite recursion 285 | else { 286 | val vPrevPos = BooleanPair(prevKey!!.time, prevKey!!.value) 287 | val vPrevTangent = prevKey!!.nextTangent.times(1.0 / 3.0).plus(vPrevPos) 288 | fPrevTangentValue = vPrevTangent.y 289 | } 290 | 291 | val fNextTangentValue: Double 292 | if (nextKey!!.prevSlopeMethod == SlopeMethod.SLOPE_PLATEAU) fNextTangentValue = 293 | nextKey!!.value // This way we don't get an infinite recursion 294 | else { 295 | val vNextPos = BooleanPair(nextKey!!.time, nextKey!!.value) 296 | val vNextTangent = vNextPos.minus(nextKey!!.prevTangent.times(1.0 / 3.0)) 297 | fNextTangentValue = vNextTangent.y 298 | } 299 | 300 | val fValue = this.value 301 | if (fPrevTangentValue > fValue && fNextTangentValue > fValue) m_nextTangent.set( 302 | nextKey!!.time - this.time, 303 | 0.0 304 | ) // Make it flat 305 | else if (fPrevTangentValue < fValue && fNextTangentValue < fValue) m_nextTangent.set( 306 | nextKey!!.time - this.time, 307 | 0.0 308 | ) // Make it flat 309 | else bCalcSmoothNext = true 310 | } 311 | 312 | SlopeMethod.SLOPE_STEPPED, SlopeMethod.SLOPE_STEPPED_NEXT -> {} 313 | else -> {} 314 | } 315 | 316 | if (bCalcSmoothPrev || bCalcSmoothNext) { 317 | if (this.prevKey != null && this.nextKey != null) { 318 | var delta = nextKey!!.timeAndValue.minus(prevKey!!.timeAndValue) 319 | val weight = abs(delta.x) 320 | if (weight == 0.0) // if keys are on top of one another (no tangents) 321 | { 322 | if (bCalcSmoothPrev) m_prevTangent.set(0.0, 0.0) 323 | if (bCalcSmoothNext) m_nextTangent.set(0.0, 0.0) 324 | } else { 325 | delta = delta.div(weight) 326 | 327 | if (bCalcSmoothPrev) { 328 | val prevWeight = this.timeAndValue.x - prevKey!!.timeAndValue.x 329 | m_prevTangent = delta.times(prevWeight) 330 | } 331 | if (bCalcSmoothNext) { 332 | val nextWeight = nextKey!!.timeAndValue.x - this.timeAndValue.x 333 | m_nextTangent = delta.times(nextWeight) 334 | } 335 | } 336 | } else { 337 | if (this.nextKey != null) { 338 | if (bCalcSmoothPrev) m_prevTangent = nextKey!!.timeAndValue.minus(this.timeAndValue) 339 | 340 | if (bCalcSmoothNext) m_nextTangent = nextKey!!.timeAndValue.minus(this.timeAndValue) 341 | } 342 | 343 | if (this.prevKey != null) { 344 | if (bCalcSmoothPrev) m_prevTangent = this.timeAndValue.minus(prevKey!!.timeAndValue) 345 | 346 | if (bCalcSmoothNext) m_nextTangent = this.timeAndValue.minus(prevKey!!.timeAndValue) 347 | } 348 | } 349 | } 350 | 351 | m_prevTangent = 352 | m_prevTangent.times(this.prevAngleAndMagnitude.y) // / 3.0 it seems like this is more of a UI only thing, and shouldn't really be done in this case. But maybe I'm wrong. Subtract the points, then take a third to get a good default tangent. Does that still appear too long in the UI? So we divide by 3 again. 353 | m_nextTangent = m_nextTangent.times(this.nextAngleAndMagnitude.y) // / 3.0 354 | } 355 | 356 | val xCoefficients: CubicCoefficients1D? 357 | get() { 358 | if (areCoefficientsDirty()) { 359 | calculateCoefficients() 360 | } 361 | return m_xCoeff 362 | } 363 | 364 | val yCoefficients: CubicCoefficients1D? 365 | get() { 366 | if (areCoefficientsDirty()) { 367 | calculateCoefficients() 368 | } else if (m_yCoeff == null) { 369 | println("m_yCoeff = null") 370 | calculateCoefficients() 371 | } 372 | return m_yCoeff 373 | } 374 | 375 | private fun calculateCoefficients() { 376 | setCoefficientsDirty(false) 377 | 378 | val pointax = this.time 379 | val pointbx = nextKey!!.time 380 | val xspan = pointbx - pointax 381 | 382 | val pointay = this.value 383 | val pointby = nextKey!!.value 384 | val pointcy = this.nextTangent.y 385 | val pointdy = nextKey!!.prevTangent.y 386 | 387 | m_yCoeff = CubicCoefficients1D(pointay, pointby, pointcy, pointdy) 388 | 389 | // if the weights are default, then the x cubic is linear and there is no need to evaluate it 390 | if (this.nextMagnitude == 1.0 && nextKey!!.prevMagnitude == 1.0) return 391 | 392 | // Spline - non default tangents means that we need a second parametric cubic for x as a function of t 393 | var pointcx = this.nextTangent.x 394 | var pointdx = nextKey!!.prevTangent.x 395 | 396 | val xspan3 = xspan * 3 397 | 398 | // if c going beyond b limit 399 | if (pointcx > xspan3) { 400 | val ratio = xspan3 / pointcx 401 | pointcx = xspan3 402 | } 403 | 404 | // if d going beyond a limit 405 | if (pointdx > xspan3) { 406 | val ratio = xspan3 / pointdx 407 | pointdx = xspan3 408 | } 409 | 410 | m_xCoeff = CubicCoefficients1D(pointax, pointbx, pointcx, pointdx) 411 | } 412 | 413 | val markbeginOrEndKeysToZeroSlope: Boolean 414 | get() = m_markBeginOrEndKeysToZeroSlope && this.motionCurve!!.markbeginOrEndKeysToZeroSlope 415 | 416 | fun setMarkBeginOrEndKeysToZeroSlope(m_setBeginOrEndKeysToZeroSlope: Boolean) { 417 | this.m_markBeginOrEndKeysToZeroSlope = m_setBeginOrEndKeysToZeroSlope 418 | } 419 | 420 | enum class SlopeMethod { 421 | SLOPE_MANUAL, SLOPE_LINEAR, SLOPE_FLAT, SLOPE_SMOOTH, SLOPE_CLAMPED, SLOPE_PLATEAU, 422 | SLOPE_STEPPED, SLOPE_STEPPED_NEXT 423 | } 424 | 425 | var magnitude: Double 426 | get() { 427 | if (this.prevKey != null) return this.prevMagnitude 428 | return this.nextMagnitude 429 | } 430 | set(magnitude) { 431 | m_prevAngleAndMagnitude.y = magnitude 432 | m_nextAngleAndMagnitude.y = magnitude 433 | this.prevSlopeMethod = SlopeMethod.SLOPE_MANUAL 434 | this.nextSlopeMethod = this.prevSlopeMethod 435 | onPositionChanged() 436 | } 437 | 438 | var angle: Double 439 | get() { 440 | if (this.prevKey != null) return this.prevAngle 441 | return this.nextAngle 442 | } 443 | set(angle) { 444 | m_prevAngleAndMagnitude.x = angle 445 | m_nextAngleAndMagnitude.x = angle 446 | this.prevSlopeMethod = SlopeMethod.SLOPE_MANUAL 447 | this.nextSlopeMethod = this.prevSlopeMethod 448 | onPositionChanged() 449 | } 450 | 451 | var prevAngle: Double 452 | get() = m_prevAngleAndMagnitude.x 453 | set(angle) { 454 | m_prevAngleAndMagnitude.x = angle 455 | this.prevSlopeMethod = SlopeMethod.SLOPE_MANUAL 456 | } 457 | var nextAngle: Double 458 | get() = m_nextAngleAndMagnitude.x 459 | set(angle) { 460 | m_nextAngleAndMagnitude.x = angle 461 | this.nextSlopeMethod = SlopeMethod.SLOPE_MANUAL 462 | } 463 | } 464 | --------------------------------------------------------------------------------