Wpi trajectory support

.gitignore

@@ -1,5 +1,6 @@
 .gradle
 /build/
+/core/build/
 
 # Ignore Gradle GUI config
 gradle-app.setting

core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt

@@ -1,14 +1,14 @@
 package org.sert2521.sertain.motors
 
-import org.sert2521.sertain.units.Angular
-import org.sert2521.sertain.units.MetricUnit
+import org.sert2521.sertain.units.AngularUnit
+import org.sert2521.sertain.units.AngularVelocityUnit
 import org.sert2521.sertain.units.Seconds
 import org.sert2521.sertain.units.div
 import kotlin.math.PI
 
 class Encoder(ticksPerRevolution: Int) {
     val ticks = EncoderTicks(ticksPerRevolution)
-    val ticksPerSecond = ticks / Seconds
+    val ticksPerSecond: AngularVelocityUnit<EncoderTicks, Seconds> = ticks / Seconds
 }
 
-class EncoderTicks(ticksPerRevolution: Int) : MetricUnit<Angular>(Angular, (PI * 2) / ticksPerRevolution, " ticks")
+class EncoderTicks(ticksPerRevolution: Int) : AngularUnit((PI * 2) / ticksPerRevolution, " ticks")

core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt

@@ -1,12 +1,12 @@
 package org.sert2521.sertain.motors
 
-import org.sert2521.sertain.units.Angular
-import org.sert2521.sertain.units.Chronic
-import org.sert2521.sertain.units.CompositeUnit
-import org.sert2521.sertain.units.CompositeUnitType
-import org.sert2521.sertain.units.MetricUnit
+import org.sert2521.sertain.units.AngularUnit
+import org.sert2521.sertain.units.AngularValue
+import org.sert2521.sertain.units.AngularVelocity
+import org.sert2521.sertain.units.AngularVelocityUnit
+import org.sert2521.sertain.units.AngularVelocityValue
+import org.sert2521.sertain.units.ChronicUnit
 import org.sert2521.sertain.units.MetricValue
-import org.sert2521.sertain.units.Per
 import org.sert2521.sertain.units.convertTo
 import com.ctre.phoenix.motorcontrol.ControlMode as CtreControlMode
 import com.ctre.phoenix.motorcontrol.NeutralMode as CtreNeutralMode
@@ -106,7 +106,7 @@ class MotorController<T : MotorId>(
                 integralZone ?: pidf[slot]?.integralZone ?: 0,
                 allowedError ?: pidf[slot]?.allowedError ?: 0,
                 maxIntegral ?: pidf[slot]?.maxIntegral ?: 0.0,
-                maxOutput ?: pidf[slot]?.maxIntegral ?: 0.0,
+                maxOutput ?: pidf[slot]?.maxOutput ?: 1.0,
                 period ?: pidf[slot]?.period ?: 0
             )
         }
@@ -172,13 +172,13 @@ class MotorController<T : MotorId>(
             ctreMotorController.selectedSensorPosition = value
         }
 
-    fun position(unit: MetricUnit<Angular>) =
+    fun <U : AngularUnit> position(unit: U) =
             MetricValue(encoder!!.ticks, position.toDouble()).convertTo(unit)
 
     val velocity: Int
         get() = ctreMotorController.getSelectedSensorVelocity(0)
 
