├── .editorconfig ├── .github └── workflows │ └── main.yml ├── .gitignore ├── LICENSE ├── LicenseHeader.txt ├── README.md ├── ThirdPartyLicenses.txt ├── build.gradle ├── core ├── build.gradle └── src │ ├── main │ └── kotlin │ │ ├── edu │ │ └── wpi │ │ │ └── first │ │ │ └── wpilibj │ │ │ └── controller │ │ │ └── RamseteController.java │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ ├── mathematics │ │ ├── Epsilon.kt │ │ ├── MathExtensions.kt │ │ ├── NumericalApproximations.kt │ │ ├── statespace │ │ │ ├── StateSpaceController.kt │ │ │ ├── StateSpaceControllerCoeffs.kt │ │ │ ├── StateSpaceObserver.kt │ │ │ ├── StateSpaceObserverCoeffs.kt │ │ │ ├── StateSpacePlant.kt │ │ │ └── StateSpacePlantCoeffs.kt │ │ ├── threedim │ │ │ └── geometry │ │ │ │ ├── Pose3d.kt │ │ │ │ ├── Quaternion.kt │ │ │ │ └── Translation3d.kt │ │ ├── twodim │ │ │ ├── geometry │ │ │ │ ├── GeometryExtensions.kt │ │ │ │ └── Rectangle2d.kt │ │ │ └── trajectory │ │ │ │ ├── FalconTrajectoryConfig.kt │ │ │ │ ├── FalconTrajectoryGenerator.kt │ │ │ │ ├── constraints │ │ │ │ ├── VelocityLimitRadiusConstraint.kt │ │ │ │ └── VelocityLimitRegionConstraint.kt │ │ │ │ └── optimization │ │ │ │ └── PathFinder.kt │ │ └── units │ │ │ ├── Ampere.kt │ │ │ ├── DeprecatedConstructors.kt │ │ │ ├── Kilogram.kt │ │ │ ├── Meter.kt │ │ │ ├── SIConstants.kt │ │ │ ├── SIUnit.kt │ │ │ ├── SIUnitBuilder.kt │ │ │ ├── Second.kt │ │ │ ├── derived │ │ │ ├── Acceleration.kt │ │ │ ├── DeprecatedConstructors.kt │ │ │ ├── Inverse.kt │ │ │ ├── Ohm.kt │ │ │ ├── Radian.kt │ │ │ ├── Velocity.kt │ │ │ ├── Volt.kt │ │ │ └── Watt.kt │ │ │ ├── nativeunit │ │ │ ├── NativeUnit.kt │ │ │ └── NativeUnitModel.kt │ │ │ ├── operations │ │ │ ├── ADivFracABEqualsB.kt │ │ │ ├── ADivUnitlessEqualsA.kt │ │ │ ├── ATimesBEqualsATimesB.kt │ │ │ ├── ATimesFracBAEqualsB.kt │ │ │ ├── ATimesUnitlessEqualsA.kt │ │ │ ├── AdivAEqualsUnitless.kt │ │ │ ├── AdivBEqualsAdivB.kt │ │ │ ├── DoubleTimesAEqualsA.kt │ │ │ ├── FracABTimesBEqualsA.kt │ │ │ ├── MultABDivAEqualsB.kt │ │ │ └── UnitlessTimesAEqualsA.kt │ │ │ └── specialops │ │ │ └── AngularMultiplication.kt │ │ ├── types │ │ ├── CSVWritable.kt │ │ ├── Interpolatable.kt │ │ └── VaryInterpolatable.kt │ │ └── utils │ │ ├── CircularBuffer.kt │ │ ├── Collections.kt │ │ ├── CoroutineUtils.kt │ │ ├── DeltaTime.kt │ │ ├── InterpolatingTreeMap.kt │ │ ├── InterpolatingTreeMapBuffer.kt │ │ ├── MiscExtensions.kt │ │ ├── MiscellaneousExtensions.kt │ │ ├── OtherUtils.kt │ │ ├── Source.kt │ │ └── SourceMonitor.kt │ └── test │ └── kotlin │ └── org │ └── ghrobotics │ └── lib │ ├── mathematics │ ├── DerivedTests.kt │ ├── LengthTest.kt │ ├── NumericalApproximationsTest.kt │ ├── TimeTest.kt │ ├── UnitTest.kt │ ├── threedim │ │ └── GeometryTests.kt │ └── twodim │ │ ├── geometry │ │ └── GeometryTests.kt │ │ └── trajectory │ │ └── PathFinderTest.kt │ └── utils │ └── SourceTests.kt ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── gradlew ├── gradlew.bat ├── jitpack.yml ├── settings.gradle.kts ├── vendorCTRE ├── build.gradle ├── src │ └── main │ │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ ├── motors │ │ └── ctre │ │ │ ├── FalconCTRE.kt │ │ │ ├── FalconCTREEncoder.kt │ │ │ ├── FalconFX.kt │ │ │ ├── FalconSPX.kt │ │ │ └── FalconSRX.kt │ │ └── utils │ │ └── CTREExtensions.kt └── vendordeps │ └── Phoenix.json ├── vendorCU ├── build.gradle ├── src │ └── main │ │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ └── utils │ │ └── CopperforgeExtensions.kt └── vendordeps │ └── LibCu-latest.json ├── vendorNAVX ├── build.gradle ├── src │ └── main │ │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ └── utils │ │ └── NavXExtensions.kt └── vendordeps │ └── navx_frc.json ├── vendorPWF ├── build.gradle ├── src │ └── main │ │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ ├── motors │ │ └── pwf │ │ │ ├── FalconVenom.kt │ │ │ └── FalconVenomEncoder.kt │ │ └── utils │ │ └── PlayingWithFusionExtensions.kt └── vendordeps │ └── playingwithfusion2021.json ├── vendorREV ├── build.gradle ├── src │ └── main │ │ ├── java │ │ └── org │ │ │ └── ghrobotics │ │ │ └── lib │ │ │ └── motors │ │ │ └── rev │ │ │ └── CANSensorShim.java │ │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ └── motors │ │ └── rev │ │ ├── FalconMAX.kt │ │ └── FalconMAXEncoder.kt └── vendordeps │ └── REVRobotics.json └── wpi ├── build.gradle ├── src ├── main │ └── kotlin │ │ └── org │ │ └── ghrobotics │ │ └── lib │ │ ├── commands │ │ ├── CommandGroupBuilder.kt │ │ ├── FalconCommand.kt │ │ ├── FalconNotifierCommand.kt │ │ ├── FalconSubsystem.kt │ │ └── FalconSubsystemHandler.kt │ │ ├── debug │ │ └── FalconDashboard.kt │ │ ├── localization │ │ ├── Localization.kt │ │ ├── TankEncoderLocalization.kt │ │ └── TimePoseInterpolatableBuffer.kt │ │ ├── motors │ │ ├── AbstractFalconEncoder.kt │ │ ├── AbstractFalconMotor.kt │ │ ├── FalconEncoder.kt │ │ └── FalconMotor.kt │ │ ├── sensors │ │ └── ADNS3080FlowSensor.kt │ │ ├── subsystems │ │ ├── SensorlessCompatibleSubsystem.kt │ │ └── drive │ │ │ ├── CharacterizationCommand.kt │ │ │ ├── FalconDriveHelper.kt │ │ │ ├── FalconWestCoastDrivetrain.kt │ │ │ ├── TrajectoryTrackerCommand.kt │ │ │ └── TrajectoryTrackerDriveBase.kt │ │ ├── utils │ │ └── WpiCoroutineUtils.kt │ │ ├── vision │ │ └── TargetTracker.kt │ │ └── wrappers │ │ ├── FalconNotifier.kt │ │ ├── FalconSolenoid.kt │ │ ├── FalconTimedRobot.kt │ │ ├── hid │ │ ├── FalconHID.kt │ │ ├── FalconPS4Controller.kt │ │ ├── FalconXboxController.kt │ │ ├── HIDControl.kt │ │ └── HIDSource.kt │ │ └── networktables │ │ ├── FalconNetworkTable.kt │ │ └── ShuffleboardExtensions.kt └── test │ └── kotlin │ └── org │ └── ghrobotics │ └── lib │ └── subsystems │ └── drive │ └── localization │ └── TimeInterpolatableBufferTest.kt └── vendordeps └── WPILibNewCommands.json /.editorconfig: -------------------------------------------------------------------------------- 1 | [*.{kt, kts}] 2 | indent_size = 4 3 | indent_style = space 4 | max_line_length = 120 -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | name: Build 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v2 11 | - uses: actions/setup-java@v1 12 | with: 13 | java-version: 11 14 | - run: ./gradlew build 15 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled class file 2 | *.class 3 | 4 | # Log file 5 | *.log 6 | 7 | # BlueJ files 8 | *.ctxt 9 | 10 | # Mobile Tools for Java (J2ME) 11 | .mtj.tmp/ 12 | 13 | # Package Files # 14 | *.jar 15 | *.war 16 | *.ear 17 | *.zip 18 | *.tar.gz 19 | *.rar 20 | 21 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 22 | hs_err_pid* 23 | 24 | .gradle/ 25 | build/ 26 | .idea/ 27 | *.iml 28 | out/ 29 | detekt.html -------------------------------------------------------------------------------- /LicenseHeader.txt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FalconLibrary 2 | [![Build Status](https://dev.azure.com/frc5190/Robot%20Code/_apis/build/status/FalconLibrary?branchName=master)](https://dev.azure.com/frc5190/Robot%20Code/_build/latest?definitionId=11&branchName=master) 3 | 4 | ## Overview 5 | 6 | Feature-rich Kotlin JVM based robotics library, primarily for use in the FIRST Robotics Competition. FalconLibrary is the backend for all Team 5190 robots. 7 | 8 | Note: Due to the usage of inline classes, the units code does NOT work in Java. 9 | 10 | ### `core`: Platform-agnostic mathematics and units code 11 | * Typesafe units of measure 12 | * Quick and easy conversions between all length, velocity, acceleration, electrical units. 13 | * Support for Talon SRX native unit length and rotation models. 14 | * Shape-safe matrix operations 15 | * Detect matrix shape mismatches at runtime by attempting to mimic C++'s templates using Generics 16 | * State-space modern control 17 | 18 | ### `wpi`: Code specific to the RoboRIO and FIRST Robotics Competition 19 | * Built-in drive subsystem abstraction with support for arbitrary localization. 20 | * Kotlin wrappers around the command based framework 21 | 22 | ### `vendorXXX`: RoboRIO vendor extensions 23 | * Talon SRX and Spark MAX wrappers that utilize Kotlin properties to set configurations. 24 | * Custom gyro `Rotation2d` sources. 25 | 26 | ## Using FalconLibrary in your project 27 | 28 | Make sure you can retrieve dependencies from JitPack. Add this to your `build.gradle`: 29 | ```groovy 30 | repositories { 31 | maven { url 'https://jitpack.io' } 32 | } 33 | ``` 34 | 35 | Under the `dependencies` section of your `build.gradle`, add the specific submodules that you want in your project. All of the submodules are presented below. 36 | 37 | ```groovy 38 | compile 'org.ghrobotics.FalconLibrary:core:2020.2.2' 39 | compile 'org.ghrobotics.FalconLibrary:wpi:2020.2.2' 40 | compile 'org.ghrobotics.FalconLibrary:vendorCTRE:2020.2.2' 41 | compile 'org.ghrobotics.FalconLibrary:vendorNAVX:2020.2.2' 42 | compile 'org.ghrobotics.FalconLibrary:vendorREV:2020.2.2' 43 | ``` 44 | 45 | Alternatively, you can include all submodules at once: 46 | ```groovy 47 | compile 'org.ghrobotics:FalconLibrary:2020.2.2' 48 | ``` 49 | 50 | Note that you must include the `vendordeps` JSON file in your own robot project to correctly use the `vendorXXX` modules. 51 | 52 | ## Contributing 53 | You are always welcome to submit a PR if you think that you can contribute something to this library. Remember that this is a FRC-game-agnostic library, so please don't ask for season-specific code to be merged. 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | plugins { 10 | id "org.jetbrains.kotlin.jvm" version "1.3.61" apply false 11 | id "edu.wpi.first.GradleRIO" version "2021.3.1" apply false 12 | id "com.diffplug.gradle.spotless" version "3.26.1" apply false 13 | id "maven" 14 | id "maven-publish" 15 | } 16 | 17 | allprojects { 18 | apply plugin: "org.jetbrains.kotlin.jvm" 19 | apply plugin: "edu.wpi.first.GradleRIO" 20 | apply plugin: "com.diffplug.gradle.spotless" 21 | apply plugin: "maven" 22 | apply plugin: "maven-publish" 23 | 24 | repositories { 25 | jcenter() 26 | maven { url "https://jitpack.io" } 27 | maven { url "https://frcmaven.wpi.edu/artifactory/release" } 28 | } 29 | 30 | dependencies { 31 | implementation "org.jetbrains.kotlin:kotlin-stdlib" 32 | implementation "org.jetbrains.kotlinx:kotlinx-coroutines-core:1.3.2" 33 | 34 | testImplementation "org.knowm.xchart:xchart:3.2.2" 35 | testImplementation "junit:junit:4.12" 36 | } 37 | 38 | compileKotlin { 39 | kotlinOptions { 40 | jvmTarget = "1.8" 41 | freeCompilerArgs = ["-Xjvm-default=compatibility"] 42 | } 43 | } 44 | 45 | spotless { 46 | kotlin { 47 | ktlint().userData(['insert_final_newline': 'true']) 48 | licenseHeaderFile "$rootDir/LicenseHeader.txt" 49 | } 50 | } 51 | } 52 | 53 | wrapper { 54 | gradleVersion = "6.0.1" 55 | } 56 | -------------------------------------------------------------------------------- /core/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api "org.apache.commons:commons-math3:3.6.1" 11 | api "com.github.FRCTeam4069:Keigen:1.4.0" 12 | api "edu.wpi.first.wpimath:wpimath-java:2021.1.1-beta-1" 13 | } -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/Epsilon.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | const val kEpsilon = 1E-9 12 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/MathExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import kotlin.math.PI 12 | import kotlin.math.absoluteValue 13 | 14 | fun Double.lerp(endValue: Double, t: Double) = this + (endValue - this) * t.coerceIn(0.0, 1.0) 15 | 16 | fun > min(a: T, b: T) = if (a < b) a else b 17 | fun > max(a: T, b: T) = if (a > b) a else b 18 | 19 | infix fun Double.epsilonEquals(other: Double) = minus(other).absoluteValue < kEpsilon 20 | 21 | infix fun Double.cos(other: Double) = times(Math.cos(other)) 22 | 23 | infix fun Double.sin(other: Double) = times(Math.sin(other)) 24 | 25 | fun Double.boundRadians(): Double { 26 | var x = this 27 | while (x >= PI) x -= (2 * PI) 28 | while (x < -PI) x += (2 * PI) 29 | return x 30 | } 31 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/NumericalApproximations.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | class NumericalApproximations private constructor() { 12 | companion object { 13 | tailrec fun newtonsMethod( 14 | numIterations: Int, 15 | x: Double, 16 | f: (x: Double) -> Double, 17 | fPrime: (x: Double) -> Double 18 | ): Double = 19 | if (numIterations < 1) { 20 | x 21 | } else { 22 | newtonsMethod(numIterations - 1, x - f(x) / fPrime(x), f, fPrime) 23 | } 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpaceController.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Num 12 | import frc.team4069.keigen.Vector 13 | import frc.team4069.keigen.get 14 | import frc.team4069.keigen.set 15 | import frc.team4069.keigen.zeros 16 | 17 | /** 18 | * Contains the controller coefficients and logic for a state-space controller. 19 | * 20 | * State-space controllers generally use the control law u = -Kx. The 21 | * feedforward used is u_ff = K_ff * (r_k+1 - A * r_k). 22 | * 23 | * For more on the underlying math, read 24 | * https://file.tavsys.net/control/state-space-guide.pdf. 25 | */ 26 | @Suppress("PrivatePropertyName") 27 | class StateSpaceController( 28 | coeffs: StateSpaceControllerCoeffs, 29 | private val plant: StateSpacePlant 30 | ) { 31 | 32 | private val states = plant.coeffs.states 33 | private val inputs = plant.coeffs.inputs 34 | 35 | private val K = coeffs.K 36 | private val Kff = coeffs.Kff 37 | private val UMin = coeffs.UMin 38 | private val UMax = coeffs.UMax 39 | 40 | private var isEnabled = false 41 | 42 | var r: Vector = zeros(states) 43 | var u: Vector = zeros(inputs) 44 | 45 | fun enable() { 46 | isEnabled = true 47 | } 48 | 49 | fun disable() { 50 | isEnabled = false 51 | u = zeros(inputs) 52 | } 53 | 54 | fun reset() { 55 | r = zeros(states) 56 | u = zeros(inputs) 57 | } 58 | 59 | fun update(x: Vector) { 60 | u = K * (r - x) + Kff * (r - plant.A * r) 61 | capU() 62 | } 63 | 64 | fun update(nextR: Vector, x: Vector) { 65 | u = K * (r - x) + Kff * (nextR - plant.A * r) 66 | r = nextR 67 | capU() 68 | } 69 | 70 | private fun capU() { 71 | for (i in 0 until inputs.i) { 72 | u[i] = u[i].coerceIn(UMin[i], UMax[i]) 73 | } 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpaceControllerCoeffs.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Matrix 12 | import frc.team4069.keigen.Num 13 | import frc.team4069.keigen.Vector 14 | 15 | data class StateSpaceControllerCoeffs( 16 | val K: Matrix, 17 | val Kff: Matrix, 18 | val UMin: Vector, 19 | val UMax: Vector 20 | ) 21 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpaceObserver.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Matrix 12 | import frc.team4069.keigen.Num 13 | import frc.team4069.keigen.Vector 14 | import frc.team4069.keigen.`1` 15 | import frc.team4069.keigen.zeros 16 | 17 | /** 18 | * Luenberger observers combine predictions from a model and measurements to 19 | * give an estimate of the true system state. 20 | * 21 | * Luenberger observers use an L gain matrix to determine whether to trust the 22 | * model or measurements more. Kalman filter theory uses statistics to compute 23 | * an optimal L gain (alternatively called the Kalman gain, K) which minimizes 24 | * the sum of squares error in the state estimate. 25 | * 26 | * Luenberger observers run the prediction and correction steps simultaneously 27 | * while Kalman filters run them sequentially. To implement a discrete-time 28 | * Kalman filter as a Luenberger observer, use the following mapping: 29 | * 
C = H, L = A * K
30 | * (H is the measurement matrix). 31 | * 32 | * For more on the underlying math, read 33 | * https://file.tavsys.net/control/state-space-guide.pdf. 34 | */ 35 | @Suppress("PrivatePropertyName") 36 | class StateSpaceObserver( 37 | coeffs: StateSpaceObserverCoeffs, 38 | private val plant: StateSpacePlant 39 | ) { 40 | 41 | private val states = plant.coeffs.states 42 | 43 | private val K = coeffs.K 44 | 45 | var xHat: Vector = zeros(states) 46 | 47 | fun reset() { 48 | xHat = zeros(states) 49 | } 50 | 51 | fun predict(newU: Matrix) { 52 | xHat = plant.updateX(xHat, newU) 53 | } 54 | 55 | fun correct(u: Vector, y: Vector) { 56 | xHat += K * (y - plant.C * xHat - plant.D * u) 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpaceObserverCoeffs.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Matrix 12 | import frc.team4069.keigen.Num 13 | 14 | data class StateSpaceObserverCoeffs( 15 | val K: Matrix 16 | ) 17 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpacePlant.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Matrix 12 | import frc.team4069.keigen.Num 13 | import frc.team4069.keigen.Vector 14 | import frc.team4069.keigen.`1` 15 | import frc.team4069.keigen.zeros 16 | 17 | /** 18 | * A plant defined using state-space notation. 19 | * 20 | * A plant is a mathematical model of a system's dynamics. 21 | * 22 | * For more on the underlying math, read 23 | * https://file.tavsys.net/control/state-space-guide.pdf. 24 | */ 25 | 26 | @Suppress("PropertyName") 27 | class StateSpacePlant( 28 | val coeffs: StateSpacePlantCoeffs 29 | ) { 30 | 31 | private val states = coeffs.states 32 | private val outputs = coeffs.outputs 33 | 34 | val A = coeffs.A 35 | val B = coeffs.B 36 | val C = coeffs.C 37 | val D = coeffs.D 38 | 39 | var x: Vector = zeros(states) 40 | var y: Vector = zeros(outputs) 41 | 42 | fun update(u: Matrix) { 43 | x = updateX(x, u) 44 | y = updateY(u) 45 | } 46 | 47 | fun updateX(x: Vector, u: Vector): Vector { 48 | return A * x + B * u 49 | } 50 | 51 | fun updateY(u: Vector): Vector { 52 | return C * x + D * u 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/statespace/StateSpacePlantCoeffs.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.statespace 10 | 11 | import frc.team4069.keigen.Matrix 12 | import frc.team4069.keigen.Nat 13 | import frc.team4069.keigen.Num 14 | 15 | data class StateSpacePlantCoeffs( 16 | val states: Nat, 17 | val inputs: Nat, 18 | val outputs: Nat, 19 | val A: Matrix, 20 | val B: Matrix, 21 | val C: Matrix, 22 | val D: Matrix 23 | ) 24 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/threedim/geometry/Pose3d.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.threedim.geometry 10 | 11 | typealias Transform = Pose3d 12 | 13 | data class Pose3d( 14 | val translation: Translation3d = Translation3d.kZero, 15 | val rotation: Quaternion = Quaternion.kIdentity 16 | ) { 17 | 18 | operator fun minus(other: Pose3d) = this + -other 19 | 20 | operator fun plus(other: Pose3d) = 21 | Pose3d( 22 | translation + (other.translation * rotation), 23 | rotation * other.rotation 24 | ) 25 | 26 | operator fun unaryMinus(): Pose3d { 27 | val invertedRotation = -rotation 28 | return Pose3d((-translation) * invertedRotation, invertedRotation) 29 | } 30 | 31 | operator fun times(scalar: Double) = Pose3d( 32 | translation * scalar, 33 | rotation * scalar 34 | ) 35 | 36 | operator fun div(scalar: Double) = times(1.0 / scalar) 37 | 38 | infix fun inFrameOfReferenceOf(fieldRelativeOrigin: Pose3d) = (-fieldRelativeOrigin) + this 39 | } 40 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/threedim/geometry/Translation3d.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.threedim.geometry 10 | 11 | import org.ghrobotics.lib.mathematics.epsilonEquals 12 | 13 | /** 14 | * +X to the right 15 | * +Y straight up 16 | * +Z axis toward viewer 17 | */ 18 | 19 | data class Translation3d( 20 | val x: Double, 21 | val y: Double, 22 | val z: Double 23 | ) { 24 | 25 | val magnitude get() = Math.sqrt(sqrMagnitude) 26 | val sqrMagnitude get() = x * x + y * y + z * z 27 | 28 | operator fun unaryMinus() = Translation3d(-x, -y, -z) 29 | 30 | operator fun get(componentId: Int) = 31 | when (componentId) { 32 | 0 -> x 33 | 1 -> y 34 | 2 -> z 35 | else -> throw IndexOutOfBoundsException() 36 | } 37 | 38 | operator fun plus(other: Translation3d) = 39 | Translation3d( 40 | x + other.x, 41 | y + other.y, 42 | z + other.z 43 | ) 44 | 45 | operator fun times(quaternion: Quaternion) = quaternion.transform(this) 46 | 47 | operator fun times(scalar: Double) = Translation3d(x * scalar, y * scalar, z * scalar) 48 | 49 | infix fun epsilonEquals(other: Translation3d) = x epsilonEquals other.x && 50 | y epsilonEquals other.y && z epsilonEquals other.z 51 | 52 | companion object { 53 | val kZero = Translation3d(0.0, 0.0, 0.0) 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/geometry/GeometryExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | @file:Suppress("FunctionName") 10 | 11 | package org.ghrobotics.lib.mathematics.twodim.geometry 12 | 13 | import edu.wpi.first.wpilibj.geometry.Pose2d 14 | import edu.wpi.first.wpilibj.geometry.Rotation2d 15 | import edu.wpi.first.wpilibj.geometry.Translation2d 16 | import edu.wpi.first.wpilibj.geometry.Twist2d 17 | import kotlin.math.absoluteValue 18 | import org.ghrobotics.lib.mathematics.kEpsilon 19 | import org.ghrobotics.lib.mathematics.units.Meter 20 | import org.ghrobotics.lib.mathematics.units.SIUnit 21 | import org.ghrobotics.lib.mathematics.units.derived.Radian 22 | import org.ghrobotics.lib.mathematics.units.derived.degrees 23 | import org.ghrobotics.lib.mathematics.units.derived.toRotation2d 24 | import org.ghrobotics.lib.mathematics.units.meters 25 | 26 | /* Translation2d Unit-Safe Constructors */ 27 | 28 | fun Translation2d(x: SIUnit, y: SIUnit) = 29 | Translation2d(x.value, y.value) 30 | 31 | fun Translation2d(distance: SIUnit, angle: Rotation2d) = 32 | Translation2d(distance * angle.cos, distance * angle.sin) 33 | 34 | /* Pose2d Unit-Safe Constructors */ 35 | 36 | fun Pose2d(x: SIUnit, y: SIUnit, angle: Rotation2d) = 37 | Pose2d(x.value, y.value, angle) 38 | 39 | fun Pose2d(x: SIUnit, y: SIUnit, angle: SIUnit = 0.degrees) = 40 | Pose2d(x.value, y.value, angle.toRotation2d()) 41 | 42 | /* Transform2d Unit-Safe Constructors */ 43 | fun Transform2d(x: SIUnit, y: SIUnit, angle: Rotation2d) = 44 | edu.wpi.first.wpilibj.geometry.Transform2d(Translation2d(x, y), angle) 45 | 46 | /* Translation2d Unit Accessors */ 47 | val Translation2d.x_u get() = x.meters 48 | val Translation2d.y_u get() = y.meters 49 | 50 | /* Interpolation */ 51 | fun Pose2d.interpolate(endValue: Pose2d, t: Double): Pose2d { 52 | return if (t < 0) { 53 | this 54 | } else if (t >= 1) { 55 | endValue 56 | } else { 57 | val twist = (endValue.relativeTo(this)).log() 58 | exp(twist * t) 59 | } 60 | } 61 | 62 | /* Misc Extensions */ 63 | fun Pose2d.mirror() = 64 | Pose2d(translation.x, 8.2296 - translation.y, -rotation) 65 | 66 | fun Pose2d.log(): Twist2d { 67 | val dtheta = rotation.radians 68 | val halfDTheta = dtheta / 2.0 69 | val cosMinusOne = rotation.cos - 1.0 70 | 71 | val halfThetaByTanOfHalfDTheta = if (cosMinusOne.absoluteValue < kEpsilon) { 72 | 1.0 - 1.0 / 12.0 * dtheta * dtheta 73 | } else { 74 | -(halfDTheta * rotation.sin) / cosMinusOne 75 | } 76 | val translationPart = translation.rotateBy( 77 | Rotation2d( 78 | halfThetaByTanOfHalfDTheta, 79 | -halfDTheta 80 | ) 81 | ) 82 | return Twist2d(translationPart.x, translationPart.y, dtheta) 83 | } 84 | 85 | operator fun Twist2d.times(scalar: Double) = 86 | Twist2d(dx * scalar, dy * scalar, dtheta * scalar) 87 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/geometry/Rectangle2d.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.geometry 10 | 11 | import edu.wpi.first.wpilibj.geometry.Translation2d 12 | import org.ghrobotics.lib.mathematics.epsilonEquals 13 | import org.ghrobotics.lib.mathematics.max 14 | import org.ghrobotics.lib.mathematics.min 15 | import org.ghrobotics.lib.mathematics.units.Meter 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | 18 | @Suppress("FunctionName") 19 | fun Rectangle2d( 20 | one: Translation2d, 21 | two: Translation2d 22 | ): Rectangle2d { 23 | val minX = min(one.x_u, two.x_u) 24 | val minY = min(one.y_u, two.y_u) 25 | val maxX = max(one.x_u, two.x_u) 26 | val maxY = max(one.y_u, two.y_u) 27 | return Rectangle2d( 28 | minX, minY, 29 | maxX - minX, maxY - minY 30 | ) 31 | } 32 | 33 | @Suppress("FunctionName", "UnsafeCallOnNullableType") 34 | fun Rectangle2d( 35 | vararg pointsToInclude: Translation2d 36 | ): Rectangle2d { 37 | val minX = pointsToInclude.minBy { it.x }!!.x_u 38 | val minY = pointsToInclude.minBy { it.y }!!.y_u 39 | val maxX = pointsToInclude.maxBy { it.x }!!.x_u 40 | val maxY = pointsToInclude.maxBy { it.y }!!.y_u 41 | return Rectangle2d( 42 | minX, minY, 43 | maxX - minX, maxY - minY 44 | ) 45 | } 46 | 47 | data class Rectangle2d constructor( 48 | val x: SIUnit, 49 | val y: SIUnit, 50 | val w: SIUnit, 51 | val h: SIUnit 52 | ) { 53 | 54 | val topLeft = Translation2d(x, y + h) 55 | val topRight = Translation2d(x + w, y + h) 56 | val bottomLeft = Translation2d(x, y) 57 | val bottomRight = Translation2d(x + w, y) 58 | 59 | val center = Translation2d(x + w / 2.0, y + h / 2.0) 60 | 61 | val maxCorner = topRight 62 | val minCorner = bottomLeft 63 | 64 | fun isIn(r: Rectangle2d) = 65 | x < r.x + r.w && x + w > r.x && y < r.y + r.h && y + h > r.y 66 | 67 | fun isWithin(r: Rectangle2d) = r.x in x..(x + w - r.w) && r.y in y..(y + h - r.h) 68 | 69 | operator fun contains(p: Translation2d) = p.x_u in x..(x + w) && p.y_u in y..(y + h) 70 | 71 | @Suppress("ComplexMethod") 72 | fun doesCollide(rectangle: Rectangle2d, translation: Translation2d): Boolean { 73 | if (translation.x epsilonEquals 0.0 && translation.y epsilonEquals 0.0) return false 74 | // Check if its even in range 75 | val boxRect = Rectangle2d( 76 | rectangle.topLeft, rectangle.bottomRight, 77 | rectangle.topLeft + translation, rectangle.bottomRight + translation 78 | ) 79 | // println(boxRect) 80 | if (!boxRect.isIn(this)) return false 81 | // AABB collision 82 | // Calculate distances 83 | val xInvEntry: SIUnit 84 | val xInvExit: SIUnit 85 | val yInvEntry: SIUnit 86 | val yInvExit: SIUnit 87 | if (translation.x > 0.0) { 88 | xInvEntry = (x - (rectangle.x + rectangle.w)) 89 | xInvExit = ((x + w) - rectangle.x) 90 | } else { 91 | xInvEntry = ((x + w) - rectangle.x) 92 | xInvExit = (x - (rectangle.x + rectangle.w)) 93 | } 94 | if (translation.y > 0.0) { 95 | yInvEntry = (y - (rectangle.y + rectangle.h)) 96 | yInvExit = ((y + h) - rectangle.y) 97 | } else { 98 | yInvEntry = ((y + h) - rectangle.y) 99 | yInvExit = (y - (rectangle.y + rectangle.h)) 100 | } 101 | // Find time of collisions 102 | val xEntry: Double 103 | val xExit: Double 104 | val yEntry: Double 105 | val yExit: Double 106 | if (translation.x epsilonEquals 0.0) { 107 | xEntry = Double.NEGATIVE_INFINITY 108 | xExit = Double.POSITIVE_INFINITY 109 | } else { 110 | xEntry = (xInvEntry / translation.x).value 111 | xExit = (xInvExit / translation.x).value 112 | } 113 | if (translation.y epsilonEquals 0.0) { 114 | yEntry = Double.NEGATIVE_INFINITY 115 | yExit = Double.POSITIVE_INFINITY 116 | } else { 117 | yEntry = (yInvEntry / translation.y).value 118 | yExit = (yInvExit / translation.y).value 119 | } 120 | val entryTime = max(xEntry, yEntry) 121 | val exitTime = min(xExit, yExit) 122 | 123 | return entryTime <= exitTime && (xEntry >= 0.0 || yEntry >= 0.0) && (xEntry < 1.0 || yEntry < 1.0) 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/FalconTrajectoryConfig.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.trajectory 10 | 11 | import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig 12 | import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.derived.LinearAcceleration 15 | import org.ghrobotics.lib.mathematics.units.derived.LinearVelocity 16 | 17 | /** 18 | * Unit-safe wrapper for TrajectoryConfig, a configuration class 19 | * used to generate trajectories. 20 | * 21 | * @param maxVelocity The maximum velocity of the trajectory. 22 | * @param maxAcceleration The maximum acceleration of the trajectory. 23 | */ 24 | @Suppress("MemberVisibilityCanBePrivate") 25 | class FalconTrajectoryConfig( 26 | maxVelocity: SIUnit, 27 | maxAcceleration: SIUnit 28 | ) : TrajectoryConfig(maxVelocity.value, maxAcceleration.value) { 29 | /** 30 | * Set the starting velocity of the trajectory. 31 | * @param startVelocity The start velocity of the trajectory. 32 | */ 33 | fun setStartVelocity(startVelocity: SIUnit): FalconTrajectoryConfig = also { 34 | super.setStartVelocity(startVelocity.value) 35 | } 36 | 37 | /** 38 | * Set the ending velocity of the trajectory. 39 | * @param endVelocity The ending velocity of the trajectory. 40 | */ 41 | fun setEndVelocity(endVelocity: SIUnit): FalconTrajectoryConfig = also { 42 | super.setEndVelocity(endVelocity.value) 43 | } 44 | 45 | override fun addConstraint(constraint: TrajectoryConstraint): FalconTrajectoryConfig = also { 46 | super.addConstraint(constraint) 47 | } 48 | 49 | override fun addConstraints(constraints: List): FalconTrajectoryConfig = also { 50 | super.addConstraints(constraints) 51 | } 52 | 53 | fun addConstraints(vararg constraints: TrajectoryConstraint): FalconTrajectoryConfig = also { 54 | super.addConstraints(constraints.asList()) 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/FalconTrajectoryGenerator.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.trajectory 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Translation2d 13 | import edu.wpi.first.wpilibj.trajectory.Trajectory 14 | import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator 15 | import org.ghrobotics.lib.mathematics.twodim.geometry.mirror 16 | 17 | /** 18 | * Wrapper class over WPILib's trajectory generator that adds 19 | * support for units. 20 | */ 21 | object FalconTrajectoryGenerator { 22 | 23 | /** 24 | * Generates a trajectory from the given waypoints and constraints. 25 | * 26 | * @param waypoints A list of waypoints. 27 | * @param config The configuration for the trajectory. 28 | * 29 | * @return The trajectory. 30 | */ 31 | fun generateTrajectory( 32 | waypoints: List, 33 | config: FalconTrajectoryConfig 34 | ): Trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config) 35 | 36 | /** 37 | * Generates a trajectory from the given waypoints and constraints. 38 | * 39 | * @param start The starting waypoint. 40 | * @param interiorWaypoints The interior waypoint translations. 41 | * @param end The ending waypoint. 42 | * @param config The configuration for the trajectory. 43 | * 44 | * @return The trajectory. 45 | */ 46 | fun generateTrajectory( 47 | start: Pose2d, 48 | interiorWaypoints: List, 49 | end: Pose2d, 50 | config: FalconTrajectoryConfig 51 | ): Trajectory = TrajectoryGenerator.generateTrajectory(start, interiorWaypoints, end, config) 52 | } 53 | 54 | fun Trajectory.mirror(): Trajectory = Trajectory( 55 | states.map { 56 | Trajectory.State( 57 | it.timeSeconds, 58 | it.velocityMetersPerSecond, 59 | it.accelerationMetersPerSecondSq, 60 | it.poseMeters.mirror(), 61 | -it.curvatureRadPerMeter 62 | ) 63 | } 64 | ) 65 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/constraints/VelocityLimitRadiusConstraint.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.trajectory.constraints 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Translation2d 13 | import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint 14 | import org.ghrobotics.lib.mathematics.units.Meter 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.derived.LinearVelocity 17 | 18 | class VelocityLimitRadiusConstraint constructor( 19 | val point: Translation2d, 20 | val radius: SIUnit, 21 | val velocityLimit: SIUnit 22 | ) : TrajectoryConstraint { 23 | 24 | override fun getMaxVelocityMetersPerSecond( 25 | poseMeters: Pose2d, 26 | curvatureRadPerMeter: Double, 27 | velocityMetersPerSecond: Double 28 | ) = if (poseMeters.translation.getDistance(point) <= radius.value) velocityLimit.value else 29 | Double.POSITIVE_INFINITY 30 | 31 | override fun getMinMaxAccelerationMetersPerSecondSq( 32 | poseMeters: Pose2d?, 33 | curvatureRadPerMeter: Double, 34 | velocityMetersPerSecond: Double 35 | ): TrajectoryConstraint.MinMax = TrajectoryConstraint.MinMax() 36 | } 37 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/constraints/VelocityLimitRegionConstraint.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | @file:Suppress("unused") 10 | 11 | package org.ghrobotics.lib.mathematics.twodim.trajectory.constraints 12 | 13 | import edu.wpi.first.wpilibj.geometry.Pose2d 14 | import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint 15 | import org.ghrobotics.lib.mathematics.twodim.geometry.Rectangle2d 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | import org.ghrobotics.lib.mathematics.units.derived.LinearVelocity 18 | 19 | class VelocityLimitRegionConstraint constructor( 20 | val region: Rectangle2d, 21 | val velocityLimit: SIUnit 22 | ) : TrajectoryConstraint { 23 | 24 | override fun getMaxVelocityMetersPerSecond( 25 | poseMeters: Pose2d, 26 | curvatureRadPerMeter: Double, 27 | velocityMetersPerSecond: Double 28 | ) = if (poseMeters.translation in region) velocityLimit.value else Double.POSITIVE_INFINITY 29 | 30 | override fun getMinMaxAccelerationMetersPerSecondSq( 31 | poseMeters: Pose2d?, 32 | curvatureRadPerMeter: Double, 33 | velocityMetersPerSecond: Double 34 | ) = TrajectoryConstraint.MinMax() 35 | } 36 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/Ampere.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units 10 | 11 | object Ampere : SIKey 12 | 13 | val Double.amps get() = SIUnit(this) 14 | 15 | val Number.amps get() = toDouble().amps 16 | 17 | @Deprecated("Replaced with Plural version", ReplaceWith("inYottaamps()")) 18 | val SIUnit.yottaamp 19 | get() = inYottaamps() 20 | @Deprecated("Replaced with Plural version", ReplaceWith("inZettaamps()")) 21 | val SIUnit.zettaamp 22 | get() = inZettaamps() 23 | @Deprecated("Replaced with Plural version", ReplaceWith("inExaamps()")) 24 | val SIUnit.exaamp 25 | get() = inExaamps() 26 | @Deprecated("Replaced with Plural version", ReplaceWith("inPetaamps()")) 27 | val SIUnit.petaamp 28 | get() = inPetaamps() 29 | @Deprecated("Replaced with Plural version", ReplaceWith("inTeraamps()")) 30 | val SIUnit.teraamp 31 | get() = inTeraamps() 32 | @Deprecated("Replaced with Plural version", ReplaceWith("inGigaamps()")) 33 | val SIUnit.gigaamp 34 | get() = inGigaamps() 35 | @Deprecated("Replaced with Plural version", ReplaceWith("inMegaamps()")) 36 | val SIUnit.megaamp 37 | get() = inMegaamps() 38 | @Deprecated("Replaced with Plural version", ReplaceWith("inKiloamps()")) 39 | val SIUnit.kiloamp 40 | get() = inKiloamps() 41 | @Deprecated("Replaced with Plural version", ReplaceWith("inHectoamps()")) 42 | val SIUnit.hectoamp 43 | get() = inHectoamps() 44 | @Deprecated("Replaced with Plural version", ReplaceWith("inDecaamps()")) 45 | val SIUnit.decaamp 46 | get() = inDecaamps() 47 | @Deprecated("Replaced with Plural version", ReplaceWith("inAmps()")) 48 | val SIUnit.amp 49 | get() = inAmps() 50 | @Deprecated("Replaced with Plural version", ReplaceWith("inDeciamps()")) 51 | val SIUnit.deciamp 52 | get() = inDeciamps() 53 | @Deprecated("Replaced with Plural version", ReplaceWith("inCentiamps()")) 54 | val SIUnit.centiamp 55 | get() = inCentiamps() 56 | @Deprecated("Replaced with Plural version", ReplaceWith("inMilliamps()")) 57 | val SIUnit.milliamp 58 | get() = inMilliamps() 59 | @Deprecated("Replaced with Plural version", ReplaceWith("inMicroamps()")) 60 | val SIUnit.microamp 61 | get() = inMicroamps() 62 | @Deprecated("Replaced with Plural version", ReplaceWith("inNanoamps()")) 63 | val SIUnit.nanoamp 64 | get() = inNanoamps() 65 | @Deprecated("Replaced with Plural version", ReplaceWith("inPicoamps()")) 66 | val SIUnit.picoamp 67 | get() = inPicoamps() 68 | @Deprecated("Replaced with Plural version", ReplaceWith("inFemtoamps()")) 69 | val SIUnit.femtoamp 70 | get() = inFemtoamps() 71 | @Deprecated("Replaced with Plural version", ReplaceWith("inAttoamps()")) 72 | val SIUnit.attoamp 73 | get() = inAttoamps() 74 | @Deprecated("Replaced with Plural version", ReplaceWith("inZeptoamps()")) 75 | val SIUnit.zeptoamp 76 | get() = inZeptoamps() 77 | @Deprecated("Replaced with Plural version", ReplaceWith("inYoctoamps()")) 78 | val SIUnit.yoctoamp 79 | get() = inYoctoamps() 80 | 81 | fun SIUnit.inYottaamps() = value.div(kYotta) 82 | fun SIUnit.inZettaamps() = value.div(kZetta) 83 | fun SIUnit.inExaamps() = value.div(kExa) 84 | fun SIUnit.inPetaamps() = value.div(kPeta) 85 | fun SIUnit.inTeraamps() = value.div(kTera) 86 | fun SIUnit.inGigaamps() = value.div(kGiga) 87 | fun SIUnit.inMegaamps() = value.div(kMega) 88 | fun SIUnit.inKiloamps() = value.div(kKilo) 89 | fun SIUnit.inHectoamps() = value.div(kHecto) 90 | fun SIUnit.inDecaamps() = value.div(kDeca) 91 | fun SIUnit.inAmps() = value 92 | fun SIUnit.inDeciamps() = value.div(kDeci) 93 | fun SIUnit.inCentiamps() = value.div(kCenti) 94 | fun SIUnit.inMilliamps() = value.div(kMilli) 95 | fun SIUnit.inMicroamps() = value.div(kMicro) 96 | fun SIUnit.inNanoamps() = value.div(kNano) 97 | fun SIUnit.inPicoamps() = value.div(kPico) 98 | fun SIUnit.inFemtoamps() = value.div(kFemto) 99 | fun SIUnit.inAttoamps() = value.div(kAtto) 100 | fun SIUnit.inZeptoamps() = value.div(kZepto) 101 | fun SIUnit.inYoctoamps() = value.div(kYocto) 102 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/DeprecatedConstructors.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units 10 | 11 | @Deprecated("Replaced with Plural version", ReplaceWith("seconds")) 12 | val Double.second 13 | get() = seconds 14 | @Deprecated("Replaced with Plural version", ReplaceWith("minutes")) 15 | val Double.minute 16 | get() = minutes 17 | @Deprecated("Replaced with Plural version", ReplaceWith("hours")) 18 | val Double.hour 19 | get() = hours 20 | @Deprecated("Replaced with Plural version", ReplaceWith("days")) 21 | val Double.day 22 | get() = days 23 | @Deprecated("Replaced with Plural version", ReplaceWith("weeks")) 24 | val Double.week 25 | get() = weeks 26 | @Deprecated("Replaced with Plural version", ReplaceWith("moments")) 27 | val Double.moment 28 | get() = moments 29 | 30 | @Deprecated("Replaced with Plural version", ReplaceWith("seconds")) 31 | val Number.second 32 | get() = seconds 33 | @Deprecated("Replaced with Plural version", ReplaceWith("minutes")) 34 | val Number.minute 35 | get() = minutes 36 | @Deprecated("Replaced with Plural version", ReplaceWith("hours")) 37 | val Number.hour 38 | get() = hours 39 | @Deprecated("Replaced with Plural version", ReplaceWith("days")) 40 | val Number.day 41 | get() = days 42 | @Deprecated("Replaced with Plural version", ReplaceWith("weeks")) 43 | val Number.week 44 | get() = weeks 45 | @Deprecated("Replaced with Plural version", ReplaceWith("moments")) 46 | val Number.moment 47 | get() = moments 48 | 49 | @Deprecated("Replaced with Plural version", ReplaceWith("meters")) 50 | val Double.meter 51 | get() = meters 52 | @Deprecated("Replaced with Plural version", ReplaceWith("lines")) 53 | val Double.line 54 | get() = lines 55 | @Deprecated("Replaced with Plural version", ReplaceWith("inches")) 56 | val Double.inch 57 | get() = inches 58 | @Deprecated("Replaced with Plural version", ReplaceWith("yards")) 59 | val Double.yard 60 | get() = yards 61 | @Deprecated("Replaced with Plural version", ReplaceWith("miles")) 62 | val Double.mile 63 | get() = miles 64 | @Deprecated("Replaced with Plural version", ReplaceWith("leagues")) 65 | val Double.league 66 | get() = leagues 67 | @Deprecated("Replaced with Plural version", ReplaceWith("nauticalMiles")) 68 | val Double.nauticalMile 69 | get() = nauticalMiles 70 | @Deprecated("Replaced with Plural version", ReplaceWith("lightYears")) 71 | val Double.lightYear 72 | get() = lightYears 73 | 74 | @Deprecated("Replaced with Plural version", ReplaceWith("meters")) 75 | val Number.meter 76 | get() = meters 77 | @Deprecated("Replaced with Plural version", ReplaceWith("lines")) 78 | val Number.line 79 | get() = lines 80 | @Deprecated("Replaced with Plural version", ReplaceWith("inches")) 81 | val Number.inch 82 | get() = inches 83 | @Deprecated("Replaced with Plural version", ReplaceWith("yards")) 84 | val Number.yard 85 | get() = yards 86 | @Deprecated("Replaced with Plural version", ReplaceWith("miles")) 87 | val Number.mile 88 | get() = miles 89 | @Deprecated("Replaced with Plural version", ReplaceWith("leagues")) 90 | val Number.league 91 | get() = leagues 92 | @Deprecated("Replaced with Plural version", ReplaceWith("nauticalMiles")) 93 | val Number.nauticalMile 94 | get() = nauticalMiles 95 | @Deprecated("Replaced with Plural version", ReplaceWith("lightYears")) 96 | val Number.lightYear 97 | get() = lightYears 98 | 99 | @Deprecated("Replaced with Plural version", ReplaceWith("lbs")) 100 | val Double.lb 101 | get() = lbs 102 | @Deprecated("Replaced with Plural version", ReplaceWith("slugs")) 103 | val Double.slug 104 | get() = slugs 105 | 106 | @Deprecated("Replaced with Plural version", ReplaceWith("lbs")) 107 | val Number.lb 108 | get() = lbs 109 | @Deprecated("Replaced with Plural version", ReplaceWith("slugs")) 110 | val Number.slug 111 | get() = slugs 112 | 113 | @Deprecated("Replaced with Plural version", ReplaceWith("amps")) 114 | val Double.amp 115 | get() = amps 116 | 117 | @Deprecated("Replaced with Plural version", ReplaceWith("amps")) 118 | val Number.amp 119 | get() = amps 120 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/SIConstants.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units 10 | 11 | const val kYotta = 1e24 12 | const val kZetta = 1e21 13 | const val kExa = 1e18 14 | const val kPeta = 1e15 15 | const val kTera = 1e12 16 | const val kGiga = 1e9 17 | const val kMega = 1e6 18 | const val kKilo = 1e3 19 | const val kHecto = 1e2 20 | const val kDeca = 1e1 21 | const val kDeci = 1e-1 22 | const val kCenti = 1e-2 23 | const val kMilli = 1e-3 24 | const val kMicro = 1e-6 25 | const val kNano = 1e-9 26 | const val kPico = 1e-12 27 | const val kFemto = 1e-15 28 | const val kAtto = 1e-18 29 | const val kZepto = 1e-21 30 | const val kYocto = 1e-24 31 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/SIUnit.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units 10 | 11 | import kotlin.math.absoluteValue 12 | import org.ghrobotics.lib.mathematics.epsilonEquals 13 | import org.ghrobotics.lib.mathematics.lerp 14 | 15 | /** 16 | * @param value This is the value expressed in its SI Base Unit 17 | */ 18 | inline class SIUnit(val value: Double) : Comparable> { 19 | 20 | val absoluteValue get() = SIUnit(value.absoluteValue) 21 | 22 | operator fun unaryMinus() = SIUnit(value.unaryMinus()) 23 | 24 | operator fun plus(other: SIUnit) = SIUnit(value.plus(other.value)) 25 | operator fun minus(other: SIUnit) = SIUnit(value.minus(other.value)) 26 | 27 | operator fun times(other: Double) = SIUnit(value.times(other)) 28 | operator fun div(other: Double) = SIUnit(value.div(other)) 29 | 30 | operator fun times(other: Number) = times(other.toDouble()) 31 | operator fun div(other: Number) = div(other.toDouble()) 32 | 33 | override operator fun compareTo(other: SIUnit): Int = value.compareTo(other.value) 34 | 35 | fun lerp(endValue: SIUnit, t: Double) = SIUnit(value.lerp(endValue.value, t)) 36 | infix fun epsilonEquals(other: SIUnit) = value.epsilonEquals(other.value) 37 | } 38 | 39 | interface SIKey 40 | 41 | class Mult : SIKey 42 | class Frac : SIKey 43 | 44 | object Unitless : SIKey 45 | 46 | val SIUnit.unitlessValue get() = value 47 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/SIUnitBuilder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units 10 | 11 | import org.ghrobotics.lib.mathematics.units.derived.Ohm 12 | import org.ghrobotics.lib.mathematics.units.derived.Volt 13 | import org.ghrobotics.lib.mathematics.units.derived.Watt 14 | 15 | inline class SIUnitBuilder(private val value: Double) { 16 | @Deprecated("Replaced with Plural version", ReplaceWith("seconds")) 17 | val second 18 | get() = seconds 19 | @Deprecated("Replaced with Plural version", ReplaceWith("meters")) 20 | val meter 21 | get() = meters 22 | @Deprecated("Replaced with Plural version", ReplaceWith("grams")) 23 | val gram 24 | get() = grams 25 | @Deprecated("Replaced with Plural version", ReplaceWith("amps")) 26 | val amp 27 | get() = amps 28 | @Deprecated("Replaced with Plural version", ReplaceWith("ohms")) 29 | val ohm 30 | get() = ohms 31 | @Deprecated("Replaced with Plural version", ReplaceWith("volts")) 32 | val volt 33 | get() = volts 34 | @Deprecated("Replaced with Plural version", ReplaceWith("watts")) 35 | val watt 36 | get() = watts 37 | 38 | val seconds get() = SIUnit(value) 39 | val meters get() = SIUnit(value) 40 | val grams get() = SIUnit(value.times(kBaseOffsetKilo)) 41 | val amps get() = SIUnit(value) 42 | val ohms get() = SIUnit(value) 43 | val volts get() = SIUnit(value) 44 | val watts get() = SIUnit(value) 45 | } 46 | 47 | val Double.yotta get() = SIUnitBuilder(times(kYotta)) 48 | val Double.zetta get() = SIUnitBuilder(times(kZetta)) 49 | val Double.exa get() = SIUnitBuilder(times(kExa)) 50 | val Double.peta get() = SIUnitBuilder(times(kPeta)) 51 | val Double.tera get() = SIUnitBuilder(times(kTera)) 52 | val Double.giga get() = SIUnitBuilder(times(kGiga)) 53 | val Double.mega get() = SIUnitBuilder(times(kMega)) 54 | val Double.kilo get() = SIUnitBuilder(times(kKilo)) 55 | val Double.hecto get() = SIUnitBuilder(times(kHecto)) 56 | val Double.deca get() = SIUnitBuilder(times(kDeca)) 57 | val Double.base get() = SIUnitBuilder(this) 58 | val Double.deci get() = SIUnitBuilder(times(kDeci)) 59 | val Double.centi get() = SIUnitBuilder(times(kCenti)) 60 | val Double.milli get() = SIUnitBuilder(times(kMilli)) 61 | val Double.micro get() = SIUnitBuilder(times(kMicro)) 62 | val Double.nano get() = SIUnitBuilder(times(kNano)) 63 | val Double.pico get() = SIUnitBuilder(times(kPico)) 64 | val Double.femto get() = SIUnitBuilder(times(kFemto)) 65 | val Double.atto get() = SIUnitBuilder(times(kAtto)) 66 | val Double.zepto get() = SIUnitBuilder(times(kZepto)) 67 | val Double.yocto get() = SIUnitBuilder(times(kYocto)) 68 | 69 | val Number.yotta get() = toDouble().yotta 70 | val Number.zetta get() = toDouble().zetta 71 | val Number.exa get() = toDouble().exa 72 | val Number.peta get() = toDouble().peta 73 | val Number.tera get() = toDouble().tera 74 | val Number.giga get() = toDouble().giga 75 | val Number.mega get() = toDouble().mega 76 | val Number.kilo get() = toDouble().kilo 77 | val Number.hecto get() = toDouble().hecto 78 | val Number.deca get() = toDouble().deca 79 | val Number.base get() = toDouble().base 80 | val Number.deci get() = toDouble().deci 81 | val Number.centi get() = toDouble().centi 82 | val Number.milli get() = toDouble().milli 83 | val Number.micro get() = toDouble().micro 84 | val Number.nano get() = toDouble().nano 85 | val Number.pico get() = toDouble().pico 86 | val Number.femto get() = toDouble().femto 87 | val Number.atto get() = toDouble().atto 88 | val Number.zepto get() = toDouble().zepto 89 | val Number.yocto get() = toDouble().yocto 90 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Acceleration.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIKey 14 | import org.ghrobotics.lib.mathematics.units.SIUnit 15 | import org.ghrobotics.lib.mathematics.units.Second 16 | 17 | typealias Acceleration = Frac, Second> 18 | 19 | typealias LinearAcceleration = Acceleration 20 | typealias AngularAcceleration = Acceleration 21 | 22 | val SIUnit.acceleration get() = SIUnit>(value) 23 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/DeprecatedConstructors.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | @Deprecated("Replaced with Plural version", ReplaceWith("radians")) 12 | val Double.radian 13 | get() = radians 14 | @Deprecated("Replaced with Plural version", ReplaceWith("degrees")) 15 | val Double.degree 16 | get() = degrees 17 | 18 | @Deprecated("Replaced with Plural version", ReplaceWith("radians")) 19 | val Number.radian 20 | get() = radians 21 | @Deprecated("Replaced with Plural version", ReplaceWith("degrees")) 22 | val Number.degree 23 | get() = degrees 24 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Inverse.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | typealias Inverse = Frac 16 | 17 | typealias Curvature = Inverse 18 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Ohm.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Ampere 12 | import org.ghrobotics.lib.mathematics.units.Frac 13 | import org.ghrobotics.lib.mathematics.units.Kilogram 14 | import org.ghrobotics.lib.mathematics.units.Meter 15 | import org.ghrobotics.lib.mathematics.units.Mult 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | import org.ghrobotics.lib.mathematics.units.Second 18 | 19 | typealias Ohm = Frac>, 20 | Mult>>>> 21 | 22 | @Deprecated("Replaced with Plural version", ReplaceWith("ohms")) 23 | val Double.ohm 24 | get() = ohms 25 | 26 | @Deprecated("Replaced with Plural version", ReplaceWith("ohms")) 27 | val Number.ohm 28 | get() = ohms 29 | 30 | val Double.ohms get() = SIUnit(this) 31 | 32 | val Number.ohms get() = toDouble().ohms 33 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Radian.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import edu.wpi.first.wpilibj.geometry.Rotation2d 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | typealias Radian = Unitless 16 | 17 | val Double.radians get() = SIUnit(this) 18 | val Double.degrees get() = SIUnit(Math.toRadians(this)) 19 | 20 | val Double.rpm get() = SIUnit(this * 0.104719755) 21 | val Number.rpm get() = toDouble().rpm 22 | 23 | val Number.radians get() = toDouble().radians 24 | val Number.degrees get() = toDouble().degrees 25 | 26 | @Deprecated("Replaced with Plural version", ReplaceWith("inRadians()")) 27 | val SIUnit.radian 28 | get() = inRadians() 29 | @Deprecated("Replaced with Plural version", ReplaceWith("inDegrees()")) 30 | val SIUnit.degree 31 | get() = inDegrees() 32 | 33 | fun SIUnit.inRadians() = value 34 | fun SIUnit.inDegrees() = Math.toDegrees(value) 35 | 36 | fun SIUnit.toRotation2d() = Rotation2d(inRadians()) 37 | fun Rotation2d.toUnbounded() = SIUnit(radians) 38 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Velocity.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIKey 14 | import org.ghrobotics.lib.mathematics.units.SIUnit 15 | import org.ghrobotics.lib.mathematics.units.Second 16 | import org.ghrobotics.lib.mathematics.units.kFeetToMeter 17 | import org.ghrobotics.lib.mathematics.units.kInchToMeter 18 | import org.ghrobotics.lib.mathematics.units.kMinuteToSecond 19 | 20 | typealias Velocity = Frac 21 | 22 | typealias LinearVelocity = Velocity 23 | typealias AngularVelocity = Velocity 24 | 25 | val SIUnit.velocity get() = SIUnit>(value) 26 | 27 | @Deprecated("", ReplaceWith("inFeetPerSecond()")) 28 | val SIUnit.feetPerSecond 29 | get() = inFeetPerSecond() 30 | @Deprecated("", ReplaceWith("inFeetPerMinute()")) 31 | val SIUnit.feetPerMinute 32 | get() = inFeetPerMinute() 33 | @Deprecated("", ReplaceWith("inInchesPerSecond()")) 34 | val SIUnit.inchesPerSecond 35 | get() = inInchesPerSecond() 36 | 37 | fun SIUnit.inFeetPerSecond() = value.div(kFeetToMeter) 38 | fun SIUnit.inFeetPerMinute() = inFeetPerSecond().times(kMinuteToSecond) 39 | fun SIUnit.inInchesPerSecond() = value.div(kInchToMeter) 40 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Volt.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Ampere 12 | import org.ghrobotics.lib.mathematics.units.Frac 13 | import org.ghrobotics.lib.mathematics.units.Kilogram 14 | import org.ghrobotics.lib.mathematics.units.Meter 15 | import org.ghrobotics.lib.mathematics.units.Mult 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | import org.ghrobotics.lib.mathematics.units.Second 18 | 19 | typealias Volt = Frac>, 20 | Mult>>> 21 | 22 | @Deprecated("Replaced with Plural version", ReplaceWith("volts")) 23 | val Double.volt 24 | get() = volts 25 | 26 | @Deprecated("Replaced with Plural version", ReplaceWith("volts")) 27 | val Number.volt 28 | get() = volts 29 | 30 | val Double.volts get() = SIUnit(this) 31 | 32 | val Number.volts get() = toDouble().volts 33 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/derived/Watt.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.derived 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.Kilogram 13 | import org.ghrobotics.lib.mathematics.units.Meter 14 | import org.ghrobotics.lib.mathematics.units.Mult 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.Second 17 | 18 | typealias Watt = Frac>, 19 | Mult>> 20 | 21 | @Deprecated("Replaced with Plural version", ReplaceWith("watts")) 22 | val Double.watt 23 | get() = watts 24 | 25 | @Deprecated("Replaced with Plural version", ReplaceWith("watts")) 26 | val Number.watt 27 | get() = watts 28 | 29 | val Double.watts get() = SIUnit(this) 30 | 31 | val Number.watts get() = toDouble().watts 32 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/nativeunit/NativeUnit.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.nativeunit 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.derived.Acceleration 14 | import org.ghrobotics.lib.mathematics.units.derived.Velocity 15 | 16 | object NativeUnit : SIKey 17 | 18 | val Double.nativeUnits get() = SIUnit(this) 19 | val Double.STU get() = nativeUnits 20 | 21 | val Number.nativeUnits get() = toDouble().nativeUnits 22 | val Number.STU get() = toDouble().STU 23 | 24 | fun SIUnit.toNativeUnitPosition(model: NativeUnitModel): SIUnit = 25 | model.toNativeUnitPosition(this) 26 | 27 | fun SIUnit.fromNativeUnitPosition(model: NativeUnitModel): SIUnit = 28 | model.fromNativeUnitPosition(this) 29 | 30 | typealias NativeUnitVelocity = Velocity 31 | 32 | val Double.nativeUnitsPer100ms get() = SIUnit(times(10.0)) 33 | val Double.STUPer100ms get() = nativeUnitsPer100ms 34 | 35 | val Number.nativeUnitsPer100ms get() = toDouble().nativeUnitsPer100ms 36 | val Number.STUPer100ms get() = toDouble().STUPer100ms 37 | 38 | @Deprecated("", ReplaceWith("inNativeUnitsPer100ms()")) 39 | val SIUnit.nativeUnitsPer100ms 40 | get() = inNativeUnitsPer100ms() 41 | @Deprecated("", ReplaceWith("inSTUPer100ms()")) 42 | val SIUnit.STUPer100ms 43 | get() = inSTUPer100ms() 44 | 45 | fun SIUnit.inNativeUnitsPer100ms() = value.div(10.0) 46 | fun SIUnit.inSTUPer100ms() = inNativeUnitsPer100ms() 47 | 48 | fun SIUnit>.toNativeUnitVelocity(model: NativeUnitModel): SIUnit = 49 | model.toNativeUnitVelocity(this) 50 | 51 | fun SIUnit.fromNativeUnitVelocity(model: NativeUnitModel): SIUnit> = 52 | model.fromNativeUnitVelocity(this) 53 | 54 | typealias NativeUnitAcceleration = Acceleration 55 | 56 | val Double.nativeUnitsPer100msPerSecond get() = SIUnit(times(10.0)) 57 | val Double.STUPer100msPerSecond get() = nativeUnitsPer100msPerSecond 58 | 59 | val Number.nativeUnitsPer100msPerSecond get() = toDouble().nativeUnitsPer100msPerSecond 60 | val Number.STUPer100msPerSecond get() = toDouble().STUPer100msPerSecond 61 | 62 | @Deprecated("", ReplaceWith("inNativeUnitsPer100msPerSecond()")) 63 | val SIUnit.nativeUnitsPer100msPerSecond 64 | get() = inNativeUnitsPer100msPerSecond() 65 | @Deprecated("", ReplaceWith("inSTUPer100msPerSecond()")) 66 | val SIUnit.STUPer100msPerSecond 67 | get() = inSTUPer100msPerSecond() 68 | 69 | fun SIUnit.inNativeUnitsPer100msPerSecond() = value.div(10.0) 70 | fun SIUnit.inSTUPer100msPerSecond() = inNativeUnitsPer100msPerSecond() 71 | 72 | fun SIUnit>.toNativeUnitAcceleration(model: NativeUnitModel): SIUnit = 73 | model.toNativeUnitAcceleration(this) 74 | 75 | fun SIUnit.fromNativeUnitAcceleration(model: NativeUnitModel): SIUnit> = 76 | model.fromNativeUnitAcceleration(this) 77 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/nativeunit/NativeUnitModel.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.nativeunit 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIKey 14 | import org.ghrobotics.lib.mathematics.units.SIUnit 15 | import org.ghrobotics.lib.mathematics.units.derived.Acceleration 16 | import org.ghrobotics.lib.mathematics.units.derived.Radian 17 | import org.ghrobotics.lib.mathematics.units.derived.Velocity 18 | import org.ghrobotics.lib.mathematics.units.operations.div 19 | import org.ghrobotics.lib.mathematics.units.operations.times 20 | 21 | abstract class NativeUnitModel { 22 | 23 | abstract fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit 24 | abstract fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit 25 | 26 | open fun toNativeUnitError(modelledUnit: SIUnit): SIUnit = 27 | toNativeUnitPosition(modelledUnit) - toNativeUnitPosition(SIUnit(0.0)) 28 | 29 | open fun fromNativeUnitVelocity(nativeUnitVelocity: SIUnit): SIUnit> = 30 | SIUnit(fromNativeUnitPosition(SIUnit(nativeUnitVelocity.value)).value) 31 | 32 | open fun toNativeUnitVelocity(modelledUnitVelocity: SIUnit>): SIUnit = 33 | SIUnit(toNativeUnitPosition(SIUnit(modelledUnitVelocity.value)).value) 34 | 35 | open fun fromNativeUnitAcceleration(nativeUnitAcceleration: SIUnit): SIUnit> = 36 | SIUnit(fromNativeUnitVelocity(SIUnit(nativeUnitAcceleration.value)).value) 37 | 38 | open fun toNativeUnitAcceleration(modelledUnitAcceleration: SIUnit>): SIUnit = 39 | SIUnit(toNativeUnitVelocity(SIUnit(modelledUnitAcceleration.value)).value) 40 | } 41 | 42 | object DefaultNativeUnitModel : NativeUnitModel() { 43 | override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = nativeUnits 44 | override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = modelledUnit 45 | } 46 | 47 | class NativeUnitLengthModel( 48 | val nativeUnitsPerRotation: SIUnit, 49 | val wheelRadius: SIUnit 50 | ) : NativeUnitModel() { 51 | override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = 52 | wheelRadius * ((nativeUnits / nativeUnitsPerRotation) * (2.0 * Math.PI)) 53 | 54 | override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = 55 | nativeUnitsPerRotation * (modelledUnit / (wheelRadius * (2.0 * Math.PI))) 56 | } 57 | 58 | class NativeUnitRotationModel( 59 | val nativeUnitsPerRotation: SIUnit 60 | ) : NativeUnitModel() { 61 | override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = 62 | (modelledUnit / (2.0 * Math.PI)) * nativeUnitsPerRotation 63 | 64 | override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = 65 | 2.0 * Math.PI * (nativeUnits / nativeUnitsPerRotation) 66 | } 67 | 68 | class SlopeNativeUnitModel( 69 | val modelledSample: SIUnit, 70 | val nativeUnitSample: SIUnit 71 | ) : NativeUnitModel() { 72 | private val slope: SIUnit> = modelledSample / nativeUnitSample 73 | 74 | override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = 75 | nativeUnits * slope 76 | 77 | override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = 78 | modelledUnit / slope 79 | } 80 | 81 | fun SlopeNativeUnitModel.wheelRadius(sensorUnitsPerRotation: SIUnit): SIUnit = 82 | modelledSample / (nativeUnitSample / sensorUnitsPerRotation) / 2.0 / Math.PI 83 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/ADivFracABEqualsB.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | operator fun SIUnit.div(other: SIUnit>) = SIUnit(value.div(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/ADivUnitlessEqualsA.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | operator fun SIUnit.div(other: SIUnit) = SIUnit(value.div(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/ATimesBEqualsATimesB.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Mult 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | operator fun SIUnit.times(other: SIUnit) = SIUnit>(value.times(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/ATimesFracBAEqualsB.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | operator fun SIUnit.times(other: SIUnit>) = SIUnit(value.times(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/ATimesUnitlessEqualsA.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | operator fun SIUnit.times(other: SIUnit) = SIUnit(value.times(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/AdivAEqualsUnitless.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | operator fun SIUnit.div(other: SIUnit) = SIUnit(value.div(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/AdivBEqualsAdivB.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | operator fun SIUnit.div(other: SIUnit) = SIUnit>(value.div(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/DoubleTimesAEqualsA.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | 14 | operator fun Double.times(other: SIUnit) = other.times(this) 15 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/FracABTimesBEqualsA.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Frac 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | operator fun SIUnit>.times(other: SIUnit) = SIUnit(value.times(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/MultABDivAEqualsB.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.Mult 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | 15 | // m^3 / m^2 = m 16 | 17 | // SIUnit, Meter> / SIUnit> = SIUnit 18 | // SIUnit> / SIUnit = SIUnit 19 | 20 | operator fun SIUnit>.div(other: SIUnit) = SIUnit(value.div(other.value)) 21 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/operations/UnitlessTimesAEqualsA.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.operations 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.Unitless 14 | 15 | operator fun SIUnit.times(other: SIUnit) = SIUnit(value.times(other.value)) 16 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/mathematics/units/specialops/AngularMultiplication.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.units.specialops 10 | 11 | import org.ghrobotics.lib.mathematics.units.Meter 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.derived.AngularVelocity 14 | import org.ghrobotics.lib.mathematics.units.derived.LinearVelocity 15 | 16 | operator fun SIUnit.times(distance: SIUnit): SIUnit { 17 | return SIUnit(value * distance.value) 18 | } 19 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/types/CSVWritable.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.types 10 | 11 | interface CSVWritable { 12 | fun toCSV(): String 13 | } 14 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/types/Interpolatable.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.types 10 | 11 | interface Interpolatable { 12 | 13 | /** 14 | * The function calculates an interpolated value along the fraction 15 | * `t` between `0.0` and `1.0`. When `t` = 1.0, 16 | * `endVal` is returned. 17 | * 18 | * @param endValue 19 | * target value 20 | * @param t 21 | * fraction between `0.0` and `1.0` 22 | * @return interpolated value 23 | */ 24 | fun interpolate(endValue: T, t: Double): T 25 | } 26 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/types/VaryInterpolatable.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.types 10 | 11 | interface VaryInterpolatable : Interpolatable { 12 | fun distance(other: S): Double 13 | } 14 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/CircularBuffer.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import java.util.ArrayList 12 | 13 | class CircularBuffer(private val size: Int) { 14 | 15 | private val buffer: ArrayList = ArrayList(size) 16 | 17 | private var numElements = 0 18 | private var sum = 0.0 19 | 20 | // Gets average of all elements 21 | val average: Double 22 | get() { 23 | return if (numElements == 0) 24 | 0.0 25 | else 26 | sum / numElements 27 | } 28 | 29 | // Adds an element to the list 30 | fun add(element: Double) { 31 | if (numElements > size - 1) { 32 | sum -= buffer[size - 1] 33 | buffer.removeAt(size - 1) 34 | numElements-- 35 | } 36 | sum += element 37 | buffer.add(0, element) 38 | numElements++ 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/Collections.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import java.util.HashSet 12 | 13 | fun Collection.plusToSet(other: Collection): Set { 14 | val result = HashSet(size + other.size) 15 | result.addAll(this) 16 | result.addAll(other) 17 | return result 18 | } 19 | 20 | fun Collection.plusToSet(other: Array): Set { 21 | val result = HashSet(size + other.size) 22 | result.addAll(this) 23 | result.addAll(other) 24 | return result 25 | } 26 | 27 | inline fun Collection.mapToSet(transform: (T) -> R): Set = mapTo(HashSet(size), transform) 28 | 29 | inline fun Collection.flatMapToSet(multiplier: Int, transform: (T) -> Iterable): Set = 30 | flatMapTo(HashSet(size * multiplier), transform) 31 | 32 | inline fun Collection.mapNotNullToSet(transform: (T) -> R?): Set = 33 | mapNotNullTo(HashSet(size), transform) 34 | 35 | inline fun Collection.filterNotToSet(predicate: (T) -> Boolean): Set = filterNotTo(HashSet(size), predicate) 36 | 37 | fun Collection.combinationPairs(): Set> { 38 | val result = HashSet>(size) 39 | for (p1 in this) { 40 | for (p2 in this) { 41 | if (p1 == p2 || result.contains(p2 to p1)) continue 42 | result += p1 to p2 43 | } 44 | } 45 | return result 46 | } 47 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/CoroutineUtils.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import kotlinx.coroutines.DisposableHandle 12 | 13 | inline fun disposableHandle(crossinline block: () -> Unit) = object : DisposableHandle { 14 | override fun dispose() { 15 | block() 16 | } 17 | } 18 | 19 | fun disposableHandle(vararg handles: DisposableHandle) = disposableHandle(handles.asList()) 20 | 21 | fun disposableHandle(handles: Collection) = disposableHandle { 22 | handles.forEach { it.dispose() } 23 | } 24 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/DeltaTime.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIUnit 12 | import org.ghrobotics.lib.mathematics.units.Second 13 | import org.ghrobotics.lib.mathematics.units.seconds 14 | 15 | class DeltaTime constructor(startTime: SIUnit = (-1.0).seconds) { 16 | 17 | var currentTime = startTime 18 | private set 19 | var deltaTime = 0.0.seconds 20 | private set 21 | 22 | fun updateTime(newTime: SIUnit): SIUnit { 23 | deltaTime = if (currentTime.value < 0.0) { 24 | 0.0.seconds 25 | } else { 26 | newTime - currentTime 27 | } 28 | currentTime = newTime 29 | return deltaTime 30 | } 31 | 32 | fun reset() { 33 | currentTime = (-1.0).seconds 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/InterpolatingTreeMap.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import java.util.TreeMap 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.operations.div 15 | import org.ghrobotics.lib.mathematics.units.unitlessValue 16 | import org.ghrobotics.lib.types.Interpolatable 17 | 18 | /** 19 | * Creates an interpolating tree map. An interpolating tree map is similar 20 | * to a tree map, except we can interpolate between the values of two keys. 21 | * 22 | * @param interpolatingFunc A function that interpolates between two [V] values. 23 | */ 24 | open class InterpolatingTreeMap(val interpolatingFunc: (start: V, end: V, t: Double) -> V) : 25 | TreeMap, V>() { 26 | /** 27 | * Returns an interpolated value based on the given key. 28 | * 29 | * @param key The key. 30 | * @return The interpolated value. 31 | */ 32 | override operator fun get(key: SIUnit): V? { 33 | // If the map is empty, return null. 34 | if (isEmpty()) return null 35 | 36 | // If we have the exact value that we're looking for, then return it. 37 | super.get(key)?.let { return it } 38 | 39 | // Get the top and bottom entries for this distance. 40 | val topBound = ceilingEntry(key) 41 | val bottomBound = floorEntry(key) 42 | 43 | return when { 44 | // When there are no more elements at the top, return the highest element. 45 | topBound == null -> bottomBound.value 46 | 47 | // When there are no more elements at the bottom, return the lowest element. 48 | bottomBound == null -> topBound.value 49 | 50 | // If there is a ceiling and a floor, interpolate between the two values. 51 | else -> interpolatingFunc( 52 | bottomBound.value, 53 | topBound.value, 54 | ((key - bottomBound.key) / (topBound.key - bottomBound.key)).unitlessValue 55 | ) 56 | } 57 | } 58 | 59 | operator fun set(key: SIUnit, value: V) = this.put(key, value) 60 | 61 | companion object { 62 | /** 63 | * Creates an interpolating tree map that contains two SIUnit types. 64 | */ 65 | fun createFromSI() = InterpolatingTreeMap> { start, end, t -> 66 | start.lerp(end, t) 67 | } 68 | 69 | /** 70 | * Creats an interpolating tree map from a type that implements [Interpolatable]. 71 | */ 72 | fun > createFromInterpolatable() = 73 | InterpolatingTreeMap { start, end, t -> 74 | start.interpolate(end, t) 75 | } 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/InterpolatingTreeMapBuffer.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.types.Interpolatable 14 | 15 | /** 16 | * Creates an [InterpolatingTreeMap], but with a certain buffer size. 17 | */ 18 | class InterpolatingTreeMapBuffer( 19 | interpolatingFunc: (start: V, end: V, t: Double) -> V, 20 | private val bufferSize: SIUnit, 21 | private val source: Source> 22 | ) : InterpolatingTreeMap(interpolatingFunc) { 23 | /** 24 | * Removes old entries from the map. 25 | */ 26 | private fun clean() { 27 | // Get the current value of the unit represented by [K]. 28 | val now = source() 29 | 30 | // Remove entries that are older than the buffer size. 31 | while (isNotEmpty()) { 32 | val entry = lastEntry() 33 | if (now - entry.key > bufferSize) { 34 | remove(entry.key) 35 | } else { 36 | return 37 | } 38 | } 39 | } 40 | 41 | /** 42 | * Adds a new value into the map. 43 | * 44 | * @param key The key. 45 | * @param value The value. 46 | * 47 | * @return The previous value associated with the same key, or null if 48 | * there was no previous value. 49 | */ 50 | override fun put(key: SIUnit, value: V): V? { 51 | // Remove old entries from the map. 52 | clean() 53 | 54 | // Add the new value into the map. 55 | return super.put(key, value) 56 | } 57 | 58 | companion object { 59 | /** 60 | * Creates an interpolating tree map buffer that contains two SIUnit types. 61 | */ 62 | fun createFromSI( 63 | bufferSize: SIUnit, 64 | source: Source> 65 | ) = InterpolatingTreeMapBuffer>({ start, end, t -> 66 | start.lerp(end, t) 67 | }, bufferSize, source) 68 | 69 | /** 70 | * Creats an interpolating tree map buffer from a type that implements [Interpolatable]. 71 | */ 72 | fun > createFromInterpolatable( 73 | bufferSize: SIUnit, 74 | source: Source> 75 | ) = InterpolatingTreeMapBuffer({ start, end, t -> 76 | start.interpolate(end, t) 77 | }, bufferSize, source) 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/MiscExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/MiscellaneousExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Transform2d 13 | 14 | /** 15 | * Converts a Pose2d to a Transform2d. 16 | */ 17 | fun Pose2d.toTransform(): Transform2d = minus(Pose2d()) 18 | 19 | /** 20 | * Converts a Transform2d to a Pose2d. 21 | */ 22 | fun Transform2d.toPose(): Pose2d = Pose2d(translation, rotation) 23 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/OtherUtils.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import kotlin.math.max 12 | import kotlin.math.min 13 | 14 | fun Double.safeRangeTo(endInclusive: Double) = min(this, endInclusive)..max(this, endInclusive) 15 | 16 | fun String.capitalizeEachWord() = buildString(length) { 17 | var previousWasSpace = true 18 | for (letter in this@capitalizeEachWord) { 19 | append( 20 | if (previousWasSpace) { 21 | previousWasSpace = false 22 | letter.toUpperCase() 23 | } else letter.toLowerCase() 24 | ) 25 | if (letter.isWhitespace()) previousWasSpace = true 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/Source.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import kotlin.math.absoluteValue 12 | import kotlin.math.sign 13 | 14 | typealias Source = () -> T 15 | 16 | typealias DoubleSource = Source 17 | typealias BooleanSource = Source 18 | 19 | fun Source.withMerge( 20 | other: Source, 21 | block: (T, O) -> P 22 | ): Source

