2019.5.10: New module structure, 3d math (#16)

* FalconMotor Recode

* make gradlew executable on unix systems

* Fix missing abstract methods in SimFalconMotor

* Introduce modeling using Components

* Add ArmComponent

* Add DefaultNativeUnitModel as a replacement for NativeFalconSRX

* Add world and local transforms

* Rename Vector3d to Translation3d

* Add Velocity and Acceleration Transforms

* Module structure

* Forgot to commit some files

* Delete .wpilib/

* Update README

* Update README.md

* Update README.md

README.md

@@ -3,39 +3,56 @@
 
 ## Overview
 
-Falcon Library is the backend library that is used on all FRC Team 5190 robots. This library was written in the Kotlin JVM Language. Some features of this library include:
-
- * Wrapped WPILib Commands and Subsystems with Kotlin Coroutines asynchronous optimzation.
-
- * High level mathematics for path generation, tracking, custom typesafe units of measure, etc.
-    * Two-dimensional parametric and functional splines.
-    * Arc length of parametric quintic hermite splines evaluated using recursive arc subdivision (from Team 254).
-    * Trajectory generation that respects constraints (i.e. centripetal acceleration, motor voltage).
-    * Custom trajectory followers
-        * Ramsete
-        * Adaptive Pure Pursuit
-        * Feedforward
-    * Typesafe units of measure
-        * Quick and easy conversions between all length, velocity, acceleration, electrical units.
-        * Support for Talon SRX native unit length and rotation models.
-
- * AHRS sensor wrapper for Pigeon IMU and NavX.
-
- * Tank Drive Subsystem abstraction with built-in odometry and command to follow trajectories.
-
- * Talon SRX wrapper that utilizes Kotlin properties to set configurations.
-
- * Custom robot base with fully implemented state machine and coroutine support.
-
- * Other WPILib wrappers for NetworkTables, etc.
+Feature-rich Kotlin JVM based robotics library, primarily for use in the FIRST Robotics Competition. FalconLibrary is the backend for all Team 5190 robots.
+
+### `core`: Platform-agnostic mathematics and units code
+* Two-dimensional parametric and functional splines.
+* Arc length of parametric quintic hermite splines evaluated using recursive arc subdivision (from Team 254).
+* Trajectory generation that respects constraints (i.e. centripetal acceleration, motor voltage).
+* Custom trajectory followers
+    * Ramsete
+    * Adaptive Pure Pursuit
+    * Feedforward
+* Typesafe units of measure
+    * Quick and easy conversions between all length, velocity, acceleration, electrical units.
+    * Support for Talon SRX native unit length and rotation models.
+
+### `wpi`: Code specific to the RoboRIO and FIRST Robotics Competition
+* Wrapped WPILib Commands and Subsystems with Kotlin Coroutines asynchronous optimization.
+* Built-in drive subsystem abstraction with support for arbitrary localization.
+* Custom robot base built on coroutines.
+
+### `vendorXXX`: RoboRIO vendor extensions
+* Talon SRX and Spark MAX wrappers that utilize Kotlin properties to set configurations.
+* Custom gyro `Rotation2d` sources.
+
+## Using FalconLibrary in your project
+
+Make sure you can retrieve dependencies from JitPack. Add this to your `build.gradle`:
+```groovy
+repositories {
+    maven { url 'https://jitpack.io' }
+}
+```
+
+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.
+
+```groovy
+compile 'org.ghrobotics:FalconLibrary:core:2019.5.10'
+compile 'org.ghrobotics:FalconLibrary:wpi:2019.5.10'
+compile 'org.ghrobotics:FalconLibrary:vendorCTRE:2019.5.10'
+compile 'org.ghrobotics:FalconLibrary:vendorNAVX:2019.5.10'
+compile 'org.ghrobotics:FalconLibrary:vendorREV:2019.5.10'
+```
+
+Alternatively, you can include all submodules at once:
+```groovy
+compile 'org.ghrobotics:FalconLibrary:2019.5.10'
+```
 
 ## Contributing
+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.
 
-This library is open source and we would love to have you contribute code to this repository. Please make sure that before submitting a pull request, your code is formatted according to `ktlint` (already in the project). The Gradle build will fail if all code is not formatted correctly.
-
-To format code automatically, run `./gradlew spotlessApply`. Please build the project locally using `./gradlew build` to make sure everything works before submitting a pull request. 
-
-When adding new features, it is encouraged that these features be game-agnostic. This library is intended to be used for robots that play any game. Also make sure to include unit-tests for any new features.
 
 
 

build.gradle.kts

@@ -1,57 +1,43 @@
-import edu.wpi.first.toolchain.NativePlatforms
-import org.gradle.api.publish.maven.MavenPublication
 import io.gitlab.arturbosch.detekt.detekt
 import org.jetbrains.kotlin.gradle.tasks.KotlinCompile
 
 plugins {
-    kotlin("jvm") version "1.3.30"
-    id("edu.wpi.first.GradleRIO") version "2019.4.1"
+    kotlin("jvm") version "1.3.30" apply false
+    id("edu.wpi.first.GradleRIO") version "2019.4.1" apply false
     id("io.gitlab.arturbosch.detekt") version "1.0.0-RC13"
     maven
     `maven-publish`
-}
-
-repositories {
-    jcenter()
-    maven { setUrl("http://dl.bintray.com/kyonifer/maven") }
-}
-
-dependencies {
-    // Kotlin Standard Library and Coroutines
-    compile(kotlin("stdlib"))
-    compile("org.jetbrains.kotlinx", "kotlinx-coroutines-core", "1.2.0")
 
-    // WPILib
-    wpi.deps.wpilib().forEach { compile(it) }
-    wpi.deps.vendor.java().forEach { compile(it) }
-    wpi.deps.vendor.jni(NativePlatforms.roborio).forEach { nativeZip(it) }
-    wpi.deps.vendor.jni(NativePlatforms.desktop).forEach { nativeDesktopZip(it) }
-
-    // Apache Commons Math
-    compile("org.apache.commons", "commons-math3", "3.6.1")
-
-    // Gson
-    compile("com.github.salomonbrys.kotson", "kotson", "2.5.0")
-
-    // Unit Testing
-    testCompile("org.knowm.xchart", "xchart", "3.2.2")
-    testCompile("junit", "junit", "4.12")
 }
 
-publishing {
-    publications {
-        create<MavenPublication>("mavenLocal") {
-            groupId = "org.ghrobotics"
-            artifactId = "FalconLibrary"
-            version = "2019.2.23"
-
-            from(components["java"])
+subprojects {
+    apply {
+        plugin("kotlin")
+    }
+    tasks {
+        withType<KotlinCompile>().configureEach {
+            kotlinOptions {
+                jvmTarget = "1.8"
+                freeCompilerArgs += "-Xjvm-default=compatibility"
+            }
         }
     }
+    repositories {
+        jcenter()
+        maven { setUrl("http://dl.bintray.com/kyonifer/maven") }
+    }
+    dependencies {
+        "compile"(kotlin("stdlib"))
+        "compile"("org.jetbrains.kotlinx", "kotlinx-coroutines-core", "1.2.0")
+
+        "testCompile"("org.knowm.xchart", "xchart", "3.2.2")
+        "testCompile"("junit", "junit", "4.12")
+    }
 }
 
 detekt {
-    config = files("$projectDir/detekt-config.yml")
+    config = files("$rootDir/detekt-config.yml")
+    println(rootDir)
 
     reports {
         html {
@@ -65,11 +51,4 @@ tasks {
     withType<Wrapper>().configureEach {
         gradleVersion = "5.0"
     }
-    withType<KotlinCompile>().configureEach {
-        kotlinOptions {
-            jvmTarget = "1.8"
-            freeCompilerArgs += "-Xjvm-default=compatibility"
-        }
-    }
 }
-

core/build.gradle.kts

@@ -0,0 +1,4 @@
+dependencies {
+    // Apache Commons Math
+    compile("org.apache.commons", "commons-math3", "3.6.1")
+}

src/main/kotlin/com/team254/lib/physics/DifferentialDrive.kt → core/src/main/kotlin/com/team254/lib/physics/DifferentialDrive.kt

@@ -61,19 +61,21 @@ class DifferentialDrive(
      * Solve forward kinematics to get chassis motion from wheel motion.
      * Could be either acceleration or velocity.
      */
-    fun solveForwardKinematics(wheelMotion: WheelState): ChassisState = ChassisState(
-        wheelRadius * (wheelMotion.right + wheelMotion.left) / 2.0,
-        wheelRadius * (wheelMotion.right - wheelMotion.left) / (2.0 * effectiveWheelBaseRadius)
-    )
+    fun solveForwardKinematics(wheelMotion: WheelState): ChassisState =
+        ChassisState(
+            wheelRadius * (wheelMotion.right + wheelMotion.left) / 2.0,
+            wheelRadius * (wheelMotion.right - wheelMotion.left) / (2.0 * effectiveWheelBaseRadius)
+        )
 
     /**
      * Solve inverse kinematics to get wheel motion from chassis motion.
      * Could be either acceleration or velocity.
      */
-    fun solveInverseKinematics(chassisMotion: ChassisState): WheelState = WheelState(
-        (chassisMotion.linear - effectiveWheelBaseRadius * chassisMotion.angular) / wheelRadius,
-        (chassisMotion.linear + effectiveWheelBaseRadius * chassisMotion.angular) / wheelRadius
-    )
+    fun solveInverseKinematics(chassisMotion: ChassisState): WheelState =
+        WheelState(
+            (chassisMotion.linear - effectiveWheelBaseRadius * chassisMotion.angular) / wheelRadius,
+            (chassisMotion.linear + effectiveWheelBaseRadius * chassisMotion.angular) / wheelRadius
+        )
 
     /**
      * Solve forward dynamics for torques and accelerations.
@@ -88,8 +90,8 @@ class DifferentialDrive(
     /**
      * Get the voltage simply from the Kv and the friction voltage of the transmissions
      */
-    fun getVoltagesFromkV(velocities: DifferentialDrive.WheelState): WheelState {
-        return DifferentialDrive.WheelState(
+    fun getVoltagesFromkV(velocities: WheelState): WheelState {
+        return WheelState(
             velocities.left / leftTransmission.speedPerVolt +
                 leftTransmission.frictionVoltage * Math.signum(velocities.left),
             velocities.right / rightTransmission.speedPerVolt +
@@ -401,9 +403,13 @@ class DifferentialDrive(
         }
 
         operator fun minus(other: ChassisState) =
-            ChassisState(this.linear - other.linear, this.angular - other.angular)
+            ChassisState(
+                this.linear - other.linear,
+                this.angular - other.angular
+            )
 
-        operator fun times(scalar: Double) = ChassisState(linear * scalar, angular * scalar)
+        operator fun times(scalar: Double) =
+            ChassisState(linear * scalar, angular * scalar)
         operator fun div(scalar: Double) = this * (1 / scalar)
     }
 

core/src/main/kotlin/org/ghrobotics/lib/mathematics/threedim/geometry/Pose3d.kt

@@ -0,0 +1,32 @@
+package org.ghrobotics.lib.mathematics.threedim.geometry
+
+typealias Transform = Pose3d
+
+data class Pose3d(
+    val translation: Translation3d = Translation3d.kZero,
+    val rotation: Quaternion = Quaternion.kIdentity
+) {
+
+    operator fun minus(other: Pose3d) = this + -other
+
+    operator fun plus(other: Pose3d) =
+        Pose3d(
+            translation + (other.translation * rotation),
+            rotation * other.rotation
+        )
+
+    operator fun unaryMinus(): Pose3d {
+        val invertedRotation = -rotation
+        return Pose3d((-translation) * invertedRotation, invertedRotation)
+    }
+
+    operator fun times(scalar: Double) = Pose3d(
+        translation * scalar,
+        rotation * scalar
+    )
+
+    operator fun div(scalar: Double) = times(1.0 / scalar)
+
+    infix fun inFrameOfReferenceOf(fieldRelativeOrigin: Pose3d) = (-fieldRelativeOrigin) + this
+
+}