Add lambda constructors (#49)

* Add lambda constructors * Version bump
team5499 · Mar 9, 2019 · bf9eba0 · bf9eba0
1 parent de5e213
commit bf9eba0
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 31 deletions.
diff --git a/build.gradle b/build.gradle
@@ -35,7 +35,7 @@ dependencies {
 }
 
 group = 'org.team5499'
-version = '2.8.0'
+version = '2.9.0'
 
 task sourcesJar(type: Jar) {
     from sourceSets.main.allJava

diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/SpaceDriveHelper.kt b/src/main/kotlin/org/team5499/monkeyLib/input/SpaceDriveHelper.kt
@@ -1,28 +1,30 @@
 package org.team5499.monkeyLib.input
 
-public class SpaceDriveHelper(deadband: Double, turnMult: Double, slowMult: Double) : DriveHelper() {
+public class SpaceDriveHelper(deadband: () -> Double, turnMult: () -> Double, slowMult: () -> Double) : DriveHelper() {
 
-    private val mDeadband: Double
-    private val mTurnMult: Double
-    private val mSlowMult: Double
+    private var mDeadband: () -> Double
+    private var mTurnMult: () -> Double
+    private var mSlowMult: () -> Double
 
     init {
-        mDeadband = deadband
-        mTurnMult = turnMult
-        mSlowMult = slowMult
+        mDeadband = { deadband() }
+        mTurnMult = { turnMult() }
+        mSlowMult = { slowMult() }
     }
 
+    constructor(deadband: Double, turnMult: Double, slowMult: Double) : this({ deadband }, { turnMult }, { slowMult })
+
     public override fun calculateOutput(
         throttle: Double,
         wheel: Double,
         isQuickTurn: Boolean,
         creep: Boolean
     ): DriveSignal {
-        val newThottle = super.handleDeadband(throttle, mDeadband)
-        var newWheel = super.handleDeadband(wheel, mDeadband)
-        val mult = if (!isQuickTurn) mTurnMult else 1.0
+        val newThottle = super.handleDeadband(throttle, mDeadband())
+        var newWheel = super.handleDeadband(wheel, mDeadband())
+        val mult = if (!isQuickTurn) mTurnMult() else 1.0
         newWheel *= mult
-        val slow = if (creep) mSlowMult else 1.0
+        val slow = if (creep) mSlowMult() else 1.0
         return DriveSignal(slow * (newThottle + newWheel), slow * (newThottle - newWheel))
     }
 }
diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/PathFollower.kt b/src/main/kotlin/org/team5499/monkeyLib/path/PathFollower.kt
@@ -11,20 +11,20 @@ import org.team5499.monkeyLib.math.geometry.Pose2d
 * @property path the path the follower will follow
 */
 @SuppressWarnings("MagicNumber")
-class PathFollower(path: Path, trackWidth: Double, lookaheadDistance: Double) {
+class PathFollower(path: Path, trackWidth: Double, initLookaheadDistance: Double) {
 
     private val mPath: Path
     private var mLastClosestPointIndex: Int
 
     private val mTrackWidth: Double
-    private val mLookaheadDistance: Double
+    public var lookaheadDistance: Double
 
     init {
         mPath = path
         mLastClosestPointIndex = 0
 
         mTrackWidth = trackWidth
-        mLookaheadDistance = lookaheadDistance
+        lookaheadDistance = initLookaheadDistance
     }
 
     /**
@@ -65,7 +65,7 @@ class PathFollower(path: Path, trackWidth: Double, lookaheadDistance: Double) {
 
             val a = d.dot(d)
             val b = 2.0 * f.dot(d)
-            val c = f.dot(f) - Math.pow(mLookaheadDistance, 2.0)
+            val c = f.dot(f) - Math.pow(lookaheadDistance, 2.0)
             var dis = (b * b) - (4.0 * a * c)
             if (dis < 0.0) {
                 continue
@@ -88,7 +88,7 @@ class PathFollower(path: Path, trackWidth: Double, lookaheadDistance: Double) {
             lookahead = mPath.endPose.translation
         } else {
             val distanceToEnd = robotPose.translation.distanceTo(mPath.endPose.translation)
-            if (distanceToEnd < mLookaheadDistance) {
+            if (distanceToEnd < lookaheadDistance) {
                 lookahead = mPath.endPose.translation
             }
         }
@@ -107,7 +107,7 @@ class PathFollower(path: Path, trackWidth: Double, lookaheadDistance: Double) {
         val b = -1
         val c = -(1 / Math.tan(robotAngle)) * robotPose.translation.y + robotPose.translation.x
         val x = Math.abs(a * lookahead.y + b * lookahead.x + c) / ((Math.sqrt(a * a + b * b)))
-        val curvature = (2.0 * x) / (Math.pow(mLookaheadDistance, 2.0))
+        val curvature = (2.0 * x) / (Math.pow(lookaheadDistance, 2.0))
         val side = Math.signum(
             Math.sin(robotAngle) * (lookahead.x - robotPose.translation.x) -
             Math.cos(robotAngle) * (lookahead.y - robotPose.translation.y)

diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt b/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt
@@ -8,21 +8,33 @@ import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature
 
 class PathGenerator {
 
-    private val mMaxVelocity: Double // i / s
-    private val mMaxAcceleration: Double // in / s / s
-    private val mStartPathVelocity: Double // i / s
-    private val mEndPathVelocity: Double // i / s
+    private val mMaxVelocity: () -> Double // i / s
+    private val mMaxAcceleration: () -> Double // in / s / s
+    private val mStartPathVelocity: () -> Double // i / s
+    private val mEndPathVelocity: () -> Double // i / s
 
     public constructor(
         defaultMaxVelocity: Double,
         defaultMaxAcceleration: Double,
         defaultStartVelocity: Double,
         defaultEndVelocity: Double
     ) {
-        mMaxVelocity = defaultMaxVelocity
-        mMaxAcceleration = defaultMaxAcceleration
-        mStartPathVelocity = defaultStartVelocity
-        mEndPathVelocity = defaultEndVelocity
+        mMaxVelocity = { defaultMaxVelocity }
+        mMaxAcceleration = { defaultMaxAcceleration }
+        mStartPathVelocity = { defaultStartVelocity }
+        mEndPathVelocity = { defaultEndVelocity }
+    }
+
+    public constructor(
+        defaultMaxVelocity: () -> Double,
+        defaultMaxAcceleration: () -> Double,
+        defaultStartVelocity: () -> Double,
+        defaultEndVelocity: () -> Double
+    ) {
+        mMaxVelocity = { defaultMaxVelocity() }
+        mMaxAcceleration = { defaultMaxAcceleration() }
+        mStartPathVelocity = { defaultStartVelocity() }
+        mEndPathVelocity = { defaultEndVelocity() }
     }
 
     @Suppress("LongParameterList", "ComplexMethod")
@@ -99,16 +111,16 @@ class PathGenerator {
     }
 
     public fun generatePath(reversed: Boolean, waypoints: Array<Pose2d>, maxVelo: Double, maxAccel: Double): Path {
-        return generatePath(reversed, waypoints, maxVelo, maxAccel, mStartPathVelocity, mEndPathVelocity)
+        return generatePath(reversed, waypoints, maxVelo, maxAccel, mStartPathVelocity(), mEndPathVelocity())
     }
 
     public fun generatePath(reversed: Boolean, waypoints: Array<Pose2d>): Path {
         return generatePath(
             reversed, waypoints,
-            mMaxVelocity,
-            mMaxAcceleration,
-            mStartPathVelocity,
-            mEndPathVelocity
+            mMaxVelocity(),
+            mMaxAcceleration(),
+            mStartPathVelocity(),
+            mEndPathVelocity()
         )
     }