= { block(this@withMerge(), other()) } 23 | 24 | fun Source.withEquals(other: O): BooleanSource = { this@withEquals() == other } 25 | fun Source.withEquals(other: Source): BooleanSource = { this@withEquals() == other() } 26 | 27 | fun Source.map(block: (T) -> O): Source = { block(this@map()) } 28 | 29 | @Suppress("FunctionName") 30 | fun Source(value: T): Source = { value } 31 | 32 | @Suppress("FunctionName") 33 | @Deprecated("Redundant", ReplaceWith("value")) 34 | fun Source(value: () -> T): Source = value 35 | 36 | fun DoubleSource.withThreshold(threshold: Double = 0.5): BooleanSource = map { this@withThreshold() >= threshold } 37 | 38 | fun DoubleSource.withDeadband( 39 | deadband: Double, 40 | scaleDeadband: Boolean = true, 41 | maxMagnitude: Double = 1.0 42 | ): DoubleSource = map { 43 | val currentValue = this@withDeadband() 44 | if (currentValue in (-deadband)..deadband) return@map 0.0 // in deadband 45 | // outside deadband 46 | if (!scaleDeadband) return@map currentValue 47 | // scale so deadband is effective 0 48 | ((currentValue.absoluteValue - deadband) / (maxMagnitude - deadband)) * currentValue.sign 49 | } 50 | 51 | // Boolean Sources 52 | 53 | fun BooleanSource.map(trueMap: T, falseMap: T) = map( 54 | Source( 55 | trueMap 56 | ), Source(falseMap) 57 | ) 58 | 59 | fun BooleanSource.map(trueMap: Source, falseMap: T) = map( 60 | trueMap, 61 | Source(falseMap) 62 | ) 63 | 64 | fun BooleanSource.map(trueMap: T, falseMap: Source) = map( 65 | Source( 66 | trueMap 67 | ), falseMap 68 | ) 69 | 70 | fun BooleanSource.map(trueMap: Source, falseMap: Source) = map { if (it) trueMap() else falseMap() } 71 | 72 | operator fun BooleanSource.not() = map { !it } 73 | infix fun BooleanSource.and(other: BooleanSource) = withMerge(other) { one, two -> one and two } 74 | infix fun BooleanSource.or(other: BooleanSource) = withMerge(other) { one, two -> one or two } 75 | infix fun BooleanSource.xor(other: BooleanSource) = withMerge(other) { one, two -> one or two } 76 | 77 | // Comparable Sources 78 | 79 | fun Source>.compareTo(other: T) = map { it.compareTo(other) } 80 | fun Source>.compareTo(other: Source) = withMerge(other) { one, two -> one.compareTo(two) } 81 | 82 | fun Source>.greaterThan(other: T) = compareTo(other).map { it > 0 } 83 | fun Source>.equalTo(other: T) = compareTo(other).map { it == 0 } 84 | fun Source>.lessThan(other: T) = compareTo(other).map { it < 0 } 85 | fun Source>.greaterThan(other: Source) = compareTo(other).map { it > 0 } 86 | fun Source>.equalTo(other: Source) = compareTo(other).map { it == 0 } 87 | fun Source>.lessThan(other: Source) = compareTo(other).map { it < 0 } 88 | 89 | @JvmName("reversedGreaterThan") 90 | fun Source.greaterThan(other: Source>) = other.compareTo(this).map { it < 0 } 91 | 92 | @JvmName("reversedEqualTo") 93 | fun Source.equalTo(other: Source>) = other.compareTo(this).map { it == 0 } 94 | 95 | @JvmName("reversedLessThan") 96 | fun Source.lessThan(other: Source>) = other.compareTo(this).map { it > 0 } 97 | -------------------------------------------------------------------------------- /core/src/main/kotlin/org/ghrobotics/lib/utils/SourceMonitor.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | val Source.monitor get() = SourceMonitor(this) 12 | 13 | class SourceMonitor( 14 | val source: Source 15 | ) { 16 | var lastValue = source() 17 | 18 | inline fun onChange(block: (T) -> Unit) { 19 | val newValue = source() 20 | if (newValue != lastValue) block(newValue) 21 | lastValue = newValue 22 | } 23 | 24 | inline fun onWhen(value: T, block: () -> Unit) { 25 | if (lastValue == value) block() 26 | onChange { if (it == value) block() } 27 | } 28 | } 29 | 30 | fun SourceMonitor.onChangeToTrue(block: () -> Unit) = onChange { if (it) block() } 31 | fun SourceMonitor.onChangeToFalse(block: () -> Unit) = onChange { if (!it) block() } 32 | fun SourceMonitor.onWhenToTrue(block: () -> Unit) = onWhen(true, block) 33 | fun SourceMonitor.onWhenToFalse(block: () -> Unit) = onWhen(false, block) 34 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/DerivedTests.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import org.ghrobotics.lib.mathematics.units.amps 12 | import org.ghrobotics.lib.mathematics.units.derived.feetPerMinute 13 | import org.ghrobotics.lib.mathematics.units.derived.feetPerSecond 14 | import org.ghrobotics.lib.mathematics.units.derived.volts 15 | import org.ghrobotics.lib.mathematics.units.inMeters 16 | import org.ghrobotics.lib.mathematics.units.inSeconds 17 | import org.ghrobotics.lib.mathematics.units.meters 18 | import org.ghrobotics.lib.mathematics.units.operations.div 19 | import org.ghrobotics.lib.mathematics.units.operations.times 20 | import org.ghrobotics.lib.mathematics.units.seconds 21 | import org.ghrobotics.lib.mathematics.units.unitlessValue 22 | import org.junit.Test 23 | 24 | class DerivedTests { 25 | 26 | @Test 27 | fun testVelocity() { 28 | val one = 5.meters 29 | val two = 2.seconds 30 | 31 | val three = one / two 32 | 33 | assert(three.value == 2.5) 34 | } 35 | 36 | @Test 37 | fun testVelocityAdjust() { 38 | val one = 5.meters 39 | val two = 2.seconds 40 | 41 | val three = one / two 42 | 43 | val four = three.feetPerMinute 44 | 45 | assert(four epsilonEquals 492.12598425196853) 46 | } 47 | 48 | @Test 49 | fun testAcceleration() { 50 | val one = 10.meters / 2.seconds / 4.seconds 51 | 52 | assert(one.value == 1.25) 53 | } 54 | 55 | @Test 56 | fun testAccelerationToVelocity() { 57 | val one = 10.meters / 1.6.seconds / 2.seconds 58 | val two = 5.seconds 59 | 60 | val three = one * two 61 | 62 | val four = three.feetPerSecond 63 | 64 | assert(four epsilonEquals 51.26312335958006) 65 | } 66 | 67 | @Test 68 | fun testVelocityToLength() { 69 | val one = 5.meters / 2.seconds 70 | val two = 6.seconds 71 | 72 | val three = one * two 73 | val four = three.inMeters() 74 | 75 | assert(four == 15.0) 76 | } 77 | 78 | @Test 79 | fun testVelocityAndAccelerationToTime() { 80 | val one = 22.meters / 2.seconds 81 | val two = 18.meters / 0.5.seconds / 4.seconds 82 | 83 | val three = one / two 84 | 85 | assert(three.inSeconds() epsilonEquals 1.2222222222222223) 86 | } 87 | 88 | @Test 89 | fun testAccelerationDividedByAcceleration() { 90 | val one = 33.meters / 1.seconds / 1.seconds 91 | val two = 22.meters / 2.seconds / 1.seconds 92 | 93 | val three = (one / two).unitlessValue 94 | 95 | assert(three == 3.0) 96 | } 97 | 98 | @Test 99 | fun testVelocityDividedByVelocity() { 100 | val one = 33.meters / 1.seconds 101 | val two = 22.meters / 2.seconds 102 | 103 | val three = (one / two).unitlessValue 104 | 105 | assert(three epsilonEquals 3.0) 106 | } 107 | 108 | @Test 109 | fun testVoltage() { 110 | val one = 1.volts 111 | val two = 5.amps 112 | 113 | val three = one * two 114 | 115 | assert(three.value epsilonEquals 5.0) 116 | } 117 | 118 | @Test 119 | fun testOhm() { 120 | val one = 2.volts 121 | val two = 5.amps 122 | 123 | val three = one / two 124 | 125 | assert(three.value epsilonEquals 0.4) 126 | } 127 | } 128 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/LengthTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import org.ghrobotics.lib.mathematics.units.centi 12 | import org.ghrobotics.lib.mathematics.units.inInches 13 | import org.ghrobotics.lib.mathematics.units.inMeters 14 | import org.ghrobotics.lib.mathematics.units.inMillimeters 15 | import org.ghrobotics.lib.mathematics.units.inches 16 | import org.ghrobotics.lib.mathematics.units.meters 17 | import org.ghrobotics.lib.mathematics.units.milli 18 | import org.junit.Test 19 | 20 | class LengthTest { 21 | 22 | @Test 23 | fun testLength() { 24 | val one = 1.0.meters 25 | val two = 12.0.inches 26 | 27 | val three = one + two 28 | 29 | assert(three.inMeters() epsilonEquals 1.3048) 30 | } 31 | 32 | @Test 33 | fun testPrefix() { 34 | val one = 1.0.meters 35 | val two = 100.centi.meters 36 | 37 | val three = one + two 38 | 39 | assert(three.inMillimeters() epsilonEquals 2000.0) 40 | } 41 | 42 | @Test 43 | fun testScalar() { 44 | val one = 12.meters 45 | 46 | val two = one / 3.0 47 | val three = two * 3.0 48 | 49 | assert(two.inMeters() epsilonEquals 4.0) 50 | assert(three.inMeters() epsilonEquals 12.0) 51 | } 52 | 53 | @Test 54 | fun testToMetric() { 55 | val one = 40.inches 56 | 57 | val two = one.inMillimeters() 58 | 59 | assert(two.toInt() == 1016) 60 | } 61 | 62 | @Test 63 | fun testFromMetric() { 64 | val one = 1016.milli.meters 65 | 66 | val two = one.inInches() 67 | 68 | assert(two.toInt() == 40) 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/NumericalApproximationsTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import kotlin.math.pow 12 | import org.junit.Assert 13 | import org.junit.Test 14 | 15 | class NumericalApproximationsTest { 16 | @Test 17 | fun testNewtonsMethod1() { 18 | val f = { x: Double -> 18 - x.pow(2) } 19 | val fPrime = { x: Double -> -2 * x } 20 | val iteratons = 3 21 | val x1 = 4.1 22 | 23 | val approximatedZero = NumericalApproximations.newtonsMethod(iteratons, x1, f, fPrime) 24 | Assert.assertEquals(4.242640687, approximatedZero, kEpsilon) 25 | } 26 | 27 | @Test 28 | fun testNewtonsMethod2() { 29 | val f = { x: Double -> x.pow(3) + 4 * x + 8 } 30 | val fPrime = { x: Double -> 3 * x.pow(2) + 4 } 31 | val iteratons = 3 32 | val x1 = -1.3 33 | 34 | val approximatedZero = NumericalApproximations.newtonsMethod(iteratons, x1, f, fPrime) 35 | Assert.assertEquals(-1.364655608, approximatedZero, kEpsilon) 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/TimeTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import org.ghrobotics.lib.mathematics.units.days 12 | import org.ghrobotics.lib.mathematics.units.inDays 13 | import org.ghrobotics.lib.mathematics.units.inMinutes 14 | import org.ghrobotics.lib.mathematics.units.minutes 15 | import org.ghrobotics.lib.mathematics.units.seconds 16 | import org.junit.Test 17 | 18 | class TimeTest { 19 | 20 | @Test 21 | fun testDivision() { 22 | val one = 45.days 23 | 24 | val two = one / 3 25 | 26 | assert(two.inDays() == 15.0) 27 | } 28 | 29 | @Test 30 | fun testAddition() { 31 | val one = 2.5.minutes 32 | val two = 360.seconds 33 | 34 | val three = one + two 35 | 36 | assert(three.inMinutes() epsilonEquals 8.5) 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/UnitTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics 10 | 11 | import org.ghrobotics.lib.mathematics.units.derived.radians 12 | import org.ghrobotics.lib.mathematics.units.derived.rpm 13 | import org.ghrobotics.lib.mathematics.units.derived.velocity 14 | import org.ghrobotics.lib.mathematics.units.feet 15 | import org.ghrobotics.lib.mathematics.units.inInches 16 | import org.ghrobotics.lib.mathematics.units.inLbs 17 | import org.ghrobotics.lib.mathematics.units.inMeters 18 | import org.ghrobotics.lib.mathematics.units.inches 19 | import org.ghrobotics.lib.mathematics.units.kilo 20 | import org.ghrobotics.lib.mathematics.units.meters 21 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitLengthModel 22 | import org.ghrobotics.lib.mathematics.units.nativeunit.fromNativeUnitPosition 23 | import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100ms 24 | import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100msPerSecond 25 | import org.ghrobotics.lib.mathematics.units.nativeunit.nativeUnits 26 | import org.ghrobotics.lib.mathematics.units.nativeunit.toNativeUnitAcceleration 27 | import org.ghrobotics.lib.mathematics.units.nativeunit.toNativeUnitVelocity 28 | import org.ghrobotics.lib.mathematics.units.operations.div 29 | import org.ghrobotics.lib.mathematics.units.seconds 30 | import org.junit.Test 31 | 32 | class UnitTest { 33 | 34 | private val settings = NativeUnitLengthModel( 35 | 1440.nativeUnits, 36 | 3.0.inches 37 | ) 38 | 39 | @Test 40 | fun testNativeUnits() { 41 | val nativeUnits = 360.nativeUnits.fromNativeUnitPosition(settings) 42 | 43 | assert(nativeUnits.inInches() epsilonEquals 4.71238898038469) 44 | } 45 | 46 | @Test 47 | fun testVelocitySTU() { 48 | val one = 1.meters / 1.seconds 49 | 50 | val two = one.toNativeUnitVelocity(settings) 51 | 52 | val three = two.inNativeUnitsPer100ms() 53 | 54 | assert(three.toInt() == 300) 55 | } 56 | 57 | @Test 58 | fun testAccelerationSTU() { 59 | val one = 1.meters / 1.seconds / 1.seconds 60 | 61 | val two = one.toNativeUnitAcceleration(settings) 62 | 63 | assert(two.inNativeUnitsPer100msPerSecond().toInt() == 300) 64 | } 65 | 66 | @Test 67 | fun testFeetToMeter() { 68 | val one = 1.feet 69 | 70 | assert(one.inMeters() epsilonEquals 0.3048) 71 | } 72 | 73 | @Test 74 | fun testKgToPound() { 75 | val kg = 2.kilo.grams 76 | assert(kg.inLbs() epsilonEquals 4.409248840367555) 77 | } 78 | 79 | @Test 80 | fun testUnboundedRotationUnits() { 81 | val speed = 250.radians.velocity 82 | assert(speed.value epsilonEquals 250.0) 83 | } 84 | 85 | @Test 86 | fun testRPM() { 87 | val speed = 3000.rpm 88 | assert(speed.value epsilonEquals 314.159265) 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/threedim/GeometryTests.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.threedim 10 | 11 | import org.ghrobotics.lib.mathematics.epsilonEquals 12 | import org.ghrobotics.lib.mathematics.threedim.geometry.Pose3d 13 | import org.ghrobotics.lib.mathematics.threedim.geometry.Quaternion 14 | import org.ghrobotics.lib.mathematics.threedim.geometry.Transform 15 | import org.ghrobotics.lib.mathematics.threedim.geometry.Translation3d 16 | import org.ghrobotics.lib.mathematics.units.milli 17 | import org.junit.Test 18 | 19 | class GeometryTests { 20 | 21 | @Test 22 | fun testQuaternionMul() { 23 | val one = Quaternion.fromEulerAngles(Math.toRadians(45.0), 0.0, 0.0) 24 | val two = Quaternion.fromEulerAngles(Math.toRadians(25.0), 0.0, 0.0) 25 | 26 | val three = one * two 27 | 28 | assert(Math.toDegrees(three.eulerAngles.x) epsilonEquals 70.0) 29 | } 30 | 31 | @Test 32 | fun testQuaternionDiv() { 33 | val one = Quaternion.fromEulerAngles(Math.toRadians(70.0), 0.0, 0.0) 34 | val two = Quaternion.fromEulerAngles(Math.toRadians(25.0), 0.0, 0.0) 35 | 36 | val three = one / two 37 | 38 | assert(Math.toDegrees(three.eulerAngles.x) epsilonEquals 45.0) 39 | } 40 | 41 | @Test 42 | fun testPose3dAdd() { 43 | val drive = Pose3d( 44 | Translation3d(0.5, 0.25, 0.5), 45 | Quaternion.fromEulerAngles(Math.toRadians(0.0), 0.0, 0.0) 46 | ) 47 | val elevator = Pose3d( 48 | Translation3d(0.0, 1.25, 0.0), 49 | Quaternion.fromEulerAngles(0.0, Math.toRadians(25.0), 0.0) 50 | ) 51 | val arm = Pose3d( 52 | Translation3d(0.75, 0.0, 0.0), 53 | Quaternion.fromEulerAngles(0.0, 0.0, 0.0) 54 | ) 55 | 56 | val actualArmPose = drive + elevator + arm 57 | 58 | assert( 59 | actualArmPose.translation epsilonEquals Translation3d( 60 | 1.1797308402774875, 61 | 1.8169636963055247, 62 | 0.5 63 | ) 64 | ) 65 | } 66 | 67 | @Test 68 | fun testAccelerationTransform() { 69 | val armRotation = Quaternion.fromAxisAngle(Math.toRadians(25.0), Translation3d(0.0, 0.0, 1.0)) 70 | 71 | val firstState = Transform( 72 | Translation3d(0.0, 0.0, 0.0), 73 | armRotation 74 | ) 75 | val secondState = Transform( 76 | Translation3d(0.0, 1.0, 0.0), 77 | armRotation 78 | ) 79 | 80 | val deltaState = (secondState - firstState) / 20.milli.seconds.value 81 | 82 | println(deltaState.translation * armRotation) 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/twodim/geometry/GeometryTests.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.geometry 10 | 11 | import org.ghrobotics.lib.mathematics.units.meters 12 | import org.junit.Test 13 | 14 | class GeometryTests { 15 | @Test 16 | fun testRectangleContains() { 17 | val rectangle = Rectangle2d(Translation2d(0.0.meters, 0.0.meters), Translation2d(10.0.meters, 10.0.meters)) 18 | val translation = Translation2d(5.0.meters, 7.0.meters) 19 | assert(rectangle.contains(translation)) 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/mathematics/twodim/trajectory/PathFinderTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.mathematics.twodim.trajectory 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Translation2d 13 | import edu.wpi.first.wpilibj.trajectory.Trajectory 14 | import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint 15 | import kotlin.system.measureTimeMillis 16 | import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d 17 | import org.ghrobotics.lib.mathematics.twodim.geometry.Rectangle2d 18 | import org.ghrobotics.lib.mathematics.twodim.geometry.Translation2d 19 | import org.ghrobotics.lib.mathematics.twodim.trajectory.optimization.PathFinder 20 | import org.ghrobotics.lib.mathematics.units.derived.acceleration 21 | import org.ghrobotics.lib.mathematics.units.derived.degrees 22 | import org.ghrobotics.lib.mathematics.units.derived.toRotation2d 23 | import org.ghrobotics.lib.mathematics.units.derived.velocity 24 | import org.ghrobotics.lib.mathematics.units.feet 25 | import org.ghrobotics.lib.mathematics.units.inches 26 | import org.ghrobotics.lib.mathematics.units.milli 27 | import org.junit.Test 28 | 29 | class PathFinderTest { 30 | 31 | @Test 32 | fun testPathFinder() { 33 | val robotSize = 33.0.inches 34 | val pathFinder = PathFinder( 35 | robotSize, 36 | PathFinder.k2018LeftSwitch, 37 | PathFinder.k2018Platform, 38 | PathFinder.k2018CubesSwitch 39 | ) 40 | lateinit var path: List 41 | val nodeCreationTime = measureTimeMillis { 42 | path = pathFinder.findPath( 43 | Pose2d(1.54.feet, 23.234167.feet, 0.0.degrees.toRotation2d()), 44 | Pose2d(23.7.feet, (27 - 20.2).feet, 0.0.degrees.toRotation2d()), 45 | Rectangle2d( 46 | Translation2d(0.0.feet, 0.0.feet), 47 | Translation2d(10.0.feet, 10.0.feet) 48 | ) 49 | )!! 50 | println(path.joinToString(separator = "\n") { 51 | "${it.translation.x}\t${it.translation.y}\t${it.rotation.degrees}" 52 | }) 53 | } 54 | println("Generated Nodes in $nodeCreationTime ms") 55 | lateinit var trajectory: Trajectory 56 | val config = FalconTrajectoryConfig(10.feet.velocity, 4.feet.acceleration) 57 | .addConstraint(CentripetalAccelerationConstraint(4.0)) 58 | 59 | val trajectoryTime = measureTimeMillis { 60 | trajectory = FalconTrajectoryGenerator.generateTrajectory( 61 | path, 62 | config 63 | ) 64 | } 65 | println( 66 | "Generated Trajectory in $trajectoryTime ms\n" + 67 | "Total: ${trajectoryTime + nodeCreationTime} ms" 68 | ) 69 | 70 | val iterator = trajectory 71 | val dt = 20.0.milli.seconds 72 | val refList = mutableListOf() 73 | 74 | // while (!iterator.isDone) { 75 | // val pt = iterator.advance(dt) 76 | // refList.add(pt.state.pose.translation) 77 | // } 78 | 79 | // val fm = DecimalFormat("#.###").format(TrajectoryGeneratorTest.trajectory.lastInterpolant.inSeconds()) 80 | // 81 | // val chart = XYChartBuilder().width(1800).height(1520).title("$fm seconds.") 82 | // .xAxisTitle("X").yAxisTitle("Y").build() 83 | // 84 | // chart.styler.markerSize = 8 85 | // chart.styler.seriesColors = arrayOf(Color.ORANGE, Color(151, 60, 67)) 86 | // 87 | // chart.styler.chartTitleFont = Font("Kanit", 1, 40) 88 | // chart.styler.chartTitlePadding = 15 89 | // 90 | // chart.styler.xAxisMin = 1.0 91 | // chart.styler.xAxisMax = 26.0 92 | // chart.styler.yAxisMin = 1.0 93 | // chart.styler.yAxisMax = 26.0 94 | // 95 | // chart.styler.chartFontColor = Color.WHITE 96 | // chart.styler.axisTickLabelsColor = Color.WHITE 97 | // 98 | // chart.styler.legendBackgroundColor = Color.GRAY 99 | // 100 | // chart.styler.isPlotGridLinesVisible = true 101 | // chart.styler.isLegendVisible = true 102 | // 103 | // chart.styler.plotGridLinesColor = Color.GRAY 104 | // chart.styler.chartBackgroundColor = Color.DARK_GRAY 105 | // chart.styler.plotBackgroundColor = Color.DARK_GRAY 106 | // 107 | // chart.addSeries( 108 | // "Trajectory", 109 | // refList.map { it.x }.toDoubleArray(), 110 | // refList.map { it.y }.toDoubleArray() 111 | // ) 112 | // SwingWrapper(chart).displayChart() 113 | // Thread.sleep(1000000) 114 | } 115 | } 116 | -------------------------------------------------------------------------------- /core/src/test/kotlin/org/ghrobotics/lib/utils/SourceTests.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import org.junit.Test 12 | 13 | class SourceTests { 14 | 15 | @Test 16 | fun constTest() { 17 | val one = Source(5) 18 | 19 | assert(one() == 5) 20 | } 21 | 22 | @Test 23 | fun variableSourceTest() { 24 | var value = 5 25 | 26 | val one: Source = { value } 27 | 28 | assert(one() == 5) 29 | value = 2 30 | assert(one() == 2) 31 | } 32 | 33 | @Test 34 | fun sourceWithProcessingTest() { 35 | var value = 1 36 | 37 | val one: Source = { value } 38 | val two = one.map { it > 2 } 39 | 40 | assert(!two()) 41 | value = 3 42 | assert(two()) 43 | } 44 | 45 | @Test 46 | fun sourceMapTest() { 47 | var value = true 48 | 49 | val constOne = 1 50 | val constTwo = 2 51 | 52 | val one: BooleanSource = { value } 53 | val two = one.map(constOne, constTwo) 54 | 55 | assert(two() == constOne) 56 | value = false 57 | assert(two() == constTwo) 58 | } 59 | 60 | @Test 61 | fun sourceEqualsTest() { 62 | var value = false 63 | 64 | val one: BooleanSource = { value } 65 | val two = one.withEquals(true) 66 | 67 | assert(!two()) 68 | value = true 69 | assert(two()) 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FRC5190/FalconLibrary/928db04e8b64435b2ebe5cb2b981757b799c7a46/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | 17 | @if "%DEBUG%" == "" @echo off 18 | @rem ########################################################################## 19 | @rem 20 | @rem Gradle startup script for Windows 21 | @rem 22 | @rem ########################################################################## 23 | 24 | @rem Set local scope for the variables with windows NT shell 25 | if "%OS%"=="Windows_NT" setlocal 26 | 27 | set DIRNAME=%~dp0 28 | if "%DIRNAME%" == "" set DIRNAME=. 29 | set APP_BASE_NAME=%~n0 30 | set APP_HOME=%DIRNAME% 31 | 32 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 33 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 34 | 35 | @rem Find java.exe 36 | if defined JAVA_HOME goto findJavaFromJavaHome 37 | 38 | set JAVA_EXE=java.exe 39 | %JAVA_EXE% -version >NUL 2>&1 40 | if "%ERRORLEVEL%" == "0" goto init 41 | 42 | echo. 43 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 44 | echo. 45 | echo Please set the JAVA_HOME variable in your environment to match the 46 | echo location of your Java installation. 47 | 48 | goto fail 49 | 50 | :findJavaFromJavaHome 51 | set JAVA_HOME=%JAVA_HOME:"=% 52 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 53 | 54 | if exist "%JAVA_EXE%" goto init 55 | 56 | echo. 57 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 58 | echo. 59 | echo Please set the JAVA_HOME variable in your environment to match the 60 | echo location of your Java installation. 61 | 62 | goto fail 63 | 64 | :init 65 | @rem Get command-line arguments, handling Windows variants 66 | 67 | if not "%OS%" == "Windows_NT" goto win9xME_args 68 | 69 | :win9xME_args 70 | @rem Slurp the command line arguments. 71 | set CMD_LINE_ARGS= 72 | set _SKIP=2 73 | 74 | :win9xME_args_slurp 75 | if "x%~1" == "x" goto execute 76 | 77 | set CMD_LINE_ARGS=%* 78 | 79 | :execute 80 | @rem Setup the command line 81 | 82 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 83 | 84 | @rem Execute Gradle 85 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 86 | 87 | :end 88 | @rem End local scope for the variables with windows NT shell 89 | if "%ERRORLEVEL%"=="0" goto mainEnd 90 | 91 | :fail 92 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 93 | rem the _cmd.exe /c_ return code! 94 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 95 | exit /b 1 96 | 97 | :mainEnd 98 | if "%OS%"=="Windows_NT" endlocal 99 | 100 | :omega 101 | -------------------------------------------------------------------------------- /jitpack.yml: -------------------------------------------------------------------------------- 1 | jdk: 2 | - openjdk11 -------------------------------------------------------------------------------- /settings.gradle.kts: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | rootProject.name = "FalconLibrary" 10 | include("core", "wpi", "vendorCTRE", "vendorREV", "vendorNAVX", "vendorCU", "vendorPWF") -------------------------------------------------------------------------------- /vendorCTRE/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api project(":wpi") 12 | api wpi.deps.vendor.java() 13 | } -------------------------------------------------------------------------------- /vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.ctre 10 | 11 | import com.ctre.phoenix.motorcontrol.IMotorController 12 | import kotlin.math.roundToInt 13 | import kotlin.properties.Delegates 14 | import org.ghrobotics.lib.mathematics.units.SIKey 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit 17 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 18 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity 19 | import org.ghrobotics.lib.mathematics.units.nativeunit.nativeUnits 20 | import org.ghrobotics.lib.mathematics.units.nativeunit.nativeUnitsPer100ms 21 | import org.ghrobotics.lib.motors.AbstractFalconEncoder 22 | 23 | /** 24 | * Represents the encoder connected to a CTRE motor controller. 25 | * 26 | * @param motorController The motor controller. 27 | * @param pidIdx The PID ID. 28 | * @param model The native unit model. 29 | */ 30 | class FalconCTREEncoder( 31 | private val motorController: IMotorController, 32 | private val pidIdx: Int = 0, 33 | model: NativeUnitModel 34 | ) : AbstractFalconEncoder(model) { 35 | /** 36 | * Returns the raw velocity from the encoder. 37 | */ 38 | override val rawVelocity: SIUnit get() = motorController.getSelectedSensorVelocity(pidIdx).nativeUnitsPer100ms 39 | 40 | /** 41 | * Returns the raw position from the encoder. 42 | */ 43 | override val rawPosition: SIUnit get() = motorController.getSelectedSensorPosition(pidIdx).nativeUnits 44 | 45 | /** 46 | * Sets the encoder phase for the encoder. 47 | */ 48 | var encoderPhase by Delegates.observable(false) { _, _, newValue -> motorController.setSensorPhase(newValue) } 49 | 50 | /** 51 | * Resets the encoder position to a certain value. 52 | * 53 | * @param newPosition The position to reset to. 54 | */ 55 | override fun resetPositionRaw(newPosition: SIUnit) { 56 | motorController.setSelectedSensorPosition(newPosition.value.roundToInt(), pidIdx, 0) 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.ctre 10 | 11 | import com.ctre.phoenix.motorcontrol.MotorCommutation 12 | import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration 13 | import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration 14 | import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice 15 | import com.ctre.phoenix.motorcontrol.can.TalonFX 16 | import com.ctre.phoenix.sensors.SensorInitializationStrategy 17 | import kotlin.properties.Delegates 18 | import org.ghrobotics.lib.mathematics.units.Ampere 19 | import org.ghrobotics.lib.mathematics.units.SIKey 20 | import org.ghrobotics.lib.mathematics.units.SIUnit 21 | import org.ghrobotics.lib.mathematics.units.amps 22 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 23 | 24 | /** 25 | * Wrapper around the TalonFX motor controller. 26 | * 27 | * @param talonFX The underlying TalonFX motor controller. 28 | * @param model The native unit model. 29 | */ 30 | @Suppress("Unused") 31 | class FalconFX( 32 | @Suppress("MemberVisibilityCanBePrivate") val talonFX: TalonFX, 33 | model: NativeUnitModel 34 | ) : FalconCTRE(talonFX, model) { 35 | 36 | /** 37 | * Alternate constructor where users can supply ID and native unit model. 38 | * 39 | * @param id The ID of the motor controller. 40 | * @param model The native unit model. 41 | */ 42 | constructor(id: Int, model: NativeUnitModel) : this(TalonFX(id), model) 43 | 44 | /** 45 | * Configures the feedback device for the motor controller. 46 | */ 47 | var feedbackDevice by Delegates.observable(TalonFXFeedbackDevice.IntegratedSensor) { _, _, newValue -> 48 | talonFX.configSelectedFeedbackSensor(newValue, 0, 0) 49 | } 50 | 51 | /** 52 | * Configures the motor commutation type for the Falcon 500. 53 | */ 54 | var motorCommutation by Delegates.observable(MotorCommutation.Trapezoidal) { _, _, newValue -> 55 | talonFX.configMotorCommutation(newValue, 0) 56 | } 57 | 58 | /** 59 | * Configures the sensor initialization strategy for the motor controller. This can be used 60 | * to set whether the sensor starts at zero or to the position reported by the absolute encoder. 61 | * The latter is useful for mechanisms such as swerve drives or turrets where initial absolute 62 | * positioning is needed, but relative mode is used after boot. 63 | */ 64 | var sensorInitializerStrategy by Delegates.observable(SensorInitializationStrategy.BootToZero) { _, _, newValue -> 65 | talonFX.configIntegratedSensorInitializationStrategy(newValue, 0) 66 | } 67 | 68 | /** 69 | * Returns the current drawn by the motor. 70 | */ 71 | override val drawnCurrent: SIUnit 72 | get() = talonFX.outputCurrent.amps 73 | 74 | /** 75 | * Configures the supply-side current limit for the motor. 76 | * 77 | * @param config The supply-side current limit configuration. 78 | */ 79 | fun configSupplyCurrentLimit(config: SupplyCurrentLimitConfiguration) { 80 | talonFX.configSupplyCurrentLimit(config, 0) 81 | } 82 | 83 | /** 84 | * Configures the current limit for the stator of the motor. 85 | * 86 | * @param config The stator current limit configuration. 87 | */ 88 | fun configStatorCurrentLimit(config: StatorCurrentLimitConfiguration) { 89 | talonFX.configStatorCurrentLimit(config, 0) 90 | } 91 | } 92 | 93 | fun falconFX( 94 | talonFX: TalonFX, 95 | model: NativeUnitModel, 96 | block: FalconFX.() -> Unit 97 | ) = FalconFX(talonFX, model).also(block) 98 | 99 | fun falconFX( 100 | id: Int, 101 | model: NativeUnitModel, 102 | block: FalconFX.() -> Unit 103 | ) = FalconFX(id, model).also(block) 104 | -------------------------------------------------------------------------------- /vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.ctre 10 | 11 | import com.ctre.phoenix.motorcontrol.can.VictorSPX 12 | import edu.wpi.first.wpilibj.DriverStation 13 | import org.ghrobotics.lib.mathematics.units.Ampere 14 | import org.ghrobotics.lib.mathematics.units.SIKey 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.amps 17 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 18 | 19 | /** 20 | * Wrapper around the VictorSPX motor controller. 21 | * 22 | * @param victorSPX The underlying motor controller. 23 | * @param model The native unit model. 24 | */ 25 | class FalconSPX( 26 | @Suppress("MemberVisibilityCanBePrivate") val victorSPX: VictorSPX, 27 | model: NativeUnitModel 28 | ) : FalconCTRE(victorSPX, model) { 29 | 30 | /** 31 | * Alternate constructor where users can supply ID and native unit model. 32 | * 33 | * @param id The ID of the motor controller. 34 | * @param model The native unit model. 35 | */ 36 | constructor(id: Int, model: NativeUnitModel) : this(VictorSPX(id), model) 37 | 38 | /** 39 | * Returns the current drawn by the motor. 40 | */ 41 | override val drawnCurrent: SIUnit 42 | get() { 43 | DriverStation.reportError("Current monitoring is not supported on the VictorSPX", false) 44 | return 0.0.amps 45 | } 46 | } 47 | 48 | fun falconSPX( 49 | victorSPX: VictorSPX, 50 | model: NativeUnitModel, 51 | block: FalconSPX.() -> Unit 52 | ) = FalconSPX(victorSPX, model).also(block) 53 | 54 | fun falconSPX( 55 | id: Int, 56 | model: NativeUnitModel, 57 | block: FalconSPX.() -> Unit 58 | ) = FalconSPX(id, model).also(block) 59 | -------------------------------------------------------------------------------- /vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.ctre 10 | 11 | import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration 12 | import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice 13 | import com.ctre.phoenix.motorcontrol.can.TalonSRX 14 | import kotlin.properties.Delegates 15 | import org.ghrobotics.lib.mathematics.units.Ampere 16 | import org.ghrobotics.lib.mathematics.units.SIKey 17 | import org.ghrobotics.lib.mathematics.units.SIUnit 18 | import org.ghrobotics.lib.mathematics.units.Second 19 | import org.ghrobotics.lib.mathematics.units.amps 20 | import org.ghrobotics.lib.mathematics.units.inAmps 21 | import org.ghrobotics.lib.mathematics.units.inMilliseconds 22 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 23 | 24 | /** 25 | * Wrapper around the TalonSRX motor controller. 26 | * 27 | * @param talonSRX The underlying TalonSRX motor controller. 28 | * @param model The native unit model. 29 | */ 30 | @Suppress("Unused") 31 | class FalconSRX( 32 | @Suppress("MemberVisibilityCanBePrivate") val talonSRX: TalonSRX, 33 | model: NativeUnitModel 34 | ) : FalconCTRE(talonSRX, model) { 35 | 36 | /** 37 | * Alternate constructor where users can supply ID and native unit model. 38 | * 39 | * @param id The ID of the motor controller. 40 | * @param model The native unit model. 41 | */ 42 | constructor(id: Int, model: NativeUnitModel) : this(TalonSRX(id), model) 43 | 44 | /** 45 | * Returns the current drawn by the motor. 46 | */ 47 | override val drawnCurrent: SIUnit 48 | get() = talonSRX.outputCurrent.amps 49 | 50 | /** 51 | * Configures the feedback device for the motor controller. 52 | */ 53 | var feedbackDevice by Delegates.observable(TalonSRXFeedbackDevice.QuadEncoder) { _, _, newValue -> 54 | talonSRX.configSelectedFeedbackSensor(newValue, 0, 0) 55 | } 56 | 57 | /** 58 | * Configure the supply-side current limit for the motor. 59 | * 60 | * @param config The supply-side current limit configuration. 61 | */ 62 | fun configSupplyCurrentLimit(config: SupplyCurrentLimitConfiguration) { 63 | talonSRX.configSupplyCurrentLimit(config, 0) 64 | } 65 | 66 | /** 67 | * Configure the current limit for the motor. 68 | * 69 | * @param enabled Whether current limiting should be enabled. 70 | * @param config The current limiting configuration. 71 | */ 72 | @Deprecated("This method has been deprecated.", ReplaceWith("configSupplyCurrentLimit")) 73 | fun configCurrentLimit(enabled: Boolean, config: CurrentLimitConfig) { 74 | talonSRX.enableCurrentLimit(enabled) 75 | if (enabled) { 76 | talonSRX.configPeakCurrentLimit(config.peakCurrentLimit.inAmps().toInt()) 77 | talonSRX.configPeakCurrentDuration(config.peakCurrentLimitDuration.inMilliseconds().toInt()) 78 | talonSRX.configContinuousCurrentLimit(config.continuousCurrentLimit.inAmps().toInt()) 79 | } 80 | } 81 | 82 | /** 83 | * Represents the TalonSRX current limit configuration. 84 | * 85 | * @param peakCurrentLimit The peak current limit. 86 | * @param peakCurrentLimitDuration How long the peak current can hold. 87 | * @param continuousCurrentLimit What the current should be dropped to. 88 | */ 89 | @Deprecated( 90 | "This data class has been deprecated.", ReplaceWith( 91 | "SupplyCurrentLimitConfiguration", 92 | "import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration" 93 | ) 94 | ) 95 | data class CurrentLimitConfig( 96 | val peakCurrentLimit: SIUnit, 97 | val peakCurrentLimitDuration: SIUnit, 98 | val continuousCurrentLimit: SIUnit 99 | ) 100 | } 101 | 102 | fun falconSRX( 103 | talonSRX: TalonSRX, 104 | model: NativeUnitModel, 105 | block: FalconSRX.() -> Unit 106 | ) = FalconSRX(talonSRX, model).also(block) 107 | 108 | fun falconSRX( 109 | id: Int, 110 | model: NativeUnitModel, 111 | block: FalconSRX.() -> Unit 112 | ) = FalconSRX(id, model).also(block) 113 | -------------------------------------------------------------------------------- /vendorCTRE/src/main/kotlin/org/ghrobotics/lib/utils/CTREExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import com.ctre.phoenix.CANifier 12 | import com.ctre.phoenix.sensors.PigeonIMU 13 | import edu.wpi.first.wpilibj.geometry.Rotation2d 14 | import java.awt.Color 15 | 16 | fun CANifier.setLEDOutput(color: Color) = setLEDOutput(color.red, color.green, color.blue) 17 | 18 | fun CANifier.setLEDOutput(r: Int, g: Int, b: Int) { 19 | setLEDOutput(r * (1.0 / 255.0), CANifier.LEDChannel.LEDChannelB) 20 | setLEDOutput(g * (1.0 / 255.0), CANifier.LEDChannel.LEDChannelA) 21 | setLEDOutput(b * (1.0 / 255.0), CANifier.LEDChannel.LEDChannelC) 22 | } 23 | 24 | fun PigeonIMU.asSource(): Source = { Rotation2d.fromDegrees(fusedHeading) } 25 | -------------------------------------------------------------------------------- /vendorCU/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api project(":wpi") 12 | api wpi.deps.vendor.java() 13 | } -------------------------------------------------------------------------------- /vendorCU/src/main/kotlin/org/ghrobotics/lib/utils/CopperforgeExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import com.cuforge.libcu.Lasershark 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.inches 15 | 16 | /** 17 | * Returns the distance from the rangefinder sensor in a typesafe 18 | * unit wrapper. 19 | */ 20 | val Lasershark.distance: SIUnit 21 | get() = distanceInches.inches 22 | -------------------------------------------------------------------------------- /vendorCU/vendordeps/LibCu-latest.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "LibCu.json", 3 | "name": "LibCu", 4 | "version": "2020.2.1", 5 | "uuid": "libcufrc-e6e8-4db6-89f0-copperforge0", 6 | "mavenUrls": [ 7 | "https://copperforge.cc/files/dev/maven" 8 | ], 9 | "jsonUrl": "https://copperforge.cc/files/dev/vendordeps/LibCu-latest.json", 10 | "cppDependencies": [ 11 | { 12 | "groupId": "com.cuforge.libcu", 13 | "artifactId": "LibCu-cpp", 14 | "version": "2020.2.1", 15 | "libName": "Cu", 16 | "headerClassifier": "headers", 17 | "sharedLibrary": true, 18 | "skipInvalidPlatforms": true, 19 | "binaryPlatforms": [ 20 | "windowsx86-64", 21 | "linuxathena" 22 | ] 23 | }, 24 | { 25 | "groupId": "com.cuforge.libcu", 26 | "artifactId": "LibCu-driver", 27 | "version": "2020.2.1", 28 | "libName": "CuDriver", 29 | "headerClassifier": "headers", 30 | "sharedLibrary": true, 31 | "skipInvalidPlatforms": true, 32 | "binaryPlatforms": [ 33 | "windowsx86-64", 34 | "linuxathena" 35 | ] 36 | } 37 | ], 38 | "javaDependencies": [ 39 | { 40 | "groupId": "com.cuforge.libcu", 41 | "artifactId": "LibCu-java", 42 | "version": "2020.2.1" 43 | } 44 | ], 45 | "jniDependencies": [ 46 | { 47 | "groupId": "com.cuforge.libcu", 48 | "artifactId": "LibCu-driver", 49 | "version": "2020.2.1", 50 | "isJar": false, 51 | "skipInvalidPlatforms": true, 52 | "validPlatforms": [ 53 | "windowsx86-64", 54 | "linuxathena" 55 | ] 56 | } 57 | ] 58 | } 59 | -------------------------------------------------------------------------------- /vendorNAVX/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api project(":wpi") 12 | api wpi.deps.vendor.java() 13 | } -------------------------------------------------------------------------------- /vendorNAVX/src/main/kotlin/org/ghrobotics/lib/utils/NavXExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import com.kauailabs.navx.frc.AHRS 12 | import edu.wpi.first.wpilibj.geometry.Rotation2d 13 | 14 | fun AHRS.asSource(): Source = { Rotation2d.fromDegrees(-fusedHeading.toDouble()) } 15 | -------------------------------------------------------------------------------- /vendorNAVX/vendordeps/navx_frc.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "navx_frc.json", 3 | "name": "KauaiLabs_navX_FRC", 4 | "version": "3.1.400", 5 | "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", 6 | "mavenUrls": [ 7 | "https://repo1.maven.org/maven2/" 8 | ], 9 | "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.kauailabs.navx.frc", 13 | "artifactId": "navx-java", 14 | "version": "3.1.400" 15 | } 16 | ], 17 | "jniDependencies": [], 18 | "cppDependencies": [ 19 | { 20 | "groupId": "com.kauailabs.navx.frc", 21 | "artifactId": "navx-cpp", 22 | "version": "3.1.400", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": false, 26 | "libName": "navx_frc", 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "linuxathena", 30 | "linuxraspbian" 31 | ] 32 | } 33 | ] 34 | } 35 | -------------------------------------------------------------------------------- /vendorPWF/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api project(":wpi") 12 | api wpi.deps.vendor.java() 13 | } -------------------------------------------------------------------------------- /vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.pwf 10 | 11 | import com.playingwithfusion.CANVenom 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit 15 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 16 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity 17 | import org.ghrobotics.lib.motors.AbstractFalconEncoder 18 | 19 | /** 20 | * Represents an encoder connected to the Venom motor controller. 21 | */ 22 | class FalconVenomEncoder( 23 | private val venom: CANVenom, 24 | model: NativeUnitModel 25 | ) : AbstractFalconEncoder(model) { 26 | /** 27 | * Returns the raw velocity from the encoder. 28 | */ 29 | override val rawVelocity: SIUnit 30 | get() = SIUnit(venom.speed / 60.0) 31 | 32 | /** 33 | * Returns the raw position from the encoder. 34 | */ 35 | override val rawPosition: SIUnit 36 | get() = SIUnit(venom.position) 37 | 38 | /** 39 | * Resets the encoder position to a certain value. 40 | * 41 | * @param newPosition The position to reset to. 42 | */ 43 | override fun resetPositionRaw(newPosition: SIUnit) { 44 | venom.position = newPosition.value 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /vendorPWF/src/main/kotlin/org/ghrobotics/lib/utils/PlayingWithFusionExtensions.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import com.playingwithfusion.TimeOfFlight 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.milli 15 | 16 | /** 17 | * Returns the distance from the time of flight sensor in a typesafe 18 | * unit wrapper. 19 | */ 20 | val TimeOfFlight.distance: SIUnit 21 | get() = range.milli.meters 22 | -------------------------------------------------------------------------------- /vendorPWF/vendordeps/playingwithfusion2021.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "playingwithfusion2021.json", 3 | "name": "PlayingWithFusion", 4 | "version": "2021.03.22", 5 | "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", 6 | "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2021.json", 7 | "mavenUrls": [ 8 | "https://www.playingwithfusion.com/frc/maven/" 9 | ], 10 | "javaDependencies": [ 11 | { 12 | "groupId": "com.playingwithfusion.frc", 13 | "artifactId": "PlayingWithFusion-java", 14 | "version": "2021.03.22" 15 | } 16 | ], 17 | "jniDependencies": [ 18 | { 19 | "groupId": "com.playingwithfusion.frc", 20 | "artifactId": "PlayingWithFusion-driver", 21 | "version": "2021.03.22", 22 | "isJar": false, 23 | "skipInvalidPlatforms": true, 24 | "validPlatforms": [ 25 | "linuxaarch64bionic", 26 | "linuxathena", 27 | "linuxraspbian", 28 | "linuxx86-64", 29 | "osxx86-64", 30 | "windowsx86-64" 31 | ] 32 | } 33 | ], 34 | "cppDependencies": [ 35 | { 36 | "groupId": "com.playingwithfusion.frc", 37 | "artifactId": "PlayingWithFusion-cpp", 38 | "version": "2021.03.22", 39 | "headerClassifier": "headers", 40 | "sharedLibrary": false, 41 | "libName": "PlayingWithFusion", 42 | "skipInvalidPlatforms": true, 43 | "binaryPlatforms": [ 44 | "linuxaarch64bionic", 45 | "linuxathena", 46 | "linuxraspbian", 47 | "linuxx86-64", 48 | "osxx86-64", 49 | "windowsx86-64" 50 | ] 51 | }, 52 | { 53 | "groupId": "com.playingwithfusion.frc", 54 | "artifactId": "PlayingWithFusion-driver", 55 | "version": "2021.03.22", 56 | "headerClassifier": "headers", 57 | "sharedLibrary": true, 58 | "libName": "PlayingWithFusionDriver", 59 | "skipInvalidPlatforms": true, 60 | "binaryPlatforms": [ 61 | "linuxaarch64bionic", 62 | "linuxathena", 63 | "linuxraspbian", 64 | "linuxx86-64", 65 | "osxx86-64", 66 | "windowsx86-64" 67 | ] 68 | } 69 | ] 70 | } -------------------------------------------------------------------------------- /vendorREV/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api project(":wpi") 12 | api wpi.deps.vendor.java() 13 | } -------------------------------------------------------------------------------- /vendorREV/src/main/java/org/ghrobotics/lib/motors/rev/CANSensorShim.java: -------------------------------------------------------------------------------- 1 | package org.ghrobotics.lib.motors.rev; 2 | 3 | import com.revrobotics.CANEncoder; 4 | import com.revrobotics.CANPIDController; 5 | 6 | public class CANSensorShim { 7 | public static void configCANEncoderonCanPIDController(CANPIDController target, CANEncoder canEncoder) { 8 | target.setFeedbackDevice(canEncoder); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors.rev 10 | 11 | import com.revrobotics.CANEncoder 12 | import org.ghrobotics.lib.mathematics.units.SIKey 13 | import org.ghrobotics.lib.mathematics.units.SIUnit 14 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit 15 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 16 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity 17 | import org.ghrobotics.lib.motors.AbstractFalconEncoder 18 | 19 | /** 20 | * Represents an encoder connected to a Spark MAX. 21 | * 22 | * @param canEncoder The underlying encoder. 23 | * @param model The native unit model. 24 | */ 25 | class FalconMAXEncoder( 26 | val canEncoder: CANEncoder, 27 | model: NativeUnitModel 28 | ) : AbstractFalconEncoder(model) { 29 | /** 30 | * Returns the raw velocity from the encoder. 31 | */ 32 | override val rawVelocity: SIUnit get() = SIUnit(canEncoder.velocity / 60.0) 33 | 34 | /** 35 | * Returns the raw position from the encoder. 36 | */ 37 | override val rawPosition: SIUnit get() = SIUnit(canEncoder.position) 38 | 39 | /** 40 | * Resets the encoder position to a certain value. 41 | * 42 | * @param newPosition The position to reset to. 43 | */ 44 | override fun resetPositionRaw(newPosition: SIUnit) { 45 | canEncoder.position = newPosition.value 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /vendorREV/vendordeps/REVRobotics.json: -------------------------------------------------------------------------------- 1 | { 2 | "cppDependencies": [ 3 | { 4 | "artifactId": "SparkMax-cpp", 5 | "binaryPlatforms": [ 6 | "windowsx86-64", 7 | "windowsx86", 8 | "linuxaarch64bionic", 9 | "linuxx86-64", 10 | "linuxathena", 11 | "linuxraspbian" 12 | ], 13 | "groupId": "com.revrobotics.frc", 14 | "headerClassifier": "headers", 15 | "libName": "SparkMax", 16 | "sharedLibrary": false, 17 | "skipInvalidPlatforms": true, 18 | "version": "1.5.1" 19 | }, 20 | { 21 | "artifactId": "SparkMax-driver", 22 | "binaryPlatforms": [ 23 | "windowsx86-64", 24 | "windowsx86", 25 | "linuxaarch64bionic", 26 | "linuxx86-64", 27 | "linuxathena", 28 | "linuxraspbian" 29 | ], 30 | "groupId": "com.revrobotics.frc", 31 | "headerClassifier": "headers", 32 | "libName": "SparkMaxDriver", 33 | "sharedLibrary": false, 34 | "skipInvalidPlatforms": true, 35 | "version": "1.5.1" 36 | } 37 | ], 38 | "fileName": "REVRobotics.json", 39 | "javaDependencies": [ 40 | { 41 | "artifactId": "SparkMax-java", 42 | "groupId": "com.revrobotics.frc", 43 | "version": "1.5.1" 44 | } 45 | ], 46 | "jniDependencies": [ 47 | { 48 | "artifactId": "SparkMax-driver", 49 | "groupId": "com.revrobotics.frc", 50 | "isJar": false, 51 | "skipInvalidPlatforms": true, 52 | "validPlatforms": [ 53 | "windowsx86-64", 54 | "windowsx86", 55 | "linuxaarch64bionic", 56 | "linuxx86-64", 57 | "linuxathena", 58 | "linuxraspbian" 59 | ], 60 | "version": "1.5.1" 61 | } 62 | ], 63 | "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", 64 | "mavenUrls": [ 65 | "http://www.revrobotics.com/content/sw/max/sdk/maven/" 66 | ], 67 | "name": "REVRobotics", 68 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 69 | "version": "1.5.1" 70 | } 71 | -------------------------------------------------------------------------------- /wpi/build.gradle: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | dependencies { 10 | api project(":core") 11 | api wpi.deps.wpilib() 12 | api wpi.deps.vendor.java() 13 | api "com.github.salomonbrys.kotson:kotson:2.5.0" 14 | } -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/commands/CommandGroupBuilder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.commands 10 | 11 | import edu.wpi.first.wpilibj2.command.Command 12 | import edu.wpi.first.wpilibj2.command.CommandGroupBase 13 | import edu.wpi.first.wpilibj2.command.ConditionalCommand 14 | import edu.wpi.first.wpilibj2.command.InstantCommand 15 | import edu.wpi.first.wpilibj2.command.ParallelCommandGroup 16 | import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup 17 | import edu.wpi.first.wpilibj2.command.ParallelRaceGroup 18 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup 19 | import java.util.function.BooleanSupplier 20 | import org.ghrobotics.lib.utils.Source 21 | 22 | fun sequential(block: BasicCommandGroupBuilder.() -> Unit) = 23 | commandGroup(BasicCommandGroupBuilder.Type.Sequential, block) 24 | 25 | fun parallel(block: BasicCommandGroupBuilder.() -> Unit) = 26 | commandGroup(BasicCommandGroupBuilder.Type.Parallel, block) 27 | 28 | fun parallelRace(block: BasicCommandGroupBuilder.() -> Unit) = 29 | commandGroup(BasicCommandGroupBuilder.Type.ParallelRace, block) 30 | 31 | fun parallelDeadline(deadline: Command, block: ParallelDeadlineGroupBuilder.() -> Unit) = 32 | parallelDeadlineGroup(deadline, block) 33 | 34 | fun stateCommandGroup(state: Source, block: StateCommandGroupBuilder.() -> Unit) = 35 | StateCommandGroupBuilder(state).apply(block).build() 36 | 37 | private fun commandGroup(type: BasicCommandGroupBuilder.Type, block: BasicCommandGroupBuilder.() -> Unit) = 38 | BasicCommandGroupBuilder(type).apply(block).build() 39 | 40 | private fun parallelDeadlineGroup(deadline: Command, block: ParallelDeadlineGroupBuilder.() -> Unit) = 41 | ParallelDeadlineGroupBuilder(deadline).apply(block).build() 42 | 43 | interface CommandGroupBuilder { 44 | fun build(): CommandGroupBase 45 | } 46 | 47 | class BasicCommandGroupBuilder(private val type: Type) : CommandGroupBuilder { 48 | 49 | private val commands = mutableListOf() 50 | 51 | operator fun Command.unaryPlus() = commands.add(this) 52 | 53 | override fun build(): CommandGroupBase { 54 | return when (type) { 55 | Type.Sequential -> SequentialCommandGroup(*commands.toTypedArray()) 56 | Type.Parallel -> ParallelCommandGroup(*commands.toTypedArray()) 57 | Type.ParallelRace -> ParallelRaceGroup(*commands.toTypedArray()) 58 | } 59 | } 60 | 61 | enum class Type { 62 | Sequential, Parallel, ParallelRace 63 | } 64 | } 65 | 66 | class ParallelDeadlineGroupBuilder(private val deadline: Command) : CommandGroupBuilder { 67 | 68 | private val commands = mutableListOf() 69 | 70 | operator fun Command.unaryPlus() = commands.add(this) 71 | 72 | override fun build() = ParallelDeadlineGroup(deadline, *commands.toTypedArray()) 73 | } 74 | 75 | class StateCommandGroupBuilder(private val state: Source) : 76 | CommandGroupBuilder { 77 | private val stateMap = mutableMapOf() 78 | 79 | fun state(vararg states: T, block: () -> Command) = states(states, block()) 80 | fun state(vararg states: T, command: Command) = states(states, command) 81 | 82 | fun states(states: Array, command: Command) = states.forEach { state(it, command) } 83 | 84 | fun state(state: T, block: () -> Command) = state(state, block()) 85 | 86 | fun state(state: T, command: Command) { 87 | if (stateMap.containsKey(state)) println("[StateCommandGroup] Warning: state $state was overwritten during building") 88 | stateMap[state] = command 89 | } 90 | 91 | override fun build() = 92 | SequentialCommandGroup( 93 | *stateMap.entries.map { (key, command) -> 94 | ConditionalCommand(command, InstantCommand(), BooleanSupplier { state() == key }) 95 | }.toTypedArray() 96 | ) 97 | } 98 | 99 | infix fun CommandGroupBase.S3ND(other: Any) = this.schedule() 100 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/commands/FalconCommand.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.commands 10 | 11 | import edu.wpi.first.wpilibj2.command.CommandBase 12 | import edu.wpi.first.wpilibj2.command.Subsystem 13 | 14 | abstract class FalconCommand(vararg requirements: Subsystem) : CommandBase() { 15 | init { 16 | addRequirements(*requirements) 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/commands/FalconNotifierCommand.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.commands 10 | 11 | import edu.wpi.first.wpilibj2.command.CommandBase 12 | import edu.wpi.first.wpilibj2.command.Subsystem 13 | import kotlinx.coroutines.CoroutineScope 14 | import kotlinx.coroutines.GlobalScope 15 | import kotlinx.coroutines.Job 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | import org.ghrobotics.lib.mathematics.units.Second 18 | import org.ghrobotics.lib.utils.launchFrequency 19 | 20 | open class FalconNotifierCommand( 21 | private val period: SIUnit, 22 | vararg requirements: Subsystem, 23 | private val block: suspend CoroutineScope.() -> Unit 24 | ) : CommandBase() { 25 | 26 | init { 27 | addRequirements(*requirements) 28 | } 29 | 30 | lateinit var job: Job 31 | 32 | override fun initialize() { 33 | job = GlobalScope.launchFrequency((1 / period.value).toInt(), block = block) 34 | } 35 | 36 | override fun end(interrupted: Boolean) { 37 | job.cancel() 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/commands/FalconSubsystem.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.commands 10 | 11 | import edu.wpi.first.wpilibj2.command.Command 12 | import edu.wpi.first.wpilibj2.command.PrintCommand 13 | import edu.wpi.first.wpilibj2.command.SubsystemBase 14 | 15 | abstract class FalconSubsystem : SubsystemBase() { 16 | open fun lateInit() {} 17 | open fun autoReset() {} 18 | open fun teleopReset() {} 19 | open fun setNeutral() {} 20 | 21 | open fun checkSubsystem(): Command { 22 | return PrintCommand( 23 | "No test routine was run. Override the" + 24 | "checkSubsystem() method in this subsystem to run tests." 25 | ) 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/commands/FalconSubsystemHandler.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.commands 10 | 11 | import edu.wpi.first.wpilibj2.command.SequentialCommandGroup 12 | 13 | internal object FalconSubsystemHandler { 14 | private val registeredSubsystems = arrayListOf() 15 | internal val testCommand = SequentialCommandGroup() 16 | 17 | fun add(subsystem: FalconSubsystem) { 18 | registeredSubsystems.add(subsystem) 19 | testCommand.addCommands(subsystem.checkSubsystem()) 20 | } 21 | 22 | fun lateInit() = registeredSubsystems.forEach { it.lateInit() } 23 | fun autoReset() = registeredSubsystems.forEach { it.autoReset() } 24 | fun teleopReset() = registeredSubsystems.forEach { it.teleopReset() } 25 | fun setNeutral() = registeredSubsystems.forEach { it.setNeutral() } 26 | } 27 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/debug/FalconDashboard.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.debug 10 | 11 | import com.github.salomonbrys.kotson.fromJson 12 | import com.github.salomonbrys.kotson.jsonObject 13 | import com.google.gson.Gson 14 | import com.google.gson.JsonObject 15 | import edu.wpi.first.wpilibj.geometry.Pose2d 16 | import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d 17 | import org.ghrobotics.lib.mathematics.units.derived.degrees 18 | import org.ghrobotics.lib.mathematics.units.derived.toRotation2d 19 | import org.ghrobotics.lib.mathematics.units.meters 20 | import org.ghrobotics.lib.wrappers.networktables.FalconNetworkTable 21 | import org.ghrobotics.lib.wrappers.networktables.delegate 22 | import org.ghrobotics.lib.wrappers.networktables.get 23 | 24 | @Deprecated("This usage will no longer be supported. Use the FalconDashboard object instead.") 25 | typealias LiveDashboard = FalconDashboard 26 | 27 | /** 28 | * Helper object for sending data to Falcon Dashboard 29 | */ 30 | object FalconDashboard { 31 | private val falconDashboardTable = FalconNetworkTable.getTable("Live_Dashboard") 32 | 33 | var robotX by falconDashboardTable["robotX"].delegate(0.0) 34 | var robotY by falconDashboardTable["robotY"].delegate(0.0) 35 | var robotHeading by falconDashboardTable["robotHeading"].delegate(0.0) 36 | 37 | var isFollowingPath by falconDashboardTable["isFollowingPath"].delegate(false) 38 | var pathX by falconDashboardTable["pathX"].delegate(0.0) 39 | var pathY by falconDashboardTable["pathY"].delegate(0.0) 40 | var pathHeading by falconDashboardTable["pathHeading"].delegate(0.0) 41 | 42 | private val visionTargetEntry = falconDashboardTable["visionTargets"] 43 | var visionTargets: List 44 | set(value) { 45 | visionTargetEntry.setStringArray( 46 | value.map { 47 | jsonObject( 48 | "x" to it.translation.x, 49 | "y" to it.translation.y, 50 | "angle" to it.rotation.degrees 51 | ).toString() 52 | }.toTypedArray() 53 | ) 54 | } 55 | get() = visionTargetEntry.getStringArray(emptyArray()) 56 | .map { 57 | val data = kGson.fromJson(it) 58 | Pose2d( 59 | data["x"].asDouble.meters, 60 | data["y"].asDouble.meters, 61 | data["angle"].asDouble.degrees.toRotation2d() 62 | ) 63 | } 64 | 65 | private val kGson = Gson() 66 | } 67 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/localization/Localization.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.localization 10 | 11 | import edu.wpi.first.wpilibj.Timer 12 | import edu.wpi.first.wpilibj.geometry.Pose2d 13 | import edu.wpi.first.wpilibj.geometry.Rotation2d 14 | import edu.wpi.first.wpilibj.geometry.Twist2d 15 | import kotlin.reflect.KProperty 16 | import org.ghrobotics.lib.mathematics.units.SIUnit 17 | import org.ghrobotics.lib.mathematics.units.Second 18 | import org.ghrobotics.lib.mathematics.units.seconds 19 | import org.ghrobotics.lib.utils.Source 20 | 21 | /** 22 | * @param localizationBuffer Stores the previous 100 states so that we can interpolate if needed. 23 | * Especially useful for Vision 24 | */ 25 | @Deprecated("The FalconLibrary localization class is no longer supported.") 26 | abstract class Localization( 27 | protected val robotHeading: Source, 28 | private val localizationBuffer: TimePoseInterpolatableBuffer = TimePoseInterpolatableBuffer() 29 | ) : Source { 30 | 31 | /** 32 | * The robot position relative to the field. 33 | */ 34 | var robotPosition = Pose2d() 35 | private set 36 | 37 | /** 38 | * Stores the previous state of the robot. 39 | */ 40 | private var prevHeading = Rotation2d(0.0) 41 | private var headingOffset = Rotation2d(0.0) 42 | 43 | @Synchronized 44 | fun reset(newPosition: Pose2d = Pose2d()) = resetInternal(newPosition) 45 | 46 | protected open fun resetInternal(newPosition: Pose2d) { 47 | robotPosition = newPosition 48 | val newHeading = robotHeading() 49 | prevHeading = newHeading 50 | headingOffset = -newHeading + newPosition.rotation 51 | localizationBuffer.clear() 52 | } 53 | 54 | @Synchronized 55 | fun update() { 56 | val newHeading = robotHeading() 57 | 58 | val deltaHeading = newHeading - prevHeading 59 | 60 | // Add the recorded motion of the robot during this iteration to the global robot pose. 61 | val newRobotPosition = robotPosition.exp(update(deltaHeading)) 62 | robotPosition = Pose2d( 63 | newRobotPosition.translation, 64 | newHeading + headingOffset 65 | ) 66 | 67 | prevHeading = newHeading 68 | 69 | // Add the global robot pose to the interpolatable buffer 70 | localizationBuffer[Timer.getFPGATimestamp().seconds] = robotPosition 71 | } 72 | 73 | protected abstract fun update(deltaHeading: Rotation2d): Twist2d 74 | 75 | override fun invoke() = robotPosition 76 | 77 | operator fun get(timestamp: SIUnit) = localizationBuffer[timestamp] ?: Pose2d() 78 | 79 | // Delegates 80 | 81 | operator fun getValue(thisRef: Any?, property: KProperty<*>): Pose2d = robotPosition 82 | operator fun setValue(thisRef: Any?, property: KProperty<*>, value: Pose2d) = reset(value) 83 | } 84 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/localization/TankEncoderLocalization.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.localization 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Rotation2d 13 | import edu.wpi.first.wpilibj.geometry.Twist2d 14 | import org.ghrobotics.lib.mathematics.units.Meter 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.meters 17 | import org.ghrobotics.lib.utils.Source 18 | 19 | @Deprecated("The TankEncoderLocalization class is no longer supported.", 20 | ReplaceWith("DifferentialDriveOdometry", "edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry")) 21 | class TankEncoderLocalization( 22 | robotHeading: Source, 23 | val leftEncoder: Source>, 24 | val rightEncoder: Source>, 25 | localizationBuffer: TimePoseInterpolatableBuffer = TimePoseInterpolatableBuffer() 26 | ) : Localization(robotHeading, localizationBuffer) { 27 | 28 | private var prevLeftEncoder = 0.0.meters 29 | private var prevRightEncoder = 0.0.meters 30 | 31 | override fun resetInternal(newPosition: Pose2d) { 32 | super.resetInternal(newPosition) 33 | prevLeftEncoder = leftEncoder() 34 | prevRightEncoder = rightEncoder() 35 | } 36 | 37 | override fun update(deltaHeading: Rotation2d): Twist2d { 38 | val newLeftEncoder = leftEncoder() 39 | val newRightEncoder = rightEncoder() 40 | 41 | val deltaLeft = newLeftEncoder - prevLeftEncoder 42 | val deltaRight = newRightEncoder - prevRightEncoder 43 | 44 | this.prevLeftEncoder = newLeftEncoder 45 | this.prevRightEncoder = newRightEncoder 46 | 47 | return forwardKinematics(deltaLeft, deltaRight, deltaHeading) 48 | } 49 | 50 | /** 51 | * Return a twist that represents the robot's motion from the left delta, the right delta, and the rotation delta. 52 | */ 53 | private fun forwardKinematics( 54 | leftDelta: SIUnit, 55 | rightDelta: SIUnit, 56 | rotationDelta: Rotation2d 57 | ): Twist2d { 58 | val dx = (leftDelta + rightDelta) / 2.0 59 | return Twist2d(dx.value, 0.0, rotationDelta.radians) 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/localization/TimePoseInterpolatableBuffer.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.localization 10 | 11 | import edu.wpi.first.wpilibj.Timer 12 | import edu.wpi.first.wpilibj.geometry.Pose2d 13 | import java.util.TreeMap 14 | import org.ghrobotics.lib.mathematics.twodim.geometry.interpolate 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.Second 17 | import org.ghrobotics.lib.mathematics.units.operations.div 18 | import org.ghrobotics.lib.mathematics.units.seconds 19 | import org.ghrobotics.lib.mathematics.units.unitlessValue 20 | import org.ghrobotics.lib.utils.Source 21 | 22 | class TimePoseInterpolatableBuffer( 23 | private val historySpan: SIUnit = 1.0.seconds, 24 | private val timeSource: Source> = { Timer.getFPGATimestamp().seconds } 25 | ) { 26 | 27 | private val bufferMap = TreeMap, Pose2d>() 28 | 29 | operator fun set(time: SIUnit, value: Pose2d): Pose2d? { 30 | cleanUp() 31 | return bufferMap.put(time, value) 32 | } 33 | 34 | private fun cleanUp() { 35 | val currentTime = timeSource() 36 | 37 | while (bufferMap.isNotEmpty()) { 38 | val entry = bufferMap.lastEntry() 39 | if (currentTime - entry.key >= historySpan) { 40 | bufferMap.remove(entry.key) 41 | } else { 42 | return 43 | } 44 | } 45 | } 46 | 47 | fun clear() { 48 | bufferMap.clear() 49 | } 50 | 51 | operator fun get(time: SIUnit): Pose2d? { 52 | if (bufferMap.isEmpty()) return null 53 | 54 | bufferMap[time]?.let { return it } 55 | 56 | val topBound = bufferMap.ceilingEntry(time) 57 | val bottomBound = bufferMap.floorEntry(time) 58 | 59 | return when { 60 | topBound == null -> bottomBound?.value 61 | bottomBound == null -> topBound.value 62 | else -> bottomBound.value.interpolate( 63 | topBound.value, 64 | ((time - bottomBound.key) / (topBound.key - bottomBound.key)).unitlessValue 65 | ) 66 | } 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.derived.Velocity 14 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel 15 | 16 | abstract class AbstractFalconEncoder( 17 | val model: NativeUnitModel 18 | ) : FalconEncoder { 19 | override val position: SIUnit get() = model.fromNativeUnitPosition(rawPosition) 20 | override val velocity: SIUnit> get() = model.fromNativeUnitVelocity(rawVelocity) 21 | 22 | override fun resetPosition(newPosition: SIUnit) { 23 | resetPositionRaw(model.toNativeUnitPosition(newPosition)) 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | 13 | abstract class AbstractFalconMotor : FalconMotor { 14 | 15 | override var useMotionProfileForPosition: Boolean = false 16 | 17 | override fun follow(motor: FalconMotor<*>): Boolean { 18 | TODO("Cross brand motor controller following not yet implemented!") 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors 10 | 11 | import org.ghrobotics.lib.mathematics.units.SIKey 12 | import org.ghrobotics.lib.mathematics.units.SIUnit 13 | import org.ghrobotics.lib.mathematics.units.derived.Velocity 14 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit 15 | import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity 16 | 17 | interface FalconEncoder { 18 | 19 | /** 20 | * The velocity of the encoder in [K]/s 21 | */ 22 | val velocity: SIUnit> 23 | /** 24 | * The position of the encoder in [K] 25 | */ 26 | val position: SIUnit 27 | 28 | /** 29 | * The velocity of the encoder in NativeUnits/s 30 | */ 31 | val rawVelocity: SIUnit 32 | /** 33 | * The position of the encoder in NativeUnits 34 | */ 35 | val rawPosition: SIUnit 36 | 37 | fun resetPosition(newPosition: SIUnit) 38 | 39 | fun resetPositionRaw(newPosition: SIUnit) 40 | } 41 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconMotor.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.motors 10 | 11 | import org.ghrobotics.lib.mathematics.units.Ampere 12 | import org.ghrobotics.lib.mathematics.units.Meter 13 | import org.ghrobotics.lib.mathematics.units.SIKey 14 | import org.ghrobotics.lib.mathematics.units.SIUnit 15 | import org.ghrobotics.lib.mathematics.units.derived.Acceleration 16 | import org.ghrobotics.lib.mathematics.units.derived.Radian 17 | import org.ghrobotics.lib.mathematics.units.derived.Velocity 18 | import org.ghrobotics.lib.mathematics.units.derived.Volt 19 | import org.ghrobotics.lib.mathematics.units.derived.volts 20 | 21 | typealias LinearFalconMotor = FalconMotor 22 | typealias AngularFalconMotor = FalconMotor 23 | 24 | interface FalconMotor { 25 | 26 | /** 27 | * The encoder attached to the motor 28 | */ 29 | val encoder: FalconEncoder 30 | /** 31 | * The voltage output of the motor controller in volts 32 | */ 33 | val voltageOutput: SIUnit 34 | 35 | /** 36 | * The current drawn by the motor 37 | */ 38 | val drawnCurrent: SIUnit 39 | 40 | /** 41 | * Inverts the output given to the motor 42 | */ 43 | var outputInverted: Boolean 44 | 45 | /** 46 | * When enabled, motor leads are commonized electrically to reduce motion 47 | */ 48 | var brakeMode: Boolean 49 | 50 | /** 51 | * Configures the max voltage output given to the motor 52 | */ 53 | var voltageCompSaturation: SIUnit 54 | 55 | /** 56 | * Peak target velocity that the on board motion profile generator will use 57 | * Unit is [K]/s 58 | */ 59 | var motionProfileCruiseVelocity: SIUnit> 60 | /** 61 | * Acceleration that the on board motion profile generator will 62 | * Unit is [K]/s/s 63 | */ 64 | var motionProfileAcceleration: SIUnit> 65 | /** 66 | * Enables the use of on board motion profiling for position mode 67 | */ 68 | var useMotionProfileForPosition: Boolean 69 | 70 | /** 71 | * Soft limit in the forward direction. 72 | */ 73 | var softLimitForward: SIUnit 74 | /** 75 | * Soft limit in the reverse direction. 76 | */ 77 | var softLimitReverse: SIUnit 78 | 79 | fun follow(motor: FalconMotor<*>): Boolean 80 | 81 | /** 82 | * Sets the output [voltage] in volts and [arbitraryFeedForward] in volts 83 | */ 84 | fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit = 0.0.volts) 85 | 86 | /** 87 | * Sets the output [dutyCycle] in percent and [arbitraryFeedForward] in volts 88 | */ 89 | fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit = 0.0.volts) 90 | 91 | /** 92 | * Sets the output [velocity] in [K]/s and [arbitraryFeedForward] in volts 93 | */ 94 | fun setVelocity(velocity: SIUnit>, arbitraryFeedForward: SIUnit = 0.0.volts) 95 | 96 | /** 97 | * Sets the output [position] in [K] and [arbitraryFeedForward] in volts 98 | */ 99 | fun setPosition(position: SIUnit, arbitraryFeedForward: SIUnit = 0.0.volts) 100 | 101 | /** 102 | * Sets the output of the motor to neutral 103 | */ 104 | fun setNeutral() 105 | } 106 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/sensors/ADNS3080FlowSensor.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.sensors 10 | 11 | import edu.wpi.first.wpilibj.SPI 12 | import edu.wpi.first.wpilibj.geometry.Rotation2d 13 | import edu.wpi.first.wpilibj.geometry.Translation2d 14 | import kotlin.coroutines.CoroutineContext 15 | import kotlinx.coroutines.CoroutineScope 16 | import kotlinx.coroutines.Job 17 | import kotlinx.coroutines.isActive 18 | import kotlinx.coroutines.launch 19 | import org.ghrobotics.lib.mathematics.twodim.geometry.Translation2d 20 | import org.ghrobotics.lib.mathematics.units.feet 21 | import org.ghrobotics.lib.utils.Source 22 | 23 | /** 24 | * Implementation of the ADNS3080 Optical Flow Sensor connected over SPI 25 | * 26 | * @param port the spi port 27 | * @param ticksPerFoot amount of ticks per foot 28 | * @param rotation2d the rotation offset (sometimes we dont mount things perfectly) 29 | */ 30 | class ADNS3080FlowSensor( 31 | private val port: SPI.Port, 32 | private val ticksPerFoot: Double, 33 | private val rotation2d: Rotation2d, 34 | parent: CoroutineContext 35 | ) : Source { 36 | 37 | private val job = Job(parent[Job]) 38 | private val scope = CoroutineScope(parent + job) 39 | 40 | private val dataSent = ByteArray(1) { 0.toByte() } 41 | private val dataReceived = ByteArray(1) { 0.toByte() } 42 | 43 | var surfaceQuality = 0 44 | private set 45 | private var accumX = 0.0 46 | private var accumY = 0.0 47 | 48 | init { 49 | scope.launch { 50 | val spi = SPI(port) 51 | spi.setChipSelectActiveLow() 52 | spi.setClockActiveHigh() 53 | spi.setClockRate(500000) 54 | 55 | while (isActive) { 56 | var rawDx = 0.0 57 | var rawDy = 0.0 58 | val motionRegister = spi.readRegister(2.toByte()).toInt() 59 | if (motionRegister and 0x80 != 0) { 60 | rawDy = spi.readRegister(3.toByte()).toDouble() 61 | rawDx = spi.readRegister(4.toByte()).toDouble() 62 | } 63 | surfaceQuality = spi.readRegister(5.toByte()).toInt() 64 | accumX += rawDx 65 | accumY += rawDy 66 | } 67 | } 68 | } 69 | 70 | private fun SPI.readRegister(register: Byte): Byte { 71 | dataSent[0] = register 72 | write(dataSent, 1) 73 | read(true, dataReceived, 1) 74 | read(false, dataReceived, 1) 75 | return dataReceived[0] 76 | } 77 | 78 | override fun invoke() = Translation2d(accumX.feet, accumY.feet).rotateBy(rotation2d) 79 | 80 | fun free() { 81 | job.cancel() 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/SensorlessCompatibleSubsystem.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.subsystems 10 | 11 | interface SensorlessCompatibleSubsystem { 12 | fun disableClosedLoopControl() 13 | fun enableClosedLoopControl() 14 | } 15 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/CharacterizationCommand.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.subsystems.drive 10 | 11 | import edu.wpi.first.networktables.NetworkTableInstance 12 | import edu.wpi.first.wpilibj.RobotController 13 | import edu.wpi.first.wpilibj.Timer 14 | import org.ghrobotics.lib.commands.FalconCommand 15 | 16 | /** 17 | * Command that characterizes the robot using the robotpy-characterization 18 | * toolsuite. 19 | * 20 | * @param drivetrain The instance of FalconWCD to use. 21 | */ 22 | class CharacterizationCommand(private val drivetrain: FalconWestCoastDrivetrain) : FalconCommand(drivetrain) { 23 | 24 | private val numberArray = DoubleArray(9) 25 | 26 | private val autoSpeedEntry = NetworkTableInstance.getDefault().getEntry("/robot/autospeed") 27 | private val telemetryEntry = NetworkTableInstance.getDefault().getEntry("/robot/telemetry") 28 | 29 | private var priorAutoSpeed = 0.0 30 | 31 | override fun execute() { 32 | val autospeed = autoSpeedEntry.getDouble(0.0) 33 | priorAutoSpeed = autospeed 34 | 35 | drivetrain.setPercent(autospeed, autospeed) 36 | 37 | numberArray[0] = Timer.getFPGATimestamp() 38 | numberArray[1] = RobotController.getBatteryVoltage() 39 | numberArray[2] = autospeed 40 | numberArray[3] = drivetrain.leftVoltage.value 41 | numberArray[4] = drivetrain.rightVoltage.value 42 | numberArray[5] = drivetrain.leftPosition.value 43 | numberArray[6] = drivetrain.rightPosition.value 44 | numberArray[7] = drivetrain.leftVelocity.value 45 | numberArray[8] = drivetrain.rightVelocity.value 46 | 47 | telemetryEntry.setNumberArray(numberArray.toTypedArray()) 48 | } 49 | 50 | override fun end(interrupted: Boolean) { 51 | drivetrain.setNeutral() 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/TrajectoryTrackerCommand.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.subsystems.drive 10 | 11 | import edu.wpi.first.wpilibj.DriverStation 12 | import edu.wpi.first.wpilibj.Timer 13 | import edu.wpi.first.wpilibj.trajectory.Trajectory 14 | import org.ghrobotics.lib.commands.FalconCommand 15 | import org.ghrobotics.lib.debug.FalconDashboard 16 | import org.ghrobotics.lib.mathematics.twodim.geometry.x_u 17 | import org.ghrobotics.lib.mathematics.twodim.geometry.y_u 18 | import org.ghrobotics.lib.mathematics.units.inFeet 19 | import org.ghrobotics.lib.utils.Source 20 | 21 | /** 22 | * Represents a command that is used to follow a trajectory. 23 | * 24 | * @param drivetrain The drivetrain being used to follow the trajectory. 25 | * @param trajectorySource The trajectory source. 26 | */ 27 | class TrajectoryTrackerCommand( 28 | private val drivetrain: TrajectoryTrackerDriveBase, 29 | private val trajectorySource: Source 30 | ) : FalconCommand(drivetrain) { 31 | 32 | private var prevLeftVelocity = 0.0 33 | private var prevRightVelocity = 0.0 34 | 35 | private val timer = Timer() 36 | private var elapsed = 0.0 37 | private lateinit var trajectory: Trajectory 38 | 39 | /** 40 | * Initializes the command, 41 | */ 42 | override fun initialize() { 43 | trajectory = trajectorySource() 44 | timer.start() 45 | 46 | prevLeftVelocity = 0.0 47 | prevRightVelocity = 0.0 48 | 49 | FalconDashboard.isFollowingPath = true 50 | } 51 | 52 | /** 53 | * Executes at 50 Hz. 54 | */ 55 | override fun execute() { 56 | // Get the elapsed time 57 | elapsed = timer.get() 58 | 59 | // Get the current trajectory state. 60 | val currentTrajectoryState = trajectory.sample(elapsed) 61 | 62 | // Get the adjusted chassis speeds from the controller. 63 | val chassisSpeeds = drivetrain.controller.calculate( 64 | drivetrain.robotPosition, 65 | currentTrajectoryState 66 | ) 67 | 68 | // Get the wheel speeds from the chassis speeds. 69 | val wheelSpeeds = drivetrain.kinematics.toWheelSpeeds(chassisSpeeds) 70 | 71 | // Calculate accelerations 72 | val leftAcceleration = (wheelSpeeds.leftMetersPerSecond - prevLeftVelocity) * 50 73 | val rightAcceleration = (wheelSpeeds.rightMetersPerSecond - prevRightVelocity) * 50 74 | 75 | prevLeftVelocity = wheelSpeeds.leftMetersPerSecond 76 | prevRightVelocity = wheelSpeeds.rightMetersPerSecond 77 | 78 | drivetrain.setOutputSI( 79 | wheelSpeeds.leftMetersPerSecond, 80 | wheelSpeeds.rightMetersPerSecond, 81 | leftAcceleration, 82 | rightAcceleration 83 | ) 84 | 85 | if (currentTrajectoryState != null) { 86 | val referencePose = currentTrajectoryState.poseMeters 87 | 88 | // Update Current Path Location on Live Dashboard 89 | FalconDashboard.pathX = referencePose.translation.x_u.inFeet() 90 | FalconDashboard.pathY = referencePose.translation.y_u.inFeet() 91 | FalconDashboard.pathHeading = referencePose.rotation.radians 92 | } 93 | } 94 | 95 | /** 96 | * Ends the command. 97 | */ 98 | override fun end(interrupted: Boolean) { 99 | drivetrain.setNeutral() 100 | FalconDashboard.isFollowingPath = false 101 | 102 | if (interrupted) { 103 | DriverStation.reportError("Trajectory tracking was interrupted.", false) 104 | } 105 | } 106 | 107 | /** 108 | * Checks if the trajectory has finished executing. 109 | */ 110 | override fun isFinished() = elapsed > trajectory.totalTimeSeconds 111 | } 112 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/TrajectoryTrackerDriveBase.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.subsystems.drive 10 | 11 | import edu.wpi.first.wpilibj.controller.RamseteController 12 | import edu.wpi.first.wpilibj.geometry.Pose2d 13 | import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics 14 | import org.ghrobotics.lib.commands.FalconSubsystem 15 | 16 | /** 17 | * An interface to implement to follow trajectories. 18 | */ 19 | abstract class TrajectoryTrackerDriveBase : FalconSubsystem() { 20 | 21 | abstract val controller: RamseteController 22 | abstract var robotPosition: Pose2d 23 | abstract val kinematics: DifferentialDriveKinematics 24 | 25 | abstract fun setOutputSI( 26 | leftVelocity: Double, 27 | rightVelocity: Double, 28 | leftAcceleration: Double, 29 | rightAcceleration: Double 30 | ) 31 | } 32 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/utils/WpiCoroutineUtils.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.utils 10 | 11 | import kotlin.coroutines.CoroutineContext 12 | import kotlin.coroutines.EmptyCoroutineContext 13 | import kotlinx.coroutines.CoroutineScope 14 | import kotlinx.coroutines.CoroutineStart 15 | import kotlinx.coroutines.Job 16 | import kotlinx.coroutines.isActive 17 | import kotlinx.coroutines.launch 18 | import org.ghrobotics.lib.wrappers.FalconNotifier 19 | 20 | fun CoroutineScope.launchFrequency( 21 | frequency: Int = 50, 22 | context: CoroutineContext = EmptyCoroutineContext, 23 | start: CoroutineStart = CoroutineStart.DEFAULT, 24 | block: suspend CoroutineScope.() -> Unit 25 | ): Job { 26 | if (frequency <= 0) throw IllegalArgumentException("Frequency cannot be lower then 1!") 27 | return launch(context, start) { 28 | loopFrequency(frequency, block) 29 | } 30 | } 31 | 32 | suspend fun CoroutineScope.loopFrequency( 33 | frequency: Int = 50, 34 | block: suspend CoroutineScope.() -> Unit 35 | ) { 36 | 37 | val notifier = FalconNotifier(frequency) 38 | notifier.updateAlarm() 39 | 40 | while (isActive) { 41 | notifier.waitForAlarm() 42 | block(this) 43 | notifier.updateAlarm() 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconNotifier.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers 10 | 11 | import edu.wpi.first.hal.NotifierJNI 12 | import edu.wpi.first.wpilibj.Notifier 13 | import edu.wpi.first.wpilibj.Timer 14 | import java.util.concurrent.TimeUnit 15 | import org.ghrobotics.lib.mathematics.units.SIUnit 16 | import org.ghrobotics.lib.mathematics.units.Second 17 | import org.ghrobotics.lib.mathematics.units.inMicroseconds 18 | import org.ghrobotics.lib.mathematics.units.seconds 19 | 20 | /** 21 | * A minimal wrapper over FPGA notifier alarms for periodic behaviour 22 | * 23 | * Unlike wpilib's [Notifier], this alarm does not spawn a new thread, nor does it take a task to run. 24 | * Rather it provides the functionality for FPGA-backed timing, and leaves task execution to the caller 25 | */ 26 | class FalconNotifier(frequency: Int) : AutoCloseable { 27 | 28 | /** 29 | * The period of this alarm in microseconds 30 | */ 31 | private val period = TimeUnit.SECONDS.toMicros(1) / frequency 32 | 33 | /** 34 | * The JNI handle for this alarm 35 | */ 36 | private val notifier = NotifierJNI.initializeNotifier() 37 | 38 | private var closed = false 39 | 40 | /** 41 | * Updates the notifier alarm. Should be called when the alarm is tripped to 42 | * reset the ticker 43 | */ 44 | fun updateAlarm(currentTime: SIUnit = Timer.getFPGATimestamp().seconds) { 45 | if (closed) { 46 | throw IllegalStateException("updateAlarm() called on a disposed notifier! Check usages of close() for the relevant instance") 47 | } 48 | NotifierJNI.updateNotifierAlarm(notifier, (currentTime.inMicroseconds() + period).toLong()) 49 | } 50 | 51 | /** 52 | * Blocks the thread until the notifier alarm trips. 53 | */ 54 | fun waitForAlarm(): Long { 55 | if (closed) { 56 | throw IllegalStateException("waitForAlarm() called on a disposed notifier! Check usages of close() for the relevant instance") 57 | } 58 | return NotifierJNI.waitForNotifierAlarm(notifier) 59 | } 60 | 61 | /** 62 | * Disposes of the internal notifier handle, and **invalidates** the associated [FalconNotifier] 63 | * 64 | * This should be the last function called before the instance goes out of scope for garbage collection. 65 | * Alarm resetting should not be done if the notifier has been closed. 66 | */ 67 | override fun close() { 68 | closed = true 69 | NotifierJNI.cleanNotifier(notifier) 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconSolenoid.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers 10 | 11 | import edu.wpi.first.wpilibj.DoubleSolenoid 12 | import edu.wpi.first.wpilibj.Solenoid 13 | import kotlin.properties.Delegates 14 | 15 | /** 16 | * Interface for both double- and single-solenoids. 17 | */ 18 | interface FalconSolenoid { 19 | enum class State { 20 | Forward, 21 | Reverse, 22 | Off 23 | } 24 | 25 | var state: State 26 | } 27 | 28 | /** 29 | * Single-acting solenoid with only one PCM Solenoid channel. 30 | * This type of solenoid can only be on or off. 31 | */ 32 | class FalconSingleSolenoid(channel: Int, module: Int? = null) : FalconSolenoid { 33 | private val wpiSolenoid: Solenoid = if (module == null) Solenoid(channel) else Solenoid(module, channel) 34 | 35 | // Set the solenoid's position. Forward -> true, Reverse -> false 36 | // If the solenoid is set to 'Off', the state is not changed. 37 | override var state: FalconSolenoid.State by Delegates.observable(FalconSolenoid.State.Reverse) { _, _, newValue -> 38 | when (newValue) { 39 | FalconSolenoid.State.Forward -> wpiSolenoid.set(true) 40 | FalconSolenoid.State.Reverse -> wpiSolenoid.set(false) 41 | else -> Unit 42 | } 43 | } 44 | } 45 | 46 | /** 47 | * Double-acting solenoid with two PCM Solenoid channels. 48 | * This type of solenoid can be forward (Forward channel is active), reverse (Reverse channel is active), 49 | * or off (Neither of the two channels are active). 50 | */ 51 | class FalconDoubleSolenoid(forwardChannel: Int, reverseChannel: Int, module: Int? = null) : 52 | FalconSolenoid { 53 | 54 | private val wpiSolenoid: DoubleSolenoid = 55 | if (module == null) DoubleSolenoid(forwardChannel, reverseChannel) 56 | else DoubleSolenoid(module, forwardChannel, reverseChannel) 57 | 58 | // Set the solenoid to the desired position 59 | override var state: FalconSolenoid.State by Delegates.observable(FalconSolenoid.State.Off) { _, _, newValue -> 60 | when (newValue) { 61 | FalconSolenoid.State.Forward -> wpiSolenoid.set(DoubleSolenoid.Value.kForward) 62 | FalconSolenoid.State.Reverse -> wpiSolenoid.set(DoubleSolenoid.Value.kReverse) 63 | FalconSolenoid.State.Off -> wpiSolenoid.set(DoubleSolenoid.Value.kOff) 64 | } 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers 10 | 11 | import edu.wpi.first.hal.FRCNetComm 12 | import edu.wpi.first.hal.HAL 13 | import edu.wpi.first.wpilibj.RobotBase 14 | import edu.wpi.first.wpilibj.TimedRobot 15 | import edu.wpi.first.wpilibj.livewindow.LiveWindow 16 | import edu.wpi.first.wpilibj2.command.Command 17 | import edu.wpi.first.wpilibj2.command.CommandScheduler 18 | import org.ghrobotics.lib.commands.FalconSubsystem 19 | import org.ghrobotics.lib.commands.FalconSubsystemHandler 20 | import org.ghrobotics.lib.subsystems.SensorlessCompatibleSubsystem 21 | 22 | abstract class FalconTimedRobot { 23 | 24 | enum class Mode { 25 | NONE, 26 | DISABLED, 27 | AUTONOMOUS, 28 | TELEOP, 29 | TEST 30 | } 31 | 32 | var currentMode = Mode.NONE 33 | private set 34 | 35 | private val sensorlessReadySystems = arrayListOf() 36 | var sensorlessModeActive = false 37 | protected set 38 | 39 | protected val wrappedValue = WpiTimedRobot() 40 | 41 | protected inner class WpiTimedRobot : TimedRobot() { 42 | 43 | private val kLanguage_Kotlin = 6 44 | 45 | init { 46 | HAL.report(FRCNetComm.tResourceType.kResourceType_Language, kLanguage_Kotlin) 47 | } 48 | 49 | override fun robotInit() { 50 | currentMode = FalconTimedRobot.Mode.NONE 51 | this@FalconTimedRobot.robotInit() 52 | FalconSubsystemHandler.lateInit() 53 | LiveWindow.disableAllTelemetry() 54 | } 55 | 56 | override fun autonomousInit() { 57 | currentMode = FalconTimedRobot.Mode.AUTONOMOUS 58 | this@FalconTimedRobot.autonomousInit() 59 | FalconSubsystemHandler.autoReset() 60 | } 61 | 62 | override fun teleopInit() { 63 | currentMode = FalconTimedRobot.Mode.TELEOP 64 | this@FalconTimedRobot.teleopInit() 65 | FalconSubsystemHandler.teleopReset() 66 | } 67 | 68 | override fun disabledInit() { 69 | currentMode = FalconTimedRobot.Mode.DISABLED 70 | this@FalconTimedRobot.disabledInit() 71 | FalconSubsystemHandler.setNeutral() 72 | } 73 | 74 | override fun testInit() { 75 | currentMode = FalconTimedRobot.Mode.TEST 76 | this@FalconTimedRobot.testInit() 77 | } 78 | 79 | override fun robotPeriodic() { 80 | this@FalconTimedRobot.robotPeriodic() 81 | CommandScheduler.getInstance().run() 82 | } 83 | 84 | override fun autonomousPeriodic() { 85 | this@FalconTimedRobot.autonomousPeriodic() 86 | } 87 | 88 | override fun teleopPeriodic() { 89 | this@FalconTimedRobot.teleopPeriodic() 90 | } 91 | 92 | override fun disabledPeriodic() { 93 | this@FalconTimedRobot.disabledPeriodic() 94 | } 95 | } 96 | 97 | protected open fun robotInit() {} 98 | protected open fun autonomousInit() {} 99 | protected open fun teleopInit() {} 100 | protected open fun disabledInit() {} 101 | private fun testInit() {} 102 | 103 | protected open fun robotPeriodic() {} 104 | protected open fun autonomousPeriodic() {} 105 | protected open fun teleopPeriodic() {} 106 | protected open fun disabledPeriodic() {} 107 | 108 | protected fun getSubsystemChecks(): Command { 109 | return FalconSubsystemHandler.testCommand 110 | } 111 | 112 | open operator fun FalconSubsystem.unaryPlus() { 113 | FalconSubsystemHandler.add(this) 114 | if (this is SensorlessCompatibleSubsystem) { 115 | sensorlessReadySystems.add(this) 116 | } 117 | } 118 | 119 | fun disableClosedLoopControl() { 120 | sensorlessReadySystems.forEach { it.disableClosedLoopControl() } 121 | sensorlessModeActive = true 122 | } 123 | 124 | fun enableClosedLoopControl() { 125 | sensorlessReadySystems.forEach { it.enableClosedLoopControl() } 126 | sensorlessModeActive = false 127 | } 128 | 129 | fun start() { 130 | RobotBase.startRobot { wrappedValue } 131 | } 132 | } 133 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/hid/FalconPS4Controller.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers.hid 10 | 11 | import edu.wpi.first.wpilibj.GenericHID 12 | import edu.wpi.first.wpilibj.Joystick 13 | 14 | typealias FalconPS4Controller = FalconHID 15 | typealias FalconPS4Builder = FalconHIDBuilder 16 | 17 | // Builder helpers 18 | fun ps4Controller(port: Int, block: FalconPS4Builder.() -> Unit): FalconPS4Controller = 19 | Joystick(port).mapControls(block) 20 | 21 | fun FalconPS4Builder.button( 22 | button: PS4Button, 23 | block: FalconHIDButtonBuilder.() -> Unit = {} 24 | ) = button(button.value, block) 25 | 26 | fun FalconPS4Builder.triggerAxisButton( 27 | hand: GenericHID.Hand, 28 | threshold: Double = HIDButton.DEFAULT_THRESHOLD, 29 | block: FalconHIDButtonBuilder.() -> Unit = {} 30 | ) = axisButton(yTriggerAxisToRawAxis(hand), threshold, block) 31 | 32 | // Source helpers 33 | fun FalconPS4Controller.getY(hand: GenericHID.Hand) = getRawAxis(yAxisToRawAxis(hand)) 34 | 35 | fun FalconPS4Controller.getX(hand: GenericHID.Hand) = getRawAxis(xAxisToRawAxis(hand)) 36 | fun FalconPS4Controller.getRawButton(button: XboxButton) = getRawButton(button.value) 37 | 38 | private fun yAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 1 else 5 39 | private fun xAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 0 else 2 40 | private fun yTriggerAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 2 else 3 41 | 42 | enum class PS4Button(val value: Int) { 43 | Square(1), 44 | X(2), 45 | Circle(3), 46 | Triangle(4), 47 | BumperLeft(5), 48 | BumperRight(6), 49 | TriggerLeft(7), 50 | TriggerRight(8), 51 | Share(9), 52 | Options(10), 53 | StickLeft(11), 54 | StickRight(12), 55 | Playstation(13), 56 | Touchpad(14) 57 | } 58 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/hid/FalconXboxController.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers.hid 10 | 11 | import edu.wpi.first.wpilibj.GenericHID 12 | import edu.wpi.first.wpilibj.XboxController 13 | 14 | typealias FalconXboxController = FalconHID 15 | typealias FalconXboxBuilder = FalconHIDBuilder 16 | 17 | // Builder Helpers 18 | fun xboxController(port: Int, block: FalconXboxBuilder.() -> Unit): FalconXboxController = 19 | XboxController(port).mapControls(block) 20 | 21 | fun FalconXboxBuilder.button( 22 | button: XboxButton, 23 | block: FalconHIDButtonBuilder.() -> Unit = {} 24 | ) = button(button.value, block) 25 | 26 | fun FalconXboxBuilder.triggerAxisButton( 27 | hand: GenericHID.Hand, 28 | threshold: Double = HIDButton.DEFAULT_THRESHOLD, 29 | block: FalconHIDButtonBuilder.() -> Unit = {} 30 | ) = axisButton(yTriggerAxisToRawAxis(hand), threshold, block) 31 | 32 | // Source Helpers 33 | fun FalconXboxController.getY(hand: GenericHID.Hand) = getRawAxis( 34 | yAxisToRawAxis( 35 | hand 36 | ) 37 | ) 38 | 39 | fun FalconXboxController.getX(hand: GenericHID.Hand) = getRawAxis( 40 | xAxisToRawAxis( 41 | hand 42 | ) 43 | ) 44 | 45 | fun FalconXboxController.getRawButton(button: XboxButton) = getRawButton(button.value) 46 | 47 | private fun yAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 1 else 5 48 | private fun xAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 0 else 4 49 | private fun yTriggerAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 2 else 3 50 | 51 | val kBumperLeft = XboxButton(5) 52 | val kBumperRight = XboxButton(6) 53 | val kStickLeft = XboxButton(9) 54 | val kStickRight = XboxButton(10) 55 | val kA = XboxButton(1) 56 | val kB = XboxButton(2) 57 | val kX = XboxButton(3) 58 | val kY = XboxButton(4) 59 | val kBack = XboxButton(7) 60 | val kStart = XboxButton(8) 61 | 62 | data class XboxButton internal constructor(val value: Int) 63 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/hid/HIDControl.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers.hid 10 | 11 | import kotlin.math.absoluteValue 12 | 13 | class HIDButton( 14 | private val source: HIDSource, 15 | private val threshold: Double, 16 | private val whileOff: List, 17 | private val whileOn: List, 18 | private val changeOn: List, 19 | private val changeOff: List 20 | ) : HIDControl { 21 | 22 | companion object { 23 | const val DEFAULT_THRESHOLD = 0.5 24 | } 25 | 26 | private var lastValue = source().absoluteValue >= threshold 27 | 28 | override fun update() { 29 | val newValue = source().absoluteValue >= threshold 30 | when { 31 | // Value has changed 32 | lastValue != newValue -> when { 33 | newValue -> changeOn 34 | else -> changeOff 35 | } 36 | // Value stayed the same 37 | else -> when { 38 | newValue -> whileOn 39 | else -> whileOff 40 | } 41 | }.forEach { it() } 42 | lastValue = newValue 43 | } 44 | } 45 | 46 | typealias HIDControlListener = () -> Unit 47 | 48 | interface HIDControl { 49 | fun update() 50 | } 51 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/hid/HIDSource.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers.hid 10 | 11 | import edu.wpi.first.wpilibj.GenericHID 12 | import org.ghrobotics.lib.utils.BooleanSource 13 | import org.ghrobotics.lib.utils.Source 14 | 15 | class HIDButtonSource( 16 | private val genericHID: GenericHID, 17 | private val buttonId: Int 18 | ) : HIDSource { 19 | val booleanSource: BooleanSource = { genericHID.getRawButton(buttonId) } 20 | 21 | override fun invoke() = if (booleanSource()) 1.0 else 0.0 22 | } 23 | 24 | class HIDAxisSource( 25 | private val genericHID: GenericHID, 26 | private val axisId: Int 27 | ) : HIDSource { 28 | override fun invoke() = genericHID.getRawAxis(axisId) 29 | } 30 | 31 | class BoundedHIDAxisSource( 32 | private val genericHID: GenericHID, 33 | private val axisId: Int, 34 | private val range: ClosedFloatingPointRange 35 | ) : HIDSource { 36 | override fun invoke() = genericHID.getRawAxis(axisId).coerceIn(range) 37 | } 38 | 39 | class HIDPOVSource( 40 | private val genericHID: GenericHID, 41 | private val povId: Int, 42 | private val angle: Int 43 | ) : HIDSource { 44 | override fun invoke() = if (genericHID.getPOV(povId) == angle) 1.0 else 0.0 45 | } 46 | 47 | interface HIDSource : Source 48 | -------------------------------------------------------------------------------- /wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/networktables/FalconNetworkTable.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.wrappers.networktables 10 | 11 | import edu.wpi.first.networktables.NetworkTable 12 | import edu.wpi.first.networktables.NetworkTableEntry 13 | import edu.wpi.first.networktables.NetworkTableInstance 14 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser 15 | import kotlin.reflect.KProperty 16 | import org.ghrobotics.lib.utils.capitalizeEachWord 17 | 18 | object FalconNetworkTable { 19 | val instance: NetworkTableInstance = NetworkTableInstance.getDefault() 20 | 21 | operator fun get(name: String): NetworkTableEntry = 22 | getEntry(name) 23 | 24 | fun getEntry(name: String): NetworkTableEntry = instance.getEntry(name) 25 | 26 | fun getTable(name: String): NetworkTable = instance.getTable(name) 27 | } 28 | 29 | operator fun NetworkTable.get(name: String): NetworkTableEntry = getEntry(name) 30 | 31 | fun NetworkTableEntry.delegate(defaultValue: String = ""): NetworkTableEntryDelegate = 32 | delegate { this.getString(defaultValue) } 33 | 34 | fun NetworkTableEntry.delegate(defaultValue: Double = 0.0): NetworkTableEntryDelegate = 35 | delegate { this.getDouble(defaultValue) } 36 | 37 | fun NetworkTableEntry.delegate(defaultValue: Boolean = false): NetworkTableEntryDelegate = 38 | delegate { this.getBoolean(defaultValue) } 39 | 40 | private fun NetworkTableEntry.delegate(get: () -> T) = 41 | NetworkTableEntryDelegate(this, get) 42 | 43 | class NetworkTableEntryDelegate( 44 | private val entry: NetworkTableEntry, 45 | private val get: () -> T 46 | ) { 47 | operator fun setValue(networkInterface: Any, property: KProperty<*>, value: T) { 48 | entry.setValue(value) 49 | } 50 | 51 | operator fun getValue(networkInterface: Any, property: KProperty<*>): T = get() 52 | } 53 | 54 | inline fun sendableChooser(crossinline block: SendableChooser.() -> Unit) = 55 | SendableChooser().apply(block) 56 | 57 | inline fun > enumSendableChooser( 58 | crossinline block: (T) -> String = { it.name.replace('_', ' ').capitalizeEachWord() } 59 | ) = sendableChooser { 60 | enumValues().forEach { enumValue -> 61 | addOption(block(enumValue), enumValue) 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /wpi/src/test/kotlin/org/ghrobotics/lib/subsystems/drive/localization/TimeInterpolatableBufferTest.kt: -------------------------------------------------------------------------------- 1 | /* 2 | * This Source Code Form is subject to the terms of the Mozilla Public 3 | * License, v. 2.0. If a copy of the MPL was not distributed with this 4 | * file, You can obtain one at https://mozilla.org/MPL/2.0/. 5 | * 6 | * Copyright 2019, Green Hope Falcons 7 | */ 8 | 9 | package org.ghrobotics.lib.subsystems.drive.localization 10 | 11 | import edu.wpi.first.wpilibj.geometry.Pose2d 12 | import edu.wpi.first.wpilibj.geometry.Rotation2d 13 | import org.ghrobotics.lib.localization.TimePoseInterpolatableBuffer 14 | import org.ghrobotics.lib.mathematics.kEpsilon 15 | import org.ghrobotics.lib.mathematics.twodim.geometry.Pose2d 16 | import org.ghrobotics.lib.mathematics.units.meters 17 | import org.ghrobotics.lib.mathematics.units.seconds 18 | import org.junit.Assert 19 | import org.junit.Test 20 | 21 | class TimeInterpolatableBufferTest { 22 | @Test 23 | fun testInterpolation() { 24 | val buffer = TimePoseInterpolatableBuffer( 25 | 2.seconds, 26 | timeSource = { 2.seconds } 27 | ) 28 | buffer[1000.seconds] = Pose2d() 29 | buffer[2000.seconds] = Pose2d(10.meters, 0.meters, Rotation2d()) 30 | 31 | Assert.assertEquals(Pose2d().translation.norm, buffer[500.seconds]!!.translation.norm, kEpsilon) 32 | Assert.assertEquals( 33 | Pose2d(2.5.meters, 0.meters, Rotation2d()).translation.norm, 34 | buffer[1250.seconds]!!.translation.norm, kEpsilon 35 | ) 36 | Assert.assertEquals( 37 | Pose2d(5.meters, 0.meters, Rotation2d()).translation.norm, 38 | buffer[1500.seconds]!!.translation.norm, kEpsilon 39 | ) 40 | Assert.assertEquals( 41 | Pose2d(10.meters, 0.meters, Rotation2d()).translation.norm, 42 | buffer[2500.seconds]!!.translation.norm, kEpsilon 43 | ) 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /wpi/vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "2020.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "mavenUrls": [], 7 | "jsonUrl": "", 8 | "javaDependencies": [ 9 | { 10 | "groupId": "edu.wpi.first.wpilibNewCommands", 11 | "artifactId": "wpilibNewCommands-java", 12 | "version": "wpilib" 13 | } 14 | ], 15 | "jniDependencies": [], 16 | "cppDependencies": [ 17 | { 18 | "groupId": "edu.wpi.first.wpilibNewCommands", 19 | "artifactId": "wpilibNewCommands-cpp", 20 | "version": "wpilib", 21 | "libName": "wpilibNewCommands", 22 | "headerClassifier": "headers", 23 | "sourcesClassifier": "sources", 24 | "sharedLibrary": true, 25 | "skipInvalidPlatforms": true, 26 | "binaryPlatforms": [ 27 | "linuxathena", 28 | "linuxraspbian", 29 | "linuxaarch64bionic", 30 | "windowsx86-64", 31 | "windowsx86", 32 | "linuxx86-64", 33 | "osxx86-64" 34 | ] 35 | } 36 | ] 37 | } 38 | --------------------------------------------------------------------------------