diff --git a/.gitignore b/.gitignore index c0a0119..e771d84 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,13 @@ +*.iml +*.ipr +*.iws .classpath .gradle/ +.idea/ .project .settings/ .vscode/ bin/ build/ +out/ virtualenv_run/ diff --git a/.idea/gradle.xml b/.idea/gradle.xml new file mode 100644 index 0000000..9707df0 --- /dev/null +++ b/.idea/gradle.xml @@ -0,0 +1,19 @@ + + + + + + + diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..11c5dcb --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..dcb6b8c --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000..a558a08 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/README.md b/README.md index 755878e..f1bcbec 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,4 @@ -# MonkeyLib - -[![Build Status](https://travis-ci.org/team5499/MonkeyLib.svg?branch=master)](https://travis-ci.org/team5499/MonkeyLib) -[![Code Coverage](https://codecov.io/gh/team5499/MonkeyLib/branch/master/graph/badge.svg)](https://codecov.io/gh/team5499/MonkeyLib) - -MonkeyLib is team 5499's base kotlin code used in all of our robots. Mostly contains math and utilities +# Fault + +My own branch of Fault + diff --git a/build.gradle b/build.gradle index cc267dd..fda9532 100644 --- a/build.gradle +++ b/build.gradle @@ -8,6 +8,7 @@ plugins { id 'jacoco' id 'maven' id 'org.jetbrains.dokka' version '0.9.17' + id 'idea' } repositories { @@ -32,10 +33,12 @@ dependencies { testCompile 'org.powermock:powermock-module-junit4:1.6.6' testCompile 'org.powermock:powermock-api-mockito2:1.6.6' + testCompile group: 'org.knowm.xchart', name: 'xchart', version: '3.5.4' + } -group = 'org.team5499' -version = '2.11.0' +group = 'org.team5419' +version = '3.0.0-SNAPSHOT' task sourcesJar(type: Jar) { @@ -106,14 +109,14 @@ task install_hooks(dependsOn: 'tox') publishing { publications { library(MavenPublication) { - artifactId = 'monkey-lib' + artifactId = 'fault' from components.java artifact sourcesJar artifact javadocJar pom { - name = 'MonkeyLib' - description = 'Commonly used utilities in FRC Team 5499\'s codebase' - url = 'https://github.com/team5499/MonkeyLib' + name = 'Fault' + description = 'Commonly used utilities in FRC Team 5419\'s codebase' + url = 'https://github.com/team5419/fault' } } } diff --git a/src/main/kotlin/org/team5419/fault/BerkeliumRobot.kt b/src/main/kotlin/org/team5419/fault/BerkeliumRobot.kt new file mode 100644 index 0000000..7b92167 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/BerkeliumRobot.kt @@ -0,0 +1,90 @@ +package org.team5419.fault + +import edu.wpi.first.hal.FRCNetComm +import edu.wpi.first.hal.HAL +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj.TimedRobot +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.subsystems.Subsystem +import org.team5419.fault.subsystems.SubsystemManager + +@Suppress("TooManyFunctions") +abstract class BerkeliumRobot(period: SIUnit) { + + protected val wrappedValue = WpiTimedRobot(period.value) + + protected inner class WpiTimedRobot(period: Double = 0.05) : TimedRobot(period) { + + private val kLanguageKotlin = 6 + + init { + HAL.report(FRCNetComm.tResourceType.kResourceType_Language, kLanguageKotlin) + } + + override fun robotInit() { + this@BerkeliumRobot.robotInit() + SubsystemManager.init() + } + + override fun autonomousInit() { + this@BerkeliumRobot.autonomousInit() + SubsystemManager.autoReset() + } + + override fun teleopInit() { + this@BerkeliumRobot.teleopInit() + SubsystemManager.teleopReset() + } + + override fun disabledInit() { + this@BerkeliumRobot.disabledInit() + SubsystemManager.zeroOutputs() + } + + override fun testInit() { + this@BerkeliumRobot.testInit() + } + + override fun robotPeriodic() { + this@BerkeliumRobot.robotPeriodic() + if (!isDisabled) SubsystemManager.periodic() + } + + override fun autonomousPeriodic() { + this@BerkeliumRobot.autonomousPeriodic() + } + + override fun teleopPeriodic() { + this@BerkeliumRobot.teleopPeriodic() + } + + override fun disabledPeriodic() { + this@BerkeliumRobot.disabledPeriodic() + } + + override fun testPeriodic() { + this@BerkeliumRobot.testPeriodic() + } + } + + protected open fun robotInit() {} + protected open fun autonomousInit() {} + protected open fun teleopInit() {} + protected open fun disabledInit() {} + protected open fun testInit() {} + + protected open fun robotPeriodic() {} + protected open fun autonomousPeriodic() {} + protected open fun teleopPeriodic() {} + protected open fun disabledPeriodic() {} + protected open fun testPeriodic() {} + + open operator fun Subsystem.unaryPlus() { + SubsystemManager.addSubsystem(this) + } + + fun start() { + RobotBase.startRobot { wrappedValue } + } +} diff --git a/src/main/kotlin/org/team5419/fault/Controller.kt b/src/main/kotlin/org/team5419/fault/Controller.kt new file mode 100644 index 0000000..39dbf01 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/Controller.kt @@ -0,0 +1,10 @@ +package org.team5419.fault + +interface Controller { + + fun start() + + fun update() + + fun reset() +} diff --git a/src/main/kotlin/org/team5419/fault/auto/Action.kt b/src/main/kotlin/org/team5419/fault/auto/Action.kt new file mode 100644 index 0000000..e00e887 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/Action.kt @@ -0,0 +1,91 @@ +package org.team5419.fault.auto + +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.SIUnitBuilder +import org.team5419.fault.math.units.Second +import org.team5419.fault.util.BooleanSource +import org.team5419.fault.util.Source +import org.team5419.fault.util.or +import org.team5419.fault.util.time.ITimer +import org.team5419.fault.util.time.WPITimer + +typealias NothingAction = Action + +open class Action( + private val timer: ITimer = WPITimer() +) { + + protected var mTimeout = SIUnitBuilder(0.0).seconds + + protected var finishCondition = FinishCondition(Source(false)) + + open fun start() { + timer.stop() + timer.reset() + timer.start() + } + + open fun update() {} + + protected fun timedOut() = (timer.get() >= mTimeout) + + open fun next(): Boolean { + return finishCondition() + } + + open fun finish() {} + + protected class FinishCondition( + private var source: BooleanSource + ) : BooleanSource { + override fun invoke() = source() + + operator fun plusAssign(other: BooleanSource) { + source = source or other + } + + fun set(other: BooleanSource) { + source = other + } + } + + fun withExit(condition: BooleanSource) = also { finishCondition += condition } + + fun overrideExit(condition: BooleanSource) = also { finishCondition.set(condition) } + + fun withTimeout(time: SIUnit) = also { + this.mTimeout = time + finishCondition += { timedOut() } + } +} + +open class ConditionalAction( + private val condition: BooleanSource, + private val ifTrue: Action? = null, + private val ifFalse: Action? = null +) : Action() { + + private var selectedAction: Action? = null + private val selectedActionDone = { if (selectedAction != null) selectedAction!!.next() else true } + + init { + finishCondition += { timedOut() } + finishCondition += selectedActionDone + } + + override fun start() { + super.start() + selectedAction = if (condition()) ifTrue else ifFalse + if (selectedAction != null) selectedAction!!.start() + } + + override fun update() { + super.update() + if (selectedAction != null) selectedAction!!.update() + } + + override fun finish() { + if (selectedAction != null) selectedAction!!.finish() + selectedAction = null + } +} diff --git a/src/main/kotlin/org/team5419/fault/auto/ActionGroup.kt b/src/main/kotlin/org/team5419/fault/auto/ActionGroup.kt new file mode 100644 index 0000000..1e4a311 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/ActionGroup.kt @@ -0,0 +1,94 @@ +package org.team5419.fault.auto + +abstract class ActionGroup internal constructor() : Action() { + + internal abstract val actions: List +} + +open class ParallelAction(actions: List) : ActionGroup() { + + override val actions: List = actions + + protected val actionMap = mutableMapOf() + + constructor(vararg actions: Action) : this(actions.toList()) + + init { + finishCondition += { + var allDone = true + actions.forEach { allDone = allDone && it.next() } + allDone + } + + actions.forEach { actionMap.put(it, false) } + } + + override fun start() { + super.start() + actionMap.keys.forEach { + actionMap.set(it, false) + it.start() + } + } + + override fun update() { + val iterator = actionMap.iterator() + while (iterator.hasNext()) { + val (action, finished) = iterator.next() + if (finished) continue + if (!finished && action.next()) { + action.finish() + actionMap.set(action, true) + } else { + action.update() + } + } + } + + override fun finish() { + actionMap.forEach { + if (!it.value) it.key.finish() + } + } +} + +open class SerialAction(actions: List) : ActionGroup() { + + constructor(vararg actions: Action) : this(actions.toList()) + + private var index = 0 + private val isLastAction = { index == (actions.size - 1) } + private val isLastActionDone = { actions.last().next() } + + override val actions: List + + init { + finishCondition += { isLastAction() } + finishCondition += { isLastActionDone() } + this.actions = actions.toMutableList() + assert(this.actions.size < 1) { "No actions added to command group" } + index = 0 + } + + override fun start() { + super.start() + index = 0 + actions[index].start() + } + + override fun update() { + super.update() + if (isLastAction() && isLastActionDone()) return + else if (actions[index].next() && !isLastAction()) { + actions[index].finish() + index++ + actions[index].start() + } + actions[index].update() + } + + override fun finish() { + super.finish() + actions.last().finish() + } +} diff --git a/src/main/kotlin/org/team5419/fault/auto/ActionUtil.kt b/src/main/kotlin/org/team5419/fault/auto/ActionUtil.kt new file mode 100644 index 0000000..e95aff9 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/ActionUtil.kt @@ -0,0 +1,26 @@ +package org.team5419.fault.auto + +enum class GroupType { Parallel, Serial } + +interface IActionGroupBuilder { + + fun build(): ActionGroup +} + +class ActionGroupBuilder(private val type: GroupType) : IActionGroupBuilder { + private val actions = mutableListOf() + + operator fun Action.unaryPlus() = actions.add(this) + + override fun build() = when (type) { + GroupType.Serial -> SerialAction(actions) + GroupType.Parallel -> ParallelAction(actions) + } +} + +fun serial(block: ActionGroupBuilder.() -> Unit) = commandGroup(GroupType.Serial, block) + +fun parallel(block: ActionGroupBuilder.() -> Unit) = commandGroup(GroupType.Parallel, block) + +private fun commandGroup(type: GroupType, block: ActionGroupBuilder.() -> Unit) = + ActionGroupBuilder(type).apply(block).build() diff --git a/src/main/kotlin/org/team5419/fault/auto/DriveStraightAction.kt b/src/main/kotlin/org/team5419/fault/auto/DriveStraightAction.kt new file mode 100644 index 0000000..c794369 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/DriveStraightAction.kt @@ -0,0 +1,39 @@ +package org.team5419.fault.auto + +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.LinearVelocity +import org.team5419.fault.math.units.derived.velocity +import org.team5419.fault.math.units.inches +import org.team5419.fault.subsystems.drivetrain.AbstractTankDrive +import kotlin.math.absoluteValue + +class DriveStraightAction( + private val drivetrain: AbstractTankDrive, + private val distance: SIUnit, + private val acceptableDistanceError: SIUnit = 2.inches, // inches + private val acceptableVelocityError: SIUnit = 2.inches.velocity // inches / s +) : Action() { + + private val distanceErrorAcceptable = { + drivetrain.leftDistanceError.absoluteValue < acceptableDistanceError && + drivetrain.rightDistanceError.absoluteValue < acceptableDistanceError + } + + private val velocityErrorAcceptable = { + drivetrain.leftMasterMotor.encoder.velocity.absoluteValue < acceptableVelocityError.value && + drivetrain.rightMasterMotor.encoder.velocity.absoluteValue < acceptableVelocityError.value + } + + init { + finishCondition += distanceErrorAcceptable + finishCondition += velocityErrorAcceptable + } + + override fun start() { + super.start() + drivetrain.setPosition(distance) + } + + override fun finish() = drivetrain.zeroOutputs() +} diff --git a/src/main/kotlin/org/team5419/fault/auto/DriveTrajectoryAction.kt b/src/main/kotlin/org/team5419/fault/auto/DriveTrajectoryAction.kt new file mode 100644 index 0000000..1b43870 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/DriveTrajectoryAction.kt @@ -0,0 +1,36 @@ +package org.team5419.fault.auto + +import org.team5419.fault.trajectory.types.Trajectory +import org.team5419.fault.trajectory.types.TimedEntry +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.subsystems.drivetrain.AbstractTankDrive +import org.team5419.fault.trajectory.followers.TrajectoryFollower + +public class DriveTrajectoryAction( + private val drivetrain: AbstractTankDrive, + private val trajectoryFollower: TrajectoryFollower, + private val trajectory: Trajectory, TimedEntry> +) : Action() { + + init { + finishCondition += { trajectoryFollower.isFinished } + } + + override fun start() { + trajectoryFollower.reset(trajectory) + } + + override fun update() { + drivetrain.setOutput(trajectoryFollower.nextState(drivetrain.robotPosition)) +// val referencePoint = trajectoryFollower.referencePoint +// if (referencePoint != null) { +// val referencePose = referencePoint.state.state.pose +// } + } + + override fun finish() { + drivetrain.zeroOutputs() + } +} diff --git a/src/main/kotlin/org/team5419/fault/auto/DriveTurnAction.kt b/src/main/kotlin/org/team5419/fault/auto/DriveTurnAction.kt new file mode 100644 index 0000000..9749c78 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/DriveTurnAction.kt @@ -0,0 +1,36 @@ +package org.team5419.fault.auto + +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.derived.velocity +import org.team5419.fault.math.units.derived.AngularVelocity + +import org.team5419.fault.subsystems.drivetrain.AbstractTankDrive + +// ALLOW for absolute turn in future (using rotation2d math maybe) +class DriveTurnAction( + private val drivetrain: AbstractTankDrive, + private val rotation: Rotation2d, + private val turnType: AbstractTankDrive.TurnType = AbstractTankDrive.TurnType.Relative, + private val acceptableTurnError: SIUnit = 3.degrees, // degrees + private val acceptableVelocityError: SIUnit = 5.0.degrees.velocity // inches / s +) : Action() { + + private val turnErrorAcceptable = { drivetrain.turnError < acceptableTurnError } + private val angularVelocityAcceptable = { drivetrain.angularVelocity < acceptableVelocityError } + + override fun start() { + super.start() + drivetrain.setTurn(rotation, turnType) + + finishCondition += { turnErrorAcceptable() } + finishCondition += { angularVelocityAcceptable() } + } + + override fun finish() { + super.finish() + drivetrain.zeroOutputs() + } +} diff --git a/src/main/kotlin/org/team5419/fault/auto/Routine.kt b/src/main/kotlin/org/team5419/fault/auto/Routine.kt new file mode 100644 index 0000000..c96d8e3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/auto/Routine.kt @@ -0,0 +1,32 @@ +package org.team5419.fault.auto + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Vector2 +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.meters +import java.util.concurrent.atomic.AtomicLong + +class Routine( + name: String? = null, + val startPose: Pose2d = Pose2d(Vector2(0.0.meters, 0.0.meters), 0.degrees), + override val actions: MutableList +) : SerialAction(actions) { + + constructor( + name: String? = null, + startPose: Pose2d = Pose2d(Vector2(0.0.meters, 0.0.meters), 0.degrees), + vararg actions: Action + ) : this(name, startPose, actions.toMutableList()) + + val name = name ?: "Routine ${routineId.incrementAndGet()}" + + companion object { + private val routineId = AtomicLong() + } +} + +fun T.toRoutine(name: String? = null, startPose: Pose2d = Pose2d()) = Routine( + name, + startPose, + this.actions.toMutableList() +) diff --git a/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumEncoder.kt b/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumEncoder.kt new file mode 100644 index 0000000..474501a --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumEncoder.kt @@ -0,0 +1,13 @@ +package org.team5419.fault.hardware + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.native.NativeUnitModel + +abstract class AbstractBerkeliumEncoder( + val model: NativeUnitModel +) : BerkeliumEncoder { + override val position: SIUnit get() = model.fromNativeUnitPosition(rawPosition) + override val velocity: SIUnit> get() = model.fromNativeUnitVelocity(rawVelocity) +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumMotor.kt b/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumMotor.kt new file mode 100644 index 0000000..b393266 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/AbstractBerkeliumMotor.kt @@ -0,0 +1,13 @@ +package org.team5419.fault.hardware + +import org.team5419.fault.math.units.SIKey + +abstract class AbstractBerkeliumMotor : BerkeliumMotor { + + override var useMotionProfileForPosition = false + + override fun follow(motor: BerkeliumMotor<*>): Boolean { + println("Cross brand following not implemented yet!") + return false + } +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/BerkeliumEncoder.kt b/src/main/kotlin/org/team5419/fault/hardware/BerkeliumEncoder.kt new file mode 100644 index 0000000..9351b15 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/BerkeliumEncoder.kt @@ -0,0 +1,32 @@ +package org.team5419.fault.hardware + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.native.NativeUnit +import org.team5419.fault.math.units.native.NativeUnitVelocity + +interface BerkeliumEncoder { + + /** + * The velocity of the encoder in [T]/s + */ + val velocity: SIUnit> + /** + * The position of the encoder in [T] + */ + val position: SIUnit + + /** + * The velocity of the encoder in NativeUnits/s + */ + val rawVelocity: SIUnit + /** + * The position of the encoder in NativeUnits + */ + val rawPosition: SIUnit + + fun resetPosition(newPosition: SIUnit = SIUnit(0.0)) + + fun resetPositionRaw(newPosition: SIUnit = SIUnit(0.0)) +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/BerkeliumMotor.kt b/src/main/kotlin/org/team5419/fault/hardware/BerkeliumMotor.kt new file mode 100644 index 0000000..38e26dd --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/BerkeliumMotor.kt @@ -0,0 +1,81 @@ +package org.team5419.fault.hardware + +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Acceleration +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.derived.Volt + +typealias LinearBerkeliumMotor = BerkeliumMotor +typealias AngularBerkeliumMotor = BerkeliumMotor + +interface BerkeliumMotor { + + /** + * The encoder attached to the motor + */ + val encoder: BerkeliumEncoder + /** + * The voltage output of the motor controller in volts + */ + val voltageOutput: SIUnit + + /** + * Inverts the output given to the motor + */ + var outputInverted: Boolean + + /** + * When enabled, motor leads are commonized electrically to reduce motion + */ + var brakeMode: Boolean + + /** + * Configures the max voltage output given to the motor + */ + var voltageCompSaturation: SIUnit + + /** + * Peak target velocity that the on board motion profile generator will use + * Unit is [T]/s + */ + var motionProfileCruiseVelocity: SIUnit> + /** + * Acceleration that the on board motion profile generator will + * Unit is [T]/s/s + */ + var motionProfileAcceleration: SIUnit> + /** + * Enables the use of on board motion profiling for position mode + */ + var useMotionProfileForPosition: Boolean + + fun follow(motor: BerkeliumMotor<*>): Boolean + + /** + * Sets the output [voltage] in volts and [arbitraryFeedForward] in volts + */ + fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit = SIUnit(0.0)) + + /** + * Sets the output [percent] in percent and [arbitraryFeedForward] in volts + */ + fun setPercent(percent: Double, arbitraryFeedForward: SIUnit = SIUnit(0.0)) + + /** + * Sets the output [velocity] in [T]/s and [arbitraryFeedForward] in volts + */ + fun setVelocity(velocity: SIUnit>, arbitraryFeedForward: SIUnit = SIUnit(0.0)) + + /** + * Sets the output [position] in [T] and [arbitraryFeedForward] in volts + */ + fun setPosition(position: SIUnit, arbitraryFeedForward: SIUnit = SIUnit(0.0)) + + /** + * Sets the output of the motor to neutral + */ + fun setNeutral() +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/Limelight.kt b/src/main/kotlin/org/team5419/fault/hardware/Limelight.kt new file mode 100644 index 0000000..5ba3793 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/Limelight.kt @@ -0,0 +1,149 @@ +package org.team5419.fault.hardware + +import edu.wpi.first.networktables.NetworkTableInstance +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.units.derived.tan +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.SIUnit + +import kotlin.math.PI +import kotlin.math.asin +import kotlin.math.tan + +open class Limelight( + networkTableName: String = "limelight", + val inverted: Boolean = false, + private val mTargetHeight: SIUnit = 0.inches, + private val mCameraHeight: SIUnit = 0.inches, + private val mCameraAngle: Rotation2d = Rotation2d() // angle below (or above) horizontal +) { + + // FEEDBACK VARIABLES + val targetFound: Boolean + get() = mLimelight.getEntry("tv").getBoolean(false) + + val horizontalOffset: Double + get() { + if (inverted) { + return -mLimelight.getEntry("tx").getDouble(0.0) + } else { + return mLimelight.getEntry("tx").getDouble(0.0) + } + } + + val verticalOffset: Double + get() { + if (inverted) { + return -mLimelight.getEntry("ty").getDouble(0.0) + } else { + return mLimelight.getEntry("ty").getDouble(0.0) + } + } + + val targetArea: Double + get() = mLimelight.getEntry("ta").getDouble(0.0) + + val targetSkew: Double + get() { + val skew = mLimelight.getEntry("ts").getDouble(0.0) + if (skew < -45.0) { /*>*/ + return skew + 90.0 + } else { + return skew + } + } + + val horizontalLength: Double + get() = mLimelight.getEntry("tlong").getDouble(0.0) + + val verticalLength: Double + get() = mLimelight.getEntry("tvert").getDouble(0.0) + + val latency: Double + get() = mLimelight.getEntry("tl").getDouble(0.0) + + // CALCULATED VARIABLES + + val horizontalDistance: SIUnit + get() = (mTargetHeight - mCameraHeight) / (mCameraAngle.radian + horizontalOffset.degrees).tan + + val calculateTargetSkew: Double + get() { + var temp = (horizontalLength / verticalLength) * kAspectRatio + if (temp < 0.0) temp = 0.0 /*>*/ + if (temp > 1.0) temp = 1.0 + return (180.0 / PI) * asin(temp) + } + + // LIGHT MODE + var lightMode: LightMode = LightMode.Off + set(value) { + if (value == field) return + when (value) { + LightMode.On -> mLimelight.getEntry("ledMode").setNumber(3) + LightMode.Off -> mLimelight.getEntry("ledMode").setNumber(1) + LightMode.Blink -> mLimelight.getEntry("ledMode").setNumber(2) + } + field = value + } + + // CAMERA MODE + var cameraMode: CameraMode = CameraMode.Vision + set(value) { + if (field == value) return + when (value) { + CameraMode.Driver -> mLimelight.getEntry("camMode").setNumber(1) + CameraMode.Vision -> mLimelight.getEntry("camMode").setNumber(0) + } + field = value + } + + // STREAM Mode + var streamMode: StreamMode = StreamMode.Standard + set(value) { + if (field == value) return + when (value) { + StreamMode.Standard -> mLimelight.getEntry("stream").setNumber(0) + StreamMode.PipMain -> mLimelight.getEntry("stream").setNumber(1) + StreamMode.PipSecondary -> mLimelight.getEntry("stream").setNumber(2) + } + field = value + } + + // SNAPSHOT MODE + var snapshotMode: SnapshotMode = SnapshotMode.Stop + set(value) { + if (field == value) return + when (value) { + SnapshotMode.Stop -> mLimelight.getEntry("snapshot").setNumber(0) + SnapshotMode.Start -> mLimelight.getEntry("snapshot").setNumber(1) + } + field = value + } + + // PIPELINE + fun setPipeline(pipeline: Pipeline) = setPipeline(pipeline.id) + fun setPipeline(id: Int) { + if (id < 0 || id > 9) { + println("Pipeline id needs to be from 0 to 9, ignoring value: $id") + return + } else { + mLimelight.getEntry("pipeline").setNumber(id) + } + } + + private val mLimelight = NetworkTableInstance.getDefault().getTable(networkTableName) + + enum class LightMode { On, Off, Blink } + enum class CameraMode { Vision, Driver } + enum class StreamMode { Standard, PipMain, PipSecondary } + enum class SnapshotMode { Stop, Start } + + data class Pipeline(val id: Int) + + companion object { + const val kAspectRatio = 6.0 / 15.0 + } +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumIMU.kt b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumIMU.kt new file mode 100644 index 0000000..61b003b --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumIMU.kt @@ -0,0 +1,8 @@ +package org.team5419.fault.hardware.ctre + +import com.ctre.phoenix.sensors.PigeonIMU +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.util.Source + +fun PigeonIMU.asSource(): Source = { Rotation2d(fusedHeading.degrees) } diff --git a/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSPX.kt b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSPX.kt new file mode 100644 index 0000000..e3a1349 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSPX.kt @@ -0,0 +1,22 @@ +package org.team5419.fault.hardware.ctre + +import com.ctre.phoenix.motorcontrol.can.VictorSPX +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.native.NativeUnitModel + +typealias LinearBerkeleiumSPX = BerkeliumSPX +typealias AngularBerkeleiumSPX = BerkeliumSPX + +class BerkeliumSPX( + val victorSPX: VictorSPX, + model: NativeUnitModel +) : CTREBerkeliumMotor(victorSPX, model) { + + init { + victorSPX.configFactoryDefault() + } + + constructor(id: Int, model: NativeUnitModel): this(VictorSPX(id), model) +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSRX.kt b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSRX.kt new file mode 100644 index 0000000..b464a6c --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/ctre/BerkeliumSRX.kt @@ -0,0 +1,42 @@ +package org.team5419.fault.hardware.ctre + +import com.ctre.phoenix.motorcontrol.can.TalonSRX +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.inMilliseconds +import org.team5419.fault.math.units.Ampere +import org.team5419.fault.math.units.inAmps +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.native.NativeUnitModel + +typealias LinearBerkeleiumSRX = BerkeliumSRX +typealias AngularBerkeleiumSRX = BerkeliumSRX + +class BerkeliumSRX( + val talonSRX: TalonSRX, + model: NativeUnitModel +) : CTREBerkeliumMotor(talonSRX, model) { + + constructor(id: Int, model: NativeUnitModel): this(TalonSRX(id), model) + + init { + talonSRX.configFactoryDefault(0) + } + + fun configCurrentLimit(enabled: Boolean, config: CurrentLimitConfig? = null) { + talonSRX.enableCurrentLimit(enabled) + if (enabled && config != null) { + talonSRX.configContinuousCurrentLimit(config.continuousCurrentLimit.inAmps().toInt()) + talonSRX.configPeakCurrentLimit(config.peakCurrentLimit.inAmps().toInt()) + talonSRX.configPeakCurrentDuration(config.peakCurrentLimitDuration.inMilliseconds().toInt()) + } + } + + data class CurrentLimitConfig( + val peakCurrentLimit: SIUnit, + val peakCurrentLimitDuration: SIUnit, + val continuousCurrentLimit: SIUnit + ) +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumEncoder.kt b/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumEncoder.kt new file mode 100644 index 0000000..6566b64 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumEncoder.kt @@ -0,0 +1,38 @@ +package org.team5419.fault.hardware.ctre + +import com.ctre.phoenix.motorcontrol.IMotorController +import org.team5419.fault.hardware.AbstractBerkeliumEncoder +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.native.NativeUnit +import org.team5419.fault.math.units.native.NativeUnitModel +import org.team5419.fault.math.units.native.nativeUnits +import org.team5419.fault.math.units.native.nativeUnitsPer100ms +import kotlin.properties.Delegates + +class CTREBerkeliumEncoder( + val motorController: IMotorController, + val pidIdx: Int = 0, + model: NativeUnitModel +) : AbstractBerkeliumEncoder(model) { + override val rawPosition get() = + motorController.getSelectedSensorPosition(pidIdx).toDouble().nativeUnits + override val rawVelocity get() = + motorController.getSelectedSensorVelocity(pidIdx).toDouble().nativeUnitsPer100ms // check this + + var encoderPhase by Delegates.observable(false) { + _, _, newValue -> motorController.setSensorPhase(newValue) + } + + /** + * sets position of encoder [newPosition] where [newPosition] is defined by the units of the[model] + */ + override fun resetPosition(newPosition: SIUnit) { + // TODO(make sure this works) + motorController.setSelectedSensorPosition(model.toNativeUnitPosition(newPosition).value.toInt(), pidIdx, 0) + } + + override fun resetPositionRaw(newPosition: SIUnit) { + motorController.setSelectedSensorPosition(newPosition.value.toInt(), pidIdx, 0) + } +} diff --git a/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumMotor.kt b/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumMotor.kt new file mode 100644 index 0000000..1392843 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/hardware/ctre/CTREBerkeliumMotor.kt @@ -0,0 +1,134 @@ +package org.team5419.fault.hardware.ctre + +import com.ctre.phoenix.motorcontrol.IMotorController +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.DemandType +import com.ctre.phoenix.motorcontrol.NeutralMode + +import org.team5419.fault.hardware.AbstractBerkeliumMotor +import org.team5419.fault.hardware.BerkeliumMotor +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Acceleration +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.derived.Volt +import org.team5419.fault.math.units.derived.volts +import org.team5419.fault.math.units.native.NativeUnitModel +import org.team5419.fault.math.units.native.inNativeUnitsPer100msPerSecond +import org.team5419.fault.math.units.native.nativeUnitsPer100ms +import org.team5419.fault.math.units.native.toNativeUnitVelocity +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div +import org.team5419.fault.math.units.unitlessValue +import kotlin.math.roundToInt + +import kotlin.properties.Delegates + +abstract class CTREBerkeliumMotor internal constructor( + val motorController: IMotorController, + val model: NativeUnitModel +) : AbstractBerkeliumMotor() { + + private var mLastDemand = Demand(ControlMode.Disabled, 0.0, DemandType.Neutral, 0.0) + + override val encoder = CTREBerkeliumEncoder(motorController, 0, model) + + override val voltageOutput: SIUnit + get() = motorController.motorOutputVoltage.volts + + override var outputInverted: Boolean by Delegates.observable(false) { _, _, newValue -> + motorController.inverted = newValue + } + + override var brakeMode: Boolean by Delegates.observable(false) { _, _, newValue -> + motorController.setNeutralMode(if (newValue) NeutralMode.Brake else NeutralMode.Coast) + } + + override var voltageCompSaturation: SIUnit by Delegates.observable(12.0.volts) { _, _, newValue -> + motorController.configVoltageCompSaturation(newValue.value, 0) + motorController.enableVoltageCompensation(true) + } + + override var motionProfileCruiseVelocity: SIUnit> + by Delegates.observable(SIUnit(0.0)) { _, _, newValue -> + motorController.configMotionCruiseVelocity( + model.toNativeUnitVelocity(newValue).nativeUnitsPer100ms.roundToInt(), 0 + ) + } + + override var motionProfileAcceleration: SIUnit> + by Delegates.observable(SIUnit(0.0)) { _, _, newValue -> + motorController.configMotionAcceleration( + model.toNativeUnitAcceleration(newValue).inNativeUnitsPer100msPerSecond().roundToInt(), 0 + ) + } + + init { + motorController.configVoltageCompSaturation(kCompVoltage.value, 0) + motorController.enableVoltageCompensation(true) + } + + override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) = sendDemand( + Demand( + ControlMode.PercentOutput, (voltage / kCompVoltage).unitlessValue, + DemandType.ArbitraryFeedForward, (arbitraryFeedForward / kCompVoltage).unitlessValue + ) + ) + + override fun setPercent(percent: Double, arbitraryFeedForward: SIUnit) = sendDemand( + Demand( + ControlMode.PercentOutput, percent, + DemandType.ArbitraryFeedForward, (arbitraryFeedForward / kCompVoltage).unitlessValue + ) + ) + + override fun setVelocity(velocity: SIUnit>, arbitraryFeedForward: SIUnit) = sendDemand( + Demand( + ControlMode.Velocity, model.toNativeUnitVelocity(velocity).nativeUnitsPer100ms, + DemandType.ArbitraryFeedForward, (arbitraryFeedForward / kCompVoltage).unitlessValue + ) + ) + + override fun setPosition(position: SIUnit, arbitraryFeedForward: SIUnit) = sendDemand( + Demand( + if (useMotionProfileForPosition) ControlMode.MotionMagic else ControlMode.Position, + model.toNativeUnitPosition(position).value, + DemandType.ArbitraryFeedForward, (arbitraryFeedForward / kCompVoltage).unitlessValue + ) + ) + + override fun setNeutral() = sendDemand( + Demand( + ControlMode.Disabled, + 0.0, + DemandType.Neutral, + 0.0 + ) + ) + + fun sendDemand(demand: Demand) { + if (demand != mLastDemand) { + motorController.set(demand.mode, demand.demand0, demand.demand1Type, demand.demand1) + mLastDemand = demand + } + } + + override fun follow(motor: BerkeliumMotor<*>): Boolean = + if (motor is CTREBerkeliumMotor<*>) { + motorController.follow(motor.motorController) + true + } else { + super.follow(motor) + } + + data class Demand( + val mode: ControlMode, + val demand0: Double, + val demand1Type: DemandType = DemandType.Neutral, + val demand1: Double = 0.0 + ) + + companion object { + private val kCompVoltage = SIUnit(12.0) + } +} diff --git a/src/main/kotlin/org/team5419/fault/input/BerkeliumHID.kt b/src/main/kotlin/org/team5419/fault/input/BerkeliumHID.kt new file mode 100644 index 0000000..8d41be1 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/BerkeliumHID.kt @@ -0,0 +1,74 @@ +package org.team5419.fault.input + +import edu.wpi.first.wpilibj.GenericHID +import org.team5419.fault.util.BooleanSource +import org.team5419.fault.util.DoubleSource + +fun T.mapControls( + block: BerkeleiumHIDBuilder.() -> Unit +) = BerkeleiumHIDBuilder(this).also(block).build() + +class BerkeleiumHIDBuilder(private val genericHID: T) { + private val controlBuilders = mutableListOf() + private val stateControlBuilders = mutableMapOf>() + + fun button( + buttonId: Int, + block: HIDButtonBuilder.() -> Unit = {} + ) = button(HIDButtonSource(genericHID, buttonId), block = block) + + fun axisButton( + axisId: Int, + threshold: Double = HIDButton.kDefaultThreshold, + block: HIDButtonBuilder.() -> Unit = {} + ) = button(HIDAxisSource(genericHID, axisId), threshold, block = block) + + fun pov(angle: Int, block: HIDButtonBuilder.() -> Unit = {}) = pov(0, angle, block) + fun pov( + povId: Int, + angle: Int, + block: HIDButtonBuilder.() -> Unit = {} + ) = button(HIDPOVSource(genericHID, povId, angle), block = block) + + fun button( + source: HIDSource, + threshold: Double = HIDButton.kDefaultThreshold, + block: HIDButtonBuilder.() -> Unit = {} + ): HIDButtonBuilder { + val builder = HIDButtonBuilder(source, threshold) + controlBuilders.add(builder) + block(builder) + return builder + } + + fun state(state: BooleanSource, block: BerkeleiumHIDBuilder.() -> Unit) = + stateControlBuilders.compute(state) { _, _ -> BerkeleiumHIDBuilder(genericHID).also(block) } + + fun build(): BerkeleiumHID { + return BerkeleiumHID( + genericHID, + controlBuilders.map { it.build() }, + stateControlBuilders.mapValues { it.value.build() } + ) + } +} + +class BerkeleiumHID( + private val genericHID: T, + private val controls: List, + private val stateControls: Map> +) { + + fun getRawAxis(axisId: Int): DoubleSource = HIDAxisSource(genericHID, axisId) + fun getRawButton(buttonId: Int): BooleanSource = HIDButtonSource( + genericHID, + buttonId + ).booleanSource + + fun update() { + controls.forEach { it.update() } + for ((state, controller) in stateControls) { + if (state()) controller.update() + } + } +} diff --git a/src/main/kotlin/org/team5419/fault/input/BerkeliumPS4.kt b/src/main/kotlin/org/team5419/fault/input/BerkeliumPS4.kt new file mode 100644 index 0000000..5d99cc6 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/BerkeliumPS4.kt @@ -0,0 +1,49 @@ +@file:Suppress("MatchingDeclarationName") +package org.team5419.fault.input + +import edu.wpi.first.wpilibj.GenericHID +import edu.wpi.first.wpilibj.Joystick + +typealias BerkeleiumPS4Controller = BerkeleiumHID +typealias BerkeleiumPS4Builder = BerkeleiumHIDBuilder + +fun ps4Controller( + port: Int, + block: BerkeleiumPS4Builder.() -> Unit +) = Joystick(port).mapControls(block) + +fun BerkeleiumPS4Builder.button( + button: PS4Button, + block: HIDButtonBuilder.() -> Unit = {} +) = button(button.id, block) + +fun BerkeleiumPS4Builder.triggerAxisButton( + hand: GenericHID.Hand, + threshold: Double = HIDButton.kDefaultThreshold, + block: HIDButtonBuilder.() -> Unit = {} +) = axisButton(yTriggerAxisToRawAxis(hand), threshold, block) + +fun BerkeleiumPS4Controller.getY(hand: GenericHID.Hand) = getRawAxis(yAxisToRawAxis(hand)) +fun BerkeleiumPS4Controller.getX(hand: GenericHID.Hand) = getRawAxis(xAxisToRawAxis(hand)) +fun BerkeleiumPS4Controller.getRawButton(button: PS4Button) = getRawButton(button.id) + +private fun yAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 1 else 5 +private fun xAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 0 else 2 +private fun yTriggerAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 2 else 3 // check this + +enum class PS4Button(val id: Int) { + Square(1), + X(2), + Circle(3), + Triangle(4), + LeftBumper(5), + RightBumper(6), + LeftTrigger(7), + RightTrigger(8), + Share(9), + Options(10), + LeftStick(11), + RightStick(12), + Playstation(13), + Touchpad(14) +} diff --git a/src/main/kotlin/org/team5419/fault/input/BerkeliumXbox.kt b/src/main/kotlin/org/team5419/fault/input/BerkeliumXbox.kt new file mode 100644 index 0000000..1090014 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/BerkeliumXbox.kt @@ -0,0 +1,45 @@ +@file:Suppress("MatchingDeclarationName") +package org.team5419.fault.input + +import edu.wpi.first.wpilibj.GenericHID +import edu.wpi.first.wpilibj.XboxController + +typealias BerkeleiumXboxController = BerkeleiumHID +typealias BerkeleiumXboxBuilder = BerkeleiumHIDBuilder + +fun xboxController(port: Int, block: BerkeleiumXboxBuilder.() -> Unit): BerkeleiumXboxController = + XboxController(port).mapControls(block) + +fun BerkeleiumXboxBuilder.button( + button: XboxButton, + block: HIDButtonBuilder.() -> Unit = {} +) = button(button.id, block) + +fun BerkeleiumXboxBuilder.triggerAxisButton( + hand: GenericHID.Hand, + threshold: Double = HIDButton.kDefaultThreshold, + block: HIDButtonBuilder.() -> Unit = {} +) = axisButton(yTriggerAxisToRawAxis(hand), threshold, block) + +fun BerkeleiumXboxController.getY(hand: GenericHID.Hand) = getRawAxis(yAxisToRawAxis(hand)) + +fun BerkeleiumXboxController.getX(hand: GenericHID.Hand) = getRawAxis(xAxisToRawAxis(hand)) + +fun BerkeleiumXboxController.getRawButton(button: XboxButton) = getRawButton(button.id) + +private fun yAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 1 else 5 +private fun xAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 0 else 4 +private fun yTriggerAxisToRawAxis(hand: GenericHID.Hand) = if (hand == GenericHID.Hand.kLeft) 2 else 3 + +enum class XboxButton(val id: Int) { + A(1), + B(2), + X(3), + Y(4), + LeftBumper(5), + RightBumper(6), + Back(7), + Start(8), + LeftStick(9), + RightStick(10) +} diff --git a/src/main/kotlin/org/team5419/fault/input/DriveHelper.kt b/src/main/kotlin/org/team5419/fault/input/DriveHelper.kt new file mode 100644 index 0000000..26693d3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/DriveHelper.kt @@ -0,0 +1,79 @@ +package org.team5419.fault.input + +import org.team5419.fault.util.BooleanSource +import org.team5419.fault.util.DoubleSource +import kotlin.math.absoluteValue + +abstract class DriveHelper : DriveSignalSource { + + abstract fun output(): DriveSignal + + override fun invoke() = output() + + protected fun handleDeadband(value: Double, deadband: Double) = when { + value.absoluteValue < deadband.absoluteValue -> 0.0 + else -> value + } +} + +class TankHelper( + private val left: DoubleSource, + private val right: DoubleSource, + private val slow: BooleanSource = { false }, + private val deadband: Double = kDefaultDeaband, + private val slowMultiplier: Double = kDefaultSlowMultiplier +) : DriveHelper() { + + override fun output(): DriveSignal { + val currentSlow = if (slow()) slowMultiplier else 1.0 + var currentLeft = left() + var currentRight = right() + currentLeft = handleDeadband(currentLeft, deadband) + currentRight = handleDeadband(currentRight, deadband) + return DriveSignal(currentLeft * currentSlow, currentRight) + } + + companion object { + const val kDefaultDeaband = 0.02 + const val kDefaultSlowMultiplier = 0.4 + } +} + +class SpaceDriveHelper( + private val throttle: DoubleSource, + private val wheel: DoubleSource, + private val quickTurn: BooleanSource, + private val slow: BooleanSource = { false }, + private val deadband: Double = kDefaultDeadband, + private val quickTurnMultiplier: Double = kDefaultQuickTurnMultiplier, + private val slowMultiplier: Double = kDefaultSlowMultiplier +) : DriveHelper() { + + override fun output(): DriveSignal { + var currentThrottle = throttle() + var currentTurn = wheel() * if (quickTurn()) 1.0 else quickTurnMultiplier + currentThrottle = handleDeadband(currentThrottle, deadband) + currentTurn = handleDeadband(currentTurn, deadband) + val currentSlow = if (slow()) slowMultiplier else 1.0 + val left = currentSlow * (currentThrottle - currentTurn) + val right = currentSlow * (currentThrottle + currentTurn) + return DriveSignal(left, right) + } + + companion object { + const val kDefaultDeadband = 0.02 + const val kDefaultQuickTurnMultiplier = 0.4 + const val kDefaultSlowMultiplier = 0.4 + } +} + +class CheesyDriveHelper( + private val throttle: DoubleSource, + private val wheel: DoubleSource, + private val quickTurn: BooleanSource +) : DriveHelper() { + + override fun output(): DriveSignal { + return DriveSignal() + } +} diff --git a/src/main/kotlin/org/team5419/fault/input/DriveSignal.kt b/src/main/kotlin/org/team5419/fault/input/DriveSignal.kt new file mode 100644 index 0000000..ae660d3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/DriveSignal.kt @@ -0,0 +1,12 @@ +package org.team5419.fault.input + +import org.team5419.fault.util.Source + +typealias DriveSignalSource = Source + +data class DriveSignal(val left: Double = 0.0, val right: Double = 0.0, val brake: Boolean = false) { + companion object { + val kBrake = DriveSignal(0.0, 0.0, true) + val kNeutral = DriveSignal() + } +} diff --git a/src/main/kotlin/org/team5419/fault/input/HIDControl.kt b/src/main/kotlin/org/team5419/fault/input/HIDControl.kt new file mode 100644 index 0000000..00e5698 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/HIDControl.kt @@ -0,0 +1,63 @@ +package org.team5419.fault.input + +import org.team5419.fault.auto.Action +import kotlin.math.absoluteValue + +typealias HIDControlListener = () -> Unit + +interface HIDControl { + fun update() +} +class HIDButton( + private val source: HIDSource, + private val threshold: Double, + private val whileOn: List, + private val whileOff: List, + private val changeToOn: List, + private val changeToOff: List +) : HIDControl { + + private var lastValue = source().absoluteValue >= threshold + + override fun update() { + val currentValue = source().absoluteValue >= threshold + when { + // Value has changed + lastValue != currentValue -> when { + currentValue -> changeToOn + else -> changeToOff + } + // Value stayed the same + else -> when { + currentValue -> whileOn + else -> whileOff + } + }.forEach { it() } + lastValue = currentValue + } + + companion object { + const val kDefaultThreshold = 0.5 + } +} + +abstract class HIDControlBuilder(val source: HIDSource) { + abstract fun build(): HIDControl +} +class HIDButtonBuilder(source: HIDSource, private val threshold: Double) : HIDControlBuilder(source) { + + private val whileOff = mutableListOf() + private val whileOn = mutableListOf() + private val changeToOn = mutableListOf() + private val changeToOff = mutableListOf() + + fun changeToOn(action: Action) = changeToOn { action.start() } + fun changeToOff(action: Action) = changeToOff { action.start() } + + fun whileOff(block: HIDControlListener) = also { whileOff.add(block) } + fun whileOn(block: HIDControlListener) = also { whileOn.add(block) } + fun changeToOn(block: HIDControlListener) = also { changeToOn.add(block) } + fun changeToOff(block: HIDControlListener) = also { changeToOff.add(block) } + + override fun build() = HIDButton(source, threshold, whileOn, whileOff, changeToOn, changeToOff) +} diff --git a/src/main/kotlin/org/team5419/fault/input/HIDSource.kt b/src/main/kotlin/org/team5419/fault/input/HIDSource.kt new file mode 100644 index 0000000..670b314 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/input/HIDSource.kt @@ -0,0 +1,31 @@ +package org.team5419.fault.input + +import edu.wpi.first.wpilibj.GenericHID +import org.team5419.fault.util.BooleanSource +import org.team5419.fault.util.Source + +interface HIDSource : Source + +class HIDButtonSource( + private val genericHID: GenericHID, + private val buttonId: Int +) : HIDSource { + val booleanSource: BooleanSource = { genericHID.getRawButton(buttonId) } + + override fun invoke() = if (booleanSource()) 1.0 else 0.0 +} + +class HIDAxisSource( + private val genericHID: GenericHID, + private val axisId: Int +) : HIDSource { + override fun invoke() = genericHID.getRawAxis(axisId) +} + +class HIDPOVSource( + private val genericHID: GenericHID, + private val povId: Int, + private val angle: Int +) : HIDSource { + override fun invoke() = if (genericHID.getPOV(povId) == angle) 1.0 else 0.0 +} diff --git a/src/main/kotlin/org/team5419/fault/math/Epsilon.kt b/src/main/kotlin/org/team5419/fault/math/Epsilon.kt new file mode 100644 index 0000000..0c9ae74 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/Epsilon.kt @@ -0,0 +1,8 @@ +package org.team5419.fault.math + +import kotlin.math.absoluteValue + +@Suppress("TopLevelPropertyNaming") +const val kEpsilon = 1E-9 + +infix fun Double.epsilonEquals(other: Double) = minus(other).absoluteValue < kEpsilon diff --git a/src/main/kotlin/org/team5419/fault/math/MathMisc.kt b/src/main/kotlin/org/team5419/fault/math/MathMisc.kt new file mode 100644 index 0000000..865d5c3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/MathMisc.kt @@ -0,0 +1,42 @@ +package org.team5419.fault.math + +import kotlin.math.PI + +fun Double.lerp(endValue: Double, t: Double) = this + (endValue - this) * t.coerceIn(0.0, 1.0) + +infix fun Double.cos(other: Double) = times(Math.cos(other)) +infix fun Double.sin(other: Double) = times(Math.sin(other)) + +fun limit(value: Double, limit: Double): Double { + return limit(value, -limit, limit) +} + +/** + * method that limits input to a certain range. + * @param value value to be limited + * @param min lower bound of the value + * @param max upper bound of the value + * @return bounded value + */ +fun limit(value: Double, min: Double, max: Double): Double { + return Math.min(max, Math.max(min, value)) +} + +/** + * method to interpolate between 2 doubles + * @param a first value + * @param b second value + * @param x value between the 2 values + * @return interpolated value + */ +fun interpolate(a: Double, b: Double, x: Double): Double { + val newX = limit(x, 0.0, 1.0) + return a + (b - a) * newX +} + +fun Double.boundRadians(): Double { + var x = this + while (x >= PI) x -= (2 * PI) + while (x < -PI) x += (2 * PI) + return x +} diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Pose2d.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Pose2d.kt new file mode 100644 index 0000000..12fea98 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Pose2d.kt @@ -0,0 +1,90 @@ +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.toRotation2d +import kotlin.math.absoluteValue + +data class Pose2d( + val translation: Vector2 = Vector2(), + val rotation: Rotation2d = Rotation2d() +) : State { + + constructor( + x: SIUnit, + y: SIUnit, + rotation: Rotation2d = Rotation2d() + ) : this(Vector2(x, y), rotation) + + constructor( + translation: Vector2 = Vector2(), + rotation: SIUnit + ) : this(translation, rotation.toRotation2d()) + + constructor( + x: SIUnit, + y: SIUnit, + rotation: SIUnit + ) : this(Vector2(x, y), rotation) + + val twist: Twist2d + get() { + val dtheta = rotation.radian.value + val halfDTheta = dtheta / 2.0 + val cosMinusOne = rotation.cos - 1.0 + + val halfThetaByTanOfHalfDTheta = if (cosMinusOne.absoluteValue < kEpsilon) { + 1.0 - 1.0 / 12.0 * dtheta * dtheta + } else { + -(halfDTheta * rotation.sin) / cosMinusOne + } + val translationPart = translation * + Rotation2d(halfThetaByTanOfHalfDTheta, -halfDTheta, false) + return Twist2d(translationPart.x, translationPart.y, rotation.radian) + } + + val mirror get() = Pose2d(Vector2(translation.x, -translation.y), -rotation) + + infix fun inFrameOfReferenceOf(fieldRelativeOrigin: Pose2d) = (-fieldRelativeOrigin) + this + + operator fun plus(other: Pose2d) = transformBy(other) + + operator fun minus(other: Pose2d) = this + -other + + fun transformBy(other: Pose2d) = + Pose2d( + translation + (other.translation * rotation), + rotation + other.rotation + ) + + operator fun unaryMinus(): Pose2d { + val invertedRotation = -rotation + return Pose2d((-translation) * invertedRotation, invertedRotation) + } + + fun isCollinear(other: Pose2d): Boolean { + if (!rotation.isParallel(other.rotation)) return false + val twist = (-this + other).twist + return twist.dy.value epsilonEquals 0.0 && twist.dTheta.value epsilonEquals 0.0 + } + + @Suppress("ReturnCount") + override fun interpolate(endValue: Pose2d, t: Double): Pose2d { + if (t <= 0) { + return Pose2d(this.translation, this.rotation) + } else if (t >= 1) { + return Pose2d(endValue.translation, endValue.rotation) + } + val twist = (-this + endValue).twist + return this + (twist * t).asPose + } + + override fun toString() = toCSV() + + override fun toCSV() = "${translation.toCSV()}, ${rotation.degree}" + + override fun distance(other: Pose2d) = (-this + other).twist.norm.value +} diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Pose2dWithCurvature.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Pose2dWithCurvature.kt new file mode 100644 index 0000000..375f5b6 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Pose2dWithCurvature.kt @@ -0,0 +1,41 @@ +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.interpolate +import org.team5419.fault.math.lerp + +data class Pose2dWithCurvature( + val pose: Pose2d, + val curvature: Double, + val dkds: Double +) : State { + + val mirror get() = Pose2dWithCurvature(pose.mirror, -curvature, -dkds) + + override fun interpolate(endValue: Pose2dWithCurvature, t: Double) = + Pose2dWithCurvature( + pose.interpolate(endValue.pose, t), + curvature.lerp(endValue.curvature, t), + dkds.lerp(endValue.dkds, t) + ) + + override fun distance(other: Pose2dWithCurvature) = pose.distance(other.pose) + + operator fun plus(other: Pose2d) = Pose2dWithCurvature(this.pose + other, curvature, dkds) + + override fun toCSV() = "${pose.toCSV()}, $curvature, $dkds" + + override fun toString() = toCSV() + + override fun hashCode(): Int { + return super.hashCode() + } + + override fun equals(other: Any?): Boolean { + if (other is Pose2dWithCurvature) { + return this.pose.equals(other.pose) && this.curvature epsilonEquals other.curvature && + this.dkds epsilonEquals other.dkds + } + return false + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Rectangle2d.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Rectangle2d.kt new file mode 100644 index 0000000..7112034 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Rectangle2d.kt @@ -0,0 +1,122 @@ +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.operations.div +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +import kotlin.math.max +import kotlin.math.min +import org.team5419.fault.math.units.operations.div + +@Suppress("FunctionName") +fun Rectangle2d( + one: Vector2d, + two: Vector2d +): Rectangle2d { + val minX = min(one.x.value, two.x.value) + val minY = min(one.y.value, two.y.value) + val maxX = max(one.x.value, two.x.value) + val maxY = max(one.y.value, two.y.value) + return Rectangle2d( + minX.meters, minY.meters, + (maxX - minX).meters, (maxY - minY).meters + ) +} + +@Suppress("FunctionName", "UnsafeCallOnNullableType") +fun Rectangle2d( + vararg pointsToInclude: Vector2d +): Rectangle2d { + val minX = pointsToInclude.minBy { it.x }!!.x + val minY = pointsToInclude.minBy { it.y }!!.y + val maxX = pointsToInclude.maxBy { it.x }!!.x + val maxY = pointsToInclude.maxBy { it.y }!!.y + return Rectangle2d( + minX, minY, + maxX - minX, maxY - minY + ) +} + +data class Rectangle2d constructor( + val x: SIUnit, + val y: SIUnit, + val w: SIUnit, + val h: SIUnit +) { + + val topLeft = Vector2(x, y + h) + val topRight = Vector2(x + w, y + h) + val bottomLeft = Vector2(x, y) + val bottomRight = Vector2(x + w, y) + + val center = Vector2(x + w / 2.0, y + h / 2.0) + + val maxCorner = topRight + val minCorner = bottomLeft + + fun isIn(r: Rectangle2d) = + x < r.x + r.w && x + w > r.x && y < r.y + r.h && y + h > r.y + + fun isWithin(r: Rectangle2d) = r.x in x..(x + w - r.w) && r.y in y..(y + h - r.h) + + operator fun contains(p: Vector2d) = p.x in x..(x + w) && p.y in y..(y + h) + + @Suppress("ComplexMethod", "ReturnCount") + fun doesCollide(rectangle: Rectangle2d, translation: Vector2d): Boolean { + if (translation.x.value epsilonEquals 0.0 && translation.y.value epsilonEquals 0.0) return false + // Check if its even in range + val boxRect = Rectangle2d( + rectangle.topLeft, rectangle.bottomRight, + rectangle.topLeft + translation, rectangle.bottomRight + translation + ) + // println(boxRect) + if (!boxRect.isIn(this)) return false + // AABB collision + // Calculate distances + val xInvEntry: SIUnit + val xInvExit: SIUnit + val yInvEntry: SIUnit + val yInvExit: SIUnit + if (translation.x.value > 0.0) { + xInvEntry = (x - (rectangle.x + rectangle.w)) + xInvExit = ((x + w) - rectangle.x) + } else { + xInvEntry = ((x + w) - rectangle.x) + xInvExit = (x - (rectangle.x + rectangle.w)) + } + if (translation.y.value > 0.0) { + yInvEntry = (y - (rectangle.y + rectangle.h)) + yInvExit = ((y + h) - rectangle.y) + } else { + yInvEntry = ((y + h) - rectangle.y) + yInvExit = (y - (rectangle.y + rectangle.h)) + } + // Find time of collisions + val xEntry: Double + val xExit: Double + val yEntry: Double + val yExit: Double + if (translation.x.value epsilonEquals 0.0) { + xEntry = Double.NEGATIVE_INFINITY + xExit = Double.POSITIVE_INFINITY + } else { + xEntry = (xInvEntry / translation.x).value + xExit = (xInvExit / translation.x).value + } + if (translation.y.value epsilonEquals 0.0) { + yEntry = Double.NEGATIVE_INFINITY + yExit = Double.POSITIVE_INFINITY + } else { + yEntry = (yInvEntry / translation.y).value + yExit = (yInvExit / translation.y).value + } + val entryTime = max(xEntry, yEntry) + val exitTime = min(xExit, yExit) + + return entryTime <= exitTime && (xEntry >= 0.0 || yEntry >= 0.0) && (xEntry < 1.0 || yEntry < 1.0) + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Rotation2d.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Rotation2d.kt new file mode 100644 index 0000000..b67bece --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Rotation2d.kt @@ -0,0 +1,62 @@ +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.cos +import org.team5419.fault.math.units.derived.sin +import org.team5419.fault.math.units.derived.radians +import org.team5419.fault.math.units.derived.inDegrees +// import org.team5419.fault.math.units.derived.radians + +@Suppress("EqualsWithHashCodeExist") +class Rotation2d { + + val value: SIUnit + val cos: Double + val sin: Double + + constructor() : this(0.0.radians) + + constructor(heading: SIUnit) : this(heading.cos, heading.sin, true) + + constructor(x: Double, y: Double, normalize: Boolean) { + if (normalize) { + val magnitude = Math.hypot(x, y) + if (magnitude > kEpsilon) { + sin = y / magnitude + cos = x / magnitude + } else { + sin = 0.0 + cos = 1.0 + } + } else { + cos = x + sin = y + } + value = Math.atan2(sin, cos).radians + } + + val radian get() = value + val degree get() = value.inDegrees() + + fun isParallel(rotation: Rotation2d) = (this - rotation).radian epsilonEquals 0.0.radians + + operator fun minus(other: Rotation2d) = plus(-other) + operator fun unaryMinus() = Rotation2d(-value) + + operator fun times(other: Double) = Rotation2d(value * other) + + operator fun plus(other: Rotation2d): Rotation2d { + return Rotation2d( + cos * other.cos - sin * other.sin, + cos * other.sin + sin * other.cos, + true + ) + } + + override fun equals(other: Any?): Boolean { + return other is Rotation2d && other.sin epsilonEquals this.sin && other.cos epsilonEquals this.cos + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Geometric.kt b/src/main/kotlin/org/team5419/fault/math/geometry/State.kt similarity index 53% rename from src/main/kotlin/org/team5499/monkeyLib/math/geometry/Geometric.kt rename to src/main/kotlin/org/team5419/fault/math/geometry/State.kt index 91aaa0c..191567c 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Geometric.kt +++ b/src/main/kotlin/org/team5419/fault/math/geometry/State.kt @@ -1,16 +1,18 @@ -package org.team5499.monkeyLib.math.geometry +package org.team5419.fault.math.geometry -import org.team5499.monkeyLib.util.Interpolable -import org.team5499.monkeyLib.util.CSVWritable +import org.team5419.fault.util.Interpolable +import org.team5419.fault.util.CSVWritable -interface Geometric : Interpolable, CSVWritable { +interface State : Interpolable, CSVWritable { - public override fun toString(): String + fun distance(other: T): Double - public override fun equals(other: Any?): Boolean + public override fun toString(): String public override fun toCSV(): String + public override fun equals(other: Any?): Boolean + public override fun interpolate(other: T, x: Double): T public override fun hashCode(): Int diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Twist2d.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Twist2d.kt new file mode 100644 index 0000000..53d4823 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Twist2d.kt @@ -0,0 +1,40 @@ +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.meters +import kotlin.math.hypot +import kotlin.math.sin +import kotlin.math.cos +import kotlin.math.abs + +class Twist2d constructor( + val dx: SIUnit, + val dy: SIUnit, + val dTheta: SIUnit +) { + + val norm get() = if (dy.value == 0.0) dx.absoluteValue else hypot(dx.value, dy.value).meters + + val asPose: Pose2d + get() { + val dTheta = this.dTheta.value + val sinTheta = sin(dTheta) + val cosTheta = cos(dTheta) + + val (s, c) = if (abs(dTheta) < kEpsilon) { + 1.0 - 1.0 / 6.0 * dTheta * dTheta to .5 * dTheta + } else { + sinTheta / dTheta to (1.0 - cosTheta) / dTheta + } + return Pose2d( + Vector2(dx * s - dy * c, dx * c + dy * s), + Rotation2d(cosTheta, sinTheta, false) + ) + } + + operator fun times(scale: Double) = + Twist2d(dx * scale, dy * scale, dTheta * scale) +} diff --git a/src/main/kotlin/org/team5419/fault/math/geometry/Vector2.kt b/src/main/kotlin/org/team5419/fault/math/geometry/Vector2.kt new file mode 100644 index 0000000..e50e2d5 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/geometry/Vector2.kt @@ -0,0 +1,77 @@ + +package org.team5419.fault.math.geometry + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.Meter +import kotlin.math.hypot + +fun Rotation2d.toTranslation() = Vector2(cos.meters, sin.meters) +typealias Vector2d = Vector2 + +@Suppress("TooManyFunctions") +data class Vector2 constructor( + val x: SIUnit, + val y: SIUnit +) : State> { + + constructor() : this(SIUnit(0.0), SIUnit(0.0)) + + // Vector to Translation3d + constructor( + rotation: Rotation2d, + distance: SIUnit = SIUnit(0.0) + ) : this (distance * rotation.cos, distance * rotation.sin) + + val norm get() = hypot(x.value, y.value).meters + + override fun interpolate(endValue: Vector2, t: Double) = when { + t <= 0 -> this + t >= 1 -> endValue + else -> Vector2( + x.lerp(endValue.x, t), + y.lerp(endValue.y, t) + ) + } + + override fun distance(other: Vector2): Double { + val x = this.x.value - other.x.value + val y = this.y.value - other.y.value + return hypot(x, y) + } + + operator fun plus(other: Vector2) = Vector2(x + other.x, y + other.y) + operator fun minus(other: Vector2) = Vector2(x - other.x, y - other.y) + + operator fun times(other: Rotation2d) = Vector2( + x * other.cos - y * other.sin, + x * other.sin + y * other.cos + ) + + operator fun times(other: Number): Vector2 { + val factor = other.toDouble() + return Vector2(x * factor, y * factor) + } + + operator fun div(other: Number): Vector2 { + val factor = other.toDouble() + return Vector2(x / factor, y / factor) + } + + operator fun unaryMinus() = Vector2(-x, -y) + + override fun toCSV() = "${x.value}, ${y.value}" + + override fun toString() = toCSV() + + override fun hashCode() = super.hashCode() + + override fun equals(other: Any?): Boolean { + if (other is Vector2<*>) { + return other.x.value epsilonEquals this.x.value && other.y.value epsilonEquals this.y.value + } + return false + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/localization/PositionTracker.kt b/src/main/kotlin/org/team5419/fault/math/localization/PositionTracker.kt new file mode 100644 index 0000000..5ac0ea3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/localization/PositionTracker.kt @@ -0,0 +1,58 @@ +package org.team5419.fault.math.localization + +import edu.wpi.first.wpilibj.Timer +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.seconds +import org.team5419.fault.util.CSVWritable +import org.team5419.fault.util.Source +import kotlin.reflect.KProperty + +abstract class PositionTracker( + val heading: Source, + private val buffer: TimeInterpolatableBuffer = TimeInterpolatableBuffer() +) : Source, CSVWritable { + + var robotPosition = Pose2d() + private set + + private var lastHeading = Rotation2d() + private var headingOffset = Rotation2d() + + override fun invoke() = robotPosition + + @Synchronized fun reset(pose: Pose2d = Pose2d()) = resetInternal(pose) + + protected open fun resetInternal(pose: Pose2d) { + robotPosition = pose + val newHeading = heading() + lastHeading = newHeading + headingOffset = -newHeading + pose.rotation + buffer.clear() + } + + @Synchronized + fun update() { + val newHeading = heading() + val headingDelta = newHeading - lastHeading + val newRobotPosition = robotPosition + update(headingDelta) + robotPosition = Pose2d( + newRobotPosition.translation, + newHeading + headingOffset // check this + ) + lastHeading = newHeading + + buffer[Timer.getFPGATimestamp().seconds] = robotPosition + } + + protected abstract fun update(deltaHeading: Rotation2d): Pose2d + + operator fun get(timestamp: SIUnit) = buffer[timestamp] ?: Pose2d() + + operator fun getValue(thisRef: Any?, property: KProperty<*>): Pose2d = robotPosition + operator fun setValue(thisRef: Any?, property: KProperty<*>, value: Pose2d) = reset(value) + + override fun toCSV() = robotPosition.toCSV() +} diff --git a/src/main/kotlin/org/team5419/fault/math/localization/TankPositionTracker.kt b/src/main/kotlin/org/team5419/fault/math/localization/TankPositionTracker.kt new file mode 100644 index 0000000..f7f9c1b --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/localization/TankPositionTracker.kt @@ -0,0 +1,33 @@ +package org.team5419.fault.math.localization + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.geometry.Twist2d +import org.team5419.fault.math.units.meters +import org.team5419.fault.util.Source + +class TankPositionTracker( + headingSource: Source, + val leftDistanceSource: Source, + val rightDistanceSource: Source, + buffer: TimeInterpolatableBuffer = TimeInterpolatableBuffer() +) : PositionTracker(headingSource, buffer) { + + private var lastLeftDistance = 0.0 + private var lastRightDistance = 0.0 + + override fun resetInternal(pose: Pose2d) { + lastLeftDistance = leftDistanceSource() + lastRightDistance = rightDistanceSource() + super.resetInternal(pose) + } + + override fun update(deltaHeading: Rotation2d): Pose2d { + val currentLeftDistance = leftDistanceSource() + val currentRightDistance = rightDistanceSource() + val leftDelta = currentLeftDistance - lastLeftDistance + val rightDelta = currentRightDistance - lastRightDistance + val dx = (leftDelta + rightDelta) / 2.0 + return Twist2d(dx.meters, 0.0.meters, deltaHeading.radian).asPose + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/localization/TimeInterpolatableBuffer.kt b/src/main/kotlin/org/team5419/fault/math/localization/TimeInterpolatableBuffer.kt new file mode 100644 index 0000000..63f0f89 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/localization/TimeInterpolatableBuffer.kt @@ -0,0 +1,62 @@ +package org.team5419.fault.math.localization + +import edu.wpi.first.wpilibj.Timer +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.seconds +import org.team5419.fault.util.Interpolable +import org.team5419.fault.util.Source +import org.team5419.fault.math.units.unitlessValue +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +import java.util.TreeMap + +class TimeInterpolatableBuffer>( + private val historySpan: SIUnit = SIUnit(1.0), + private val timeSource: Source> = { Timer.getFPGATimestamp().seconds } +) { + + private val bufferMap = TreeMap, T>() + + operator fun set(time: SIUnit, value: T): T? { + cleanUp() + return bufferMap.put(time, value) + } + + private fun cleanUp() { + val currentTime = timeSource() + + while (bufferMap.isNotEmpty()) { + val entry = bufferMap.lastEntry() + if (currentTime - entry.key >= historySpan) { + bufferMap.remove(entry.key) + } else { + return + } + } + } + + fun clear() { + bufferMap.clear() + } + + @Suppress("ReturnCount") + operator fun get(time: SIUnit): T? { + if (bufferMap.isEmpty()) return null + + bufferMap[time]?.let { return it } + + val topBound = bufferMap.ceilingEntry(time) + val bottomBound = bufferMap.floorEntry(time) + + return when { + topBound == null -> bottomBound.value + bottomBound == null -> topBound.value + else -> bottomBound.value.interpolate( + topBound.value, + ((time - bottomBound.key) / (topBound.key - bottomBound.key)).unitlessValue + ) + } + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/physics/DCMotorTransmission.kt b/src/main/kotlin/org/team5419/fault/math/physics/DCMotorTransmission.kt new file mode 100644 index 0000000..0b05466 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/physics/DCMotorTransmission.kt @@ -0,0 +1,78 @@ +package org.team5419.fault.math.physics + +import org.team5419.fault.math.kEpsilon + +/* + * Implementation from: + * NASA Ames Robotics "The Cheesy Poofs" + * Team 254 + */ + +/** + * Model of a DC motor rotating a shaft. All parameters refer to the output (e.g. should already consider gearing + * and efficiency losses). The motor is assumed to be symmetric forward/reverse. + */ +class DCMotorTransmission( + val speedPerVolt: Double, // rad/s per V (no load), + private val torquePerVolt: Double, // N m per V (stall), + val frictionVoltage: Double // V +) { + + /** + * Returns the free speed of the motor at the specified voltage + * @param voltage specified voltage + * @return free speed + */ + fun getFreeSpeedAtVoltage(voltage: Double): Double { + return when { + voltage > kEpsilon -> Math.max(0.0, voltage - frictionVoltage) * speedPerVolt + voltage < kEpsilon -> Math.min(0.0, voltage + frictionVoltage) * speedPerVolt + else -> 0.0 + } + } + + /** + * Returns the torque produced by the motor + * @param outputSpeed The speed that is being outputted by the motor + * @param voltage The voltage through the motor + * @return torque + */ + fun getTorqueForVoltage(outputSpeed: Double, voltage: Double): Double { + var effectiveVoltage = voltage + when { + outputSpeed > kEpsilon -> // Forward motion, rolling friction. + effectiveVoltage -= frictionVoltage + outputSpeed < kEpsilon -> // Reverse motion, rolling friction. + effectiveVoltage += frictionVoltage + voltage > kEpsilon -> // System is static, forward torque. + effectiveVoltage = Math.max(0.0, voltage - frictionVoltage) + voltage < kEpsilon -> // System is static, reverse torque. + effectiveVoltage = Math.min(0.0, voltage + frictionVoltage) + else -> // System is idle. + return 0.0 + } + return torquePerVolt * (-outputSpeed / speedPerVolt + effectiveVoltage) + } + + /** + * Returns the voltage going through the motor + * @param outputSpeed The speed that is being outputted by the motor + * @param torque Torque produced by the motor + * @return voltage + */ + fun getVoltageForTorque(outputSpeed: Double, torque: Double): Double { + val fv: Double = when { + outputSpeed > kEpsilon -> // Forward motion, rolling friction. + frictionVoltage + outputSpeed < kEpsilon -> // Reverse motion, rolling friction. + -frictionVoltage + torque > kEpsilon -> // System is static, forward torque. + frictionVoltage + torque < kEpsilon -> // System is static, reverse torque. + -frictionVoltage + else -> // System is idle. + return 0.0 + } + return torque / torquePerVolt + outputSpeed / speedPerVolt + fv + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/physics/DifferentialDrive.kt b/src/main/kotlin/org/team5419/fault/math/physics/DifferentialDrive.kt new file mode 100644 index 0000000..2f306e6 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/physics/DifferentialDrive.kt @@ -0,0 +1,438 @@ +@file:Suppress("MemberVisibilityCanBePrivate", "unused") + +package org.team5419.fault.math.physics + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.util.CSVWritable +import java.text.DecimalFormat +import java.util.Arrays + +/* + * Implementation from: + * NASA Ames Robotics "The Cheesy Poofs" + * Team 254 + */ + +/** + * Dynamic model a differential drive robot. Note: to simplify things, this math assumes the center of mass is + * coincident with the kinematic center of rotation (e.g. midpoint of the center axle). + */ +@Suppress("LongParameterList", "TooManyFunctions") +class DifferentialDrive( + /** + * Equivalent mass when accelerating purely linearly, in kg. + * This is equivalent in that it also absorbs the effects of drivetrain inertia. + * Measure by doing drivetrain acceleration characterizaton in a straight line. + */ + private val mass: Double, + /** + * Equivalent moment of inertia when accelerating purely angularly in kg m^2. + * This is equivalent in that it also absorbs the effects of drivetrain inertia. + * Measure by doing drivetrain acceleration characterization while turing in place. + */ + private val moi: Double, + /** + * Drag torque (proportional to angular velocity) that resists turning in Nm/rad/s. + * Empirical testing of 254's drivebase showed that there was an unexplained loss in torque ~proportional to angular + * velocity, likely due to scrub of wheels. + * NOTE: this may not be a purely linear tern, and 254 has done limited testing, but this factor may help a model + * to better match reality. + */ + private val angularDrag: Double, + /** + * The radius of the wheel + */ + val wheelRadius: Double, // m, + /** + * Effective kinematic wheelbase radius. Might be larger than theoretical to compensate for skid steer. Measure by + * turning the robot in place several times and figuring out what the equivalent wheel base radius is. + */ + private val effectiveWheelBaseRadius: Double, // m + /** + * DC Motor transmission for both sides of the drivetrain. + */ + private val leftTransmission: DCMotorTransmission, + private val rightTransmission: DCMotorTransmission +) { + + /** + * 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) + ) + + /** + * 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 + ) + + /** + * Solve forward dynamics for torques and accelerations. + */ + fun solveForwardDynamics(chassisVelocity: ChassisState, voltage: WheelState): DriveDynamics = solveForwardDynamics( + solveInverseKinematics(chassisVelocity), + chassisVelocity, + (chassisVelocity.angular / chassisVelocity.linear).let { if (it.isNaN()) 0.0 else it }, + voltage + ) + + /** + * Get the voltage simply from the Kv and the friction voltage of the transmissions + */ + fun getVoltagesFromkV(velocities: DifferentialDrive.WheelState): WheelState { + return DifferentialDrive.WheelState( + velocities.left / leftTransmission.speedPerVolt + + leftTransmission.frictionVoltage * Math.signum(velocities.left), + velocities.right / rightTransmission.speedPerVolt + + rightTransmission.frictionVoltage * Math.signum(velocities.right) + ) + } + + /** + * Solve forward dynamics for torques and accelerations. + */ + fun solveForwardDynamics(wheelVelocity: WheelState, voltage: WheelState): DriveDynamics { + val chassisVelocity = solveForwardKinematics(wheelVelocity) + return solveForwardDynamics( + wheelVelocity, + chassisVelocity, + (chassisVelocity.angular / chassisVelocity.linear).let { if (it.isNaN()) 0.0 else it }, + voltage + ) + } + + /** + * Solve forward dynamics for torques and accelerations. + */ + fun solveForwardDynamics( + wheelVelocity: WheelState, + chassisVelocity: ChassisState, + curvature: Double, + voltage: WheelState + ): DriveDynamics { + + val wheelTorque: WheelState + val chassisAcceleration: ChassisState + val wheelAcceleration: WheelState + val dcurvature: Double + + val leftStationary = wheelVelocity.left epsilonEquals 0.0 && Math.abs( + voltage.left + ) < leftTransmission.frictionVoltage + val rightStationary = wheelVelocity.right epsilonEquals 0.0 && Math.abs( + voltage.right + ) < rightTransmission.frictionVoltage + + // Neither side breaks static friction, so we remain stationary. + if (leftStationary && rightStationary) { + wheelTorque = WheelState() + chassisAcceleration = ChassisState() + wheelAcceleration = WheelState() + dcurvature = 0.0 + } else { + // Solve for motor torques generated on each side. + wheelTorque = WheelState( + leftTransmission.getTorqueForVoltage(wheelVelocity.left, voltage.left), + rightTransmission.getTorqueForVoltage(wheelVelocity.right, voltage.right) + ) + + // Add forces and torques about the center of mass. + chassisAcceleration = ChassisState( + (wheelTorque.right + wheelTorque.left) / (wheelRadius * mass), + // (Tr - Tl) / r_w * r_wb - drag * w = I * angular_accel + effectiveWheelBaseRadius * (wheelTorque.right - wheelTorque.left) / + (wheelRadius * moi) - chassisVelocity.angular * angularDrag / moi + ) + + // Solve for change in curvature from angular acceleration. + // total angular accel = linear_accel * curvature + v^2 * dcurvature + dcurvature = ((chassisAcceleration.angular - chassisAcceleration.linear * curvature) / + (chassisVelocity.linear * chassisVelocity.linear)).let { if (it.isNaN()) 0.0 else it } + + // Resolve chassis accelerations to each wheel. + wheelAcceleration = WheelState( + chassisAcceleration.linear - chassisAcceleration.angular * effectiveWheelBaseRadius, + chassisAcceleration.linear + chassisAcceleration.angular * effectiveWheelBaseRadius + ) + } + return DriveDynamics( + curvature, + dcurvature, + chassisVelocity, + chassisAcceleration, + wheelVelocity, + wheelAcceleration, + voltage, + wheelTorque + ) + } + + /** + * Solve inverse dynamics for torques and voltages + */ + fun solveInverseDynamics(chassisVelocity: ChassisState, chassisAcceleration: ChassisState): DriveDynamics { + + val curvature = (chassisVelocity.angular / chassisVelocity.linear) + .let { if (it.isNaN()) 0.0 else it } + + val dcurvature = ((chassisAcceleration.angular - chassisAcceleration.linear * curvature) / + (chassisVelocity.linear * chassisVelocity.linear)) + .let { if (it.isNaN()) 0.0 else it } + + return solveInverseDynamics( + solveInverseKinematics(chassisVelocity), + chassisVelocity, + solveInverseKinematics(chassisAcceleration), + chassisAcceleration, + curvature, + dcurvature + ) + } + + /** + * Solve inverse dynamics for torques and voltages + */ + fun solveInverseDynamics(wheelVelocity: WheelState, wheelAcceleration: WheelState): DriveDynamics { + + val chassisVelocity = solveForwardKinematics(wheelVelocity) + val chassisAcceleration = solveForwardKinematics(wheelAcceleration) + + val curvature = (chassisVelocity.angular / chassisVelocity.linear) + .let { if (it.isNaN()) 0.0 else it } + + val dcurvature = ((chassisAcceleration.angular - chassisAcceleration.linear * curvature) / + (chassisVelocity.linear * chassisVelocity.linear)) + .let { if (it.isNaN()) 0.0 else it } + + return solveInverseDynamics( + wheelVelocity, + chassisVelocity, + wheelAcceleration, + chassisAcceleration, + curvature, + dcurvature + ) + } + + /** + * Solve inverse dynamics for torques and voltages + */ + fun solveInverseDynamics( + wheelVelocity: WheelState, + chassisVelocity: ChassisState, + wheelAcceleration: WheelState, + chassisAcceleration: ChassisState, + curvature: Double, + dcurvature: Double + ): DriveDynamics { + + // Determine the necessary torques on the left and right wheels to produce the desired wheel accelerations. + val wheelTorque = WheelState( + wheelRadius / 2.0 * (chassisAcceleration.linear * mass - + chassisAcceleration.angular * moi / effectiveWheelBaseRadius - + chassisVelocity.angular * angularDrag / effectiveWheelBaseRadius), + + wheelRadius / 2.0 * (chassisAcceleration.linear * mass + + chassisAcceleration.angular * moi / effectiveWheelBaseRadius + + chassisVelocity.angular * angularDrag / effectiveWheelBaseRadius) + ) + + // Solve for input voltages + val voltage = WheelState( + leftTransmission.getVoltageForTorque(wheelVelocity.left, wheelTorque.left), + rightTransmission.getVoltageForTorque(wheelVelocity.right, wheelTorque.right) + ) + + return DriveDynamics( + curvature, + dcurvature, + chassisVelocity, + chassisAcceleration, + wheelVelocity, + wheelAcceleration, + voltage, + wheelTorque + ) + } + + /** + * Solve for the max absolute velocity that the drivetrain is capable of given a max voltage and curvature + */ + @Suppress("ReturnCount") + fun getMaxAbsVelocity(curvature: Double, /*double dcurvature, */ maxAbsVoltage: Double): Double { + // Alternative implementation: + // (Tr - Tl) * r_wb / r_w = I * v^2 * dk + // (Tr + Tl) / r_w = 0 + // T = Tr = -Tl + // 2T * r_wb / r_w = I*v^2*dk + // T = 2*I*v^2*dk*r_w/r_wb + // T = kt*(-vR/kv + V) = -kt*(-vL/vmax + V) + // Vr = v * (1 + k*r_wb) + // 0 = 2*I*dk*r_w/r_wb * v^2 + kt * ((1 + k*r_wb) * v / kv) - kt * V + // solve using quadratic formula? + // -b +/- sqrt(b^2 - 4*a*c) / (2a) + + // k = w / v + // v = r_w*(wr + wl) / 2 + // w = r_w*(wr - wl) / (2 * r_wb) + // Plug in maxAbsVoltage for each wheel. + + val leftSpeedAtMaxVoltage = leftTransmission.getFreeSpeedAtVoltage(maxAbsVoltage) + val rightSpeedAtMaxVoltage = rightTransmission.getFreeSpeedAtVoltage(maxAbsVoltage) + + if (curvature epsilonEquals 0.0) { + return wheelRadius * Math.min(leftSpeedAtMaxVoltage, rightSpeedAtMaxVoltage) + } + if (curvature.isInfinite()) { + // Turn in place. Return value meaning becomes angular velocity. + val wheelSpeed = Math.min(leftSpeedAtMaxVoltage, rightSpeedAtMaxVoltage) + return Math.signum(curvature) * wheelRadius * wheelSpeed / effectiveWheelBaseRadius + } + + val rightSpeedIfLeftMax = + leftSpeedAtMaxVoltage * (effectiveWheelBaseRadius * curvature + 1.0) / + (1.0 - effectiveWheelBaseRadius * curvature) + + if (Math.abs(rightSpeedIfLeftMax) <= rightSpeedAtMaxVoltage + kEpsilon) { + // Left max is active constraint. + return wheelRadius * (leftSpeedAtMaxVoltage + rightSpeedIfLeftMax) / 2.0 + } + val leftSpeedIfRightMax = + rightSpeedAtMaxVoltage * (1.0 - effectiveWheelBaseRadius * curvature) / + (1.0 + effectiveWheelBaseRadius * curvature) + + // Right at max is active constraint. + return wheelRadius * (rightSpeedAtMaxVoltage + leftSpeedIfRightMax) / 2.0 + } + + data class MinMax(val min: Double, val max: Double) + + // Curvature is redundant here in the case that chassisVelocity is not purely angular. It is the responsibility of + // the caller to ensure that curvature = angular vel / linear vel in these cases. + @Suppress("ComplexMethod", "NestedBlockDepth") + fun getMinMaxAcceleration( + chassisVelocity: ChassisState, + curvature: Double, /*double dcurvature,*/ + maxAbsVoltage: Double + ): MinMax { + val wheelVelocities = solveInverseKinematics(chassisVelocity) + + var min = java.lang.Double.POSITIVE_INFINITY + var max = java.lang.Double.NEGATIVE_INFINITY + + // Math: + // (Tl + Tr) / r_w = m*a + // (Tr - Tl) / r_w * r_wb - drag*w = i*(a * k + v^2 * dk) + + // 2 equations, 2 unknowns. + // Solve for a and (Tl|Tr) + + val linearTerm = if (java.lang.Double.isInfinite(curvature)) 0.0 else mass * effectiveWheelBaseRadius + val angularTerm = if (java.lang.Double.isInfinite(curvature)) moi else moi * curvature + + val dragTorque = chassisVelocity.angular * angularDrag + + // Check all four cases and record the min and max valid accelerations. + for (left in Arrays.asList(false, true)) { + for (sign in Arrays.asList(1.0, -1.0)) { + + val fixedTransmission = if (left) leftTransmission else rightTransmission + val variableTransmission = if (left) rightTransmission else leftTransmission + val fixedTorque = fixedTransmission.getTorqueForVoltage(wheelVelocities[left], sign * maxAbsVoltage) + var variableTorque: Double + // NOTE: variable_torque is wrong. Units don't work out correctly. We made a math error somewhere... + // Leaving this "as is" for code release so as not to be disingenuous, but this whole function needs + // revisiting in the future... + variableTorque = if (left) { + (/*-moi * chassisVelocity.linear * chassisVelocity.linear * dcurvature*/ + -dragTorque * mass * wheelRadius + fixedTorque * (linearTerm + angularTerm)) / + (linearTerm - angularTerm) + } else { + (/*moi * chassisVelocity.linear * chassisVelocity.linear * dcurvature*/ + +dragTorque * mass * wheelRadius + fixedTorque * (linearTerm - angularTerm)) / + (linearTerm + angularTerm) + } + val variableVoltage = variableTransmission.getVoltageForTorque(wheelVelocities[!left], variableTorque) + if (Math.abs(variableVoltage) <= maxAbsVoltage + kEpsilon) { + val accel = if (java.lang.Double.isInfinite(curvature)) { + (if (left) -1.0 else 1.0) * (fixedTorque - variableTorque) * + effectiveWheelBaseRadius / (moi * wheelRadius) - dragTorque / moi + /*- chassisVelocity.linear * chassisVelocity.linear * dcurvature*/ + } else { + (fixedTorque + variableTorque) / (mass * wheelRadius) + } + min = Math.min(min, accel) + max = Math.max(max, accel) + } + } + } + return MinMax(min, max) + } + + /** + * Can refer to velocity or acceleration depending on context. + */ + class ChassisState { + var linear: Double = 0.toDouble() + var angular: Double = 0.toDouble() + + constructor(linear: Double, angular: Double) { + this.linear = linear + this.angular = angular + } + + constructor() + + override fun toString(): String { + val fmt = DecimalFormat("#0.000") + return fmt.format(linear) + ", " + fmt.format(angular) + } + + operator fun minus(other: ChassisState) = + ChassisState(this.linear - other.linear, this.angular - other.angular) + + operator fun times(scalar: Double) = ChassisState(linear * scalar, angular * scalar) + operator fun div(scalar: Double) = this * (1 / scalar) + } + + /** + * Can refer to velocity, acceleration, torque, voltage, etc., depending on context. + */ + class WheelState(val left: Double, val right: Double) { + constructor() : this(0.0, 0.0) + + operator fun get(isLeft: Boolean) = if (isLeft) left else right + + override fun toString(): String { + val fmt = DecimalFormat("#0.000") + return fmt.format(left) + ", " + fmt.format(right) + } + } + + /** + * Full state dynamics of the drivetrain + */ + class DriveDynamics( + val curvature: Double, // m^-1 + val dcurvature: Double, // m^-2 + val chassisVelocity: ChassisState, // m/s + val chassisAcceleration: ChassisState, // m/s^2 + val wheelVelocity: WheelState, // rad/s + val wheelAcceleration: WheelState, // rad/s^2 + val voltage: WheelState, // V + val wheelTorque: WheelState // N m + ) : CSVWritable { + override fun toCSV(): String { + return "$curvature,$dcurvature,$chassisVelocity, $chassisAcceleration, " + + "$wheelVelocity, $wheelAcceleration, $voltage, $wheelTorque" + } + } +} diff --git a/src/main/kotlin/org/team5419/fault/math/pid/PIDF.kt b/src/main/kotlin/org/team5419/fault/math/pid/PIDF.kt new file mode 100644 index 0000000..ddf0bdf --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/pid/PIDF.kt @@ -0,0 +1,137 @@ +package org.team5419.fault.math.pid + +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.util.DoubleSource +import kotlin.math.absoluteValue + +// adapted from: +// https://github.com/Team1323/2019DeepSpace/blob/13d5d4ac56c2da11d112ab9ab84241c8b6878018/src/main/java/com/team1323/lib/util/SynchronousPIDF.java +class PIDF( + var kP: Double = 0.0, + var kI: Double = 0.0, + var kD: Double = 0.0, + var kF: Double = 0.0, + private val feedbackVariable: DoubleSource +) { + + constructor( + kP: Double = 0.0, + kI: Double = 0.0, + kD: Double = 0.0, + feedbackVariable: DoubleSource + ): this(kP, kI, kD, 0.0, feedbackVariable) + + constructor( + kP: Double = 0.0, + kD: Double = 0.0, + feedbackVariable: DoubleSource + ): this(kP, 0.0, kD, 0.0, feedbackVariable) + + var setpoint = 0.0 + set(value) { + if (value == field) return + field = if (maximumInput > minimumInput) { + if (value > maximumInput) maximumInput + else if (value < minimumInput) minimumInput + else value + } else value + } + + var maximumOutput = 1.0 + private set + var minimumOutput = -1.0 + private set + + var maximumInput = 0.0 + private set + var minimumInput = 0.0 + private set + + var deadband = 0.0 + + var continuous = false + var inverted = false + + var error = 0.0 + private set + var output = 0.0 + private set + + private var lastError = 0.0 + private var lastInput = Double.NaN + private var accumulator = 0.0 + + @Suppress("ComplexMethod") + fun calculate(dt: Double): Double { + var newDt = dt + if (newDt < kEpsilon) newDt = kEpsilon + val input = feedbackVariable() + lastInput = input + error = setpoint - input + if (continuous) { + if (error.absoluteValue > (maximumInput - minimumInput) / 2.0) { + if (error > 0) { + error = error - maximumInput + minimumInput + } else { + error = error + maximumInput - minimumInput + } + } + } + if ((error * kP < maximumOutput) && (error * kP > minimumOutput)) { + accumulator += error * newDt + } else { + accumulator = 0.0 + } + + val propError = if (error.absoluteValue < deadband) 0.0 else error + + output = kP * propError + kI * accumulator + kD * (error - lastError) / dt + kF * setpoint + lastError = error + + if (output > maximumOutput) { + output = maximumOutput + } else if (output < minimumOutput) { + output = minimumOutput + } + + return output * if (inverted) -1.0 else 1.0 + } + + fun setPID(kP: Double, kI: Double, kD: Double) = setPID(kP, kI, kD, 0.0) + fun setPID(kP: Double, kI: Double, kD: Double, kF: Double) { + this.kP = kP + this.kI = kI + this.kD = kD + this.kF = kF + } + + fun setInputRange(min: Double, max: Double) { + if (min > max) { + println("ERROR: tried to set min input to greater than max input in PID") + return + } + minimumInput = min + maximumInput = max + } + + fun setOutputRange(min: Double, max: Double) { + if (min > max) { + println("ERROR: tried to set min output to greater than max output in PID") + return + } + minimumOutput = min + maximumOutput = max + } + + fun reset() { + lastInput = Double.NaN + lastError = 0.0 + output = 0.0 + setpoint = 0.0 + resetAccumulator() + } + + fun resetAccumulator() { + accumulator = 0.0 + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/splines/CubicHermiteSpline.kt b/src/main/kotlin/org/team5419/fault/math/splines/CubicHermiteSpline.kt similarity index 67% rename from src/main/kotlin/org/team5499/monkeyLib/math/splines/CubicHermiteSpline.kt rename to src/main/kotlin/org/team5419/fault/math/splines/CubicHermiteSpline.kt index 63b03d8..342a66f 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/splines/CubicHermiteSpline.kt +++ b/src/main/kotlin/org/team5419/fault/math/splines/CubicHermiteSpline.kt @@ -1,11 +1,12 @@ -package org.team5499.monkeyLib.math.splines +package org.team5419.fault.math.splines -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.units.meters @SuppressWarnings("MagicNumber") -class CubicHermiteSpline(p0: Vector2, h0: Rotation2d, p1: Vector2, h1: Rotation2d) : Spline() { +class CubicHermiteSpline(p0: Vector2d, h0: Rotation2d, p1: Vector2d, h1: Rotation2d) : Spline() { private val ax: Double private val bx: Double @@ -18,17 +19,17 @@ class CubicHermiteSpline(p0: Vector2, h0: Rotation2d, p1: Vector2, h1: Rotation2 private val dy: Double init { - val scale = 2.0 * p0.distanceTo(p1) + val scale = 2.0 * p0.distance(p1) - val x0 = p0.x - val x1 = p1.x - val dx0 = h0.cosAngle * scale - val dx1 = h1.cosAngle * scale + val x0 = p0.x.value + val x1 = p1.x.value + val dx0 = h0.cos * scale + val dx1 = h1.cos * scale - val y0 = p0.y - val y1 = p1.y - val dy0 = h0.sinAngle * scale - val dy1 = h1.sinAngle * scale + val y0 = p0.y.value + val y1 = p1.y.value + val dy0 = h0.sin * scale + val dy1 = h1.sin * scale ax = dx0 + dx1 + 2.0 * x0 - 2.0 * x1 bx = -2.0 * dx0 - dx1 - 3.0 * x0 + 3.0 * x1 @@ -44,10 +45,10 @@ class CubicHermiteSpline(p0: Vector2, h0: Rotation2d, p1: Vector2, h1: Rotation2 constructor(p0: Pose2d, p1: Pose2d): this(p0.translation, p0.rotation, p1.translation, p1.rotation) constructor(): this(Pose2d(), Pose2d()) - override fun getPoint(t: Double): Vector2 { + override fun getPoint(t: Double): Vector2d { val x = t * t * t * ax + t * t * bx + t * cx + dx val y = t * t * t * ay + t * t * by + t * cy + dy - return Vector2(x, y) + return Vector2d(x.meters, y.meters) } override fun getHeading(t: Double): Rotation2d { diff --git a/src/main/kotlin/org/team5419/fault/math/splines/FunctionalQuadraticSpline.kt b/src/main/kotlin/org/team5419/fault/math/splines/FunctionalQuadraticSpline.kt new file mode 100644 index 0000000..e17dc68 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/splines/FunctionalQuadraticSpline.kt @@ -0,0 +1,22 @@ +package org.team5419.fault.math.splines + +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +data class FunctionalQuadraticSpline( + private val p1: Vector2d, + private val p2: Vector2d, + private val p3: Vector2d +) { + + private val a: SIUnit> + get() = p3.x * (p2.y - p1.y) + p2.x * (p1.y - p3.y) + p1.x * (p3.y - p2.y) + private val b: SIUnit, Meter>> + get() = p3.x * p3.x * (p1.y - p2.y) + p2.x * p2.x * (p3.y - p1.y) + p1.x * p1.x * (p2.y - p3.y) + + val vertexXCoordinate: SIUnit get() = -b / (2.0 * a) +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/splines/QuinticHermiteSpline.kt b/src/main/kotlin/org/team5419/fault/math/splines/QuinticHermiteSpline.kt similarity index 79% rename from src/main/kotlin/org/team5499/monkeyLib/math/splines/QuinticHermiteSpline.kt rename to src/main/kotlin/org/team5419/fault/math/splines/QuinticHermiteSpline.kt index 83b96c7..b4a0728 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/splines/QuinticHermiteSpline.kt +++ b/src/main/kotlin/org/team5419/fault/math/splines/QuinticHermiteSpline.kt @@ -1,26 +1,29 @@ -package org.team5499.monkeyLib.math.splines +package org.team5419.fault.math.splines -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div // yes external contructors are disgusting. but they work @Suppress("FunctionNaming", "MagicNumber") -fun QuinticHermiteSpline(p0: Vector2, p1: Vector2, h0: Rotation2d, h1: Rotation2d): QuinticHermiteSpline { - val scale = 1.2 * p0.distanceTo(p1) +fun QuinticHermiteSpline(p0: Vector2d, p1: Vector2d, h0: Rotation2d, h1: Rotation2d): QuinticHermiteSpline { + val scale = 1.2 * p0.distance(p1) - val x0 = p0.x - val x1 = p1.x - val dx0 = h0.cosAngle * scale - val dx1 = h1.cosAngle * scale + val x0 = p0.x.value + val x1 = p1.x.value + val dx0 = h0.cos * scale + val dx1 = h1.cos * scale val ddx0 = 0.0 val ddx1 = 0.0 // y vars - val y0 = p0.y - val y1 = p1.y - val dy0 = h0.sinAngle * scale - val dy1 = h1.sinAngle * scale + val y0 = p0.y.value + val y1 = p1.y.value + val dy0 = h0.sin * scale + val dy1 = h1.sin * scale val ddy0 = 0.0 val ddy1 = 0.0 @@ -111,14 +114,14 @@ class QuinticHermiteSpline( } val startPose: Pose2d - get() = Pose2d(Vector2(x0, y0), Rotation2d(dx0, dy0, true)) + get() = Pose2d(Vector2d(x0.meters, y0.meters), Rotation2d(dx0, dy0, true)) val endPose: Pose2d - get() = Pose2d(Vector2(x1, y1), Rotation2d(dx1, dy1, true)) + get() = Pose2d(Vector2d(x1.meters, y1.meters), Rotation2d(dx1, dy1, true)) - override fun getPoint(t: Double): Vector2 { + override fun getPoint(t: Double): Vector2d { val x = ax * t * t * t * t * t + bx * t * t * t * t + cx * t * t * t + dx * t * t + ex * t + fx val y = ay * t * t * t * t * t + by * t * t * t * t + cy * t * t * t + dy * t * t + ey * t + fy - return Vector2(x, y) + return Vector2d(x.meters, y.meters) } private fun dx(t: Double) = 5 * ax * t * t * t * t + 4 * bx * t * t * t + 3 * cx * t * t + 2 * dx * t + ex @@ -154,7 +157,7 @@ class QuinticHermiteSpline( override fun getHeading(t: Double) = Rotation2d(dx(t), dy(t), true) - public fun sumDCurvature2(): Double { + fun sumDCurvature2(): Double { val dt = 1.0 / kSamples var sum = 0.0 var t = 0.0 @@ -241,11 +244,11 @@ class QuinticHermiteSpline( } magnitude = Math.sqrt(magnitude) - val p2 = Vector2(0.0, sumDCurvature2(splines)) + val p2 = Vector2d(0.0.meters, sumDCurvature2(splines).meters) for (i in 0..splines.size - 2) { - if (splines.get(i).startPose.isColinear(splines.get(i + 1).startPose) && - splines.get(i).endPose.isColinear(splines.get(i + 1).endPose)) { + if (splines.get(i).startPose.isCollinear(splines.get(i + 1).startPose) && + splines.get(i).endPose.isCollinear(splines.get(i + 1).endPose)) { continue } controlPoints[i]!!.ddx *= (kStepSize / magnitude) @@ -260,11 +263,11 @@ class QuinticHermiteSpline( splines.get(i + 1).calcCoeffs() } - val p1 = Vector2(-kStepSize, sumDCurvature2(splines)) + val p1 = Vector2d(-kStepSize.meters, sumDCurvature2(splines).meters) for (i in 0..splines.size - 2) { - if (splines.get(i).startPose.isColinear(splines.get(i + 1).startPose) && - splines.get(i).endPose.isColinear(splines.get(i + 1).endPose)) { + if (splines.get(i).startPose.isCollinear(splines.get(i + 1).startPose) && + splines.get(i).endPose.isCollinear(splines.get(i + 1).endPose)) { continue } splines.get(i).ddx1 += (2.0 * controlPoints[i]!!.ddx) @@ -277,16 +280,19 @@ class QuinticHermiteSpline( splines.get(i + 1).calcCoeffs() } - val p3 = Vector2(kStepSize, sumDCurvature2(splines)) + val p3 = Vector2d(kStepSize.meters, sumDCurvature2(splines).meters) + + val stepSize = FunctionalQuadraticSpline( + p1, p2, p3 + ).vertexXCoordinate - val stepSize = fitParabola(p1, p2, p3) for (i in 0..splines.size - 2) { - if (splines.get(i).startPose.isColinear(splines.get(i + 1).startPose) && - splines.get(i).endPose.isColinear(splines.get(i + 1).endPose)) { + if (splines.get(i).startPose.isCollinear(splines.get(i + 1).startPose) && + splines.get(i).endPose.isCollinear(splines.get(i + 1).endPose)) { continue } - controlPoints[i]!!.ddx *= (1.0 + stepSize / kStepSize) - controlPoints[i]!!.ddy *= (1.0 + stepSize / kStepSize) + controlPoints[i]!!.ddx *= (1.0 + stepSize.value / kStepSize) + controlPoints[i]!!.ddy *= (1.0 + stepSize.value / kStepSize) splines.get(i).ddx1 += controlPoints[i]!!.ddx splines.get(i).ddy1 += controlPoints[i]!!.ddy @@ -298,21 +304,5 @@ class QuinticHermiteSpline( splines.get(i + 1).calcCoeffs() } } - - fun fitParabola(p1: Vector2, p2: Vector2, p3: Vector2): Double { - // println("$p1 - $p2 - $p3") - val t1 = p3.x * (p2.y - p1.y) - val t2 = p2.x * (p1.y - p3.y) - val t3 = p1.x * (p3.y - p2.y) - val A = (t1 + t2 + t3) - - val t4 = (p3.x * p3.x) * (p1.y - p2.y) - val t5 = (p2.x * p2.x) * (p3.y - p1.y) - val t6 = (p1.x * p1.x) * (p2.y - p3.y) - val B = (t4 + t5 + t6) - // println("$t1, $t2, $t3, $t4, $t5, $t6") - // println("a:$A, b:$B, result: ${-B / (2 * A)}") - return -B / (2 * A) - } } } diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/splines/Spline.kt b/src/main/kotlin/org/team5419/fault/math/splines/Spline.kt similarity index 60% rename from src/main/kotlin/org/team5499/monkeyLib/math/splines/Spline.kt rename to src/main/kotlin/org/team5419/fault/math/splines/Spline.kt index acb4c7f..49b4417 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/splines/Spline.kt +++ b/src/main/kotlin/org/team5419/fault/math/splines/Spline.kt @@ -1,13 +1,13 @@ -package org.team5499.monkeyLib.math.splines +package org.team5419.fault.math.splines -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Pose2dWithCurvature abstract class Spline { - abstract fun getPoint(t: Double): Vector2 + abstract fun getPoint(t: Double): Vector2d abstract fun getHeading(t: Double): Rotation2d diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/splines/SplineGenerator.kt b/src/main/kotlin/org/team5419/fault/math/splines/SplineGenerator.kt similarity index 57% rename from src/main/kotlin/org/team5499/monkeyLib/math/splines/SplineGenerator.kt rename to src/main/kotlin/org/team5419/fault/math/splines/SplineGenerator.kt index da6c4ac..6b76bff 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/splines/SplineGenerator.kt +++ b/src/main/kotlin/org/team5419/fault/math/splines/SplineGenerator.kt @@ -1,24 +1,23 @@ -package org.team5499.monkeyLib.math.splines +package org.team5419.fault.math.splines -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Radian @SuppressWarnings("LongParameterList") object SplineGenerator { - private const val kMaxDX = 2.0 - private const val kMaxDY = 0.05 - private const val kMaxDTheta = 0.1 private const val kMinSampleSize = 1 fun parameterizeSpline( s: Spline, - maxDx: Double, - maxDy: Double, - maxDTheta: Double, - t0: Double, - t1: Double + maxDx: SIUnit, + maxDy: SIUnit, + maxDTheta: SIUnit, + t0: Double = 0.0, + t1: Double = 1.0 ): MutableList { val rv: MutableList = mutableListOf() rv.add(s.getPoseWithCurvature(0.0)) @@ -31,24 +30,20 @@ object SplineGenerator { return rv } - fun parameterizeSpline(s: Spline): MutableList { - return parameterizeSpline(s, kMaxDX, kMaxDY, kMaxDTheta, 0.0, 1.0) - } - fun parameterizeSpline( s: Spline, - maxDx: Double, - maxDy: Double, - maxDTheta: Double + maxDx: SIUnit, + maxDy: SIUnit, + maxDTheta: SIUnit ): MutableList { return parameterizeSpline(s, maxDx, maxDy, maxDTheta, 0.0, 1.0) } fun parameterizeSplines( splines: List, - maxDx: Double, - maxDy: Double, - maxDTheta: Double + maxDx: SIUnit, + maxDy: SIUnit, + maxDTheta: SIUnit ): MutableList { val rv: MutableList = mutableListOf() if (splines.isEmpty()) return rv @@ -61,34 +56,26 @@ object SplineGenerator { return rv } - fun parameterizeSplines(splines: List): MutableList { - return parameterizeSplines(splines, kMaxDX, kMaxDY, kMaxDTheta) - } - - fun parameterizeSplines(vararg s: Spline): MutableList { - return parameterizeSplines(s.toMutableList()) - } - private fun getSegmentArc( s: Spline, rv: MutableList, t0: Double, t1: Double, - maxDx: Double, - maxDy: Double, - maxDTheta: Double + maxDx: SIUnit, + maxDy: SIUnit, + maxDTheta: SIUnit ) { val p0 = s.getPoint(t0) val p1 = s.getPoint(t1) val r0 = s.getHeading(t0) val r1 = s.getHeading(t1) - val transformation = Pose2d(Vector2(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse())) - val twist = Pose2d.log(transformation) + val transformation = Pose2d((p1 - p0) * -r0, r1 + -r0) +// val transformation = Pose2d(Vector2(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse())) + val twist = transformation.twist if (twist.dy > maxDy || twist.dx > maxDx || twist.dTheta > maxDTheta) { getSegmentArc(s, rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta) getSegmentArc(s, rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta) } else { - // println(twist) rv.add(s.getPoseWithCurvature(t1)) } } diff --git a/src/main/kotlin/org/team5419/fault/math/units/Ampere.kt b/src/main/kotlin/org/team5419/fault/math/units/Ampere.kt new file mode 100644 index 0000000..a02fbe8 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/Ampere.kt @@ -0,0 +1,11 @@ +package org.team5419.fault.math.units + +object Ampere : SIKey + +val Double.amps get() = SIUnit(this) + +val Number.amps get() = toDouble().amps + +fun SIUnit.inMilliamps() = value.div(kMilli) +fun SIUnit.inMicroamps() = value.div(kMicro) +fun SIUnit.inAmps() = value diff --git a/src/main/kotlin/org/team5419/fault/math/units/Kilogram.kt b/src/main/kotlin/org/team5419/fault/math/units/Kilogram.kt new file mode 100644 index 0000000..31a1207 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/Kilogram.kt @@ -0,0 +1,23 @@ +@file:SuppressWarnings("TopLevelPropertyNaming") +package org.team5419.fault.math.units + +object Kilogram : SIKey + +const val kCentiOffsetKilo = 1e-5 +const val kBaseOffsetKilo = 1e-3 +const val kMilliOffsetKilo = 1e-6 +const val kLbOffsetKilo = 0.453592 + +val Double.lbs get() = pounds +val Double.pounds get() = SIUnit(times(kLbOffsetKilo)) + +val Number.lbs get() = toDouble().lbs +val Number.pounds get() = toDouble().pounds + +fun SIUnit.inKilograms() = value +fun SIUnit.inGrams() = value.div(kBaseOffsetKilo) +fun SIUnit.inCentigrams() = value.div(kCentiOffsetKilo) +fun SIUnit.inMilligrams() = value.div(kMilliOffsetKilo) + +fun SIUnit.inLbs() = inPounds() +fun SIUnit.inPounds() = value.div(kLbOffsetKilo) diff --git a/src/main/kotlin/org/team5419/fault/math/units/Meter.kt b/src/main/kotlin/org/team5419/fault/math/units/Meter.kt new file mode 100644 index 0000000..51e17d6 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/Meter.kt @@ -0,0 +1,34 @@ +@file:SuppressWarnings("TopLevelPropertyNaming", "TooManyFunctions") +package org.team5419.fault.math.units + +object Meter : SIKey + +const val kInchToMeter = 0.0254 +const val kFeetToMeter = kInchToMeter * 12.0 +const val kThouToMeter = kInchToMeter * 0.001 +const val kYardToMeter = kFeetToMeter * 3.0 + +val Double.meters get() = SIUnit(this) +val Double.thou get() = SIUnit(times(kThouToMeter)) +val Double.inches get() = SIUnit(times(kInchToMeter)) +val Double.feet get() = SIUnit(times(kFeetToMeter)) +val Double.yards get() = SIUnit(times(kYardToMeter)) + +val Number.meters get() = toDouble().meters +val Number.thou get() = toDouble().thou +val Number.inches get() = toDouble().inches +val Number.feet get() = toDouble().feet + +fun SIUnit.inThou() = value.div(kThouToMeter) +fun SIUnit.inInches() = value.div(kInchToMeter) +fun SIUnit.inFeet() = value.div(kFeetToMeter) +fun SIUnit.inYards() = value.div(kYardToMeter) + +fun SIUnit.inKilometers() = value.div(kKilo) +fun SIUnit.inHectometers() = value.div(kHecto) +fun SIUnit.inDecameters() = value.div(kDeca) +fun SIUnit.inMeters() = value +fun SIUnit.inDecimeters() = value.div(kDeci) +fun SIUnit.inCentimeters() = value.div(kCenti) +fun SIUnit.inMillimeters() = value.div(kMilli) +fun SIUnit.inMicrometers() = value.div(kMicro) diff --git a/src/main/kotlin/org/team5419/fault/math/units/SIUnit.kt b/src/main/kotlin/org/team5419/fault/math/units/SIUnit.kt new file mode 100644 index 0000000..e5b7c09 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/SIUnit.kt @@ -0,0 +1,75 @@ +@file:SuppressWarnings("TopLevelPropertyNaming", "TooManyFunctions") +package org.team5419.fault.math.units + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.lerp +import org.team5419.fault.math.units.derived.Ohm +import org.team5419.fault.math.units.derived.Volt +import org.team5419.fault.math.units.derived.Watt +import kotlin.math.absoluteValue + +// SI Constants +const val kKilo = 1e3 +const val kHecto = 1e2 +const val kDeca = 1e1 +const val kDeci = 1e-1 +const val kCenti = 1e-2 +const val kMilli = 1e-3 +const val kMicro = 1e-6 +const val kNano = 1e-9 + +// SI Unit +inline class SIUnit(val value: Double) : Comparable> { + + val absoluteValue get() = SIUnit(value.absoluteValue) + + operator fun unaryMinus() = SIUnit(-value) + operator fun plus(other: SIUnit) = SIUnit(value.plus(other.value)) + operator fun minus(other: SIUnit) = SIUnit(value.minus(other.value)) + operator fun times(other: Double) = SIUnit(value.times(other)) + operator fun div(other: Double) = SIUnit(value.div(other)) + operator fun times(other: Number) = times(other.toDouble()) + operator fun div(other: Number) = div(other.toDouble()) + + override operator fun compareTo(other: SIUnit) = value.compareTo(other.value) + operator fun compareTo(other: Double) = value.compareTo(other) + operator fun compareTo(other: Number) = compareTo(other.toDouble()) + + fun lerp(endValue: SIUnit, t: Double) = SIUnit(value.lerp(endValue.value, t)) + infix fun epsilonEquals(other: SIUnit) = value.epsilonEquals(other.value) +} + +val Double.kilo get() = SIUnitBuilder(times(kKilo)) +val Double.deca get() = SIUnitBuilder(times(kDeca)) +val Double.base get() = SIUnitBuilder(this) +val Double.deci get() = SIUnitBuilder(times(kDeci)) +val Double.centi get() = SIUnitBuilder(times(kCenti)) +val Double.milli get() = SIUnitBuilder(times(kMilli)) +val Double.micro get() = SIUnitBuilder(times(kMicro)) + +val Number.kilo get() = toDouble().kilo +val Number.deca get() = toDouble().deca +val Number.base get() = toDouble().base +val Number.deci get() = toDouble().deci +val Number.centi get() = toDouble().centi +val Number.milli get() = toDouble().milli +val Number.micro get() = toDouble().micro + +inline class SIUnitBuilder(private val value: Double) { + val seconds get() = SIUnit(value) + val meters get() = SIUnit(value) + val grams get() = SIUnit(value.times(kBaseOffsetKilo)) + val amps get() = SIUnit(value) + val ohms get() = SIUnit(value) + val volts get() = SIUnit(value) + val watts get() = SIUnit(value) +} + +interface SIKey + +class Mult : SIKey +class Frac : SIKey + +object Unitless : SIKey + +val SIUnit.unitlessValue get() = value diff --git a/src/main/kotlin/org/team5419/fault/math/units/Second.kt b/src/main/kotlin/org/team5419/fault/math/units/Second.kt new file mode 100644 index 0000000..1c60eb5 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/Second.kt @@ -0,0 +1,32 @@ +@file:SuppressWarnings("TooManyFunctions") +package org.team5419.fault.math.units + +object Second : SIKey + +@Suppress("TopLevelPropertyNaming") +const val kMinuteToSecond = 60.0 +@Suppress("TopLevelPropertyNaming") +const val kHourToSecond = kMinuteToSecond * 60.0 + +val Double.seconds get() = SIUnit(this) +val Double.minutes get() = SIUnit(times(kMinuteToSecond)) +val Double.hours get() = SIUnit(times(kHourToSecond)) +val Double.milliseconds get() = SIUnit(div(1000.0)) + +val Number.seconds get() = toDouble().seconds +val Number.minutes get() = toDouble().minutes +val Number.hours get() = toDouble().hours +val Number.milliseconds get() = toDouble().milliseconds + +fun SIUnit.inMinutes() = value.div(kMinuteToSecond) +fun SIUnit.inHours() = value.div(kHourToSecond) + +fun SIUnit.inKiloseconds() = value.div(kKilo) +fun SIUnit.inHectoseconds() = value.div(kHecto) +fun SIUnit.inDecaseconds() = value.div(kDeca) +fun SIUnit.inSeconds() = value +fun SIUnit.inDeciseconds() = value.div(kDeci) +fun SIUnit.inCentiseconds() = value.div(kCenti) +fun SIUnit.inMilliseconds() = value.div(kMilli) +fun SIUnit.inMicroseconds() = value.div(kMicro) +fun SIUnit.inNanoseconds() = value.div(kNano) diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Acceleration.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Acceleration.kt new file mode 100644 index 0000000..bdb341a --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Acceleration.kt @@ -0,0 +1,13 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +typealias Acceleration = Frac, Second> +typealias LinearAcceleration = Acceleration +typealias AngularAcceleration = Acceleration + +val SIUnit.acceleration get() = SIUnit>(value) diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Inverse.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Inverse.kt new file mode 100644 index 0000000..fa3f185 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Inverse.kt @@ -0,0 +1,8 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Unitless + +typealias Inverse = Frac +typealias Curvature = Inverse diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Ohm.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Ohm.kt new file mode 100644 index 0000000..2a9cf5d --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Ohm.kt @@ -0,0 +1,16 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.Kilogram +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.Ampere +import org.team5419.fault.math.units.SIUnit + +typealias Ohm = Frac>, + Mult>>>> + +val Double.ohms get() = SIUnit(this) + +val Number.ohms get() = toDouble().ohms diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Radian.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Radian.kt new file mode 100644 index 0000000..99a1c85 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Radian.kt @@ -0,0 +1,22 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Unitless + +typealias Radian = Unitless + +val Double.radians get() = SIUnit(this) +val Double.degrees get() = SIUnit(Math.toRadians(this)) + +val Number.radians get() = toDouble().radians +val Number.degrees get() = toDouble().degrees + +fun SIUnit.inRadians() = value +fun SIUnit.inDegrees() = Math.toDegrees(value) + +fun SIUnit.toRotation2d() = Rotation2d(this) + +val SIUnit.cos get() = Math.cos(value) +val SIUnit.sin get() = Math.sin(value) +val SIUnit.tan get() = Math.tan(value) diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Velocity.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Velocity.kt new file mode 100644 index 0000000..c7059b3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Velocity.kt @@ -0,0 +1,19 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.kMinuteToSecond +import org.team5419.fault.math.units.kFeetToMeter +import org.team5419.fault.math.units.kInchToMeter +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +typealias Velocity = Frac +typealias LinearVelocity = Velocity +typealias AngularVelocity = Velocity + +val SIUnit.velocity get() = SIUnit>(value) +fun SIUnit.inFeetPerSecond() = value.div(kFeetToMeter) +fun SIUnit.inFeetPerMinute() = inFeetPerSecond().times(kMinuteToSecond) +fun SIUnit.inInchesPerSecond() = value.div(kInchToMeter) diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Volt.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Volt.kt new file mode 100644 index 0000000..47d649c --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Volt.kt @@ -0,0 +1,16 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.Kilogram +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.Ampere +import org.team5419.fault.math.units.SIUnit + +typealias Volt = Frac>, + Mult>>> + +val Double.volts get() = SIUnit(this) + +val Number.volts get() = toDouble().volts diff --git a/src/main/kotlin/org/team5419/fault/math/units/derived/Watt.kt b/src/main/kotlin/org/team5419/fault/math/units/derived/Watt.kt new file mode 100644 index 0000000..9d3cc32 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/derived/Watt.kt @@ -0,0 +1,13 @@ +package org.team5419.fault.math.units.derived + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.Kilogram +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.SIUnit + +typealias Watt = Frac>, Mult>> + +val Double.watts get() = SIUnit(this) +val Number.watts get() = toDouble().watts diff --git a/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnit.kt b/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnit.kt new file mode 100644 index 0000000..35b8ef9 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnit.kt @@ -0,0 +1,68 @@ +package org.team5419.fault.math.units.native + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Acceleration +import org.team5419.fault.math.units.derived.Velocity + +object NativeUnit : SIKey + +val Double.nativeUnits get() = SIUnit(this) +val Double.STU get() = nativeUnits + +val Number.nativeUnits get() = toDouble().nativeUnits +val Number.STU get() = toDouble().STU + +fun SIUnit.toNativeUnitPosition(model: NativeUnitModel): SIUnit = + model.toNativeUnitPosition(this) + +fun SIUnit.fromNativeUnitPosition(model: NativeUnitModel): SIUnit = + model.fromNativeUnitPosition(this) + +typealias NativeUnitVelocity = Velocity + +val Double.nativeUnitsPer100ms get() = SIUnit(times(10.0)) +val Double.STUPer100ms get() = nativeUnitsPer100ms + +val Number.nativeUnitsPer100ms get() = toDouble().nativeUnitsPer100ms +val Number.STUPer100ms get() = toDouble().STUPer100ms + +@Deprecated("", ReplaceWith("inNativeUnitsPer100ms()")) +val SIUnit.nativeUnitsPer100ms + get() = inNativeUnitsPer100ms() +@Deprecated("", ReplaceWith("inSTUPer100ms()")) +val SIUnit.STUPer100ms + get() = inSTUPer100ms() + +fun SIUnit.inNativeUnitsPer100ms() = value.div(10.0) +fun SIUnit.inSTUPer100ms() = inNativeUnitsPer100ms() + +fun SIUnit>.toNativeUnitVelocity(model: NativeUnitModel): SIUnit = + model.toNativeUnitVelocity(this) + +fun SIUnit.fromNativeUnitVelocity(model: NativeUnitModel): SIUnit> = + model.fromNativeUnitVelocity(this) + +typealias NativeUnitAcceleration = Acceleration + +val Double.nativeUnitsPer100msPerSecond get() = SIUnit(times(10.0)) +val Double.STUPer100msPerSecond get() = nativeUnitsPer100msPerSecond + +val Number.nativeUnitsPer100msPerSecond get() = toDouble().nativeUnitsPer100msPerSecond +val Number.STUPer100msPerSecond get() = toDouble().STUPer100msPerSecond + +@Deprecated("", ReplaceWith("inNativeUnitsPer100msPerSecond()")) +val SIUnit.nativeUnitsPer100msPerSecond + get() = inNativeUnitsPer100msPerSecond() +@Deprecated("", ReplaceWith("inSTUPer100msPerSecond()")) +val SIUnit.STUPer100msPerSecond + get() = inSTUPer100msPerSecond() + +fun SIUnit.inNativeUnitsPer100msPerSecond() = value.div(10.0) +fun SIUnit.inSTUPer100msPerSecond() = inNativeUnitsPer100msPerSecond() + +fun SIUnit>.toNativeUnitAcceleration(model: NativeUnitModel): + SIUnit = model.toNativeUnitAcceleration(this) + +fun SIUnit.fromNativeUnitAcceleration(model: NativeUnitModel): + SIUnit> = model.fromNativeUnitAcceleration(this) diff --git a/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnitModel.kt b/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnitModel.kt new file mode 100644 index 0000000..fcf0915 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/native/NativeUnitModel.kt @@ -0,0 +1,74 @@ +package org.team5419.fault.math.units.native + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Acceleration +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +abstract class NativeUnitModel { + + abstract fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit + abstract fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit + + open fun toNativeUnitError(modelledUnit: SIUnit): SIUnit = + toNativeUnitPosition(modelledUnit) - toNativeUnitPosition(SIUnit(0.0)) + + open fun fromNativeUnitVelocity(nativeUnitVelocity: SIUnit): SIUnit> = + SIUnit(fromNativeUnitPosition(SIUnit(nativeUnitVelocity.value)).value) + + open fun toNativeUnitVelocity(modelledUnitVelocity: SIUnit>): SIUnit = + SIUnit(toNativeUnitPosition(SIUnit(modelledUnitVelocity.value)).value) + + open fun fromNativeUnitAcceleration(nativeUnitAcceleration: SIUnit): + SIUnit> = SIUnit(fromNativeUnitVelocity(SIUnit(nativeUnitAcceleration.value)).value) + + open fun toNativeUnitAcceleration(modelledUnitAcceleration: SIUnit>): + SIUnit = SIUnit(toNativeUnitVelocity(SIUnit(modelledUnitAcceleration.value)).value) +} + +object DefaultNativeUnitModel : NativeUnitModel() { + override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = nativeUnits + override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = modelledUnit +} + +class NativeUnitLengthModel( + val nativeUnitsPerRotation: SIUnit, + val wheelRadius: SIUnit +) : NativeUnitModel() { + override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = + wheelRadius * ((nativeUnits / nativeUnitsPerRotation) * (2.0 * Math.PI)) + + override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = + nativeUnitsPerRotation * (modelledUnit / (wheelRadius * (2.0 * Math.PI))) +} + +class NativeUnitRotationModel( + val nativeUnitsPerRotation: SIUnit +) : NativeUnitModel() { + override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = + (modelledUnit / (2.0 * Math.PI)) * nativeUnitsPerRotation + + override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = + 2.0 * Math.PI * (nativeUnits / nativeUnitsPerRotation) +} + +class SlopeNativeUnitModel( + val modelledSample: SIUnit, + val nativeUnitSample: SIUnit +) : NativeUnitModel() { + private val slope: SIUnit> = modelledSample / nativeUnitSample + + override fun fromNativeUnitPosition(nativeUnits: SIUnit): SIUnit = + nativeUnits * slope + + override fun toNativeUnitPosition(modelledUnit: SIUnit): SIUnit = + modelledUnit / slope +} + +fun SlopeNativeUnitModel.wheelRadius(sensorUnitsPerRotation: SIUnit): SIUnit = + modelledSample / (nativeUnitSample / sensorUnitsPerRotation) / 2.0 / Math.PI diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/ADivFracABEqualsB.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/ADivFracABEqualsB.kt new file mode 100644 index 0000000..79df52c --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/ADivFracABEqualsB.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun SIUnit.div(other: SIUnit>) = SIUnit(value.div(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/ADivUnitlessEqualsA.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/ADivUnitlessEqualsA.kt new file mode 100644 index 0000000..7372e8e --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/ADivUnitlessEqualsA.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Unitless + +operator fun SIUnit.div(other: SIUnit) = SIUnit(value.div(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesBEqualsATimesB.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesBEqualsATimesB.kt new file mode 100644 index 0000000..99c3385 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesBEqualsATimesB.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun SIUnit.times(other: SIUnit) = SIUnit>(value.times(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesFracBAEqualsB.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesFracBAEqualsB.kt new file mode 100644 index 0000000..7d583d0 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesFracBAEqualsB.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun SIUnit.times(other: SIUnit>) = SIUnit(value.times(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesUnitlessEqualsA.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesUnitlessEqualsA.kt new file mode 100644 index 0000000..1656ebe --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/ATimesUnitlessEqualsA.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Unitless + +operator fun SIUnit.times(other: SIUnit) = SIUnit(value.times(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/AdivAEqualsUnitless.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/AdivAEqualsUnitless.kt new file mode 100644 index 0000000..0879af2 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/AdivAEqualsUnitless.kt @@ -0,0 +1,7 @@ +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Unitless + +operator fun SIUnit.div(other: SIUnit) = SIUnit(value.div(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/AdivBEqualsAdivB.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/AdivBEqualsAdivB.kt new file mode 100644 index 0000000..8bd62bd --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/AdivBEqualsAdivB.kt @@ -0,0 +1,7 @@ +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun SIUnit.div(other: SIUnit) = SIUnit>(value.div(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/DoubleTimesAEqualsA.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/DoubleTimesAEqualsA.kt new file mode 100644 index 0000000..e85da23 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/DoubleTimesAEqualsA.kt @@ -0,0 +1,14 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun Double.times(other: SIUnit) = other.times(this) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/FracABTimesBEqualsA.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/FracABTimesBEqualsA.kt new file mode 100644 index 0000000..ec042e2 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/FracABTimesBEqualsA.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Frac +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +operator fun SIUnit>.times(other: SIUnit) = SIUnit(value.times(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/MultABDivAEqualsB.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/MultABDivAEqualsB.kt new file mode 100644 index 0000000..42ce9a0 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/MultABDivAEqualsB.kt @@ -0,0 +1,20 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.Mult +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit + +// m^3 / m^2 = m + +// SIUnit, Meter> / SIUnit> = SIUnit +// SIUnit> / SIUnit = SIUnit + +operator fun SIUnit>.div(other: SIUnit) = SIUnit(value.div(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/math/units/operations/UnitlessTimesAEqualsA.kt b/src/main/kotlin/org/team5419/fault/math/units/operations/UnitlessTimesAEqualsA.kt new file mode 100644 index 0000000..1252e01 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/math/units/operations/UnitlessTimesAEqualsA.kt @@ -0,0 +1,15 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + * + * Copyright 2019, Green Hope Falcons + */ + +package org.team5419.fault.math.units.operations + +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Unitless + +operator fun SIUnit.times(other: SIUnit) = SIUnit(value.times(other.value)) diff --git a/src/main/kotlin/org/team5419/fault/simulation/SimulatedBerkeliumMotor.kt b/src/main/kotlin/org/team5419/fault/simulation/SimulatedBerkeliumMotor.kt new file mode 100644 index 0000000..77c18d5 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/simulation/SimulatedBerkeliumMotor.kt @@ -0,0 +1,80 @@ +package org.team5419.fault.simulation + +import org.team5419.fault.hardware.BerkeliumEncoder +import org.team5419.fault.hardware.BerkeliumMotor +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIKey +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Acceleration +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.Velocity +import org.team5419.fault.math.units.derived.Volt +import org.team5419.fault.math.units.native.NativeUnit +import org.team5419.fault.math.units.native.NativeUnitVelocity + +typealias LinearSimulatedBerkeleiumMotor = SimulatedBerkeliumMotor +typealias AngularSimulatedBerkeleiumMotor = SimulatedBerkeliumMotor + +class SimulatedBerkeliumMotor : BerkeliumMotor { + + var velocity = SIUnit>(0.0) + override val voltageOutput = SIUnit(0.0) + + override val encoder = object : BerkeliumEncoder { + override val position: SIUnit get() = SIUnit(0.0) + override val velocity: SIUnit> get() = this@SimulatedBerkeliumMotor.velocity + override val rawPosition: SIUnit get() = SIUnit(position.value) + override val rawVelocity: SIUnit get() = SIUnit(velocity.value) + + override fun resetPosition(newPosition: SIUnit) {} + override fun resetPositionRaw(newPosition: SIUnit) {} + } + + override var outputInverted: Boolean + get() = TODO("not implemented") + set(value) {} + + override var brakeMode: Boolean + get() = TODO("not implemented") + set(value) {} + + override fun follow(motor: BerkeliumMotor<*>): Boolean { + TODO("not implemented") + } + + override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) { + TODO("not implemented") + } + + override fun setPercent(percent: Double, arbitraryFeedForward: SIUnit) { + TODO("not implemented") + } + + override fun setVelocity(velocity: SIUnit>, arbitraryFeedForward: SIUnit) { + this.velocity = velocity + } + + override fun setPosition(position: SIUnit, arbitraryFeedForward: SIUnit) { + TODO("not implemented") + } + + override fun setNeutral() { + velocity = SIUnit(0.0) + } + + override var voltageCompSaturation: SIUnit + get() = TODO("not implemented") + set(value) {} + + override var motionProfileCruiseVelocity: SIUnit> + get() = TODO("not implemented") + set(value) {} + + override var motionProfileAcceleration: SIUnit> + get() = TODO("not implemented") + set(value) {} + + override var useMotionProfileForPosition: Boolean + get() = TODO("not implemented") + set(value) {} +} diff --git a/src/main/kotlin/org/team5419/fault/simulation/SimulatedDifferentialDrive.kt b/src/main/kotlin/org/team5419/fault/simulation/SimulatedDifferentialDrive.kt new file mode 100644 index 0000000..ec752f4 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/simulation/SimulatedDifferentialDrive.kt @@ -0,0 +1,38 @@ +package org.team5419.fault.simulation + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Twist2d +import org.team5419.fault.math.physics.DifferentialDrive +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.derived.radians +import org.team5419.fault.math.units.meters +import org.team5419.fault.subsystems.drivetrain.IDifferentialFollowerDrive +import org.team5419.fault.trajectory.followers.TrajectoryFollower + +class SimulatedDifferentialDrive( + override val differentialDrive: DifferentialDrive, + override val leftMasterMotor: LinearSimulatedBerkeleiumMotor, + override val rightMasterMotor: LinearSimulatedBerkeleiumMotor, + override val trajectoryFollower: TrajectoryFollower, + private val angularFactor: Double = 1.0 + +) : IDifferentialFollowerDrive { + + override var robotPosition = Pose2d() + + fun update(deltaTime: SIUnit) { + val wheelState = DifferentialDrive.WheelState( + (leftMasterMotor.velocity * deltaTime.value / differentialDrive.wheelRadius).value, + (rightMasterMotor.velocity * deltaTime.value / differentialDrive.wheelRadius).value + ) + + val forwardKinematics = differentialDrive.solveForwardKinematics(wheelState) + + robotPosition += Twist2d( + forwardKinematics.linear.meters, + 0.meters, + (forwardKinematics.angular * angularFactor).radians + ).asPose + } +} diff --git a/src/main/kotlin/org/team5419/fault/subsystems/Subsystem.kt b/src/main/kotlin/org/team5419/fault/subsystems/Subsystem.kt new file mode 100644 index 0000000..d6d2714 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/subsystems/Subsystem.kt @@ -0,0 +1,45 @@ +package org.team5419.fault.subsystems + +import org.team5419.fault.util.loops.ILooper +import java.util.concurrent.atomic.AtomicLong + +object SubsystemManager { + + private val subsystems = mutableListOf() + + fun addSubsystem(subsystem: Subsystem) { + print("Adding ${subsystem.name} to subsystem manager.") + subsystems.add(subsystem) + } + + fun periodic() = subsystems.forEach { it.periodic() } + + fun init() = subsystems.forEach { it.init() } + + fun autoReset() = subsystems.forEach { it.autoReset() } + + fun teleopReset() = subsystems.forEach { it.teleopReset() } + + fun zeroOutputs() = subsystems.forEach { it.zeroOutputs() } +} + +abstract class Subsystem(name: String? = null) { + + companion object { + private val subsystemId = AtomicLong() + } + + val name = name ?: "Subsystem ${subsystemId.incrementAndGet()}" + + open fun periodic() {} + + open fun init() {} + + open fun autoReset() {} + + open fun teleopReset() {} + + open fun zeroOutputs() {} + + open fun registerLoops(looper: ILooper) {} +} diff --git a/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/AbstractTankDrive.kt b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/AbstractTankDrive.kt new file mode 100644 index 0000000..8822f5b --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/AbstractTankDrive.kt @@ -0,0 +1,51 @@ +package org.team5419.fault.subsystems.drivetrain + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Rotation2d +import org.team5419.fault.math.localization.PositionTracker +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.AngularVelocity +import org.team5419.fault.math.units.derived.LinearVelocity +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.Volt +import org.team5419.fault.subsystems.Subsystem +import kotlin.properties.Delegates + +abstract class AbstractTankDrive : IDifferentialFollowerDrive, Subsystem() { + + protected abstract val localization: PositionTracker + + override var robotPosition: Pose2d + get() = localization.robotPosition + set(value) = localization.reset(value) + + var isBraking by Delegates.observable(false) { _, _, wantBrake -> + leftMasterMotor.brakeMode = wantBrake + rightMasterMotor.brakeMode = wantBrake + } + + abstract val leftDistance: SIUnit + abstract val rightDistance: SIUnit + abstract val leftDistanceError: SIUnit + abstract val rightDistanceError: SIUnit + + abstract val angularVelocity: SIUnit + abstract val turnError: SIUnit + + override fun zeroOutputs() { + setPercent(0.0, 0.0) + } + + abstract fun setPercent(left: Double, right: Double) + abstract fun setTurn(angle: Rotation2d, type: TurnType = TurnType.Relative) + abstract fun setPosition(distance: SIUnit) + abstract fun setVelocity( + leftVelocity: SIUnit, + rightVelocity: SIUnit, + leftFF: SIUnit, + rightFF: SIUnit + ) + + enum class TurnType { Relative, Absolute } +} diff --git a/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/CharacterizationData.kt b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/CharacterizationData.kt new file mode 100644 index 0000000..26a75f9 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/CharacterizationData.kt @@ -0,0 +1,12 @@ +package org.team5419.fault.subsystems.drivetrain + +import org.team5419.fault.util.CSVWritable + +data class CharacterizationData( + val voltage: Double, // volts + val velocity: Double, // rads / s + val dt: Double // s +) : CSVWritable { + + override fun toCSV() = "$voltage,$velocity,$dt" +} diff --git a/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/IDifferentialFollowerDrive.kt b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/IDifferentialFollowerDrive.kt new file mode 100644 index 0000000..101dba3 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/IDifferentialFollowerDrive.kt @@ -0,0 +1,49 @@ +package org.team5419.fault.subsystems.drivetrain + +import org.team5419.fault.math.physics.DifferentialDrive +import org.team5419.fault.math.units.derived.velocity +import org.team5419.fault.math.units.derived.volts +import org.team5419.fault.math.units.meters +import org.team5419.fault.trajectory.followers.TrajectoryFollowerOutput + +interface IDifferentialFollowerDrive : ITrajectoryFollowingDrive { + + val differentialDrive: DifferentialDrive + + override fun setOutput(output: TrajectoryFollowerOutput) { + setOutputFromDynamics( + output.differentialDriveVelocity, + output.differentialDriveAcceleration + ) + } + + fun setOutputFromKinematics(chassisVelocity: DifferentialDrive.ChassisState) { + val wheelVelocities = differentialDrive.solveInverseKinematics(chassisVelocity) + val feedForwardVoltages = differentialDrive.getVoltagesFromkV(wheelVelocities) + + setOutput(wheelVelocities, feedForwardVoltages) + } + + fun setOutputFromDynamics( + chassisVelocity: DifferentialDrive.ChassisState, + chassisAcceleration: DifferentialDrive.ChassisState + ) { + val dynamics = differentialDrive.solveInverseDynamics(chassisVelocity, chassisAcceleration) + + setOutput(dynamics.wheelVelocity, dynamics.voltage) + } + + fun setOutput( + wheelVelocities: DifferentialDrive.WheelState, + wheelVoltages: DifferentialDrive.WheelState + ) { + leftMasterMotor.setVelocity( + (wheelVelocities.left * differentialDrive.wheelRadius).meters.velocity, + wheelVoltages.left.volts + ) + rightMasterMotor.setVelocity( + (wheelVelocities.right * differentialDrive.wheelRadius).meters.velocity, + wheelVoltages.right.volts + ) + } +} diff --git a/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/ITrajectoryFollowingDrive.kt b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/ITrajectoryFollowingDrive.kt new file mode 100644 index 0000000..9309131 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/subsystems/drivetrain/ITrajectoryFollowingDrive.kt @@ -0,0 +1,23 @@ +package org.team5419.fault.subsystems.drivetrain + +import org.team5419.fault.hardware.LinearBerkeliumMotor +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.trajectory.followers.TrajectoryFollower +import org.team5419.fault.trajectory.followers.TrajectoryFollowerOutput + +interface ITrajectoryFollowingDrive { + + val leftMasterMotor: LinearBerkeliumMotor + val rightMasterMotor: LinearBerkeliumMotor + + val trajectoryFollower: TrajectoryFollower + + val robotPosition: Pose2d + + fun setOutput(output: TrajectoryFollowerOutput) + + fun zeroOutputs() { + leftMasterMotor.setNeutral() + rightMasterMotor.setNeutral() + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryGenerator.kt b/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryGenerator.kt new file mode 100644 index 0000000..179b6e4 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryGenerator.kt @@ -0,0 +1,341 @@ +package org.team5419.fault.trajectory + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Vector2 +import org.team5419.fault.math.geometry.State + +import org.team5419.fault.trajectory.constraints.TimingConstraint +import org.team5419.fault.trajectory.types.TimedTrajectory +import org.team5419.fault.trajectory.types.TimedEntry +import org.team5419.fault.trajectory.types.IndexedTrajectory +import org.team5419.fault.trajectory.types.DistanceTrajectory + +import org.team5419.fault.math.splines.QuinticHermiteSpline +import org.team5419.fault.math.splines.Spline +import org.team5419.fault.math.splines.SplineGenerator +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit + +import org.team5419.fault.math.units.derived.LinearAcceleration +import org.team5419.fault.math.units.derived.LinearVelocity +import org.team5419.fault.math.units.derived.Radian +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.util.Oof + +import kotlin.math.absoluteValue +import kotlin.math.pow + +val DefaultTrajectoryGenerator = TrajectoryGenerator( + 2.0.inches, + 0.25.inches, + 5.0.degrees +) + +public class TrajectoryGenerator( + val maxDx: SIUnit, + val maxDy: SIUnit, + val maxDTheta: SIUnit // degrees +) { + + @Suppress("LongParameterList") + fun generateTrajectory( + waypoints: List, + constraints: List>, + startVelocity: SIUnit, + endVelocity: SIUnit, + maxVelocity: SIUnit, + maxAcceleration: SIUnit, + reversed: Boolean, + optimizeSplines: Boolean = true + ): TimedTrajectory { + val flippedPosition = Pose2d(Vector2(), 180.0.degrees) + val newWaypoints = waypoints.asSequence().map { point -> + if (reversed) point + flippedPosition else point + } + var trajectory = trajectoryFromSplineWaypoints(newWaypoints, optimizeSplines).points + if (reversed) { + trajectory = trajectory.map { state -> + Pose2dWithCurvature( + pose = state.pose + flippedPosition, + curvature = -state.curvature, + dkds = state.dkds + ) + } + } + + return timeParameterizeTrajectory( + DistanceTrajectory(trajectory), + constraints, + startVelocity.value, + endVelocity.value, + maxVelocity.value, + maxAcceleration.value.absoluteValue, + maxDx.value, + reversed + ) + } + + private fun trajectoryFromSplineWaypoints( + wayPoints: Sequence, + optimizeSplines: Boolean + ): IndexedTrajectory { + val splines = wayPoints.zipWithNext { a, b -> QuinticHermiteSpline(a, b) }.toMutableList() + + if (optimizeSplines) QuinticHermiteSpline.optimizeSpline(splines) + + return trajectoryFromSplines(splines) + } + + private fun trajectoryFromSplines( + splines: List + ) = IndexedTrajectory( + SplineGenerator.parameterizeSplines( + splines, + maxDx, + maxDy, + maxDTheta + ) + ) + + @Suppress( + "LongParameterList", + "LongMethod", + "ComplexMethod", + "NestedBlockDepth", + "ComplexCondition", + "TooGenericExceptionThrown", + "ThrowsCount" + ) + private fun > timeParameterizeTrajectory( + distanceTrajectory: DistanceTrajectory, + constraints: List>, + startVelocity: Double, + endVelocity: Double, + maxVelocity: Double, + maxAcceleration: Double, + stepSize: Double, + reversed: Boolean + ): TimedTrajectory { + + class ConstrainedState> { + lateinit var state: S + var distance: Double = 0.0 + var maxVelocity: Double = 0.0 + var minAcceleration: Double = 0.0 + var maxAcceleration: Double = 0.0 + + override fun toString(): String { + return state.toString() + ", distance: " + distance + ", maxVelocity: " + maxVelocity + ", " + + "minAcceleration: " + minAcceleration + ", maxAcceleration: " + maxAcceleration + } + } + + fun enforceAccelerationLimits( + reverse: Boolean, + constraints: List>, + constraintState: ConstrainedState + ) { + + for (constraint in constraints) { + val minMaxAccel = constraint.getMinMaxAcceleration( + constraintState.state, + (if (reverse) -1.0 else 1.0) * constraintState.maxVelocity + ) + if (!minMaxAccel.valid) { + throw RuntimeException() + } + constraintState.minAcceleration = Math.max( + constraintState.minAcceleration, + if (reverse) -minMaxAccel.maxAcceleration else minMaxAccel.minAcceleration + ) + constraintState.maxAcceleration = Math.min( + constraintState.maxAcceleration, + if (reverse) -minMaxAccel.minAcceleration else minMaxAccel.maxAcceleration + ) + } + } + + val distanceViewRange = + distanceTrajectory.firstInterpolant.value..distanceTrajectory.lastInterpolant.value + val distanceViewSteps = + Math.ceil((distanceTrajectory.lastInterpolant.value - distanceTrajectory.firstInterpolant.value) / + stepSize + 1).toInt() + + val states = (0 until distanceViewSteps).map { step -> + distanceTrajectory.sample( + (step * stepSize + distanceTrajectory.firstInterpolant.value).coerceIn( + distanceViewRange + ) + ) + .state + } + + val constraintStates = ArrayList>(states.size) + val epsilon = 1E-6 + + var predecessor = ConstrainedState() + predecessor.state = states[0] + predecessor.distance = 0.0 + predecessor.maxVelocity = startVelocity + predecessor.minAcceleration = -maxAcceleration + predecessor.maxAcceleration = maxAcceleration + + for (i in states.indices) { + // Add the new state. + constraintStates.add(ConstrainedState()) + val constraintState = constraintStates[i] + constraintState.state = states[i] + val ds = constraintState.state.distance(predecessor.state) + constraintState.distance = ds + predecessor.distance + + // We may need to iterate to find the maximum end velocity and common acceleration, since acceleration + // limits may be a function of velocity. + while (true) { + // Enforce global max velocity and max reachable velocity by global acceleration limit. + // vf = sqrt(vi^2 + 2*a*d) + constraintState.maxVelocity = Math.min( + maxVelocity, + Math.sqrt(predecessor.maxVelocity.pow(2) + 2.0 * predecessor.maxAcceleration * ds) + ) + if (constraintState.maxVelocity.isNaN()) { + throw RuntimeException() + } + // Enforce global max absolute acceleration. + constraintState.minAcceleration = -maxAcceleration + constraintState.maxAcceleration = maxAcceleration + + // At this point, the state is full constructed, but no constraints have been applied aside from + // predecessor + // state max accel. + + // Enforce all velocity constraints. + for (constraint in constraints) { + constraintState.maxVelocity = Math.min( + constraintState.maxVelocity, + constraint.getMaxVelocity(constraintState.state) + ) + } + if (constraintState.maxVelocity < 0.0) { + // This should never happen if constraints are well-behaved. + throw RuntimeException() + } + + // Now enforce all acceleration constraints. + enforceAccelerationLimits(reversed, constraints, constraintState) + if (constraintState.minAcceleration > constraintState.maxAcceleration) { + // This should never happen if constraints are well-behaved. + throw RuntimeException() + } + + if (ds < epsilon) { + break + } + + // If the max acceleration for this constraint state is more conservative than what we had applied, we + // need to reduce the max accel at the predecessor state and try again. + val actualAcceleration = + (constraintState.maxVelocity.pow(2) - predecessor.maxVelocity.pow(2)) / (2.0 * ds) + if (constraintState.maxAcceleration < actualAcceleration - epsilon) { + predecessor.maxAcceleration = constraintState.maxAcceleration + } else { + if (actualAcceleration > predecessor.minAcceleration + epsilon) { + predecessor.maxAcceleration = actualAcceleration + } + // If actual acceleration is less than predecessor min accel, we will repair during the backward + // pass. + break + } + // System.out.println("(intermediate) i: " + i + ", " + constraint_state.toString()); + } + // System.out.println("i: " + i + ", " + constraint_state.toString()); + predecessor = constraintState + } + + var successor = ConstrainedState() + successor.state = states[states.size - 1] + successor.distance = constraintStates[states.size - 1].distance + successor.maxVelocity = endVelocity + successor.minAcceleration = -maxAcceleration + successor.maxAcceleration = maxAcceleration + + for (i in states.indices.reversed()) { + val constraintState = constraintStates[i] + val ds = constraintState.distance - successor.distance // will be negative. + + while (true) { + // Enforce reverse max reachable velocity limit. + // vf = sqrt(vi^2 + 2*a*d), where vi = successor. + val newMaxVelocity = Math.sqrt(successor.maxVelocity.pow(2) + 2.0 * successor.minAcceleration * ds) + if (newMaxVelocity >= constraintState.maxVelocity) { + // No new limits to impose. + break + } + constraintState.maxVelocity = newMaxVelocity + if (java.lang.Double.isNaN(constraintState.maxVelocity)) { + throw RuntimeException() + } + + // Now check all acceleration constraints with the lower max velocity. + enforceAccelerationLimits(reversed, constraints, constraintState) + if (constraintState.minAcceleration > constraintState.maxAcceleration) { + throw RuntimeException() + } + + if (ds > epsilon) { + break + } + // If the min acceleration for this constraint state is more conservative than what we have applied, we + // need to reduce the min accel and try again. + val actualAcceleration = + (constraintState.maxVelocity.pow(2) - successor.maxVelocity.pow(2)) / (2.0 * ds) + if (constraintState.minAcceleration > actualAcceleration + epsilon) { + successor.minAcceleration = constraintState.minAcceleration + } else { + successor.minAcceleration = actualAcceleration + break + } + } + successor = constraintState + } + val timedStates = ArrayList>(states.size) + var t = 0.0 + var s = 0.0 + var v = 0.0 + for (i in states.indices) { + val constrainedState = constraintStates[i] + // Advance t. + val ds = constrainedState.distance - s + val accel = (constrainedState.maxVelocity.pow(2) - v.pow(2)) / (2.0 * ds) + var dt = 0.0 + if (i > 0) { + timedStates[i - 1] = timedStates[i - 1].copy( + acceleration = SIUnit(if (reversed) -accel else accel) + ) + + dt = when { + Math.abs(accel) > epsilon -> (constrainedState.maxVelocity - v) / accel + Math.abs(v) > epsilon -> ds / v + else -> throw RuntimeException() + } + } + t += dt + if (t.isNaN() || t.isInfinite()) { + throw Oof("FUCK!") + } + + v = constrainedState.maxVelocity + s = constrainedState.distance + timedStates.add( + TimedEntry( + constrainedState.state, + SIUnit(t), + SIUnit(if (reversed) -v else v), + SIUnit(if (reversed) -accel else accel) + ) + ) + } + return TimedTrajectory(timedStates, reversed) + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryIterator.kt b/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryIterator.kt new file mode 100644 index 0000000..14beefb --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/TrajectoryIterator.kt @@ -0,0 +1,30 @@ +package org.team5419.fault.trajectory + +import org.team5419.fault.math.geometry.State +import org.team5419.fault.trajectory.types.Trajectory +import org.team5419.fault.trajectory.types.TrajectorySamplePoint + +abstract class TrajectoryIterator, S : State>( + val trajectory: Trajectory +) { + + protected abstract fun addition(a: U, b: U): U + + var progress = trajectory.firstInterpolant + private var sample = trajectory.sample(progress) + + val isDone: Boolean + get() = progress >= trajectory.lastInterpolant + val currentState get() = sample + + fun advance(amount: U): TrajectorySamplePoint { + progress = addition(progress, amount).coerceIn(trajectory.firstInterpolant, trajectory.lastInterpolant) + sample = trajectory.sample(progress) + return sample + } + + fun preview(amount: U): TrajectorySamplePoint { + progress = addition(progress, amount).coerceIn(trajectory.firstInterpolant, trajectory.lastInterpolant) + return trajectory.sample(progress) + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/AngularAccelerationConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/AngularAccelerationConstraint.kt new file mode 100644 index 0000000..c82c207 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/AngularAccelerationConstraint.kt @@ -0,0 +1,34 @@ +package org.team5419.fault.trajectory.constraints + +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.AngularAcceleration + +import kotlin.math.absoluteValue +import kotlin.math.sqrt +import kotlin.math.abs + +class AngularAccelerationConstraint internal constructor( + val maxAngularAcceleration: SIUnit +) : TimingConstraint { + + init { + require(maxAngularAcceleration > 0.0) { + "Cannot have a negative angular acceleration" + } + } + + override fun getMaxVelocity(state: Pose2dWithCurvature): Double { + return sqrt(maxAngularAcceleration.value / state.dkds.absoluteValue) + } + + override fun getMinMaxAcceleration( + state: Pose2dWithCurvature, + velocity: Double + ): TimingConstraint.MinMaxAcceleration { + val maxAbsoluteAcceleration = abs( + (maxAngularAcceleration.value - (velocity * velocity * state.dkds)) / state.curvature + ) + return TimingConstraint.MinMaxAcceleration(-maxAbsoluteAcceleration, maxAbsoluteAcceleration) + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/CentripetalAccelerationConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/CentripetalAccelerationConstraint.kt new file mode 100644 index 0000000..e2e2f46 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/CentripetalAccelerationConstraint.kt @@ -0,0 +1,21 @@ +package org.team5419.fault.trajectory.constraints + +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.LinearAcceleration + +import kotlin.math.absoluteValue +import kotlin.math.sqrt + +class CentripetalAccelerationConstraint internal constructor( + private val maxCentripetalAcceleration: SIUnit +) : TimingConstraint { + + override fun getMaxVelocity(state: Pose2dWithCurvature) = + sqrt((maxCentripetalAcceleration.value / state.curvature).absoluteValue) + + override fun getMinMaxAcceleration( + state: Pose2dWithCurvature, + velocity: Double + ) = TimingConstraint.MinMaxAcceleration.noLimits +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/DifferentialDriveDynamicsConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/DifferentialDriveDynamicsConstraint.kt new file mode 100644 index 0000000..6220c12 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/DifferentialDriveDynamicsConstraint.kt @@ -0,0 +1,27 @@ +package org.team5419.fault.trajectory.constraints + +import org.team5419.fault.math.physics.DifferentialDrive +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.Volt + +class DifferentialDriveDynamicsConstraint internal constructor( + private val drive: DifferentialDrive, + private val maxVoltage: SIUnit +) : TimingConstraint { + + override fun getMaxVelocity(state: Pose2dWithCurvature) = + drive.getMaxAbsVelocity(state.curvature, maxVoltage.value) + + override fun getMinMaxAcceleration( + state: Pose2dWithCurvature, + velocity: Double + ): TimingConstraint.MinMaxAcceleration { + val minMax = drive.getMinMaxAcceleration( + DifferentialDrive.ChassisState(velocity, velocity * state.curvature), + state.curvature, + maxVoltage.value + ) + return TimingConstraint.MinMaxAcceleration(minMax.min, minMax.max) + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/TimingConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/TimingConstraint.kt new file mode 100644 index 0000000..233ca7c --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/TimingConstraint.kt @@ -0,0 +1,20 @@ +package org.team5419.fault.trajectory.constraints + +interface TimingConstraint { + fun getMaxVelocity(state: S): Double + + fun getMinMaxAcceleration(state: S, velocity: Double): MinMaxAcceleration + + data class MinMaxAcceleration( + val minAcceleration: Double, + val maxAcceleration: Double + ) { + constructor(): this(Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY) + + val valid = minAcceleration <= maxAcceleration + + companion object { + val noLimits = MinMaxAcceleration() + } + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRadiusConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRadiusConstraint.kt new file mode 100644 index 0000000..8054de5 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRadiusConstraint.kt @@ -0,0 +1,22 @@ +package org.team5419.fault.trajectory.constraints + +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.LinearVelocity + +class VelocityLimitRadiusConstraint internal constructor( + private val point: Vector2d, + private val radius: SIUnit, + private val velocityLimit: SIUnit +) : TimingConstraint { + + override fun getMaxVelocity(state: Pose2dWithCurvature) = + if (state.pose.translation.distance(point) <= radius.value) velocityLimit.value else Double.POSITIVE_INFINITY + + override fun getMinMaxAcceleration( + state: Pose2dWithCurvature, + velocity: Double + ) = TimingConstraint.MinMaxAcceleration.noLimits +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRegionConstraint.kt b/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRegionConstraint.kt new file mode 100644 index 0000000..b39641b --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/constraints/VelocityLimitRegionConstraint.kt @@ -0,0 +1,18 @@ +package org.team5419.fault.trajectory.constraints + +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Rectangle2d +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.derived.LinearVelocity + +public class VelocityLimitRegionConstraint internal constructor( + private val region: Rectangle2d, + private val velocityLimit: SIUnit +) : TimingConstraint { + + override fun getMaxVelocity(state: Pose2dWithCurvature) = + if (state.pose.translation in region) velocityLimit.value else Double.POSITIVE_INFINITY + + override fun getMinMaxAcceleration(state: Pose2dWithCurvature, velocity: Double) = + TimingConstraint.MinMaxAcceleration.noLimits +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/followers/PurePursuitFollower.kt b/src/main/kotlin/org/team5419/fault/trajectory/followers/PurePursuitFollower.kt new file mode 100644 index 0000000..73fb551 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/followers/PurePursuitFollower.kt @@ -0,0 +1,97 @@ +package org.team5419.fault.trajectory.followers + +import org.team5419.fault.trajectory.TrajectoryIterator +import org.team5419.fault.trajectory.types.TimedEntry + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Vector2 +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.meters + +import kotlin.math.pow + +class PurePursuitFollower( + private val kLat: Double, + private val kLookaheadTime: SIUnit, + private val kMinLookaheadDistance: SIUnit = 18.inches // inches +) : TrajectoryFollower() { + + override fun calculateState( + iterator: TrajectoryIterator, TimedEntry>, + robotPose: Pose2d + ): TrajectoryFollower.TrajectoryFollowerVelocityOutput { + val referencePoint = iterator.currentState + + // Compute the lookahead state. + val lookaheadState: Pose2d = calculateLookaheadPose2d(iterator, robotPose) + + // Find the appropriate lookahead point. + val lookaheadTransform = lookaheadState inFrameOfReferenceOf robotPose + + // Calculate latitude error. + val xError = (referencePoint.state.state.pose inFrameOfReferenceOf robotPose).translation.x.value + + // Calculate the velocity at the reference point. + val vd = referencePoint.state.velocity.value + + // Calculate the distance from the robot to the lookahead. + val l = lookaheadTransform.translation.norm.value + + // Calculate the curvature of the arc that connects the robot and the lookahead point. + val curvature = 2 * lookaheadTransform.translation.y.value / l.pow(2) + + // Adjust the linear velocity to compensate for the robot lagging behind. + val adjustedLinearVelocity = vd * lookaheadTransform.rotation.cos + kLat * xError + + return TrajectoryFollower.TrajectoryFollowerVelocityOutput( + linearVelocity = SIUnit(adjustedLinearVelocity), + // v * curvature = omega + angularVelocity = SIUnit(adjustedLinearVelocity * curvature) + ) + } + + @Suppress("ReturnCount") + private fun calculateLookaheadPose2d( + iterator: TrajectoryIterator, TimedEntry>, + robotPose: Pose2d + ): Pose2d { + val lookaheadPoseByTime = iterator.preview(kLookaheadTime).state.state.pose + + // The lookahead point is farther from the robot than the minimum lookahead distance. + // Therefore we can use this point. + if ((lookaheadPoseByTime inFrameOfReferenceOf robotPose).translation.norm >= kMinLookaheadDistance.value) { + return lookaheadPoseByTime + } + + var lookaheadPoseByDistance = iterator.currentState.state.state.pose + var previewedTime = 0.seconds + + // Run the loop until a distance that is greater than the minimum lookahead distance is found or until + // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory. + while (iterator.progress > previewedTime) { + previewedTime += 0.02.seconds + + lookaheadPoseByDistance = iterator.preview(previewedTime).state.state.pose + val lookaheadDistance = (lookaheadPoseByDistance inFrameOfReferenceOf robotPose).translation.norm + + if (lookaheadDistance > kMinLookaheadDistance.value) { + return lookaheadPoseByDistance + } + } + + // Extend the trajectory. + val remaining = + kMinLookaheadDistance - (lookaheadPoseByDistance inFrameOfReferenceOf robotPose).translation.norm + + return lookaheadPoseByDistance.transformBy( + Pose2d( + Vector2(remaining * if (iterator.trajectory.reversed) -1 else 1, 0.0.meters) + ) + ) + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/followers/RamseteFollower.kt b/src/main/kotlin/org/team5419/fault/trajectory/followers/RamseteFollower.kt new file mode 100644 index 0000000..479e83b --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/followers/RamseteFollower.kt @@ -0,0 +1,54 @@ +package org.team5419.fault.trajectory.followers + +import org.team5419.fault.trajectory.TrajectoryIterator +import org.team5419.fault.trajectory.types.TimedEntry +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Pose2d + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +import kotlin.math.sqrt +import kotlin.math.sin + +class RamseteFollower( + private val kBeta: Double, + private val kZeta: Double +) : TrajectoryFollower() { + + override fun calculateState( + iterator: TrajectoryIterator, TimedEntry>, + robotPose: Pose2d + ): TrajectoryFollower.TrajectoryFollowerVelocityOutput { + val referenceState = iterator.currentState.state + + // Calculate goal in robot's coordinates + val error = referenceState.state.pose inFrameOfReferenceOf robotPose + + // Get reference linear and angular velocities + val vd = referenceState.velocity.value + val wd = vd * referenceState.state.curvature + + // Compute gain + val k1 = 2 * kZeta * sqrt(wd * wd + kBeta * vd * vd) + + // Get angular error in bounded radians + val angleError = error.rotation.radian + + return TrajectoryFollower.TrajectoryFollowerVelocityOutput( + linearVelocity = SIUnit(vd * error.rotation.cos + k1 * error.translation.x.value), + angularVelocity = SIUnit(wd + kBeta * vd * sinc(angleError.value) * + error.translation.y.value + k1 * angleError.value) + ) + } + + companion object { + private fun sinc(theta: Double) = + if (theta epsilonEquals 0.0) { + 1.0 - 1.0 / 6.0 * theta * theta + } else sin(theta) / theta + } +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/followers/TrajectoryFollower.kt b/src/main/kotlin/org/team5419/fault/trajectory/followers/TrajectoryFollower.kt new file mode 100644 index 0000000..b34585e --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/followers/TrajectoryFollower.kt @@ -0,0 +1,101 @@ +@file:Suppress("ConstructorParameterNaming") +package org.team5419.fault.trajectory.followers + +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.physics.DifferentialDrive +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.derived.AngularVelocity +import org.team5419.fault.math.units.derived.LinearVelocity +import org.team5419.fault.math.units.derived.LinearAcceleration +import org.team5419.fault.math.units.derived.AngularAcceleration +import org.team5419.fault.math.units.derived.acceleration +import org.team5419.fault.math.units.derived.radians +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.milliseconds +import org.team5419.fault.math.units.operations.div +import org.team5419.fault.trajectory.TrajectoryIterator +import org.team5419.fault.trajectory.types.TimedEntry +import org.team5419.fault.trajectory.types.Trajectory +import org.team5419.fault.util.time.DeltaTime + +abstract class TrajectoryFollower { + + private var trajectoryIterator: TrajectoryIterator, TimedEntry>? = null + private var deltaTimeController = DeltaTime() + private var previousVelocity: TrajectoryFollowerVelocityOutput? = null + + val referencePoint get() = trajectoryIterator?.currentState + val isFinished get() = trajectoryIterator?.isDone ?: true + + fun reset(trajectory: Trajectory, TimedEntry>) { + trajectoryIterator = trajectory.iterator() + deltaTimeController.reset() + previousVelocity = null + } + + fun nextState( + currentRobotPose: Pose2d, + currentTime: SIUnit = System.currentTimeMillis().toDouble().milliseconds + ): TrajectoryFollowerOutput { + val iterator = trajectoryIterator + require(iterator != null) { + "You cannot get the next state from the TrajectoryTracker without a" + + "trajectory! Call TrajectoryTracker#reset first!" + } + val deltaTime = deltaTimeController.updateTime(currentTime) + iterator.advance(deltaTime) + + val velocity = calculateState(iterator, currentRobotPose) + val previousVelocity = this.previousVelocity + this.previousVelocity = velocity + + // Calculate Acceleration (useful for drive dynamics) + return if (previousVelocity == null || deltaTime.value <= 0) { + TrajectoryFollowerOutput( + linearVelocity = velocity.linearVelocity, + linearAcceleration = 0.0.meters.acceleration, + angularVelocity = velocity.angularVelocity, + angularAcceleration = 0.0.radians.acceleration + ) + } else { + TrajectoryFollowerOutput( + linearVelocity = velocity.linearVelocity, + linearAcceleration = (velocity.linearVelocity - previousVelocity.linearVelocity) / deltaTime, + angularVelocity = velocity.angularVelocity, + angularAcceleration = (velocity.angularVelocity - previousVelocity.angularVelocity) / deltaTime + ) + } + } + + protected abstract fun calculateState( + iterator: TrajectoryIterator, TimedEntry>, + robotPose: Pose2d + ): TrajectoryFollowerVelocityOutput + + protected data class TrajectoryFollowerVelocityOutput constructor( + val linearVelocity: SIUnit, + val angularVelocity: SIUnit + ) +} + +data class TrajectoryFollowerOutput constructor( + val linearVelocity: SIUnit, + val linearAcceleration: SIUnit, + val angularVelocity: SIUnit, + val angularAcceleration: SIUnit +) { + + val differentialDriveVelocity + get() = DifferentialDrive.ChassisState( + linearVelocity.value, + angularVelocity.value + ) + + val differentialDriveAcceleration + get() = DifferentialDrive.ChassisState( + linearAcceleration.value, + angularAcceleration.value + ) +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/types/DistanceTrajectory.kt b/src/main/kotlin/org/team5419/fault/trajectory/types/DistanceTrajectory.kt new file mode 100644 index 0000000..3e7612e --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/types/DistanceTrajectory.kt @@ -0,0 +1,64 @@ +package org.team5419.fault.trajectory.types + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.geometry.State +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.meters +import org.team5419.fault.trajectory.TrajectoryIterator + +// distance and state +class DistanceTrajectory>( + override val points: List +) : Trajectory, S> { + + private val distances: List + + init { + val tempDistances = mutableListOf() + tempDistances += 0.0 + points.zipWithNext { + c, n -> tempDistances += tempDistances.last() + c.distance(n) + } + distances = tempDistances + } + + override fun sample(interpolant: SIUnit) = sample(interpolant.value) + + fun sample(interpolant: Double) = when { + interpolant >= lastInterpolant.value -> TrajectorySamplePoint(getPoint(points.size - 1)) + interpolant <= 0.0 -> TrajectorySamplePoint(getPoint(0)) + else -> { + val (index, entry) = points.asSequence() + .withIndex() + .first { (index, _) -> index != 0 && distances[index] >= interpolant } + val prevEntry = points[index - 1] + if (distances[index] epsilonEquals distances[index - 1]) { + TrajectorySamplePoint(entry, index, index) + } else { + TrajectorySamplePoint( + prevEntry.interpolate( + entry, + (interpolant - distances[index - 1]) / (distances[index] - distances[index - 1]) + ), + index - 1, + index + ) + } + } + } + + override val firstState get() = points.first() + override val lastState get() = points.last() + + override val firstInterpolant get() = 0.0.meters + override val lastInterpolant get() = distances.last().toDouble().meters + + override fun iterator() = DistanceIterator(this) +} + +class DistanceIterator>( + trajectory: DistanceTrajectory +) : TrajectoryIterator, S>(trajectory) { + override fun addition(a: SIUnit, b: SIUnit) = a + b +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/types/IndexedTrajectory.kt b/src/main/kotlin/org/team5419/fault/trajectory/types/IndexedTrajectory.kt new file mode 100644 index 0000000..0bcae19 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/types/IndexedTrajectory.kt @@ -0,0 +1,41 @@ +package org.team5419.fault.trajectory.types + +import org.team5419.fault.trajectory.TrajectoryIterator +import org.team5419.fault.math.geometry.State + +class IndexedTrajectory>( + override val points: List +) : Trajectory { + override fun sample(interpolant: Double) = when { + points.isEmpty() -> throw IndexOutOfBoundsException("Trajectory is empty!") + interpolant <= 0.0 -> TrajectorySamplePoint(getPoint(0)) + interpolant >= points.size - 1 -> TrajectorySamplePoint(getPoint(points.size - 1)) + else -> { + val index = Math.floor(interpolant).toInt() + val percent = interpolant - index + when { + percent <= Double.MIN_VALUE -> TrajectorySamplePoint(getPoint(index)) + percent >= 1.0 - Double.MIN_VALUE -> TrajectorySamplePoint(getPoint(index + 1)) + else -> TrajectorySamplePoint( + points[index].interpolate(points[index], percent), + index, + index + 1 + ) + } + } + } + + override val firstState get() = points.first() + override val lastState get() = points.last() + + override val firstInterpolant get() = 0.0 + override val lastInterpolant get() = (points.size - 1.0).coerceAtLeast(0.0) + + override fun iterator() = IndexedIterator(this) +} + +class IndexedIterator>( + trajectory: IndexedTrajectory +) : TrajectoryIterator(trajectory) { + override fun addition(a: Double, b: Double) = a + b +} diff --git a/src/main/kotlin/org/team5419/fault/trajectory/types/TimedTrajectory.kt b/src/main/kotlin/org/team5419/fault/trajectory/types/TimedTrajectory.kt new file mode 100644 index 0000000..1bde450 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/types/TimedTrajectory.kt @@ -0,0 +1,101 @@ +@file:Suppress("ConstructorParameterNaming") +package org.team5419.fault.trajectory.types + +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.trajectory.TrajectoryIterator +import org.team5419.fault.math.geometry.State +import org.team5419.fault.math.lerp +import org.team5419.fault.math.geometry.Pose2dWithCurvature +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.derived.LinearAcceleration +import org.team5419.fault.math.units.derived.LinearVelocity +import org.team5419.fault.math.units.operations.times + +class TimedTrajectory>( + override val points: List>, + override val reversed: Boolean +) : Trajectory, TimedEntry> { + + override fun sample(interpolant: SIUnit) = sample(interpolant.value) + + fun sample(interpolant: Double) = when { + interpolant >= lastInterpolant.value -> TrajectorySamplePoint(getPoint(points.size - 1)) + interpolant <= firstInterpolant.value -> TrajectorySamplePoint(getPoint(0)) + else -> { + val (index, entry) = points.asSequence() + .withIndex() + .first { (index, entry) -> index != 0 && entry.t.value >= interpolant } + val prevEntry = points[ index - 1 ] + if (entry.t epsilonEquals prevEntry.t) { + TrajectorySamplePoint(entry, index, index) + } else { + TrajectorySamplePoint( + prevEntry.interpolate( + entry, + (interpolant - prevEntry.t.value) / (entry.t.value - prevEntry.t.value) + ), + index - 1, + index + ) + } + } + } + + override val firstState get() = points.first() + override val lastState get() = points.last() + + override val firstInterpolant get() = firstState.t + override val lastInterpolant get() = lastState.t + + override fun iterator() = TimedIterator(this) +} + +data class TimedEntry> internal constructor( + val state: S, + val t: SIUnit = SIUnit(0.0), + val velocity: SIUnit = SIUnit(0.0), + val acceleration: SIUnit = SIUnit(0.0) +) : State> { + + override fun interpolate(other: TimedEntry, t: Double): TimedEntry { + val newT = this.t.lerp(other.t, t) + val deltaT = newT - this.t + if (deltaT.value < 0.0) return other.interpolate(this, 1.0 - t) + + val reversing = + this.velocity.value < 0.0 || this.velocity.value epsilonEquals 0.0 && this.acceleration.value < 0.0 + + val newV = this.velocity + this.acceleration * deltaT + val newS = (if (reversing) -1.0 else 1.0) * (this.velocity * deltaT + 0.5 * this.acceleration * deltaT * deltaT) + + return TimedEntry( + state.interpolate(other.state, (newS / state.distance(other.state)).value), + newT, + newV, + this.acceleration + ) + } + + override fun distance(other: TimedEntry) = state.distance(other.state) + + override fun toCSV() = "$t, $state, $velocity, $acceleration" +} + +class TimedIterator>( + trajectory: TimedTrajectory +) : TrajectoryIterator, TimedEntry>(trajectory) { + override fun addition(a: SIUnit, b: SIUnit) = a + b +} + +fun Trajectory, TimedEntry>.mirror() = + TimedTrajectory(points.map { TimedEntry(it.state.mirror, it.t, it.velocity, it.acceleration) }, this.reversed) + +fun Trajectory, TimedEntry>.transform(transform: Pose2d) = + TimedTrajectory( + points.map { TimedEntry(it.state + transform, it.t, it.velocity, it.acceleration) }, + this.reversed + ) + +val Trajectory, TimedEntry>.duration get() = this.lastState.t diff --git a/src/main/kotlin/org/team5419/fault/trajectory/types/Trajectory.kt b/src/main/kotlin/org/team5419/fault/trajectory/types/Trajectory.kt new file mode 100644 index 0000000..0401bf2 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/trajectory/types/Trajectory.kt @@ -0,0 +1,34 @@ +package org.team5419.fault.trajectory.types + +import org.team5419.fault.math.geometry.State +import org.team5419.fault.trajectory.TrajectoryIterator + +interface Trajectory, S : State> { + val points: List + val reversed get() = false + + fun getPoint(index: Int) = TrajectoryPoint(index, points[index]) + + fun sample(interpolant: U): TrajectorySamplePoint + + val firstInterpolant: U + val lastInterpolant: U + + val firstState: S + val lastState: S + + operator fun iterator(): TrajectoryIterator +} + +data class TrajectoryPoint( + val index: Int, + val state: S +) + +data class TrajectorySamplePoint ( + val state: S, + val floorIndex: Int, + val ceilIndex: Int +) { + constructor(point: TrajectoryPoint): this(point.state, point.index, point.index) +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/CSVWritable.kt b/src/main/kotlin/org/team5419/fault/util/CSVWritable.kt similarity index 61% rename from src/main/kotlin/org/team5499/monkeyLib/util/CSVWritable.kt rename to src/main/kotlin/org/team5419/fault/util/CSVWritable.kt index e2c2789..e06ff2d 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/CSVWritable.kt +++ b/src/main/kotlin/org/team5419/fault/util/CSVWritable.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util +package org.team5419.fault.util interface CSVWritable { public fun toCSV(): String diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/CircularBuffer.kt b/src/main/kotlin/org/team5419/fault/util/CircularBuffer.kt similarity index 94% rename from src/main/kotlin/org/team5499/monkeyLib/util/CircularBuffer.kt rename to src/main/kotlin/org/team5419/fault/util/CircularBuffer.kt index f4cd0d6..9916e87 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/CircularBuffer.kt +++ b/src/main/kotlin/org/team5419/fault/util/CircularBuffer.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util +package org.team5419.fault.util public open class CircularBuffer(maxSize: Int) { diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/CircularDoubleBuffer.kt b/src/main/kotlin/org/team5419/fault/util/CircularDoubleBuffer.kt similarity index 91% rename from src/main/kotlin/org/team5499/monkeyLib/util/CircularDoubleBuffer.kt rename to src/main/kotlin/org/team5419/fault/util/CircularDoubleBuffer.kt index c5a244d..b4f77b0 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/CircularDoubleBuffer.kt +++ b/src/main/kotlin/org/team5419/fault/util/CircularDoubleBuffer.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util +package org.team5419.fault.util public class CircularDoubleBuffer(maxSize: Int) : CircularBuffer(maxSize) { diff --git a/src/main/kotlin/org/team5419/fault/util/CumultiveAverageFilter.kt b/src/main/kotlin/org/team5419/fault/util/CumultiveAverageFilter.kt new file mode 100644 index 0000000..30663f9 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/util/CumultiveAverageFilter.kt @@ -0,0 +1,20 @@ +package org.team5419.fault.util + +class CumultiveAverageFilter( + private val weight: Double, + private val initial: Double = 0.0 +) { + + private var prevValue = 0.0 + + fun update(currentValue: Double): Double { + prevValue = prevValue * (weight) + (1.0 - weight) * currentValue + return prevValue + } + + fun reset() { + prevValue = initial + } + + fun getLast() = prevValue +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/Interpolable.kt b/src/main/kotlin/org/team5419/fault/util/Interpolable.kt similarity index 69% rename from src/main/kotlin/org/team5499/monkeyLib/util/Interpolable.kt rename to src/main/kotlin/org/team5419/fault/util/Interpolable.kt index 5ea6f7f..54e2f75 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Interpolable.kt +++ b/src/main/kotlin/org/team5419/fault/util/Interpolable.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util +package org.team5419.fault.util interface Interpolable { public fun interpolate(other: T, x: Double): T diff --git a/src/main/kotlin/org/team5419/fault/util/MovingAverageFilter.kt b/src/main/kotlin/org/team5419/fault/util/MovingAverageFilter.kt new file mode 100644 index 0000000..923c334 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/util/MovingAverageFilter.kt @@ -0,0 +1,31 @@ +package org.team5419.fault.util + +import java.util.LinkedList + +class MovingAverageFilter(numberOfValues: Int) { + private val values: LinkedList + private val numberOfValues: Int + public val average: Double + get() = values.toDoubleArray().sum() / values.size + + init { + this.values = LinkedList() + this.numberOfValues = numberOfValues + } + + private var staticAddValue: (Double) -> Unit = { value -> + this.values.add(value) + this.values.remove() + } + + private var initAddValue: (Double) -> Unit = { value -> + this.values.add(value) + if (this.values.size == numberOfValues) { + addValue = staticAddValue + } + } + + private var addValue: (Double) -> Unit = initAddValue + + operator fun plusAssign(value: Double) = addValue(value) +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/Oof.kt b/src/main/kotlin/org/team5419/fault/util/Oof.kt similarity index 78% rename from src/main/kotlin/org/team5499/monkeyLib/util/Oof.kt rename to src/main/kotlin/org/team5419/fault/util/Oof.kt index 8954310..5d9e695 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Oof.kt +++ b/src/main/kotlin/org/team5419/fault/util/Oof.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util +package org.team5419.fault.util /* Thanks 1323! diff --git a/src/main/kotlin/org/team5419/fault/util/Source.kt b/src/main/kotlin/org/team5419/fault/util/Source.kt new file mode 100644 index 0000000..4eca1cf --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/util/Source.kt @@ -0,0 +1,86 @@ +@file:Suppress("TooManyFunctions") +package org.team5419.fault.util + +import kotlin.math.absoluteValue +import kotlin.math.sign + +typealias Source = () -> T + +typealias DoubleSource = Source +typealias BooleanSource = Source +typealias IntSource = Source +typealias StringSource = Source + +fun Source.withMerge( + other: Source, + block: (T, O) -> P +): Source