-    fun velocity(unit: CompositeUnit<Per, Angular, Chronic>) =
+    fun <U1 : AngularVelocity, U2 : ChronicUnit, U : AngularVelocityUnit<U1, U2>> velocity(unit: U) =
             MetricValue(encoder!!.ticksPerSecond, velocity.toDouble()).convertTo(unit)
 
     fun setPercentOutput(output: Double) {
@@ -189,7 +189,7 @@ class MotorController<T : MotorId>(
         ctreMotorController.set(CtreControlMode.Position, position.toDouble())
     }
 
-    fun <U : MetricUnit<Angular>> setTargetPosition(position: MetricValue<Angular, U>) {
+    fun <U : AngularUnit, V : AngularValue<U>> setTargetPosition(position: V) {
         checkNotNull(encoder) { "You must configure your encoder to use units." }
         setTargetPosition(position.convertTo(encoder!!.ticks).value.toInt())
     }
@@ -198,9 +198,7 @@ class MotorController<T : MotorId>(
         ctreMotorController.set(CtreControlMode.Velocity, velocity.toDouble())
     }
 
-    fun setTargetVelocity(
-        velocity: MetricValue<CompositeUnitType<Per, Angular, Chronic>, CompositeUnit<Per, Angular, Chronic>>
-    ) {
+    fun <U1 : AngularUnit, U2 : ChronicUnit, V : AngularVelocityValue<U1, U2>> setTargetVelocity(velocity: V) {
         checkNotNull(encoder) { "You must configure your encoder to use units." }
         setTargetVelocity(velocity.convertTo(encoder!!.ticksPerSecond).value.toInt())
     }

core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt

@@ -0,0 +1,87 @@
+package org.sert2521.sertain.pathfinding
+
+import edu.wpi.first.wpilibj.controller.RamseteController
+import edu.wpi.first.wpilibj.geometry.Pose2d
+import edu.wpi.first.wpilibj.geometry.Rotation2d
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics
+import edu.wpi.first.wpilibj.trajectory.Trajectory
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator
+
+data class PathConstraints(
+        val maxVel: Double,
+        val maxAcc: Double
+)
+
+class PathFollowerConfig {
+    internal var getPose = { Pose2d(0.0, 0.0, Rotation2d(0.0)) }
+    internal var outputVelocity = { l: Double, r: Double -> Unit }
+
+    var constraints = DriveSettings(1.0)
+        private set
+
+    var trackWidth
+        get() = constraints.trackWidth
+        set(value) {
+            constraints = constraints.copy(trackWidth = value)
+        }
+
+    fun pose(get: () -> Pose2d) {
+        getPose = get
+    }
+
+    fun outputVelocity(set: (left: Double, right: Double) -> Unit) {
+        outputVelocity = set
+    }
+}
+
+data class DriveSettings(
+        val trackWidth: Double
+)
+
+class PathFollower(val settings: DriveSettings, val getPose: () -> Pose2d, val outputVelocity: (left: Double, right: Double) -> Unit) {
+    suspend fun follow(trajectory: Trajectory) {
+        val kinematics = DifferentialDriveKinematics(settings.trackWidth)
+        followWpilibPath(
+                trajectory,
+                getPose,
+                RamseteController(),
+                kinematics,
+                outputVelocity
+        )
+    }
+
+    suspend fun followPath(configure: WaypointPathConfig.() -> Unit) {
+        val config = WaypointPathConfig().apply(configure)
+        val kinematics = DifferentialDriveKinematics(settings.trackWidth)
+        follow(
+                TrajectoryGenerator.generateTrajectory(
+                        config.waypoints,
+                        TrajectoryConfig(
+                                config.maxVel,
+                                config.maxAcc
+                        ).setKinematics(kinematics)
+                )
+        )
+    }
+
+    suspend fun followPath(start: Pose2d, finish: Pose2d, configure: PointPathConfig.() -> Unit) {
+        val config = PointPathConfig().apply(configure)
+        val kinematics = DifferentialDriveKinematics(settings.trackWidth)
+        follow(
+                TrajectoryGenerator.generateTrajectory(
+                        start,
+                        config.points,
+                        finish,
+                        TrajectoryConfig(
+                                config.maxVel,
+                                config.maxAcc
+                        ).setKinematics(kinematics)
+                )
+        )
+    }
+}
+
+fun follower(configure: PathFollowerConfig.() -> Unit) = with(PathFollowerConfig().apply(configure)) {
+    PathFollower(DriveSettings(trackWidth), getPose, outputVelocity)
+}

core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt

@@ -0,0 +1,58 @@
+package org.sert2521.sertain.pathfinding
+
+import edu.wpi.first.wpilibj.geometry.Pose2d
+import edu.wpi.first.wpilibj.geometry.Rotation2d
+import edu.wpi.first.wpilibj.geometry.Translation2d
+
+class WaypointPathConfig {
+    var constraints = PathConstraints(1.0, 1.0)
+        private set
+
+    var maxVel
+        get() = constraints.maxVel
+        set(value) {
+            constraints = constraints.copy(maxVel = value)
+        }
+
+    var maxAcc
+        get() = constraints.maxAcc
+        set(value) {
+            constraints = constraints.copy(maxAcc = value)
+        }
+
+    val waypoints = mutableListOf<Pose2d>()
+
+    fun wp(x: Double, y: Double, ang: Double) {
+        waypoints += Pose2d(x, y, Rotation2d(ang))
+    }
+
+    fun p(x: Double, y: Double) =
+            Translation2d(x, y)
+
+    infix fun Translation2d.ang(angle: Double) {
+        waypoints += Pose2d(this, Rotation2d(angle))
+    }
+}
+
+class PointPathConfig {
+    var constraints = PathConstraints(0.0, 0.0)
+        private set
+
+    var maxVel
+        get() = constraints.maxVel
+        set(value) {
+            constraints = constraints.copy(maxVel = value)
+        }
+
+    var maxAcc
+        get() = constraints.maxAcc
+        set(value) {
+            constraints = constraints.copy(maxAcc = value)
+        }
+
+    internal val points = mutableListOf<Translation2d>()
+
+    fun p(x: Double, y: Double) {
+        points += Translation2d(x, y)
+    }
+}

core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt

@@ -0,0 +1,28 @@
+package org.sert2521.sertain.pathfinding
+
+import edu.wpi.first.wpilibj.controller.RamseteController
+import edu.wpi.first.wpilibj.geometry.Pose2d
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics
+import edu.wpi.first.wpilibj.trajectory.Trajectory
+import org.sert2521.sertain.utils.timer
+
+suspend fun followWpilibPath(
+        trajectory: Trajectory,
+        getPose: () -> Pose2d,
+        follower: RamseteController,
+        kinematics: DifferentialDriveKinematics,
+        outputMetersPerSecond: (left: Double, right: Double) -> Unit
+) {
+    timer(20, 0, (trajectory.totalTimeSeconds * 1000).toLong().also { println("Path time: $it") }) {
+        val curTime = it.toDouble() / 1000
+
+        val targetWheelSpeeds = kinematics.toWheelSpeeds(
+                follower.calculate(getPose(), trajectory.sample(curTime))
+        )
+
+        val leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond
+        val rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond
+
+        outputMetersPerSecond(leftSpeedSetpoint, rightSpeedSetpoint)
+    }
+}

core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt

@@ -6,16 +6,22 @@ object Per : CompositionOperation()
 object By : CompositionOperation()
 
 open class CompositeUnitType<OP : CompositionOperation, T1 : MetricUnitType, T2 : MetricUnitType>(
-    val operation: OP,
-    val type1: T1,
-    val type2: T2
+        val operation: OP,
+        val type1: T1,
+        val type2: T2
 ) : MetricUnitType()
 
 // By operation is not well tested!
-open class CompositeUnit<OP : CompositionOperation, T1 : MetricUnitType, T2 : MetricUnitType>(
-    val operation: OP,
-    val unit1: MetricUnit<T1>,
-    val unit2: MetricUnit<T2>
+open class CompositeUnit<
+        OP : CompositionOperation,
+        T1 : MetricUnitType,
+        T2 : MetricUnitType,
+        U1 : MetricUnit<T1>,
+        U2 : MetricUnit<T2>
+>(
+        val operation: OP,
+        val unit1: U1,
+        val unit2: U2
 ) : MetricUnit<CompositeUnitType<OP, T1, T2>>(
         CompositeUnitType(operation, unit1.type, unit2.type),
         when (operation) {
@@ -38,8 +44,18 @@ open class CompositeUnit<OP : CompositionOperation, T1 : MetricUnitType, T2 : Me
         }
 )
 
-operator fun <T1 : MetricUnitType, T2 : MetricUnitType> MetricUnit<T1>.div(other: MetricUnit<T2>) =
+operator fun <T1 : MetricUnitType, T2 : MetricUnitType, U1 : MetricUnit<T1>, U2 : MetricUnit<T2>> U1.div(other: U2) =
         CompositeUnit(Per, this, other)
 
-operator fun <T1 : MetricUnitType, T2 : MetricUnitType> MetricUnit<T1>.times(other: MetricUnit<T2>) =
+operator fun <T1 : MetricUnitType, T2 : MetricUnitType, U1 : MetricUnit<T1>, U2 : MetricUnit<T2>> U1.times(other: U2) =
         CompositeUnit(By, this, other)
+
+typealias Velocity = CompositeUnitType<Per, Linear, Chronic>
+typealias AngularVelocity = CompositeUnitType<Per, Angular, Chronic>
+typealias Acceleration = CompositeUnitType<Per, Linear, CompositeUnitType<By, Chronic, Chronic>>
+typealias AngularAcceleration = CompositeUnitType<Per, Angular, CompositeUnitType<By, Chronic, Chronic>>
+
+typealias VelocityUnit<U1, U2> = CompositeUnit<Per, Linear, Chronic, U1, U2>
+typealias AngularVelocityUnit<U1, U2> = CompositeUnit<Per, Angular, Chronic, U1, U2>
+typealias AccelerationUnit<U1, U2> = CompositeUnit<Per, Linear, CompositeUnitType<By, Chronic, Chronic>, U1, CompositeUnit<By, Chronic, Chronic, U2, U2>>
+typealias AngularAccelerationUnit<U1, U2> = CompositeUnit<Per, Angular, CompositeUnitType<By, Chronic, Chronic>, U1, CompositeUnit<By, Chronic, Chronic, U2, U2>>

core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt

@@ -1,9 +1,7 @@
 package org.sert2521.sertain.units
 
-fun <T : MetricUnitType, U1 : MetricUnit<T>, U2 : MetricUnit<T>> Double.convert(units: Pair<U1, U2>): Double {
-    return this * units.first.base / units.second.base
-}
+fun <T : MetricUnitType, U1 : MetricUnit<T>, U2 : MetricUnit<T>> Double.convert(units: Pair<U1, U2>) =
+        this * units.first.base / units.second.base
 
-fun <T : MetricUnitType, U1 : MetricUnit<T>, U2 : MetricUnit<T>> MetricValue<T, U1>.convertTo(other: U2): MetricValue<T, U2> {
-    return MetricValue(other, value.convert(unit to other))
-}
+fun <T : MetricUnitType, U1 : MetricUnit<T>, U2 : MetricUnit<T>> MetricValue<T, U1>.convertTo(other: U2) =
+        MetricValue(other, value.convert(unit to other))

core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt

@@ -2,7 +2,7 @@ package org.sert2521.sertain.units
 
 import kotlin.math.PI
 
-abstract class MetricUnit<T : MetricUnitType>(val type: T, val base: Double, val symbol: String)
+open class MetricUnit<T : MetricUnitType>(val type: T, val base: Double, val symbol: String)
 
 abstract class ChronicUnit(seconds: Double, symbol: String) : MetricUnit<Chronic>(Chronic, seconds, symbol)
 
@@ -16,7 +16,7 @@ object Meters : LinearUnit(1.0, " m")
 object Centimeters : LinearUnit(0.01, " cm")
 object Millimeters : LinearUnit(0.001, " mm")
 
-abstract class AngularUnit(radians: Double, symbol: String) : MetricUnit<Angular>(Angular, radians, symbol)
+abstract class AngularUnit(meters: Double, symbol: String) : MetricUnit<Angular>(Angular, meters, symbol)
 
 object Degrees : AngularUnit(PI / 180, "°")
 object Radians : AngularUnit(1.0, " rad")