From bf9eba015a4bf4b2f42118d1fb86a394638e7cd4 Mon Sep 17 00:00:00 2001 From: Andy Cate Date: Fri, 8 Mar 2019 19:04:27 -0800 Subject: [PATCH] Add lambda constructors (#49) * Add lambda constructors * Version bump --- build.gradle | 2 +- .../monkeyLib/input/SpaceDriveHelper.kt | 24 ++++++------ .../team5499/monkeyLib/path/PathFollower.kt | 12 +++--- .../team5499/monkeyLib/path/PathGenerator.kt | 38 ++++++++++++------- 4 files changed, 45 insertions(+), 31 deletions(-) diff --git a/build.gradle b/build.gradle index 5b9a1ec..56b44b7 100644 --- 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 index 25773da..9b6e628 100644 --- 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 index e2f5db3..9d08131 100644 --- 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 index efdfdd8..cbf7502 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt @@ -8,10 +8,10 @@ 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, @@ -19,10 +19,22 @@ class PathGenerator { 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, 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): Path { return generatePath( reversed, waypoints, - mMaxVelocity, - mMaxAcceleration, - mStartPathVelocity, - mEndPathVelocity + mMaxVelocity(), + mMaxAcceleration(), + mStartPathVelocity(), + mEndPathVelocity() ) }