= { block(this@withMerge(), other()) } + +fun Source.withEquals(other: O): BooleanSource = { this@withEquals() == other } +fun Source.withEquals(other: Source): BooleanSource = { this@withEquals() == other() } + +fun Source.map(block: (T) -> O): Source = { block(this@map()) } + +@Suppress("FunctionName") +fun Source(value: T): Source = { value } + +@Suppress("FunctionName") +@Deprecated("Redundant", ReplaceWith("value")) +fun Source(value: () -> T): Source = value + +fun DoubleSource.withThreshold(threshold: Double = 0.5): BooleanSource = map { this@withThreshold() >= threshold } + +fun DoubleSource.withDeadband( + deadband: Double, + scaleDeadband: Boolean = true, + maxMagnitude: Double = 1.0 +): DoubleSource = map { + val currentValue = this@withDeadband() + if (currentValue in (-deadband)..deadband) return@map 0.0 // in deadband + // outside deadband + if (!scaleDeadband) return@map currentValue + // scale so deadband is effective 0 + ((currentValue.absoluteValue - deadband) / (maxMagnitude - deadband)) * currentValue.sign +} + +// Boolean Sources + +fun BooleanSource.map(trueMap: T, falseMap: T) = map( + Source( + trueMap + ), Source(falseMap) +) +fun BooleanSource.map(trueMap: Source, falseMap: T) = map(trueMap, + Source(falseMap) +) +fun BooleanSource.map(trueMap: T, falseMap: Source) = map( + Source( + trueMap + ), falseMap) +fun BooleanSource.map(trueMap: Source, falseMap: Source) = map { if (it) trueMap() else falseMap() } + +operator fun BooleanSource.not() = map { !it } +infix fun BooleanSource.and(other: BooleanSource) = withMerge(other) { one, two -> one and two } +infix fun BooleanSource.or(other: BooleanSource) = withMerge(other) { one, two -> one or two } +infix fun BooleanSource.xor(other: BooleanSource) = withMerge(other) { one, two -> one or two } + +// Comparable Sources + +fun Source>.compareTo(other: T) = map { it.compareTo(other) } +fun Source>.compareTo(other: Source) = withMerge(other) { one, two -> one.compareTo(two) } + +fun Source>.greaterThan(other: T) = compareTo(other).map { it > 0 } +fun Source>.equalTo(other: T) = compareTo(other).map { it == 0 } +fun Source>.lessThan(other: T) = compareTo(other).map { it < 0 } +fun Source>.greaterThan(other: Source) = compareTo(other).map { it > 0 } +fun Source>.equalTo(other: Source) = compareTo(other).map { it == 0 } +fun Source>.lessThan(other: Source) = compareTo(other).map { it < 0 } + +@JvmName("reversedGreaterThan") +fun Source.greaterThan(other: Source>) = other.compareTo(this).map { it < 0 } + +@JvmName("reversedEqualTo") +fun Source.equalTo(other: Source>) = other.compareTo(this).map { it == 0 } + +@JvmName("reversedLessThan") +fun Source.lessThan(other: Source>) = other.compareTo(this).map { it > 0 } diff --git a/src/main/kotlin/org/team5419/fault/util/SourceMonitor.kt b/src/main/kotlin/org/team5419/fault/util/SourceMonitor.kt new file mode 100644 index 0000000..06feec8 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/util/SourceMonitor.kt @@ -0,0 +1,25 @@ +package org.team5419.fault.util + +val Source.monitor get() = SourceMonitor(this) + +class SourceMonitor( + val source: Source +) { + var lastValue = source() + + inline fun onChange(block: (T) -> Unit) { + val newValue = source() + if (newValue != lastValue) block(newValue) + lastValue = newValue + } + + inline fun onWhen(value: T, block: () -> Unit) { + if (lastValue == value) block() + onChange { if (it == value) block() } + } +} + +fun SourceMonitor.onChangeToTrue(block: () -> Unit) = onChange { if (it) block() } +fun SourceMonitor.onChangeToFalse(block: () -> Unit) = onChange { if (!it) block() } +fun SourceMonitor.onWhenToTrue(block: () -> Unit) = onWhen(true, block) +fun SourceMonitor.onWhenToFalse(block: () -> Unit) = onWhen(false, block) diff --git a/src/main/kotlin/org/team5499/monkeyLib/logging/ReflectingCSVWriter.kt b/src/main/kotlin/org/team5419/fault/util/logging/ReflectingCSVWriter.kt similarity index 95% rename from src/main/kotlin/org/team5499/monkeyLib/logging/ReflectingCSVWriter.kt rename to src/main/kotlin/org/team5419/fault/util/logging/ReflectingCSVWriter.kt index 74dc91f..0b60487 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/logging/ReflectingCSVWriter.kt +++ b/src/main/kotlin/org/team5419/fault/util/logging/ReflectingCSVWriter.kt @@ -1,11 +1,11 @@ -package org.team5499.monkeyLib.logging +package org.team5419.fault.util.logging import java.io.FileNotFoundException import java.io.PrintWriter import java.lang.reflect.Field import java.util.concurrent.ConcurrentLinkedDeque -import org.team5499.monkeyLib.util.CSVWritable +import org.team5419.fault.util.CSVWritable class ReflectingCSVWriter(filename: String, typeClass: Class) { private val mLinesToWrite: ConcurrentLinkedDeque = ConcurrentLinkedDeque() diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/loops/ILooper.kt b/src/main/kotlin/org/team5419/fault/util/loops/ILooper.kt similarity index 58% rename from src/main/kotlin/org/team5499/monkeyLib/util/loops/ILooper.kt rename to src/main/kotlin/org/team5419/fault/util/loops/ILooper.kt index fdc75ed..8744c69 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/loops/ILooper.kt +++ b/src/main/kotlin/org/team5419/fault/util/loops/ILooper.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util.loops +package org.team5419.fault.util.loops public interface ILooper { diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/loops/Loop.kt b/src/main/kotlin/org/team5419/fault/util/loops/Loop.kt similarity index 78% rename from src/main/kotlin/org/team5499/monkeyLib/util/loops/Loop.kt rename to src/main/kotlin/org/team5419/fault/util/loops/Loop.kt index 5be980c..16d779c 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/loops/Loop.kt +++ b/src/main/kotlin/org/team5419/fault/util/loops/Loop.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util.loops +package org.team5419.fault.util.loops public interface Loop { diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/loops/Looper.kt b/src/main/kotlin/org/team5419/fault/util/loops/Looper.kt similarity index 97% rename from src/main/kotlin/org/team5499/monkeyLib/util/loops/Looper.kt rename to src/main/kotlin/org/team5419/fault/util/loops/Looper.kt index 8e0cbe0..e3cab32 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/loops/Looper.kt +++ b/src/main/kotlin/org/team5419/fault/util/loops/Looper.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.util.loops +package org.team5419.fault.util.loops import edu.wpi.first.wpilibj.Notifier import edu.wpi.first.wpilibj.Timer diff --git a/src/main/kotlin/org/team5419/fault/util/time/DeltaTime.kt b/src/main/kotlin/org/team5419/fault/util/time/DeltaTime.kt new file mode 100644 index 0000000..95844a7 --- /dev/null +++ b/src/main/kotlin/org/team5419/fault/util/time/DeltaTime.kt @@ -0,0 +1,31 @@ +package org.team5419.fault.util.time + +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second + +class DeltaTime internal constructor(startTime: SIUnit = SIUnit(0.0)) { + + @Suppress("VariableNaming") + internal var _currentTime = startTime + private set + @Suppress("VariableNaming") + internal var _deltaTime = SIUnit(0.0) + private set + + val deltaTime get() = _deltaTime + val currentTime get() = _currentTime + + fun updateTime(newTime: SIUnit): SIUnit { + _deltaTime = if (_currentTime < 0.0) { + SIUnit(0.0) + } else { + newTime - _currentTime + } + _currentTime = newTime + return _deltaTime + } + + fun reset() { + _currentTime = SIUnit(-1.0) + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/time/ITimer.kt b/src/main/kotlin/org/team5419/fault/util/time/ITimer.kt similarity index 67% rename from src/main/kotlin/org/team5499/monkeyLib/util/time/ITimer.kt rename to src/main/kotlin/org/team5419/fault/util/time/ITimer.kt index 2446377..13e1c93 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/time/ITimer.kt +++ b/src/main/kotlin/org/team5419/fault/util/time/ITimer.kt @@ -1,15 +1,17 @@ -package org.team5499.monkeyLib.util.time +package org.team5419.fault.util.time +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second /** * Timer abstraction layer to allow unit testing */ -public interface ITimer { +interface ITimer { /** * Function to get time since timer started * @return number of ellapsed seconds */ - public fun get(): Double + public fun get(): SIUnit /** * Starts the timer diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/time/SystemTimer.kt b/src/main/kotlin/org/team5419/fault/util/time/SystemTimer.kt similarity index 79% rename from src/main/kotlin/org/team5499/monkeyLib/util/time/SystemTimer.kt rename to src/main/kotlin/org/team5419/fault/util/time/SystemTimer.kt index 2bada95..716d12a 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/time/SystemTimer.kt +++ b/src/main/kotlin/org/team5419/fault/util/time/SystemTimer.kt @@ -1,4 +1,8 @@ -package org.team5499.monkeyLib.util.time +package org.team5419.fault.util.time + +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.Second +import org.team5419.fault.math.units.milliseconds public class SystemTimer : ITimer { @@ -15,9 +19,9 @@ public class SystemTimer : ITimer { mPaused = false } - public override fun get(): Double { + public override fun get(): SIUnit { @Suppress("MagicNumber") - return (mStartTime - System.currentTimeMillis() - mTotalPauseTime) / 1000.0 + return (mStartTime - System.currentTimeMillis() - mTotalPauseTime).milliseconds } public override fun start() { diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/time/WPITimer.kt b/src/main/kotlin/org/team5419/fault/util/time/WPITimer.kt similarity index 70% rename from src/main/kotlin/org/team5499/monkeyLib/util/time/WPITimer.kt rename to src/main/kotlin/org/team5419/fault/util/time/WPITimer.kt index 5717678..492a9a6 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/time/WPITimer.kt +++ b/src/main/kotlin/org/team5419/fault/util/time/WPITimer.kt @@ -1,6 +1,7 @@ -package org.team5499.monkeyLib.util.time +package org.team5419.fault.util.time import edu.wpi.first.wpilibj.Timer +import org.team5419.fault.math.units.seconds public class WPITimer(timer: Timer = Timer()) : ITimer { @@ -10,7 +11,7 @@ public class WPITimer(timer: Timer = Timer()) : ITimer { mTimer = timer } - public override fun get() = mTimer.get() + public override fun get() = mTimer.get().seconds public override fun start() = mTimer.start() diff --git a/src/main/kotlin/org/team5499/monkeyLib/Controller.kt b/src/main/kotlin/org/team5499/monkeyLib/Controller.kt deleted file mode 100644 index 4d337ca..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/Controller.kt +++ /dev/null @@ -1,19 +0,0 @@ -package org.team5499.monkeyLib - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -public abstract class Controller(timer: ITimer = WPITimer()) { - - protected val timer: ITimer - - init { - this.timer = timer - } - - abstract fun start() - - abstract fun update() - - abstract fun reset() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/Subsystem.kt b/src/main/kotlin/org/team5499/monkeyLib/Subsystem.kt deleted file mode 100644 index b736591..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/Subsystem.kt +++ /dev/null @@ -1,19 +0,0 @@ -package org.team5499.monkeyLib - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -abstract class Subsystem(timer: ITimer = WPITimer()) { - - protected val timer: ITimer - - init { - this.timer = timer - } - - abstract fun update() - - abstract fun stop() - - abstract fun reset() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/auto/Action.kt b/src/main/kotlin/org/team5499/monkeyLib/auto/Action.kt deleted file mode 100644 index 0a1bb93..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/auto/Action.kt +++ /dev/null @@ -1,34 +0,0 @@ -package org.team5499.monkeyLib.auto - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -public open class Action(timeoutSeconds: Double, timer: ITimer = WPITimer()) { - - private val mTimer: ITimer - private val mTimeoutSeconds: Double - - init { - mTimer = timer - mTimeoutSeconds = timeoutSeconds - } - - public open fun start() { - mTimer.stop() - mTimer.reset() - mTimer.start() - } - - public open fun update() {} - - protected fun timedOut(): Boolean { - val t = mTimer.get() - return (t >= mTimeoutSeconds) - } - - public open fun next(): Boolean { - return timedOut() - } - - public open fun finish() {} -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/auto/NothingAction.kt b/src/main/kotlin/org/team5499/monkeyLib/auto/NothingAction.kt deleted file mode 100644 index 16be064..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/auto/NothingAction.kt +++ /dev/null @@ -1,6 +0,0 @@ -package org.team5499.monkeyLib.auto - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -public class NothingAction(timeout: Double, timer: ITimer = WPITimer()) : Action(timeout, timer) diff --git a/src/main/kotlin/org/team5499/monkeyLib/auto/ParallelAction.kt b/src/main/kotlin/org/team5499/monkeyLib/auto/ParallelAction.kt deleted file mode 100644 index 44c1e2e..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/auto/ParallelAction.kt +++ /dev/null @@ -1,44 +0,0 @@ -package org.team5499.monkeyLib.auto - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -class ParallelAction(vararg actions: Action, timer: ITimer = WPITimer()) : Action(0.0, timer) { - - private val mActions: Array - - init { - mActions = actions.copyOf() - } - - override fun start() { - super.start() - for (a: Action in mActions) { - a.start() - } - } - - override fun update() { - super.update() - for (a: Action in mActions) { - a.update() - } - } - - @Suppress("ReturnCount") - override fun next(): Boolean { - for (a: Action in mActions) { - if (!a.next()) { - return false - } - } - return true - } - - override fun finish() { - super.finish() - for (a: Action in mActions) { - a.finish() - } - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/auto/Routine.kt b/src/main/kotlin/org/team5499/monkeyLib/auto/Routine.kt deleted file mode 100644 index 319e6da..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/auto/Routine.kt +++ /dev/null @@ -1,47 +0,0 @@ -package org.team5499.monkeyLib.auto - -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Pose2d - -class Routine(name: String, startPose: Pose2d, vararg actions: Action) { - - private val actions: Array - var stepNumber: Int - get() = field - val name: String - get() = field - val startPose: Pose2d - get() = field - - init { - this.stepNumber = 0 - this.name = name - this.startPose = startPose - this.actions = actions.copyOf() - } - - @Suppress("SpreadOperator") - public constructor(name: String, vararg actions: Action) : - this(name, Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0.0)), *actions) - - public fun getCurrentAction(): Action { - return actions.get(stepNumber) - } - - public fun advanceRoutine(): Boolean { - if (isLastStep()) { - return false - } - stepNumber++ - return true - } - - public fun reset() { - this.stepNumber = 0 - } - - public fun isLastStep(): Boolean { - return (stepNumber >= (actions.size - 1)) - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/auto/SerialAction.kt b/src/main/kotlin/org/team5499/monkeyLib/auto/SerialAction.kt deleted file mode 100644 index baa0a55..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/auto/SerialAction.kt +++ /dev/null @@ -1,36 +0,0 @@ - -package org.team5499.monkeyLib.auto - -public class SerialAction(vararg actions: Action) : Action(0.0) { - - private val childActions: Array - private var index: Int = 0 - - init { - childActions = actions - } - - public override fun start() { - this.index = 0 - super.start() - childActions[index].start() - } - - public override fun update() { - if (index == childActions.size - 1) return - if (childActions[index].next()) { - childActions[index].finish() - index++ - childActions[index].start() - } - childActions[index].update() - } - - public override fun next(): Boolean { - return index == (childActions.size - 1) && childActions[childActions.size - 1].next() - } - - public override fun finish() { - super.finish() - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt deleted file mode 100644 index c6562f0..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt +++ /dev/null @@ -1,44 +0,0 @@ -package org.team5499.monkeyLib.hardware - -import com.ctre.phoenix.motorcontrol.can.TalonSRX -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.DemandType -import com.ctre.phoenix.motorcontrol.NeutralMode - -class LazyTalonSRX(deviceNumber: Int) : TalonSRX(deviceNumber) { - - private var lastSet = Double.NaN - private var lastSecondarySet = Double.NaN - private var lastControlMode: ControlMode? = null - private var lastDemandType: DemandType? = null - private var lastBrakeMode: NeutralMode? = null - - public override fun set(mode: ControlMode, value: Double) { - if (value != lastSet || mode != lastControlMode) { - lastSet = value - lastControlMode = mode - lastSecondarySet = Double.NaN - lastDemandType = null - super.set(mode, value) - } - } - - public override fun set(mode: ControlMode, value0: Double, demandType: DemandType, value1: Double) { - @Suppress("ComplexCondition") - if (value0 != lastSet || mode != lastControlMode || - demandType != lastDemandType || value1 != lastSecondarySet) { - lastSet = value0 - lastControlMode = mode - lastDemandType = demandType - lastSecondarySet = value1 - super.set(mode, value0, demandType, value1) - } - } - - public override fun setNeutralMode(brakeMode: NeutralMode) { - if (lastBrakeMode != brakeMode) { - lastBrakeMode = brakeMode - super.setNeutralMode(brakeMode) - } - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyVictorSPX.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyVictorSPX.kt deleted file mode 100644 index 814cb57..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyVictorSPX.kt +++ /dev/null @@ -1,44 +0,0 @@ -package org.team5499.monkeyLib.hardware - -import com.ctre.phoenix.motorcontrol.can.VictorSPX -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.DemandType -import com.ctre.phoenix.motorcontrol.NeutralMode - -class LazyVictorSPX(deviceNumber: Int) : VictorSPX(deviceNumber) { - - private var lastSet = Double.NaN - private var lastSecondarySet = Double.NaN - private var lastControlMode: ControlMode? = null - private var lastDemandType: DemandType? = null - private var lastBrakeMode: NeutralMode? = null - - public override fun set(mode: ControlMode, value: Double) { - if (value != lastSet || mode != lastControlMode) { - lastSet = value - lastControlMode = mode - lastSecondarySet = Double.NaN - lastDemandType = null - super.set(mode, value) - } - } - - public override fun set(mode: ControlMode, value0: Double, demandType: DemandType, value1: Double) { - @Suppress("ComplexCondition") - if (value0 != lastSet || mode != lastControlMode || - demandType != lastDemandType || value1 != lastSecondarySet) { - lastSet = value0 - lastControlMode = mode - lastDemandType = demandType - lastSecondarySet = value1 - super.set(mode, value0, demandType, value1) - } - } - - public override fun setNeutralMode(brakeMode: NeutralMode) { - if (lastBrakeMode != brakeMode) { - lastBrakeMode = brakeMode - super.setNeutralMode(brakeMode) - } - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/XboxControllerPlus.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/XboxControllerPlus.kt deleted file mode 100644 index ebc854b..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/XboxControllerPlus.kt +++ /dev/null @@ -1,67 +0,0 @@ -package org.team5499.monkeyLib.hardware - -import edu.wpi.first.wpilibj.XboxController - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.WPITimer - -import java.util.concurrent.atomic.AtomicBoolean - -// adapted from 1323 -public class XboxControllerPlus(portNumber: Int, timer: ITimer = WPITimer()) : XboxController(portNumber) { - - private val mRumbling: AtomicBoolean - private val mTimer: ITimer - - public val isRumbling: Boolean - get() = mRumbling.get() - - init { - mRumbling = AtomicBoolean(false) - mTimer = timer - } - - public fun cancelRumble() { - mRumbling.set(false) - } - - public fun rumble(rumblesPerSecond: Double, numberOfSeconds: Double) { - if (!mRumbling.get()) { - val thread = RumbleThread(rumblesPerSecond, numberOfSeconds) - thread.start() - } - } - - private inner class RumbleThread(rumblesPerSecond: Double, numberOfSeconds: Double) : Thread() { - private val mRumblesPerSecond: Double - private val mInterval: Long - private val mSeconds: Double - - init { - mRumblesPerSecond = rumblesPerSecond - mSeconds = numberOfSeconds - mInterval = (1 / (rumblesPerSecond * 2.0) * 1000.0).toLong() - } - - public override fun start() { - mRumbling.set(true) - mTimer.stop() - mTimer.reset() - mTimer.start() - try { - while (mTimer.get() < mSeconds && mRumbling.get()) { - setRumble(RumbleType.kLeftRumble, 1.0) - setRumble(RumbleType.kRightRumble, 1.0) - sleep(mInterval) - setRumble(RumbleType.kLeftRumble, 0.0) - setRumble(RumbleType.kRightRumble, 0.0) - sleep(mInterval) - } - } catch (e: InterruptedException) { - mRumbling.set(false) - e.printStackTrace() - } - mRumbling.set(false) - } - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/ButtonState.kt b/src/main/kotlin/org/team5499/monkeyLib/input/ButtonState.kt deleted file mode 100644 index faa4c19..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/ButtonState.kt +++ /dev/null @@ -1,3 +0,0 @@ -package org.team5499.monkeyLib.input - -public data class ButtonState(val down: Boolean, val pressed: Boolean, val released: Boolean) diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/CheesyDriveHelper.kt b/src/main/kotlin/org/team5499/monkeyLib/input/CheesyDriveHelper.kt deleted file mode 100644 index 84a89f8..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/CheesyDriveHelper.kt +++ /dev/null @@ -1,158 +0,0 @@ -package org.team5499.monkeyLib.input - -import org.team5499.monkeyLib.util.Utils - -public class CheesyDriveHelper(config: CheesyDriveConfig) : DriveHelper() { - - public class CheesyDriveConfig { - public var deadband = 0.0 - public var quickstopDeadband = 0.0 - public var quickstopWeight = 0.0 - public var quickstopScalar = 0.0 - public var highWheelNonlinearity = 0.0 - public var lowWheelNonlinearity = 0.0 - public var highNeginertiaScalar = 0.0 - public var highSensitivity = 0.0 - public var lowNeginertiaTurnScalar = 0.0 - public var lowNeginertiaThreshold = 0.0 - public var lowNeginertiaFarScalar = 0.0 - public var lowNeginertiaCloseScalar = 0.0 - public var lowSensitivity = 0.0 - } - - private var config: CheesyDriveConfig - - private var mOldWheel = 0.0 - private var mQuickStopAccumlator = 0.0 - private var mNegInertiaAccumlator = 0.0 - private var mDebugCounter = 0 - - init { - this.config = config - } - - public constructor(): this(CheesyDriveConfig()) - - @Suppress("LongMethod", "ComplexMethod") - public override fun calculateOutput( - throttle: Double, - wheel: Double, - isQuickTurn: Boolean, - isHighGear: Boolean - ): DriveSignal { - var newWheel = handleDeadband(wheel, config.deadband) - var newThrottle = handleDeadband(throttle, config.deadband) - - val negInertia = newWheel - mOldWheel - mOldWheel = newWheel - - newWheel = calculateCheesyNonlinearity(newWheel, isHighGear) - - var leftPwm: Double - var rightPwm: Double - var overPower: Double - var angularPower: Double - var linearPower: Double - - val pair = calculateCheesyNegIntertialScalar(newWheel, negInertia, isHighGear) - val negInertiaScalar = pair.first - val sensitivity = pair.second - - val negInertiaPower = negInertia * negInertiaScalar - mNegInertiaAccumlator += negInertiaPower - - newWheel = newWheel + mNegInertiaAccumlator - if (mNegInertiaAccumlator > 1) { - mNegInertiaAccumlator -= 1 - } else if (mNegInertiaAccumlator < -1) { - mNegInertiaAccumlator += 1 - } else { - mNegInertiaAccumlator = 0.0 - } - linearPower = newThrottle - - if (isQuickTurn) { - if (Math.abs(linearPower) < config.quickstopDeadband) { - val alpha = config.quickstopWeight - mQuickStopAccumlator = (1 - alpha) * mQuickStopAccumlator - + alpha * Utils.limit(wheel, 1.0) * config.quickstopScalar - } - overPower = 1.0 - angularPower = newWheel - } else { - overPower = 0.0 - angularPower = Math.abs(throttle) * newWheel * sensitivity - mQuickStopAccumlator - if (mQuickStopAccumlator > 1) { - mQuickStopAccumlator -= 1 - } else if (mQuickStopAccumlator < -1) { - mQuickStopAccumlator += 1 - } else { - mQuickStopAccumlator = 0.0 - } - } - leftPwm = linearPower - rightPwm = linearPower - leftPwm += angularPower - rightPwm -= angularPower - - if (leftPwm > 1.0) { - rightPwm -= overPower * (leftPwm - 1.0) - leftPwm = 1.0 - } else if (rightPwm > 1.0) { - leftPwm -= overPower * (rightPwm - 1.0) - rightPwm = 1.0 - } else if (leftPwm < -1.0) { - rightPwm += overPower * (-1.0 - leftPwm) - leftPwm = -1.0 - } else if (rightPwm < -1.0) { - leftPwm += overPower * (-1.0 - rightPwm) - rightPwm = -1.0 - } - - return DriveSignal(leftPwm, rightPwm) - } - - private fun calculateCheesyNonlinearity(wheel: Double, isHighGear: Boolean): Double { - var newWheel = wheel - if (isHighGear) { - val wheelNonLinearity = config.highWheelNonlinearity - val denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity) - newWheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * newWheel) / denominator - newWheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * newWheel) / denominator - } else { - val wheelNonLinearity = config.lowWheelNonlinearity - val denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity) - newWheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * newWheel) / denominator - newWheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * newWheel) / denominator - newWheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * newWheel) / denominator - } - return newWheel - } - - private fun calculateCheesyNegIntertialScalar( - wheel: Double, - negInertia: Double, - isHighGear: Boolean - ): Pair { - val sensitivity: Double - val negInertiaScalar: Double - if (isHighGear) { - negInertiaScalar = config.highNeginertiaScalar - sensitivity = config.highSensitivity - } else { - if (wheel * negInertia > 0) { - // If we are moving away from 0.0, aka, trying to get more wheel. - negInertiaScalar = config.lowNeginertiaTurnScalar - } else { - // Otherwise, we are attempting to go back to 0.0. - if (Math.abs(wheel) > config.lowNeginertiaThreshold) { - negInertiaScalar = config.lowNeginertiaFarScalar - } else { - negInertiaScalar = config.lowNeginertiaCloseScalar - } - } - sensitivity = config.lowSensitivity - } - return Pair(negInertiaScalar, sensitivity) - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/DriveHelper.kt b/src/main/kotlin/org/team5499/monkeyLib/input/DriveHelper.kt deleted file mode 100644 index 591f617..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/DriveHelper.kt +++ /dev/null @@ -1,10 +0,0 @@ -package org.team5499.monkeyLib.input - -abstract class DriveHelper { - - public abstract fun calculateOutput(arg0: Double, arg1: Double, arg2: Boolean, arg3: Boolean = false): DriveSignal - - protected fun handleDeadband(value: Double, deadband: Double): Double { - return if (Math.abs(value) > Math.abs(deadband)) value else 0.0 - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/DriveSignal.kt b/src/main/kotlin/org/team5499/monkeyLib/input/DriveSignal.kt deleted file mode 100644 index 4ab40c1..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/DriveSignal.kt +++ /dev/null @@ -1,20 +0,0 @@ -package org.team5499.monkeyLib.input - -import org.team5499.monkeyLib.util.CSVWritable - -data class DriveSignal(val left: Double, val right: Double, val brakeMode: Boolean) : CSVWritable { - - constructor(): this(0.0, 0.0) - constructor(left: Double, right: Double) : this(left, right, false) - - companion object { - val NEUTRAL = DriveSignal(0.0, 0.0, false) - val BRAKE = DriveSignal(0.0, 0.0, true) - } - - override fun toString(): String { - return "L: $left - R: $right - Brake Mode: $brakeMode " - } - - override fun toCSV() = "$left,$right,$brakeMode" -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/input/SpaceDriveHelper.kt b/src/main/kotlin/org/team5499/monkeyLib/input/SpaceDriveHelper.kt deleted file mode 100644 index 9b6e628..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/SpaceDriveHelper.kt +++ /dev/null @@ -1,30 +0,0 @@ -package org.team5499.monkeyLib.input - -public class SpaceDriveHelper(deadband: () -> Double, turnMult: () -> Double, slowMult: () -> Double) : DriveHelper() { - - private var mDeadband: () -> Double - private var mTurnMult: () -> Double - private var mSlowMult: () -> Double - - init { - 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 - newWheel *= mult - 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/input/TankDriveHelper.kt b/src/main/kotlin/org/team5499/monkeyLib/input/TankDriveHelper.kt deleted file mode 100644 index f7c2848..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/input/TankDriveHelper.kt +++ /dev/null @@ -1,22 +0,0 @@ -package org.team5499.monkeyLib.input - -public class TankDriveHelper(deadband: Double, slowMultiplier: Double) : DriveHelper() { - - private val mDeadband: Double - private val mSlowMult: Double - - init { - mDeadband = deadband - mSlowMult = slowMultiplier - } - - public override fun calculateOutput(left: Double, right: Double, isSlow: Boolean, notUsed: Boolean): DriveSignal { - var newLeft = super.handleDeadband(left, mDeadband) - var newRight = super.handleDeadband(right, mDeadband) - if (isSlow) { - newLeft *= mSlowMult - newRight *= mSlowMult - } - return DriveSignal(newLeft, newRight) - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/logging/Logger.kt b/src/main/kotlin/org/team5499/monkeyLib/logging/Logger.kt deleted file mode 100644 index 1947d0c..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/logging/Logger.kt +++ /dev/null @@ -1,3 +0,0 @@ -package org.team5499.monkeyLib.logging - -// main logging class diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/Epsilon.kt b/src/main/kotlin/org/team5499/monkeyLib/math/Epsilon.kt deleted file mode 100644 index 9253b49..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/Epsilon.kt +++ /dev/null @@ -1,13 +0,0 @@ -package org.team5499.monkeyLib.math - -object Epsilon { - - public const val EPSILON = 1E-5 - - public fun epsilonEquals(value: Double, other: Double, epsilon: Double = EPSILON): Boolean { - return (Math.abs(value - other) < epsilon) - } - - public fun epsilonEquals(value: Double, epsilon: Double = EPSILON) = - epsilonEquals(value, 0.0, epsilon) -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/Position.kt b/src/main/kotlin/org/team5499/monkeyLib/math/Position.kt deleted file mode 100644 index 85270fb..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/Position.kt +++ /dev/null @@ -1,51 +0,0 @@ -package org.team5499.monkeyLib.math - -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.util.CSVWritable - -class Position : CSVWritable { - - private var x = 0.0 - private var y = 0.0 - private var lastLeft = 0.0 - private var lastRight = 0.0 - private var lastAngle = 0.0 - - var positionVector: Vector2 - get() = Vector2(x, y) - set(value) { - x = value.x - y = value.y - } - - fun update(leftDistance: Double, rightDistance: Double, angle: Double) { - val newAngle = Math.toRadians(angle) - var angleDelta = newAngle - lastAngle - if (angleDelta == 0.0) angleDelta = Epsilon.EPSILON - val leftDelta = leftDistance - lastLeft - val rightDelta = rightDistance - lastRight - val distance = (leftDelta + rightDelta) / 2.0 - val radiusOfCurvature = distance / angleDelta - val dy = radiusOfCurvature * Math.sin(angleDelta) - val dx = radiusOfCurvature * (Math.cos(angleDelta) - 1) - y -= dx * Math.cos(lastAngle) - dy * Math.sin(lastAngle) - x += dx * Math.sin(lastAngle) + dy * Math.cos(lastAngle) - lastLeft = leftDistance - lastRight = rightDistance - lastAngle = newAngle - } - - fun reset() { - x = 0.0 - y = 0.0 - lastLeft = 0.0 - lastRight = 0.0 - lastAngle = 0.0 - } - - override fun toString(): String { - return "Robot Position -- X: $x, Y: $y" - } - - override fun toCSV(): String = "$x,$y" -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/UnitConversion.kt b/src/main/kotlin/org/team5499/monkeyLib/math/UnitConversion.kt deleted file mode 100644 index 209a32c..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/UnitConversion.kt +++ /dev/null @@ -1,5 +0,0 @@ -package org.team5499.monkeyLib.math - -// public object UnitConversion { -// // functions that convert between units -// } diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2d.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2d.kt deleted file mode 100644 index d05f0f0..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2d.kt +++ /dev/null @@ -1,110 +0,0 @@ -package org.team5499.monkeyLib.math.geometry - -import org.team5499.monkeyLib.math.Epsilon - -@Suppress("TooManyFunctions") -class Pose2d(translation: Vector2, rotation: Rotation2d) : Geometric { - - companion object { - - @Suppress("MagicNumber") - fun log(transform: Pose2d): Twist2d { - val dTheta = transform.rotation.radians - val halfTheta = 0.5 * dTheta - val cosMinusOne = transform.rotation.cosAngle - 1.0 - val halfThetaByTanOfHalfDtheta: Double - if (Math.abs(cosMinusOne) < Epsilon.EPSILON) { - halfThetaByTanOfHalfDtheta = 1.0 - ((1.0 / 12.0) * (dTheta * dTheta)) - } else { - halfThetaByTanOfHalfDtheta = -(halfTheta * transform.rotation.sinAngle) / cosMinusOne - } - val translationPart = transform.translation.rotateBy( - Rotation2d(halfThetaByTanOfHalfDtheta, -halfTheta, false) - ) - return Twist2d(translationPart.x, translationPart.y, dTheta) - } - - @Suppress("MagicNumber") - fun exp(delta: Twist2d): Pose2d { - val sinTheta = Math.sin(delta.dTheta) - val cosTheta = Math.cos(delta.dTheta) - val s: Double - val c: Double - if (Math.abs(delta.dTheta) < Epsilon.EPSILON) { - s = 1.0 - 1.0 / 6.0 * delta.dTheta * delta.dTheta - c = .5 * delta.dTheta - } else { - s = sinTheta / delta.dTheta - c = (1.0 - cosTheta) / delta.dTheta - } - return Pose2d(Vector2(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), - Rotation2d(cosTheta, sinTheta, false)) - } - - fun fromRotation(rotation: Rotation2d) = Pose2d(Vector2(), rotation) - } - - val translation: Vector2 - get() = field - val rotation: Rotation2d - get() = field - - init { - this.translation = translation - this.rotation = rotation - } - - constructor(): this(Vector2(), Rotation2d()) - constructor(other: Pose2d): this(other.translation, other.rotation) - - fun inverse(): Pose2d { - val rotationInverted = rotation.inverse() - return Pose2d((-translation).rotateBy(rotationInverted), rotationInverted) - } - - fun normal(): Pose2d { - return Pose2d(translation, rotation.normal()) - } - - fun isColinear(other: Pose2d): Boolean { - if (other.rotation.isParallel(rotation)) return false - val twist = log(inverse().transformBy(other)) - return Math.abs(twist.dx) < Epsilon.EPSILON && Math.abs(twist.dTheta) < Epsilon.EPSILON - } - - @Suppress("ReturnCount") - override fun interpolate(other: Pose2d, x: Double): Pose2d { - if (x <= 0) { - return Pose2d(this) - } else if (x >= 1) { - return Pose2d(other) - } - val twist = Pose2d.log(inverse().transformBy(other)) - return transformBy(Pose2d.exp(twist.scaled(x))) - } - - fun transformBy(other: Pose2d): Pose2d { - return Pose2d(translation.translateBy(other.translation.rotateBy(rotation)), rotation.rotateBy(other.rotation)) - } - - fun transformBy(other: Vector2): Pose2d { - return Pose2d(translation.translateBy(other), rotation) - } - - fun mirror(): Pose2d { - return Pose2d(Vector2(translation.x, -translation.y), rotation.inverse()) - } - - override fun toString(): String { - return "Translation: $translation, Rotation: $rotation" - } - - override fun toCSV() = "${translation.toCSV()},${rotation.toCSV()}" - - override fun equals(other: Any?): Boolean { - if (other == null || other !is Pose2d) return false - return (other.translation.equals(translation) && other.rotation.equals(rotation)) - } - - override fun hashCode() = super.hashCode() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt deleted file mode 100644 index 16e6995..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt +++ /dev/null @@ -1,57 +0,0 @@ -package org.team5499.monkeyLib.math.geometry - -import org.team5499.monkeyLib.util.Utils - -class Pose2dWithCurvature( - translation: Vector2, - rotation: Rotation2d, - curvature: Double, - dCurvature: Double = 0.0 -) : Geometric { - - val curvature: Double - get() = field - val dCurvature: Double - get() = field - val pose: Pose2d - get() = field - val translation: Vector2 - get() = pose.translation - val rotation: Rotation2d - get() = pose.rotation - - init { - this.curvature = curvature - this.dCurvature = dCurvature - this.pose = Pose2d(translation, rotation) - } - - constructor(pose: Pose2d, curvature: Double, dCurvature: Double = 0.0): - this(pose.translation, pose.rotation, curvature, dCurvature) - constructor(): this(Vector2(), Rotation2d(), 0.0, 0.0) - constructor(other: Pose2dWithCurvature): this(other.translation, other.rotation, other.curvature, other.dCurvature) - - public fun mirror(): Pose2dWithCurvature { - return Pose2dWithCurvature(pose.mirror(), -curvature, -dCurvature) - } - - override fun interpolate(other: Pose2dWithCurvature, x: Double): Pose2dWithCurvature { - return Pose2dWithCurvature(pose.interpolate(other.pose, x), - Utils.interpolate(curvature, other.curvature, x), - Utils.interpolate(dCurvature, other.dCurvature, x) - ) - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is Pose2dWithCurvature) return false - return pose.equals(other.pose) && curvature == other.curvature && dCurvature == other.dCurvature - } - - override fun toString(): String { - return pose.toString() + ", Curvature: $curvature, dCurvature: $dCurvature" - } - - override fun toCSV() = "${pose.toCSV()},$curvature,$dCurvature" - - override fun hashCode() = super.hashCode() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Rotation2d.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Rotation2d.kt deleted file mode 100644 index eb42561..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Rotation2d.kt +++ /dev/null @@ -1,120 +0,0 @@ -package org.team5499.monkeyLib.math.geometry - -import org.team5499.monkeyLib.math.Epsilon - -import java.text.DecimalFormat - -@Suppress("TooManyFunctions") -class Rotation2d(x: Double, y: Double, normalize: Boolean) : Geometric { - - companion object { - val identity = Rotation2d() - get() = field - - fun fromRadians(radians: Double): Rotation2d { - return Rotation2d(Math.cos(radians), Math.sin(radians), false) - } - - fun fromDegrees(degrees: Double): Rotation2d { - return fromRadians(Math.toRadians(degrees)) - } - - fun fromDegrees(degrees: Int) = fromDegrees(degrees.toDouble()) - } - - val cosAngle: Double - get() = field - val sinAngle: Double - get() = field - val tan: Double - get() { - if (Math.abs(cosAngle) < Epsilon.EPSILON) { - if (sinAngle > 0.0) return Double.POSITIVE_INFINITY - else return Double.NEGATIVE_INFINITY - } - return sinAngle / cosAngle - } - val radians: Double - get() = Math.atan2(sinAngle, cosAngle) - val degrees: Double - get() = Math.toDegrees(radians) - - init { - if (normalize) { - val mag = Math.hypot(x, y) - if (mag > Epsilon.EPSILON) { - cosAngle = x / mag - sinAngle = y / mag - } else { - cosAngle = 0.0 - sinAngle = 1.0 - } - } else { - cosAngle = x - sinAngle = y - } - } - - constructor(): this(1.0, 0.0, false) - constructor(other: Rotation2d): this(other.cosAngle, other.sinAngle, false) - constructor(translation: Vector2, normalize: Boolean): this(translation.x, translation.y, normalize) - constructor(x: Int, y: Int, normalize: Boolean): this(x.toDouble(), y.toDouble(), normalize) - - fun equals(other: Rotation2d): Boolean { - val diff = Math.abs(other.degrees - degrees) - if (diff < Epsilon.EPSILON) return false - return true - } - - operator fun plus(other: Rotation2d): Rotation2d = fromDegrees(degrees + other.degrees) - operator fun minus(other: Rotation2d): Rotation2d = fromDegrees(degrees - other.degrees) - - fun rotateBy(other: Rotation2d): Rotation2d { - return Rotation2d( - cosAngle * other.cosAngle - sinAngle * other.sinAngle, - cosAngle* other.sinAngle + sinAngle * other.cosAngle, - true - ) - } - - fun normal() = Rotation2d(-cosAngle, sinAngle, false) - - fun inverse() = Rotation2d(cosAngle, -sinAngle, false) - - @Suppress("ReturnCount") - override fun interpolate(other: Rotation2d, x: Double): Rotation2d { - if (x <= 0) { - return Rotation2d(this) - } else if (x >= 1) { - return Rotation2d(other) - } - val angleDiff = inverse().rotateBy(other).radians - return this.rotateBy(Rotation2d.fromRadians(angleDiff * x)) - } - - fun isParallel(other: Rotation2d): Boolean { - val temp = Vector2.cross(toVector(), other.toVector()) - return Math.abs(temp) < Epsilon.EPSILON - } - - fun toVector(): Vector2 { - return Vector2(cosAngle, sinAngle) - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is Rotation2d) return false - return degrees == other.degrees - } - - override fun toCSV(): String { - val format = DecimalFormat("#0.000") - return format.format(degrees) - } - - override fun toString(): String { - val format = DecimalFormat("#0.000") - return "${format.format(degrees)} degrees" - } - - override fun hashCode() = super.hashCode() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Twist2d.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Twist2d.kt deleted file mode 100644 index f0e1fea..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Twist2d.kt +++ /dev/null @@ -1,65 +0,0 @@ -package org.team5499.monkeyLib.math.geometry - -import java.text.DecimalFormat - -import org.team5499.monkeyLib.math.Epsilon - -class Twist2d(dx: Double, dy: Double, dTheta: Double) : Geometric { - - companion object { - val identity = Twist2d() - } - - val dx: Double - get() = field - val dy: Double - get() = field - val dTheta: Double - get() = field - - init { - this.dx = dx - this.dy = dy - this.dTheta = dTheta - } - - constructor(): this(0.0, 0.0, 0.0) - - operator fun times(other: Double) = Twist2d(dx * other, dy * other, dTheta * other) - - fun scaled(scale: Double): Twist2d { - return Twist2d(dx * scale, dy * scale, dTheta * scale) - } - - fun norm(): Double { - if (dy == 0.0) - return Math.abs(dx) - return Math.hypot(dx, dy) - } - - fun curvature(): Double { - if (Math.abs(dTheta) < Epsilon.EPSILON && norm() < Epsilon.EPSILON) { - return 0.0 - } - return dTheta / norm() - } - - override fun interpolate(other: Twist2d, x: Double): Twist2d { - println("Cant interpolate Twist2d") - return Twist2d() - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is Twist2d) return false - return dx == other.dx && dy == other.dy && dTheta == other.dTheta - } - - override fun toString(): String { - val format = DecimalFormat("###0.000") - return "(${format.format(dx)}, ${format.format(dy)}, ${format.format(Math.toDegrees(dTheta))} deg)" - } - - override fun toCSV() = "$dx,$dy,$dTheta" - - override fun hashCode() = super.hashCode() -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Vector2.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Vector2.kt deleted file mode 100644 index fa3c879..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Vector2.kt +++ /dev/null @@ -1,68 +0,0 @@ -package org.team5499.monkeyLib.math.geometry - -@Suppress("TooManyFunctions") -class Vector2(val x: Double, val y: Double) : Geometric { - - companion object { - fun distanceBetween(a: Vector2, b: Vector2) = (a - b).magnitude - fun cross(a: Vector2, b: Vector2) = a.x * b.y - a.y * b.x - } - - val magnitude by lazy { Math.hypot(y, x) } - val angle by lazy { Math.atan2(y, x) } - val normalized by lazy { - val len = magnitude - Vector2(x / len.toDouble(), y / len.toDouble()) - } - - constructor(other: Vector2): this(other.x, other.y) - constructor(): this(0.0, 0.0) - constructor(x: Int, y: Int): this(x.toDouble(), y.toDouble()) - constructor(start: Vector2, end: Vector2): this(end.x - start.x, end.y - start.y) - - operator fun plus(other: Vector2) = Vector2(x + other.x, y + other.y) - - operator fun minus(other: Vector2) = Vector2(x - other.x, y - other.y) - - operator fun times(coef: Int) = Vector2(this * coef.toDouble()) - - operator fun times(coef: Double) = Vector2(x * coef, y * coef) - - operator fun div(coef: Double) = when (coef) { - 0.0 -> throw IllegalArgumentException("Division by 0") - else -> Vector2(x / coef, y / coef) - } - - operator fun unaryMinus() = Vector2(-x, -y) - - fun dot(other: Vector2) = x * other.x + y * other.y - - fun distanceTo(other: Vector2) = (this - other).magnitude - - fun translateBy(other: Vector2) = Vector2(this + other) - fun translateBy(x: Double, y: Double) = Vector2(this.x + x, this.y + y) - - fun rotateBy(r: Rotation2d) = Vector2(x * r.cosAngle - y * r.sinAngle, x * r.sinAngle + y * r.cosAngle) - - fun extrapolate(other: Vector2, x: Double) = Vector2( - x * (other.x - this.x) + this.x, x * (other.y - this.y) + this.y - ) - - @Suppress("ReturnCount") - override fun interpolate(other: Vector2, x: Double): Vector2 { - if (x <= 0) return Vector2(this) - else if (x >= 1) return Vector2(other) - else return extrapolate(other, x) - } - - override fun equals(other: Any?): Boolean { - if (other == null || other !is Vector2) return false - return x == other.x && y == other.y - } - - override fun hashCode() = super.hashCode() - - override fun toString(): String = "(X: %.3f, Y: %.3f)".format(x, y) - - override fun toCSV() = "$x,$y" -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt b/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt deleted file mode 100644 index cc92245..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/physics/DCMotorTransmission.kt +++ /dev/null @@ -1,111 +0,0 @@ -package org.team5499.monkeyLib.math.physics - -/** - * Model of a DC motor rotating a shaft. All parameters refer to the output (e.g. should already consider gearing - * and efficiency losses). The motor is assumed to be symmetric forward/reverse. - * - * @property speedPerVolt kV, or rad/s per V (no load) - * @property torquePerVolt N•m per Amp (stall) - * @property frictionVoltage the voltage needed to overcome static - * @property stallAmperage the stall amperage at 12 volts - */ -class DCMotorTransmission( - val speedPerVolt: Double, - val torquePerVolt: Double, - val frictionVoltage: Double, - val stallAmperage: Double -) { - // TODO add electrical constants? (e.g. current) - - val torquePerAmp: Double - val internalResistance: Double - - init { - torquePerAmp = torquePerVolt * 12.0 / stallAmperage - internalResistance = 12.0 / stallAmperage - } - /** - * Returns the idle speed of the motor at this voltage - * - * @param voltage The voltage across the motor - * @return The theoretical speed in rad/s - */ - @Suppress("ReturnCount") - fun freeSpeedAtVoltage(voltage: Double): Double { - if (voltage > 0.0) { - return Math.max(0.0, voltage - frictionVoltage) * speedPerVolt - } else if (0.0 > voltage) { - return Math.min(0.0, voltage + frictionVoltage) * speedPerVolt - } else { - return 0.0 - } - } - - /** - * Get the theoretical torque applied by the motor at a given speed and voltage - * - * @param outputSpeed The speed of the motor in rad/s - * @param voltage The voltage across the motor - * @return The theoretical torque in N•m - */ - fun getTorqueForVoltage(outputSpeed: Double, voltage: Double): Double { - var effectiveVoltage = voltage - if (outputSpeed > 0.0) { - // Forward motion, rolling friction. - effectiveVoltage -= frictionVoltage - } else if (0.0 > outputSpeed) { - // Reverse motion, rolling friction. - effectiveVoltage += frictionVoltage - } else if (voltage > 0.0) { - // System is static, forward torque. - effectiveVoltage = Math.max(0.0, voltage - frictionVoltage) - } else if (0.0 > voltage) { - // System is static, reverse torque. - effectiveVoltage = Math.min(0.0, voltage + frictionVoltage) - } else { - // System is idle. - return 0.0 - } - return torquePerVolt * (-outputSpeed / speedPerVolt + effectiveVoltage) - } - - /** - * Get the required voltage for the requested torque at a speed. - * - * @param outputSpeed The output speed of the motor in rad/s - * @param torque The output torque of the motor in N•m - * @return The theoretical voltage for the requested torque and speed - */ - fun getVoltageForTorque(outputSpeed: Double, torque: Double): Double { - var modifiedFrictionVoltage: Double = 0.0 - if (outputSpeed > 0.0) { - // Forward motion, rolling friction. - modifiedFrictionVoltage = frictionVoltage - } else if (0.0 > outputSpeed) { - // Reverse motion, rolling friction. - modifiedFrictionVoltage = -frictionVoltage - } else if (torque > 0.0) { - // System is static, forward torque. - modifiedFrictionVoltage = frictionVoltage - } else if (0.0 > torque) { - // System is static, reverse torque. - modifiedFrictionVoltage = -frictionVoltage - } else { - // System is idle. - return 0.0 - } - return torque / torquePerVolt + outputSpeed / speedPerVolt + modifiedFrictionVoltage - } - - /** - * Get the theoretical speed for voltage and amperage. - * - * @param voltage - * @param amperage - * @return speed rad/s - */ - fun getSpeedForVoltageAndAmperage(voltage: Double, amperage: Double): Double { - var modifiedVoltage = voltage - frictionVoltage - return (modifiedVoltage - amperage * internalResistance) * speedPerVolt - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/pid/PIDF.kt b/src/main/kotlin/org/team5499/monkeyLib/math/pid/PIDF.kt deleted file mode 100644 index c1c5ff2..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/math/pid/PIDF.kt +++ /dev/null @@ -1,82 +0,0 @@ -package org.team5499.monkeyLib.math.pid - -import edu.wpi.first.wpilibj.Timer - -public class PIDF { - - public var kP: Double - public var kI: Double - public var kD: Double - public var kF: Double - public var setpoint: Double - public var processVariable: Double - - // number of iterations the accumulator keeps track of. - // 0 disables this (keeps track of all loops) - public var integralZone: Int - - public var inverted: Boolean - - private var lastError: Double - private var accumulator: Double - private var integralZoneBuffer: MutableList - private val timer: Timer - - val error: Double - get() = (setpoint - processVariable) - - init { - this.timer = Timer() - this.timer.reset() - this.accumulator = 0.0 - this.lastError = 0.0 - this.setpoint = 0.0 - this.processVariable = 0.0 - this.integralZoneBuffer = mutableListOf() - this.integralZone = 0 - } - - public constructor(kP: Double, kI: Double, kD: Double, kF: Double, inverted: Boolean = false) { - this.kP = kP - this.kI = kI - this.kD = kD - this.kF = kF - this.inverted = inverted - } - - public constructor(): this(0.0, 0.0, 0.0, 0.0, false) - - public fun calculate(): Double { - if (integralZone == 0) { - accumulator += error * timer.get() - } else { - accumulator = 0.0 - integralZoneBuffer.add(error * timer.get()) - while (integralZoneBuffer.size > integralZone) { - integralZoneBuffer.removeAt(0) - } - for (value in integralZoneBuffer) { - accumulator += value - } - } - - val pt = kP * error - val it = kI * accumulator - val dt = kD * (error - lastError) / timer.get() - val ft = kF * setpoint - timer.stop() - timer.reset() - timer.start() - lastError = error - val total = pt + it + dt + ft - return total - } - - public fun reset() { - timer.stop() - timer.reset() - accumulator = 0.0 - lastError = 0.0 - setpoint = 0.0 - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt b/src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt deleted file mode 100644 index 9c8de64..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt +++ /dev/null @@ -1,22 +0,0 @@ -package org.team5499.monkeyLib.path - -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature - -/** -* this class represents a path that can be mirrored over the centerline of the field -* @property left is path on the left side of the field -*/ -public class MirroredPath(left: Path) { - - public val left: Path - public val right: Path - - init { - this.left = left - val newPoints: MutableList = left.points.toMutableList() - for (i in 0..newPoints.size - 1) { - newPoints.set(i, newPoints.get(i).mirror()) - } - this.right = Path(newPoints, left.velocities, left.reversed) - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt b/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt deleted file mode 100644 index e2021fe..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt +++ /dev/null @@ -1,96 +0,0 @@ -package org.team5499.monkeyLib.path - -import org.team5499.monkeyLib.util.CSVWritable - -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature - -class Path( - points: MutableList, - velocities: MutableList, - reversed: Boolean = false -) : CSVWritable { - - internal val points: MutableList - internal val velocities: MutableList - - val pathLength: Int - get() = points.size - - val reversed: Boolean - get() = field - - val startPose: Pose2dWithCurvature - get() = Pose2dWithCurvature(points.get(0)) - - val endPose: Pose2dWithCurvature - get() = Pose2dWithCurvature(points.get(pathLength - 1)) - - val startVelocity: Double - get() = velocities.get(0) - - val endVelocity: Double - get() = velocities.get(velocities.size - 1) - - init { - if (points.size != velocities.size) { - println("coords length: ${points.size}, velo length: ${velocities.size}") - throw IllegalArgumentException("Velocity and Coordinate arrays need to be same length.") - } - if (points.size < 2) throw IllegalArgumentException("Needs to be more than 2 points for a path") - this.reversed = reversed - this.points = points.toMutableList() - this.velocities = velocities.toMutableList() - } - - constructor(other: Path): this(other.points.toMutableList(), other.velocities, other.reversed) - - fun getPose(index: Int): Pose2dWithCurvature { - if (index >= points.size || index < 0) { - throw IndexOutOfBoundsException("Point $index not in path") - } - return points.get(index) - } - - fun getVelocity(index: Int): Double { - if (index >= points.size || index < 0) { - throw IndexOutOfBoundsException("Point $index not in velocities") - } - return velocities.get(index) - } - - fun findClosestPointIndex(point: Pose2d, lastIndex: Int): Int { - val lastPose: Vector2 = points.get(lastIndex).translation - var minDistance: Double = Vector2.distanceBetween(point.translation, lastPose) - var index: Int = lastIndex - for (i in lastIndex..points.size - 1) { - val tempDistance: Double = Vector2.distanceBetween(point.translation, points.get(i).translation) - if (tempDistance < minDistance) { - index = i - minDistance = tempDistance - } - } - return index - } - - override fun toString(): String { - val buffer: StringBuilder = StringBuilder() - for (i in 0..points.size - 1) { - buffer.append(points.get(i).toString()) - buffer.append(System.lineSeparator()) - } - return buffer.toString() - } - - override fun toCSV(): String { - val buffer: StringBuilder = StringBuilder() - for (i in 0..points.size - 1) { - buffer.append(points.get(i).pose.toCSV()) - buffer.append(", ") - buffer.append(velocities.get(i)) - buffer.append(System.lineSeparator()) - } - return buffer.toString() - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/PathFollower.kt b/src/main/kotlin/org/team5499/monkeyLib/path/PathFollower.kt deleted file mode 100644 index 9cf1795..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/path/PathFollower.kt +++ /dev/null @@ -1,139 +0,0 @@ -package org.team5499.monkeyLib.path - -import org.team5499.monkeyLib.math.Epsilon - -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Pose2d - -/** -* class that contains methods to follow a list of coordinates -* used during autonomous -* @property path the path the follower will follow -*/ -@SuppressWarnings("MagicNumber") -class PathFollower(path: Path, trackWidth: Double, initLookaheadDistance: Double) { - - private val mPath: Path - private var mLastClosestPointIndex: Int - - private val mTrackWidth: Double - public var lookaheadDistance: Double - - init { - mPath = path - mLastClosestPointIndex = 0 - - mTrackWidth = trackWidth - lookaheadDistance = initLookaheadDistance - } - - /** - * Function used by path follower to be run in every periodic tick to calculate - * velocities based on current path, robot location, robot angle, and target velocities - * @param currentRobotPose the current position and rotation of the drivetrain - * @return velocities for the left and right sides of the drivetrain - */ - fun update(currentRobotPose: Pose2d): PathFollowerOutput { - var robotAngle = currentRobotPose.rotation.radians - if (robotAngle == 0.0) robotAngle = Epsilon.EPSILON - val lookahead = calculateLookahead(currentRobotPose) - val curvature = calculateCurvature(currentRobotPose, lookahead, robotAngle) - val velocityTarget = mPath.getVelocity(mLastClosestPointIndex) - val negate = if (mPath.reversed) -1.0 else 1.0 - val leftVelo = negate * (velocityTarget * (2.0 + (curvature * mTrackWidth)) / 2.0) - val rightVelo = negate * (velocityTarget * (2.0 - (curvature * mTrackWidth)) / 2.0) - return PathFollowerOutput(leftVelo, rightVelo) - } - - /** - * Calcuates the lookahead point based on robot position and desired lookahead distance. - * Algorithm basically creates a circle around the robot with radius = lookahead distance - * then finds the intersection point with the path line. It then chooses the most suitable point. - * If it doesnt find a lookahead point, it sets the lookahead to the last point in the path - * @param robotPose current pose of the robot - * @return calculated lookahead of the robot as a Vector2 - */ - @Suppress("ComplexMethod") - private fun calculateLookahead(robotPose: Pose2d): Vector2 { - mLastClosestPointIndex = mPath.findClosestPointIndex(robotPose, mLastClosestPointIndex) - var lookahead: Vector2? = null - for (i in mLastClosestPointIndex..mPath.pathLength - 2) { - val begin = mPath.getPose(i) - val end = mPath.getPose(i + 1) - val d = end.translation - begin.translation - val f = begin.translation - robotPose.translation - - val a = d.dot(d) - val b = 2.0 * f.dot(d) - val c = f.dot(f) - Math.pow(lookaheadDistance, 2.0) - var dis = (b * b) - (4.0 * a * c) - if (dis < 0.0) { - continue - } else { - dis = Math.sqrt(dis) - val t1 = (-b - dis) / (2.0 * a) - val t2 = (-b + dis) / (2.0 * a) - if (t1 >= 0 && t1 <= 1) { - val temp: Vector2 = d * t1 - lookahead = begin.translation + temp - break - } else if (t2 >= 0 && t2 <= 1) { - val temp = d * t2 - lookahead = begin.translation + temp - break - } - } - } - if (lookahead == null) { - lookahead = mPath.endPose.translation - } else { - val distanceToEnd = robotPose.translation.distanceTo(mPath.endPose.translation) - if (distanceToEnd < lookaheadDistance) { - lookahead = mPath.endPose.translation - } - } - return lookahead - } - - /** - * calculates curvature between robot point and lookahead point - * @param robotPose current pose of the robot - * @param lookahead lookahead point - * @param robotAngle the modified robot angle to prevent undefined curvatures - * @return the curvature of the arc that the robot must follow to reach the lookahead point - */ - private fun calculateCurvature(robotPose: Pose2d, lookahead: Vector2, robotAngle: Double): Double { - val a = (1 / Math.tan(robotAngle)) - 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(lookaheadDistance, 2.0)) - val side = Math.signum( - Math.sin(robotAngle) * (lookahead.x - robotPose.translation.x) - - Math.cos(robotAngle) * (lookahead.y - robotPose.translation.y) - ) - return curvature * side - } - /** - * @param robotPos position of the robot - * @return whether the robot is done with the the designated path based on robot location - */ - fun doneWithPath(robotPose: Pose2d): Boolean { - val distance = robotPose.translation.distanceTo(mPath.endPose.translation) - val done = distance < PATH_EXTENSION_LENGTH - return done - } - - /** - * resets follower to first point - */ - fun reset() { - mLastClosestPointIndex = 0 - } - - /** - * @property leftVelocity target velocity of the right side of the drivetrain - * @property rightVelocity target velocity of the right side of the drivetrain - */ - public data class PathFollowerOutput(val leftVelocity: Double = 0.0, val rightVelocity: Double = 0.0) -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt b/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt deleted file mode 100644 index a81d52d..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/path/PathGenerator.kt +++ /dev/null @@ -1,175 +0,0 @@ -package org.team5499.monkeyLib.path - -import org.team5499.monkeyLib.math.splines.QuinticHermiteSpline -import org.team5499.monkeyLib.math.splines.SplineGenerator -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature - -// length added to end of path to allow easier path following -const val PATH_EXTENSION_LENGTH = 12.0 // inches - -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 - - public constructor( - defaultMaxVelocity: Double, - defaultMaxAcceleration: Double, - defaultStartVelocity: Double, - defaultEndVelocity: Double - ) { - 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") - public fun generatePath( - reversed: Boolean, - waypoints: Array, - maxVelo: Double, - maxAccel: Double, - startVelo: Double, - endVelo: Double - ): Path { - val waypointsMaybeFlipped = waypoints.toMutableList() - val flip = Pose2d.fromRotation(Rotation2d(-1.0, 0.0, false)) - if (reversed) { - waypointsMaybeFlipped.clear() - for (pose in waypoints) { - waypointsMaybeFlipped.add(pose.transformBy(flip)) - } - } - - val splines: MutableList = mutableListOf() - for (i in 0..waypointsMaybeFlipped.size - 2) { - splines.add(QuinticHermiteSpline(waypointsMaybeFlipped.get(i), waypointsMaybeFlipped.get(i + 1))) - } - QuinticHermiteSpline.optimizeSpline(splines) - - var samples = SplineGenerator.parameterizeSplines(splines) - if (reversed) { - val flipped = samples.toMutableList() - flipped.clear() - for (i in 0..samples.size - 1) { - flipped.add(Pose2dWithCurvature( - samples.get(i).pose.transformBy(flip), - -samples.get(i).curvature, - samples.get(i).dCurvature - )) - } - samples = flipped - } - - // extend last segment by lookahead distance - val lastNorm = (samples.get(samples.size - 1).translation - - samples.get(samples.size - 2).translation).normalized - val newSegment = samples.get(samples.size - 1).translation + (lastNorm * PATH_EXTENSION_LENGTH) - samples.set(samples.size - 1, Pose2dWithCurvature( - Pose2d(newSegment, samples.get(samples.size - 1).rotation), - samples.get(samples.size - 1).curvature, - samples.get(samples.size - 1).dCurvature) - ) - - val velocities = mutableListOf() - for (i in 0..samples.size - 1) { - velocities.add(Math.min(maxVelo, Math.abs(3.0 / samples.get(i).curvature))) - } - velocities.set(velocities.size - 1, endVelo) - for (i in (samples.size - 2).downTo(0)) { - val distance = samples.get(i).translation.distanceTo(samples.get(i + 1).translation) - val value = Math.min( - velocities.get(i), - Math.sqrt(Math.pow(velocities.get(i + 1), 2.0) + 2.0 * maxAccel * distance) - ) - velocities.set(i, value) - } - - velocities.set(0, startVelo) - for (i in 0..samples.size - 2) { - val distance = samples.get(i).translation.distanceTo(samples.get(i + 1).translation) - val value = Math.sqrt(Math.pow(velocities.get(i), 2.0) + 2.0 * maxAccel * distance) - if (value < velocities.get(i + 1)) - velocities.set(i + 1, value) - } - - return Path(samples, velocities, reversed) - } - - public fun generatePath(reversed: Boolean, waypoints: Array, maxVelo: Double, maxAccel: Double): Path { - return generatePath(reversed, waypoints, maxVelo, maxAccel, mStartPathVelocity(), mEndPathVelocity()) - } - - public fun generatePath(reversed: Boolean, waypoints: Array): Path { - return generatePath( - reversed, waypoints, - mMaxVelocity(), - mMaxAcceleration(), - mStartPathVelocity(), - mEndPathVelocity() - ) - } - - @Suppress("LongParameterList") - public fun generateMirroredPath( - reversed: Boolean, - waypoints: Array, - maxVelo: Double, - maxAccel: Double, - startVelocity: Double, - endVelocity: Double - ): MirroredPath { - return MirroredPath( - generatePath( - reversed, - waypoints, - maxVelo, - maxAccel, - startVelocity, - endVelocity - ) - ) - } - - public fun generateMirroredPath( - reversed: Boolean, - waypoints: Array, - maxVelo: Double, - maxAccel: Double - ): MirroredPath { - return MirroredPath( - generatePath( - reversed, - waypoints, - maxVelo, - maxAccel - ) - ) - } - - public fun generateMirroredPath(reversed: Boolean, waypoints: Array): MirroredPath { - return MirroredPath( - generatePath( - reversed, - waypoints - ) - ) - } -} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/PathUtils.kt b/src/main/kotlin/org/team5499/monkeyLib/path/PathUtils.kt deleted file mode 100644 index f7c12d2..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/path/PathUtils.kt +++ /dev/null @@ -1,17 +0,0 @@ -package org.team5499.monkeyLib.path - -typealias PathSet = MutableList - -typealias MirroredPathSet = MutableList - -@Suppress("FunctionNaming") -public fun PathSet() = pathSetOf() - -@Suppress("FunctionNaming") -public fun MirroredPathSet() = mirroredPathSetOf() - -@Suppress("SpreadOperator") -fun pathSetOf(vararg paths: Path) = mutableListOf(*paths) - -@Suppress("SpreadOperator") -fun mirroredPathSetOf(vararg paths: MirroredPath) = mutableListOf(*paths) diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt b/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt deleted file mode 100644 index f522bcb..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt +++ /dev/null @@ -1,107 +0,0 @@ -package org.team5499.monkeyLib.util - -@SuppressWarnings("MagicNumber") -object Utils { - - /** - * method that limits input to a certain range. - * @param value value to be limited - * @param limit upper and lower bound of the value - * @return bounded value - */ - public fun limit(value: Double, limit: Double): Double { - return limit(value, -limit, limit) - } - - /** - * method that limits input to a certain range. - * @param value value to be limited - * @param min lower bound of the value - * @param max upper bound of the value - * @return bounded value - */ - public fun limit(value: Double, min: Double, max: Double): Double { - return Math.min(max, Math.max(min, value)) - } - - /** - * method to interpolate between 2 doubles - * @param a first value - * @param b second value - * @param x value between the 2 values - * @return interpolated value - */ - public fun interpolate(a: Double, b: Double, x: Double): Double { - val newX = limit(x, 0.0, 1.0) - return a + (b - a) * newX - } - - /** - * @param encoderTicksPerRotation encoder ticks per 360 degrees of rotation of the encoder shaft - * @param circumference circumference of the object being rotated - * @param inchesPerSecond velocity to be converted - * @param reduction reduction from the encoder shaft to the output shaft - * @return velocity in ticks per 100ms - */ - public fun inchesPerSecondToEncoderTicksPer100Ms( - encoderTicksPerRotation: Int, - circumference: Double, - inchesPerSecond: Double, - reduction: Double = 1.0 - ): Double { - return inchesToEncoderTicks(encoderTicksPerRotation, circumference, inchesPerSecond, reduction) / 10.0 - } - - /** - * @param encoderTicksPerRotation encoder ticks per 360 degrees of rotation of the encoder shaft - * @param circumference circumference of the object being rotated - * @param encoderTicksPer100ms velocity to be converted - * @param reduction reduction from the encoder shaft to the output shaft - */ - public fun encoderTicksPer100MsToInchesPerSecond( - encoderTicksPerRotation: Int, - circumference: Double, - encoderTicksPer100ms: Int, - reduction: Double = 1.0 - ): Double { - return encoderTicksToInches(encoderTicksPerRotation, circumference, encoderTicksPer100ms, reduction) * 10.0 - } - - /** - * @param encoderTicksPerRotation encoder ticks per 360 degrees of rotation of the encoder shaft - * @param circumference circumference of the object being rotated - * @param inches inches of travel to be converted to ticks of travel - * @param reduction reduction from the encoder shaft to the output shaft - */ - public fun inchesToEncoderTicks( - encoderTicksPerRotation: Int, - circumference: Double, - inches: Double, - reduction: Double = 1.0 - ): Int { - return (encoderTicksPerRotation * inches * reduction / circumference).toInt() - } - - /** - * @param encoderTicksPerRotation encoder ticks per 360 degrees of rotation of the encoder shaft - * @param circumference circumference of the object being rotated - * @param ticks ticks of travel to be converted to inches of travel - * @param reduction reduction from the encoder shaft to the output shaft - */ - public fun encoderTicksToInches( - encoderTicksPerRotation: Int, - circumference: Double, - ticks: Int, - reduction: Double = 1.0 - ): Double { - return (circumference * ticks) / (encoderTicksPerRotation * reduction) - } - - public fun talonAngleToDegrees(gyroTicksPerRotation: Int, ticks: Int): Double { - return (360.0 / gyroTicksPerRotation) * ticks - } - - public fun degreesToTalonAngle(gyroTicksPerRotation: Int, degrees: Double): Int { - return ((gyroTicksPerRotation / 360.0) * degrees).toInt() - } -} diff --git a/src/test/kotlin/tests/input/DriveHelperTest.kt b/src/test/kotlin/tests/input/DriveHelperTest.kt deleted file mode 100644 index d0c1824..0000000 --- a/src/test/kotlin/tests/input/DriveHelperTest.kt +++ /dev/null @@ -1,196 +0,0 @@ -package tests.input - -import org.team5499.monkeyLib.input.DriveSignal -import org.team5499.monkeyLib.input.TankDriveHelper -import org.team5499.monkeyLib.input.SpaceDriveHelper -import org.team5499.monkeyLib.input.CheesyDriveHelper - -import org.team5499.monkeyLib.math.Epsilon - -import org.junit.Test -import org.junit.Assert.assertEquals -import org.junit.Assert.assertFalse - -class DriveHelperTest { - - @Suppress("LongMethod") - @Test - fun testCheesyDriveHelper() { - val config = CheesyDriveHelper.CheesyDriveConfig() - config.deadband = 0.02 - config.quickstopDeadband = 1.0 - config.quickstopWeight = 1.0 - config.quickstopScalar = 1.0 - config.highWheelNonlinearity = 1.0 - config.lowWheelNonlinearity = 1.0 - config.highNeginertiaScalar = 1.0 - config.highSensitivity = 1.0 - config.lowNeginertiaTurnScalar = 1.0 - config.lowNeginertiaThreshold = 1.0 - config.lowNeginertiaFarScalar = 1.0 - config.lowNeginertiaCloseScalar = 1.0 - config.lowSensitivity = 1.0 - - // need to reset helper every time because it contains references to past calls - var helper = CheesyDriveHelper(config) - var output = helper.calculateOutput(0.5, 0.0, false, false) - assertEquals(output.left, 0.5, Epsilon.EPSILON) - assertEquals(output.right, 0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(-0.5, 0.0, false, false) - assertEquals(output.left, -0.5, Epsilon.EPSILON) - assertEquals(output.right, -0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(0.01, 0.01, false, false) - assertEquals(output.left, 0.0, Epsilon.EPSILON) - assertEquals(output.right, 0.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(1.0, 0.5, false, false) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, -0.486690, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(1.0, -0.5, false, false) - assertEquals(output.left, -0.486690, Epsilon.EPSILON) - assertEquals(output.right, 1.0, Epsilon.EPSILON) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(0.0, 1.0, true, false) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, -3.0, Epsilon.EPSILON) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(1.0, 0.0, true, false) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, 1.0, Epsilon.EPSILON) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(1.0, 0.0, false, true) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, 1.0, Epsilon.EPSILON) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(-1.0, 0.0, false, true) - assertEquals(output.left, -1.0, Epsilon.EPSILON) - assertEquals(output.right, -1.0, Epsilon.EPSILON) - - helper = CheesyDriveHelper(config) - output = helper.calculateOutput(1.0, 0.5, true, true) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, -1.792037871853613, Epsilon.EPSILON) - - println(output.toString()) - } - - @Suppress("LongMethod") - @Test - fun testSpaceDriveHelper() { - val helper = SpaceDriveHelper(0.02, 0.4, 0.5) - var output = helper.calculateOutput(0.01, 0.01, false) - assertEquals(output.left, 0.0, Epsilon.EPSILON) - assertEquals(output.right, 0.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(1.0, 0.0, false) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, 1.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(-1.0, 0.0, false) - assertEquals(output.left, -1.0, Epsilon.EPSILON) - assertEquals(output.right, -1.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.0, 0.5, true) - assertEquals(output.left, 0.5, Epsilon.EPSILON) - assertEquals(output.right, -0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.0, -0.5, true) - assertEquals(output.left, -0.5, Epsilon.EPSILON) - assertEquals(output.right, 0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.0, 0.5, false) - assertEquals(output.left, 0.5 * 0.4, Epsilon.EPSILON) - assertEquals(output.right, -0.5 * 0.4, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.0, -0.5, false) - assertEquals(output.left, -0.5 * 0.4, Epsilon.EPSILON) - assertEquals(output.right, 0.5 * 0.4, Epsilon.EPSILON) - assertFalse(output.brakeMode) - } - - @Suppress("LongMethod") - @Test - fun testTankDriveHelper() { - val helper = TankDriveHelper(0.02, 0.5) - var output = helper.calculateOutput(0.01, 0.01, false) - assertEquals(output.left, 0.0, Epsilon.EPSILON) - assertEquals(output.right, 0.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(-0.01, -0.01, false) - assertEquals(output.left, 0.0, Epsilon.EPSILON) - assertEquals(output.right, 0.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.5, 0.5, false) - assertEquals(output.left, 0.5, Epsilon.EPSILON) - assertEquals(output.right, 0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(1.0, 1.0, false) - assertEquals(output.left, 1.0, Epsilon.EPSILON) - assertEquals(output.right, 1.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(1.0, 1.0, true) - assertEquals(output.left, 0.5, Epsilon.EPSILON) - assertEquals(output.right, 0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(0.01, 1.0, true) - assertEquals(output.left, 0.0, Epsilon.EPSILON) - assertEquals(output.right, 0.5, Epsilon.EPSILON) - assertFalse(output.brakeMode) - - output = helper.calculateOutput(1.0, 0.01, true) - assertEquals(output.left, 0.5, Epsilon.EPSILON) - assertEquals(output.right, 0.0, Epsilon.EPSILON) - assertFalse(output.brakeMode) - } - - @Test - fun testDriveSignal() { - var signal = DriveSignal() - assertEquals(signal.left, 0.0, Epsilon.EPSILON) - assertEquals(signal.right, 0.0, Epsilon.EPSILON) - assert(signal.brakeMode == false) - println("toString: " + signal.toString()) - println("toCSV: " + signal.toCSV()) - - signal = DriveSignal.NEUTRAL - assertEquals(signal.left, 0.0, Epsilon.EPSILON) - assertEquals(signal.right, 0.0, Epsilon.EPSILON) - assert(signal.brakeMode == false) - - signal = DriveSignal.BRAKE - assertEquals(signal.left, 0.0, Epsilon.EPSILON) - assertEquals(signal.right, 0.0, Epsilon.EPSILON) - assert(signal.brakeMode == true) - - signal = DriveSignal(0.973, -0.254) - assertEquals(signal.left, 0.973, Epsilon.EPSILON) - assertEquals(signal.right, -0.254, Epsilon.EPSILON) - assert(signal.brakeMode == false) - } -} diff --git a/src/test/kotlin/tests/math/EpsilonTest.kt b/src/test/kotlin/tests/math/EpsilonTest.kt index 137a89b..a4d6041 100644 --- a/src/test/kotlin/tests/math/EpsilonTest.kt +++ b/src/test/kotlin/tests/math/EpsilonTest.kt @@ -1,17 +1,16 @@ package tests.math -import org.team5499.monkeyLib.math.Epsilon - import org.junit.Test import org.junit.Assert.assertTrue +import org.team5419.fault.math.epsilonEquals class EpsilonTest { @Test - public fun testEpsilon() { - var temp = 3.3333333 - assertTrue(Epsilon.epsilonEquals(temp, 3.333333334)) + fun testEpsilon() { + var temp = 3.3333333333 + assertTrue(temp epsilonEquals 3.3333333333) temp = 1E-10 - assertTrue(Epsilon.epsilonEquals(temp)) + assertTrue(temp epsilonEquals 0.0) } } diff --git a/src/test/kotlin/tests/math/GeometryTests.kt b/src/test/kotlin/tests/math/GeometryTests.kt index 69cadea..9bbff9b 100644 --- a/src/test/kotlin/tests/math/GeometryTests.kt +++ b/src/test/kotlin/tests/math/GeometryTests.kt @@ -1,284 +1,72 @@ package tests.math -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Twist2d - -import org.team5499.monkeyLib.math.Epsilon - +import org.junit.Assert import org.junit.Test -import org.junit.Assert.assertEquals -import org.junit.Assert.assertTrue -import org.junit.Assert.assertFalse +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Rectangle2d +import org.team5419.fault.math.geometry.Vector2 +import org.team5419.fault.math.kEpsilon +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.derived.toRotation2d +import org.team5419.fault.math.units.meters @Suppress("LargeClass") -public class GeometryTests { +class GeometryTests { - companion object { - private const val testEpsilon = Epsilon.EPSILON - } - - @Suppress("LongMethod") @Test - fun testVector2() { - var pos1 = Vector2() - assertEquals(0.0, pos1.x, testEpsilon) - assertEquals(0.0, pos1.y, testEpsilon) - assertEquals(0.0, pos1.magnitude, testEpsilon) + fun testTransforms() { + // Position of the static object + val staticObjectPose = Pose2d(10.meters, 10.meters, 0.degrees) - pos1 = Vector2(3.0, 4.0) - assertEquals(3.0, pos1.x, testEpsilon) - assertEquals(4.0, pos1.y, testEpsilon) - assertEquals(5.0, pos1.magnitude, testEpsilon) + // Position of the camera on the robot. + // Camera is on the back of the robot (1 foot behind the center) + // Camera is facing backward + val robotToCamera = Pose2d((-1).meters, 0.meters, 180.degrees) - pos1 = Vector2(3.152, 4.1666) - var pos2 = -pos1 - assertEquals(-pos2.x, pos1.x, testEpsilon) - assertEquals(-pos2.y, pos1.y, testEpsilon) - assertEquals(pos1.magnitude, pos2.magnitude, testEpsilon) + // The camera detects the static object 9 meter in front and 2 meter to the right of it. + val cameraToStaticObject = Pose2d(9.meters, 2.meters, 0.degrees) - pos1 = Vector2(2, 0) - var rot1 = Rotation2d.fromDegrees(90.0) - pos2 = pos1.rotateBy(rot1) - assertEquals(0.0, pos2.x, testEpsilon) - assertEquals(2.0, pos2.y, testEpsilon) - assertEquals(pos1.magnitude, pos2.magnitude, testEpsilon) + // Transform the static object into the robot's coordinates + val robotToStaticObject = robotToCamera + cameraToStaticObject - pos1 = Vector2(2, 0) - rot1 = Rotation2d.fromDegrees(-45.0) - pos2 = pos1.rotateBy(rot1) - assertEquals(Math.sqrt(2.0), pos2.x, testEpsilon) - assertEquals(-Math.sqrt(2.0), pos2.y, testEpsilon) + // Get the global robot pose + val globalRobotPose = staticObjectPose - robotToStaticObject - pos1 = Vector2(2, 0) - pos2 = Vector2(-2, 1) - var pos3 = pos1.translateBy(pos2) - assertEquals(0.0, pos3.x, testEpsilon) - assertEquals(1.0, pos3.y, testEpsilon) - assertEquals(1.0, pos3.magnitude, testEpsilon) + println( + "X: ${globalRobotPose.translation.x.value}, Y: ${globalRobotPose.translation.y.value}, " + + "Angle: ${globalRobotPose.rotation.degree}" + ) - val identity = Vector2() - pos1 = Vector2(2.16612, -23.55) - pos2 = pos1.translateBy(-pos1) - assertEquals(identity.x, pos2.x, testEpsilon) - assertEquals(identity.y, pos2.y, testEpsilon) - assertEquals(identity.magnitude, pos2.magnitude, testEpsilon) + Assert.assertEquals(0.0, globalRobotPose.translation.x.value, kEpsilon) + Assert.assertEquals(8.0, globalRobotPose.translation.y.value, kEpsilon) + println(globalRobotPose.rotation.toString()) + Assert.assertEquals((-180).degrees.toRotation2d(), globalRobotPose.rotation) + } - pos1 = Vector2(0, 1) - pos2 = Vector2(10, -1) - pos3 = pos1.interpolate(pos2, 0.5) - assertEquals(5.0, pos3.x, testEpsilon) - assertEquals(0.0, pos3.y, testEpsilon) + @Test + fun testRectangleContains() { + val rectangle = Rectangle2d(Vector2(0.meters, 0.meters), Vector2(10.meters, 10.meters)) + val translation = Vector2(5.meters, 7.meters) + assert(rectangle.contains(translation)) + } - pos1 = Vector2(0, 1) - pos2 = Vector2(10, -1) - pos3 = pos1.interpolate(pos2, 0.75) - assertEquals(7.5, pos3.x, testEpsilon) - assertEquals(-0.5, pos3.y, testEpsilon) + @Suppress("LongMethod") + @Test + fun testPose() { } @Suppress("LongMethod") @Test fun testTwist() { - var twist = Twist2d(1.0, 0.0, 0.0) - var pose = Pose2d.exp(twist) - assertEquals(1.0, pose.translation.x, testEpsilon) - assertEquals(0.0, pose.translation.y, testEpsilon) - assertEquals(0.0, pose.rotation.degrees, testEpsilon) - - twist = Twist2d(1.0, 0.0, 0.0) - pose = Pose2d.exp(twist * 2.5) - assertEquals(2.5, pose.translation.x, testEpsilon) - assertEquals(0.0, pose.translation.y, testEpsilon) - assertEquals(0.0, pose.rotation.degrees, testEpsilon) - - pose = Pose2d(Vector2(2.0, 2.0), Rotation2d.fromRadians(Math.PI / 2)) - twist = Pose2d.log(pose) - assertEquals(Math.PI, twist.dx, testEpsilon) - assertEquals(0.0, twist.dy, testEpsilon) - assertEquals(Math.PI / 2, twist.dTheta, testEpsilon) - - val new_pose = Pose2d.exp(twist) - assertEquals(new_pose.translation.x, pose.translation.x, testEpsilon) - // println("pose y: ${new_pose.translation.y}, pose y: ${pose.translation.y}") - assertEquals(new_pose.translation.y, pose.translation.y, testEpsilon) - assertEquals(new_pose.rotation.degrees, pose.rotation.degrees, testEpsilon) } @Suppress("LongMethod") @Test fun testRotation() { - // Test constructors - var rot1 = Rotation2d() - assertEquals(1.0, rot1.cosAngle, testEpsilon) - assertEquals(0.0, rot1.sinAngle, testEpsilon) - // println(rot1.tan) - assertEquals(0.0, rot1.tan, testEpsilon) - assertEquals(0.0, rot1.degrees, testEpsilon) - assertEquals(0.0, rot1.radians, testEpsilon) - - rot1 = Rotation2d(1.0, 1.0, true) - assertEquals(Math.sqrt(2.0) / 2.0, rot1.cosAngle, testEpsilon) - assertEquals(Math.sqrt(2.0) / 2.0, rot1.sinAngle, testEpsilon) - assertEquals(1.0, rot1.tan, testEpsilon) - assertEquals(45.0, rot1.degrees, testEpsilon) - assertEquals(Math.PI / 4.0, rot1.radians, testEpsilon) - - rot1 = Rotation2d.fromRadians(Math.PI / 2) - assertEquals(0.0, rot1.cosAngle, testEpsilon) - assertEquals(1.0, rot1.sinAngle, testEpsilon) - assertTrue(1.0 / testEpsilon < rot1.tan) - assertEquals(90.0, rot1.degrees, testEpsilon) - assertEquals(Math.PI / 2.0, rot1.radians, testEpsilon) - - rot1 = Rotation2d.fromDegrees(270.0) - assertEquals(0.0, rot1.cosAngle, testEpsilon) - assertEquals(-1.0, rot1.sinAngle, testEpsilon) - // println(rot1.tan) - assertTrue(-1.0 / testEpsilon > rot1.tan) - assertEquals(-90.0, rot1.degrees, testEpsilon) - assertEquals(-Math.PI / 2.0, rot1.radians, testEpsilon) - - // Test inversion - rot1 = Rotation2d.fromDegrees(270.0) - var rot2 = rot1.inverse() - // println(rot2.cosAngle) - assertEquals(0.0, rot2.cosAngle, testEpsilon) - assertEquals(1.0, rot2.sinAngle, testEpsilon) - assertTrue(1.0 / testEpsilon < rot2.tan) - assertEquals(90.0, rot2.degrees, testEpsilon) - assertEquals(Math.PI / 2, rot2.radians, testEpsilon) - - rot1 = Rotation2d.fromDegrees(1.0) - rot2 = rot1.inverse() - assertEquals(rot1.cosAngle, rot2.cosAngle, testEpsilon) - assertEquals(-rot1.sinAngle, rot2.sinAngle, testEpsilon) - assertEquals(-1.0, rot2.degrees, testEpsilon) - - // Test rotateBy - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(45.0) - var rot3 = rot1.rotateBy(rot2) - assertEquals(0.0, rot3.cosAngle, testEpsilon) - assertEquals(1.0, rot3.sinAngle, testEpsilon) - assertTrue(1.0 / testEpsilon < rot3.tan) - assertEquals(90.0, rot3.degrees, testEpsilon) - assertEquals(Math.PI / 2.0, rot3.radians, testEpsilon) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(-45.0) - rot3 = rot1.rotateBy(rot2) - assertEquals(1.0, rot3.cosAngle, testEpsilon) - assertEquals(0.0, rot3.sinAngle, testEpsilon) - assertEquals(0.0, rot3.tan, testEpsilon) - assertEquals(0.0, rot3.degrees, testEpsilon) - assertEquals(0.0, rot3.radians, testEpsilon) - - // A rotation times its inverse should be the identity - val identity = Rotation2d() - rot1 = Rotation2d.fromDegrees(21.45) - rot2 = rot1.rotateBy(rot1.inverse()) - assertEquals(identity.cosAngle, rot2.cosAngle, testEpsilon) - assertEquals(identity.sinAngle, rot2.sinAngle, testEpsilon) - assertEquals(identity.degrees, rot2.degrees, testEpsilon) - - // Test interpolation - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(135.0) - rot3 = rot1.interpolate(rot2, 0.5) - assertEquals(90.0, rot3.degrees, testEpsilon) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(135.0) - rot3 = rot1.interpolate(rot2, 0.75) - assertEquals(112.5, rot3.degrees, testEpsilon) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(-45.0) - rot3 = rot1.interpolate(rot2, 0.5) - assertEquals(0.0, rot3.degrees, testEpsilon) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(45.0) - rot3 = rot1.interpolate(rot2, 0.5) - assertEquals(45.0, rot3.degrees, testEpsilon) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(45.0) - rot3 = rot1.interpolate(rot2, 0.5) - assertEquals(45.0, rot3.degrees, testEpsilon) - - // Test parallel. - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(45.0) - assertTrue(rot1.isParallel(rot2)) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(-45.0) - assertFalse(rot1.isParallel(rot2)) - - rot1 = Rotation2d.fromDegrees(45.0) - rot2 = Rotation2d.fromDegrees(-135.0) - assertTrue(rot1.isParallel(rot2)) } @Suppress("LongMethod") @Test fun testPose2d() { - var pose1 = Pose2d() - var pose2 = Pose2d() - - // assertTrue(pose1.equals(pose2)) - println(pose1.toString()) - println(pose1.toCSV()) - println(pose1.hashCode()) - - assertEquals(0.0, pose1.translation.x, testEpsilon) - assertEquals(0.0, pose1.translation.y, testEpsilon) - assertEquals(0.0, pose1.rotation.degrees, testEpsilon) - - pose1 = Pose2d(Vector2(3, 4), Rotation2d.fromDegrees(45.0)) - assertEquals(3.0, pose1.translation.x, testEpsilon) - assertEquals(4.0, pose1.translation.y, testEpsilon) - assertEquals(45.0, pose1.rotation.degrees, testEpsilon) - - pose1 = Pose2d(Vector2(3, 4), Rotation2d.fromDegrees(90.0)) - pose2 = Pose2d(Vector2(1, 0), Rotation2d.fromDegrees(0.0)) - var pose3 = pose1.transformBy(pose2) - assertEquals(3.0, pose3.translation.x, testEpsilon) - assertEquals(5.0, pose3.translation.y, testEpsilon) - assertEquals(90.0, pose3.rotation.degrees, testEpsilon) - - pose1 = Pose2d(Vector2(3, 4), Rotation2d.fromDegrees(90.0)) - pose2 = Pose2d(Vector2(1, 0), Rotation2d.fromDegrees(-90.0)) - pose3 = pose1.transformBy(pose2) - assertEquals(3.0, pose3.translation.x, testEpsilon) - assertEquals(5.0, pose3.translation.y, testEpsilon) - assertEquals(0.0, pose3.rotation.degrees, testEpsilon) - - val identity = Pose2d() - pose1 = Pose2d(Vector2(3.51512152, 4.23), Rotation2d.fromDegrees(91.6)) - pose2 = pose1.transformBy(pose1.inverse()) - assertEquals(identity.translation.x, pose2.translation.x, testEpsilon) - assertEquals(identity.translation.y, pose2.translation.y, testEpsilon) - assertEquals(identity.rotation.degrees, pose2.rotation.degrees, testEpsilon) - - pose1 = Pose2d(Vector2(3, 4), Rotation2d.fromDegrees(90.0)) - pose2 = Pose2d(Vector2(13, -6), Rotation2d.fromDegrees(0.0)) - pose3 = pose1.interpolate(pose2, 0.5) - var expected_angle_rads = Math.PI / 4.0 - assertEquals(3.0 + 10.0 * Math.cos(expected_angle_rads), pose3.translation.x, testEpsilon) - assertEquals(-6.0 + 10.0 * Math.sin(expected_angle_rads), pose3.translation.y, testEpsilon) - assertEquals(expected_angle_rads, pose3.rotation.radians, testEpsilon) - - pose1 = Pose2d(Vector2(3, 4), Rotation2d.fromDegrees(90.0)) - pose2 = Pose2d(Vector2(13, -6), Rotation2d.fromDegrees(0.0)) - pose3 = pose1.interpolate(pose2, 0.75) - expected_angle_rads = Math.PI / 8.0 - assertEquals(3.0 + 10.0 * Math.cos(expected_angle_rads), pose3.translation.x, testEpsilon) - assertEquals(-6.0 + 10.0 * Math.sin(expected_angle_rads), pose3.translation.y, testEpsilon) - assertEquals(expected_angle_rads, pose3.rotation.radians, testEpsilon) } } diff --git a/src/test/kotlin/tests/math/control/RamseteFollowerTest.kt b/src/test/kotlin/tests/math/control/RamseteFollowerTest.kt new file mode 100644 index 0000000..209325f --- /dev/null +++ b/src/test/kotlin/tests/math/control/RamseteFollowerTest.kt @@ -0,0 +1,124 @@ +package tests.math.control + +import org.junit.Test +import org.knowm.xchart.XYChartBuilder +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.units.Meter +import org.team5419.fault.math.units.inFeet +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.feet +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.milliseconds +import org.team5419.fault.math.units.inSeconds + +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.simulation.SimulatedBerkeliumMotor +import org.team5419.fault.simulation.SimulatedDifferentialDrive +import org.team5419.fault.trajectory.followers.RamseteFollower +import tests.math.splines.TrajectoryGeneratorTest +import java.awt.Color +import java.awt.Font +import java.text.DecimalFormat + +class RamseteFollowerTest { + + private val kBeta = 2.0 + private val kZeta = 0.85 + + @Suppress("LongMethod") + @Test + fun testTrajectoryFollower() { + val ramseteTracker = RamseteFollower( + kBeta, + kZeta + ) + + val drive = SimulatedDifferentialDrive( + TrajectoryGeneratorTest.drive, + SimulatedBerkeliumMotor(), + SimulatedBerkeliumMotor(), + ramseteTracker, + 1.05 + ) + + var currentTime = 0.seconds + val deltaTime = 20.milliseconds + + val xList = arrayListOf() + val yList = arrayListOf() + + val refXList = arrayListOf() + val refYList = arrayListOf() + + ramseteTracker.reset(TrajectoryGeneratorTest.trajectory) + + drive.robotPosition = ramseteTracker.referencePoint!!.state.state.pose + .transformBy(Pose2d(1.feet, 50.inches, 5.degrees)) + + while (!ramseteTracker.isFinished) { + currentTime += deltaTime + drive.setOutput(ramseteTracker.nextState(drive.robotPosition, currentTime)) + drive.update(deltaTime) + + xList += drive.robotPosition.translation.x.inFeet() + yList += drive.robotPosition.translation.y.inFeet() + + val referenceTranslation = ramseteTracker.referencePoint!!.state.state.pose.translation + refXList += referenceTranslation.x.inFeet() + refYList += referenceTranslation.y.inFeet() + + // TODO add voltage to the sim +// System.out.printf( +// "Left Voltage: %3.3f, Right Voltage: %3.3f%n", +// drive.leftMotor.voltageOutput.value, drive.rightMotor.voltageOutput.value +// ) + } + + val fm = DecimalFormat("#.###").format(TrajectoryGeneratorTest.trajectory.lastInterpolant.inSeconds()) + + val chart = XYChartBuilder().width(1800).height(1520).title("$fm seconds.") + .xAxisTitle("X").yAxisTitle("Y").build() + + chart.styler.markerSize = 8 + chart.styler.seriesColors = arrayOf(Color.ORANGE, Color(151, 60, 67)) + + chart.styler.chartTitleFont = Font("Kanit", 1, 40) + chart.styler.chartTitlePadding = 15 + + chart.styler.xAxisMin = 1.0 + chart.styler.xAxisMax = 26.0 + chart.styler.yAxisMin = 1.0 + chart.styler.yAxisMax = 26.0 + + chart.styler.chartFontColor = Color.WHITE + chart.styler.axisTickLabelsColor = Color.WHITE + + chart.styler.legendBackgroundColor = Color.GRAY + + chart.styler.isPlotGridLinesVisible = true + chart.styler.isLegendVisible = true + + chart.styler.plotGridLinesColor = Color.GRAY + chart.styler.chartBackgroundColor = Color.DARK_GRAY + chart.styler.plotBackgroundColor = Color.DARK_GRAY + + chart.addSeries("Trajectory", refXList.toDoubleArray(), refYList.toDoubleArray()) + chart.addSeries("Robot", xList.toDoubleArray(), yList.toDoubleArray()) + + val terror = + TrajectoryGeneratorTest.trajectory.lastState.state.pose.translation - drive.robotPosition.translation + val rerror = TrajectoryGeneratorTest.trajectory.lastState.state.pose.rotation - drive.robotPosition.rotation + + System.out.printf("%n[Test] X Error: %3.3f, Y Error: %3.3f%n", terror.x.inFeet(), terror.y.inFeet()) + + assert(terror.norm.value.also { + println("[Test] Norm of Translational Error: $it") + } < 0.50) + assert(rerror.degree.also { + println("[Test] Rotational Error: $it degrees") + } < 5.0) + +// SwingWrapper(chart).displayChart() +// Thread.sleep(1000000) + } +} diff --git a/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt b/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt index e62edc3..55f2207 100644 --- a/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt +++ b/src/test/kotlin/tests/math/physics/DCMotorTransmissionTest.kt @@ -3,11 +3,11 @@ package tests.math.physics import org.junit.Test import org.junit.Assert.assertEquals -import org.team5499.monkeyLib.math.physics.DCMotorTransmission +import org.team5419.fault.math.physics.DCMotorTransmission class DCMotorTransmissionTest { - val motor = DCMotorTransmission(1000.0, 0.5, 1.0, 120.0) - val stallTorque = 5.5 // N•m + val motor = DCMotorTransmission(1000.0, 0.5, 1.0) + val stallTorque = 6.5 // N•m TODO(check this) val freeSpeed = 11000.0 // rad/s val epsilon = 0.1 @@ -20,7 +20,7 @@ class DCMotorTransmissionTest { @Test fun freeSpeedTest() { - val calculatedFreeSpeed = motor.freeSpeedAtVoltage(12.0) + val calculatedFreeSpeed = motor.getFreeSpeedAtVoltage(12.0) println("Expected $freeSpeed, but got $calculatedFreeSpeed.") assertEquals(freeSpeed, calculatedFreeSpeed, epsilon) } @@ -28,14 +28,13 @@ class DCMotorTransmissionTest { @Test fun constantsTest() { assertEquals(1000.0, motor.speedPerVolt, 0.0) - assertEquals(0.5, motor.torquePerVolt, 0.0) assertEquals(1.0, motor.frictionVoltage, 0.0) } @Test fun speedForVoltageAndAmperageTest() { - val predictedSpeed = motor.getSpeedForVoltageAndAmperage(6.0, 24.0) - println(predictedSpeed) - assertEquals(2600.0, predictedSpeed, epsilon) +// val predictedSpeed = motor.getSpeedForVoltageAndAmperage(6.0, 24.0) +// println(predictedSpeed) +// assertEquals(2600.0, predictedSpeed, epsilon) } } diff --git a/src/test/kotlin/tests/math/splines/QuinticHermiteOptimizerTest.kt b/src/test/kotlin/tests/math/splines/QuinticHermiteOptimizerTest.kt index aea1573..c54e9f3 100644 --- a/src/test/kotlin/tests/math/splines/QuinticHermiteOptimizerTest.kt +++ b/src/test/kotlin/tests/math/splines/QuinticHermiteOptimizerTest.kt @@ -4,11 +4,13 @@ import org.junit.Test import org.junit.Assert.assertTrue import org.junit.Assert.assertEquals -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Vector2 +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.splines.FunctionalQuadraticSpline -import org.team5499.monkeyLib.math.splines.QuinticHermiteSpline +import org.team5419.fault.math.splines.QuinticHermiteSpline +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.meters class QuinticHermiteOptimizerTest { @@ -16,23 +18,23 @@ class QuinticHermiteOptimizerTest { @Test fun testOptimizer() { // test porabola thing - val t = QuinticHermiteSpline.fitParabola(Vector2(-1.0, 0.015), Vector2(0.0, 0.015), Vector2(1.0, 0.016)) - assertEquals(t, -0.5, 0.1) - - val a = Pose2d(Vector2(0, 100), Rotation2d.fromDegrees(270)) - val b = Pose2d(Vector2(50, 0), Rotation2d.fromDegrees(0)) - val c = Pose2d(Vector2(100, 100), Rotation2d.fromDegrees(90)) - - val d = Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(90)) - val e = Pose2d(Vector2(0, 50), Rotation2d.fromDegrees(0)) - val f = Pose2d(Vector2(100, 0), Rotation2d.fromDegrees(90)) - val g = Pose2d(Vector2(100, 100), Rotation2d.fromDegrees(0)) - - val h = Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0)) - val i = Pose2d(Vector2(50, 0), Rotation2d.fromDegrees(0)) - val j = Pose2d(Vector2(100, 50), Rotation2d.fromDegrees(45)) - val k = Pose2d(Vector2(150, 0), Rotation2d.fromDegrees(270)) - val l = Pose2d(Vector2(150, -50), Rotation2d.fromDegrees(270)) + val t = FunctionalQuadraticSpline(Vector2(-1.0.meters, 0.015.meters), Vector2(0.0.meters, 0.015.meters), Vector2(1.0.meters, 0.016.meters)).vertexXCoordinate + assertEquals(t.value, -0.5, 0.1) + + val a = Pose2d(Vector2(0.meters, 100.meters), 270.degrees) + val b = Pose2d(Vector2(50.meters, 0.meters), 0.degrees) + val c = Pose2d(Vector2(100.meters, 100.meters), 90.degrees) + + val d = Pose2d(Vector2(0.meters, 0.meters), 90.degrees) + val e = Pose2d(Vector2(0.meters, 50.meters), 0.degrees) + val f = Pose2d(Vector2(100.meters, 0.meters), 90.degrees) + val g = Pose2d(Vector2(100.meters, 100.meters), 0.degrees) + + val h = Pose2d(Vector2(0.meters, 0.meters), 0.degrees) + val i = Pose2d(Vector2(50.meters, 0.meters), 0.degrees) + val j = Pose2d(Vector2(100.meters, 50.meters), 45.degrees) + val k = Pose2d(Vector2(150.meters, 0.meters), 270.degrees) + val l = Pose2d(Vector2(150.meters, -50.meters), 270.degrees) var startTime = System.currentTimeMillis() diff --git a/src/test/kotlin/tests/math/splines/SplineGeneratorTest.kt b/src/test/kotlin/tests/math/splines/SplineGeneratorTest.kt index 25ad71e..831cc5e 100644 --- a/src/test/kotlin/tests/math/splines/SplineGeneratorTest.kt +++ b/src/test/kotlin/tests/math/splines/SplineGeneratorTest.kt @@ -1,41 +1,10 @@ package tests.math.splines import org.junit.Test -import org.junit.Assert.assertEquals - -import org.team5499.monkeyLib.math.Epsilon - -import org.team5499.monkeyLib.math.geometry.Vector2 -import org.team5499.monkeyLib.math.geometry.Rotation2d -import org.team5499.monkeyLib.math.geometry.Pose2d -import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature - -import org.team5499.monkeyLib.math.splines.SplineGenerator -import org.team5499.monkeyLib.math.splines.QuinticHermiteSpline class SplineGeneratorTest { @Test fun testGenerator() { - val p1 = Pose2d(Vector2(0, 0), Rotation2d()) - val p2 = Pose2d(Vector2(15, 10), Rotation2d(1, -5, true)) - var s = QuinticHermiteSpline(p1, p2) - - var samples: MutableList = SplineGenerator.parameterizeSpline(s) - var arclength = 0.0 - var curPose = samples.get(0) - for (pose in samples) { - val t = Pose2d.log(curPose.pose.inverse().transformBy(pose.pose)) - arclength += t.dx - curPose = pose - } - assertEquals(curPose.pose.translation.x, 15.0, Epsilon.EPSILON) - assertEquals(curPose.pose.translation.y, 10.0, Epsilon.EPSILON) - assertEquals(curPose.pose.rotation.degrees, -78.69006752597981, Epsilon.EPSILON) - assertEquals(arclength, 23.17291953186379, Epsilon.EPSILON) - - // for (p in samples) { - // println(p.toString()) - // } } } diff --git a/src/test/kotlin/tests/math/splines/TrajectoryGeneratorTest.kt b/src/test/kotlin/tests/math/splines/TrajectoryGeneratorTest.kt new file mode 100644 index 0000000..d2784db --- /dev/null +++ b/src/test/kotlin/tests/math/splines/TrajectoryGeneratorTest.kt @@ -0,0 +1,176 @@ +package tests.math.splines + +import org.junit.Test +import org.knowm.xchart.XYChartBuilder +import org.team5419.fault.math.geometry.Pose2d +import org.team5419.fault.math.geometry.Vector2d +import org.team5419.fault.math.physics.DCMotorTransmission +import org.team5419.fault.math.physics.DifferentialDrive +import org.team5419.fault.math.units.derived.velocity +import org.team5419.fault.math.units.derived.acceleration +import org.team5419.fault.math.units.derived.degrees +import org.team5419.fault.math.units.derived.radians +import org.team5419.fault.math.units.derived.volts +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.feet +import org.team5419.fault.math.units.milliseconds +import org.team5419.fault.math.units.inFeet +import org.team5419.fault.math.units.inSeconds +import org.team5419.fault.math.units.milli +import org.team5419.fault.trajectory.DefaultTrajectoryGenerator +import org.team5419.fault.trajectory.constraints.AngularAccelerationConstraint +import org.team5419.fault.trajectory.constraints.CentripetalAccelerationConstraint +import org.team5419.fault.trajectory.constraints.DifferentialDriveDynamicsConstraint +import java.awt.Color +import java.awt.Font +import java.text.DecimalFormat +import kotlin.math.absoluteValue +import kotlin.math.pow + +class TrajectoryGeneratorTest { + + companion object { + private const val kRobotLinearInertia = 60.0 // kg + private const val kRobotAngularInertia = 10.0 // kg m^2 + private const val kRobotAngularDrag = 12.0 // N*m / (rad/sec) + private const val kDriveVIntercept = 1.055 // V + private const val kDriveKv = 0.135 // V per rad/s + private const val kDriveKa = 0.012 // V per rad/s^2 + + private val kDriveWheelRadiusInches = 3.0.inches + private val kWheelBaseDiameter = 29.5.inches + + private val transmission = DCMotorTransmission( + speedPerVolt = 1 / kDriveKv, + torquePerVolt = kDriveWheelRadiusInches.value.pow(2) * kRobotLinearInertia / (2.0 * kDriveKa), + frictionVoltage = kDriveVIntercept + ) + + val drive = DifferentialDrive( + mass = kRobotLinearInertia, + moi = kRobotAngularInertia, + angularDrag = kRobotAngularDrag, + wheelRadius = kDriveWheelRadiusInches.value, + effectiveWheelBaseRadius = kWheelBaseDiameter.value / 2.0, + leftTransmission = transmission, + rightTransmission = transmission + ) + + private val kMaxCentripetalAcceleration = 4.0.feet.acceleration + private val kMaxAcceleration = 4.0.feet.acceleration + private val kMaxAngularAcceleration = 2.0.radians.acceleration + private val kMaxVelocity = 10.0.feet.velocity + + private const val kTolerance = 0.1 + + private val kSideStart = Pose2d(1.54.feet, 23.234167.feet, 180.0.degrees) + private val kNearScaleEmpty = Pose2d(23.7.feet, 20.2.feet, 160.0.degrees) + + val trajectory = DefaultTrajectoryGenerator.generateTrajectory( + listOf( + kSideStart, + kSideStart + Pose2d((-13.0).feet, 0.0.feet, 0.0.degrees), + kSideStart + Pose2d((-19.5).feet, 5.0.feet, (-90.0).degrees), + kSideStart + Pose2d((-19.5).feet, 14.0.feet, (-90.0).degrees), + kNearScaleEmpty.mirror + ), + listOf( + CentripetalAccelerationConstraint(kMaxCentripetalAcceleration), + DifferentialDriveDynamicsConstraint(drive, 9.0.volts), + AngularAccelerationConstraint(kMaxAngularAcceleration) + ), + 0.0.feet.velocity, + 0.0.feet.velocity, + kMaxVelocity, + kMaxAcceleration, + true + ) + } + + @Test + fun testTrajectoryGenerationAndConstraints() { + val iterator = trajectory.iterator() + + var time = 0.0.seconds + val dt = 20.0.milli.seconds + + while (!iterator.isDone) { + val pt = iterator.advance(dt) + time += dt + + val ac = pt.state.velocity.value.pow(2) * pt.state.state.curvature + + assert(ac <= kMaxCentripetalAcceleration.value + kTolerance) + assert(pt.state.velocity.value.absoluteValue < kMaxVelocity.value + kTolerance) + assert(pt.state.acceleration.value.absoluteValue < kMaxAcceleration.value + kTolerance) + + assert( + pt.state.acceleration.value * pt.state.state.curvature + + pt.state.velocity.value.pow(2) * pt.state.state.dkds + < kMaxAngularAcceleration.value + kTolerance + ) + } + } + + @Suppress("LongMethod") + @Test + fun testTrajectoryVisualization() { + val iterator = trajectory.iterator() + var time = 0.seconds + var dt = 20.milliseconds + val refList = mutableListOf() + while (!iterator.isDone) { + val pt = iterator.advance(dt) + time += dt + val ac = pt.state.velocity.value.pow(2) * pt.state.state.curvature + + refList.add(pt.state.state.pose.translation) + + assert(ac <= kMaxCentripetalAcceleration.value + kTolerance) + assert(pt.state.velocity.value.absoluteValue < kMaxVelocity.value + kTolerance) + assert(pt.state.acceleration.value.absoluteValue < kMaxAcceleration.value + kTolerance) + + assert( + pt.state.acceleration.value * pt.state.state.curvature + + pt.state.velocity.value.pow(2) * pt.state.state.dkds + < kMaxAngularAcceleration.value + kTolerance + ) + } + + val fm = DecimalFormat("#.###").format(TrajectoryGeneratorTest.trajectory.lastInterpolant.inSeconds()) + val chart = XYChartBuilder().width(1800).height(1520).title("$fm seconds.") + .xAxisTitle("X").yAxisTitle("Y").build() + chart.styler.markerSize = 8 + chart.styler.seriesColors = arrayOf(Color.ORANGE, Color(151, 60, 67)) + + chart.styler.chartTitleFont = Font("Kanit", 1, 40) + chart.styler.chartTitlePadding = 15 + + chart.styler.xAxisMin = 1.0 + chart.styler.xAxisMax = 26.0 + chart.styler.yAxisMin = 1.0 + chart.styler.yAxisMax = 26.0 + + chart.styler.chartFontColor = Color.WHITE + chart.styler.axisTickLabelsColor = Color.WHITE + + chart.styler.legendBackgroundColor = Color.GRAY + + chart.styler.isPlotGridLinesVisible = true + chart.styler.isLegendVisible = true + + chart.styler.plotGridLinesColor = Color.GRAY + chart.styler.chartBackgroundColor = Color.DARK_GRAY + chart.styler.plotBackgroundColor = Color.DARK_GRAY + + chart.addSeries( + "Trajectory", + refList.map { it.x.inFeet() }.toDoubleArray(), + refList.map { it.y.inFeet() }.toDoubleArray() + ) + // Uncomment these to see generation +// SwingWrapper(chart).displayChart() +// Thread.sleep(1000000) + } +} diff --git a/src/test/kotlin/tests/math/units/DerivedTests.kt b/src/test/kotlin/tests/math/units/DerivedTests.kt new file mode 100644 index 0000000..b3bff74 --- /dev/null +++ b/src/test/kotlin/tests/math/units/DerivedTests.kt @@ -0,0 +1,120 @@ +package tests.math.units + +import org.junit.Test +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.unitlessValue +import org.team5419.fault.math.units.inSeconds +import org.team5419.fault.math.units.inMeters +import org.team5419.fault.math.units.amps +import org.team5419.fault.math.units.derived.inFeetPerMinute +import org.team5419.fault.math.units.derived.inFeetPerSecond +import org.team5419.fault.math.units.derived.volts +import org.team5419.fault.math.units.operations.div +import org.team5419.fault.math.units.operations.times + +class DerivedTests { + + @Test + fun testVelocity() { + val one = 5.meters + val two = 2.seconds + + val three = one / two + + assert(three.value == 2.5) + } + + @Test + fun testVelocityAdjust() { + val one = 5.meters + val two = 2.seconds + + val three = one / two + + val four = three.inFeetPerMinute() + + assert(four epsilonEquals 492.12598425196853) + } + + @Test + fun testAcceleration() { + val one = 10.meters / 2.seconds / 4.seconds + + assert(one.value == 1.25) + } + + @Test + fun testAccelerationToVelocity() { + val one = 10.meters / 1.6.seconds / 2.seconds + val two = 5.seconds + + val three = one * two + + val four = three.inFeetPerSecond() + + assert(four epsilonEquals 51.26312335958006) + } + + @Test + fun testVelocityToLength() { + val one = 5.meters / 2.seconds + val two = 6.seconds + + val three = one * two + val four = three.inMeters() + + assert(four == 15.0) + } + + @Test + fun testVelocityAndAccelerationToTime() { + val one = 22.meters / 2.seconds + val two = 18.meters / 0.5.seconds / 4.seconds + + val three = one / two + + assert(three.inSeconds() epsilonEquals 1.2222222222222223) + } + + @Test + fun testAccelerationDividedByAcceleration() { + val one = 33.meters / 1.seconds / 1.seconds + val two = 22.meters / 2.seconds / 1.seconds + + val three = (one / two).unitlessValue + + assert(three == 3.0) + } + + @Test + fun testVelocityDividedByVelocity() { + val one = 33.meters / 1.seconds + val two = 22.meters / 2.seconds + + val three = (one / two).unitlessValue + + assert(three epsilonEquals 3.0) + } + + @Test + fun testVoltage() { + val one = 1.volts + val two = 5.amps + + val three = one * two + + assert(three.value epsilonEquals 5.0) + } + + @Test + fun testOhm() { + val one = 2.volts + val two = 5.amps + + val three = one / two + + assert(three.value epsilonEquals 0.4) + } +} diff --git a/src/test/kotlin/tests/math/units/LengthTest.kt b/src/test/kotlin/tests/math/units/LengthTest.kt new file mode 100644 index 0000000..4e37544 --- /dev/null +++ b/src/test/kotlin/tests/math/units/LengthTest.kt @@ -0,0 +1,63 @@ +package tests.math.units + +import org.junit.Test +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.inInches +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.milli +import org.team5419.fault.math.units.centi +import org.team5419.fault.math.units.inMillimeters +import org.team5419.fault.math.units.inMeters + +class LengthTest { + + @Test + fun testLength() { + val one = 1.0.meters + val two = 12.0.inches + + val three = one + two + + assert(three.inMeters() epsilonEquals 1.3048) + } + + @Test + fun testPrefix() { + val one = 1.0.meters + val two = 100.centi.meters + + val three = one + two + + assert(three.inMillimeters() epsilonEquals 2000.0) + } + + @Test + fun testScalar() { + val one = 12.meters + + val two = one / 3.0 + val three = two * 3.0 + + assert(two.inMeters() epsilonEquals 4.0) + assert(three.inMeters() epsilonEquals 12.0) + } + + @Test + fun testToMetric() { + val one = 40.inches + + val two = one.inMillimeters() + + assert(two.toInt() == 1016) + } + + @Test + fun testFromMetric() { + val one = 1016.milli.meters + + val two = one.inInches() + + assert(two.toInt() == 40) + } +} diff --git a/src/test/kotlin/tests/math/units/TimeTest.kt b/src/test/kotlin/tests/math/units/TimeTest.kt new file mode 100644 index 0000000..84a87f4 --- /dev/null +++ b/src/test/kotlin/tests/math/units/TimeTest.kt @@ -0,0 +1,31 @@ +package tests.math.units + +import org.junit.Test +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.hours +import org.team5419.fault.math.units.inHours +import org.team5419.fault.math.units.minutes +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.inMinutes + +class TimeTest { + + @Test + fun testDivision() { + val one = 45.hours + + val two = one / 3 + + assert(two.inHours() == 15.0) + } + + @Test + fun testAddition() { + val one = 2.5.minutes + val two = 360.seconds + + val three = one + two + + assert(three.inMinutes() epsilonEquals 8.5) + } +} diff --git a/src/test/kotlin/tests/math/units/UnitTest.kt b/src/test/kotlin/tests/math/units/UnitTest.kt new file mode 100644 index 0000000..e08ab2b --- /dev/null +++ b/src/test/kotlin/tests/math/units/UnitTest.kt @@ -0,0 +1,80 @@ +package tests.math.units + +import org.junit.Test +import org.team5419.fault.math.epsilonEquals +import org.team5419.fault.math.units.SIUnit +import org.team5419.fault.math.units.inches +import org.team5419.fault.math.units.inInches +import org.team5419.fault.math.units.meters +import org.team5419.fault.math.units.seconds +import org.team5419.fault.math.units.feet +import org.team5419.fault.math.units.Kilogram +import org.team5419.fault.math.units.inMeters +import org.team5419.fault.math.units.inLbs + +import org.team5419.fault.math.units.derived.radians +import org.team5419.fault.math.units.derived.velocity +import org.team5419.fault.math.units.native.nativeUnits +import org.team5419.fault.math.units.native.NativeUnitLengthModel +import org.team5419.fault.math.units.native.toNativeUnitVelocity +import org.team5419.fault.math.units.native.toNativeUnitAcceleration +import org.team5419.fault.math.units.native.fromNativeUnitPosition +import org.team5419.fault.math.units.native.nativeUnitsPer100ms +import org.team5419.fault.math.units.native.nativeUnitsPer100msPerSecond + +import org.team5419.fault.math.units.operations.times +import org.team5419.fault.math.units.operations.div + +class UnitTest { + + private val settings = NativeUnitLengthModel( + 1440.nativeUnits, + 3.0.inches + ) + + @Test + fun testNativeUnits() { + val nativeUnits = 360.nativeUnits.fromNativeUnitPosition(settings) + + assert(nativeUnits.inInches() epsilonEquals 4.71238898038469) + } + + @Test + fun testVelocitySTU() { + val one = 1.meters / 1.seconds + + val two = one.toNativeUnitVelocity(settings) + + val three = two.nativeUnitsPer100ms + + assert(three.toInt() == 300) + } + + @Test + fun testAccelerationSTU() { + val one = 1.meters / 1.seconds / 1.seconds + + val two = one.toNativeUnitAcceleration(settings) + + assert(two.nativeUnitsPer100msPerSecond.toInt() == 300) + } + + @Test + fun testFeetToMeter() { + val one = 1.feet + + assert(one.inMeters() epsilonEquals 0.3048) + } + + @Test + fun testKgToPound() { + val kg = SIUnit(2.0) + assert(kg.inLbs() epsilonEquals 4.409248840367555) + } + + @Test + fun testUnboundedRotationUnits() { + val speed = 250.radians.velocity + assert(speed.value epsilonEquals 250.0) + } +} diff --git a/src/test/kotlin/tests/util/SourceTests.kt b/src/test/kotlin/tests/util/SourceTests.kt new file mode 100644 index 0000000..cfb0994 --- /dev/null +++ b/src/test/kotlin/tests/util/SourceTests.kt @@ -0,0 +1,61 @@ +package tests.util + +import org.junit.Test +import org.team5419.fault.util.BooleanSource +import org.team5419.fault.util.Source +import org.team5419.fault.util.map +import org.team5419.fault.util.withEquals + +class SourceTests { + + @Test + fun constTest() { + val one = Source(5) + assert(one() == 5) + } + + @Test + fun variableSourceTest() { + var value = 5 + val one: Source = { value } + assert(one() == 5) + value = 2 + assert(one() == 2) + } + + @Test + fun sourceWithProcessingTest() { + var value = 1 + val one: Source = { value } + val two = one.map { it > 2 } + assert(!two()) + value = 3 + assert(two()) + } + + @Test + fun sourceMapTest() { + var value = true + + val constOne = 1 + val constTwo = 2 + + val one: BooleanSource = { value } + val two = one.map(constOne, constTwo) + + assert(two() == constOne) + value = false + assert(two() == constTwo) + } + + @Test + fun sourceEqualsTest() { + var value = false + + val one: BooleanSource = { value } + val two = one.withEquals(true) + assert(!two()) + value = true + assert(two()) + } +} diff --git a/src/test/kotlin/tests/util/TimerTest.kt b/src/test/kotlin/tests/util/TimerTest.kt index e3dd49c..55e2418 100644 --- a/src/test/kotlin/tests/util/TimerTest.kt +++ b/src/test/kotlin/tests/util/TimerTest.kt @@ -1,30 +1,24 @@ package tests.utils import org.junit.Test -import org.junit.Assert.assertEquals - -import org.team5499.monkeyLib.util.time.ITimer -import org.team5499.monkeyLib.util.time.SystemTimer public class TimerTest { - private val kEpsilon = 1E6 - @Test fun timerSystemTest() { - val timer: ITimer = SystemTimer() - timer.start() - assertEquals(timer.get(), 0.0, kEpsilon) - Thread.sleep(2000L) - assertEquals(timer.get(), 2.0, kEpsilon) - timer.stop() - Thread.sleep(1000L) - timer.start() - Thread.sleep(1000L) - assertEquals(timer.get(), 3.0, kEpsilon) - timer.stop() - timer.reset() - timer.start() - assertEquals(timer.get(), 0.0, kEpsilon) +// val timer: ITimer = SystemTimer() +// timer.start() +// assertEquals(timer.get(), 0.0, kEpsilon) +// Thread.sleep(2000L) +// assertEquals(timer.get(), 2.0, kEpsilon) +// timer.stop() +// Thread.sleep(1000L) +// timer.start() +// Thread.sleep(1000L) +// assertEquals(timer.get(), 3.0, kEpsilon) +// timer.stop() +// timer.reset() +// timer.start() +// assertEquals(timer.get(), 0.0, kEpsilon) } } diff --git a/src/test/kotlin/tests/util/UtilTest.kt b/src/test/kotlin/tests/util/UtilTest.kt deleted file mode 100644 index 2352dd6..0000000 --- a/src/test/kotlin/tests/util/UtilTest.kt +++ /dev/null @@ -1,19 +0,0 @@ -package tests.utils - -import org.team5499.monkeyLib.util.Utils - -import org.junit.Test -import org.junit.Assert.assertTrue - -public class UtilTest { - - private val kTestWheelCir = 6.0 * Math.PI - private val kTestTicksPerRotation = 1000 - - @Test - fun testInchesToEncoderTicks() { - val inches = 2.0 - val output = Utils.inchesToEncoderTicks(kTestTicksPerRotation, kTestWheelCir, inches) - assertTrue(output == (2.0 * kTestTicksPerRotation / kTestWheelCir).toInt()) - } -}