From 0d994871e44f9ca80a415d1f0dba3daa1ca15bf5 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Tue, 21 Jan 2020 15:54:00 -0800 Subject: [PATCH 1/9] Brokt --- drivetrain/build.gradle.kts | 44 +++++++++++++++++++ .../subsystems/drivetrain/Drivetrain.kt | 14 ++++++ .../subsystems/drivetrain/DrivetrainConfig.kt | 33 ++++++++++++++ settings.gradle.kts | 1 + 4 files changed, 92 insertions(+) create mode 100644 drivetrain/build.gradle.kts create mode 100644 drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt create mode 100644 drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt diff --git a/drivetrain/build.gradle.kts b/drivetrain/build.gradle.kts new file mode 100644 index 0000000..3a816fb --- /dev/null +++ b/drivetrain/build.gradle.kts @@ -0,0 +1,44 @@ +import org.jetbrains.kotlin.gradle.tasks.KotlinCompile + +plugins { + kotlin("jvm") + java + `maven-publish` + maven +} + +dependencies { + implementation(kotlin("stdlib-jdk8", "1.3.61")) + implementation(kotlin("reflect", "1.3.61")) + implementation("org.jetbrains.kotlinx:kotlinx-coroutines-core:1.3.3") + implementation("edu.wpi.first.wpilibj:wpilibj-java:2020.1.2") + implementation("edu.wpi.first.hal:hal-java:2020.1.2") + implementation("edu.wpi.first.wpiutil:wpiutil-java:2020.1.2") + implementation("edu.wpi.first.ntcore:ntcore-java:2020.1.2") + implementation("com.ctre.phoenix:api-java:5.14.1") + implementation("com.revrobotics.frc:SparkMax-java:1.4.1") + implementation("com.kauailabs.navx.frc:navx-java:3.1.344") + implementation(project(":core")) +} + +configure { + sourceCompatibility = JavaVersion.VERSION_11 +} + +tasks.withType { + kotlinOptions.jvmTarget = "11" +} + +val compileKotlin: KotlinCompile by tasks + +compileKotlin.kotlinOptions { + freeCompilerArgs = listOf("-XXLanguage:+InlineClasses", "-Xuse-experimental=kotlin.Experimental") +} + +publishing { + publications { + create("maven") { + artifact("$buildDir/libs/${project.name}-${project.version}.jar") + } + } +} diff --git a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt new file mode 100644 index 0000000..ffab976 --- /dev/null +++ b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt @@ -0,0 +1,14 @@ +package org.sert2521.sertain.subsystems.drivetrain + +import edu.wpi.first.wpilibj.I2C +import org.sert2521.sertain.subsystems.Subsystem +import com.kauailabs.navx.frc.AHRS +import edu.wpi.first.wpilibj.I2C + +class Drivetrain(options: Map) : Subsystem(options["name"] as? String ?: "Drivetrain") { + val navx = (options["navxPort"] as? NavxPort)?.let { + AHRS(SPI.Port.kMXP) + } + + val heading get() = navx +} diff --git a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt new file mode 100644 index 0000000..3957baf --- /dev/null +++ b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt @@ -0,0 +1,33 @@ +package org.sert2521.sertain.subsystems.drivetrain + +import com.kauailabs.navx.frc.AHRS +import edu.wpi.first.wpilibj.I2C +import edu.wpi.first.wpilibj.SPI +import kotlin.properties.ReadWriteProperty +import kotlin.reflect.KProperty + +sealed class NavxPort + +object Kmpx : NavxPort() +object KOnboard : NavxPort() + +class DrivetrainConfig { + internal val map = mutableMapOf() + + var navx: AHRS? by DrivetrainConfigProperty(null) +} + +class DrivetrainConfigProperty(initialValue: T) : ReadWriteProperty { + override fun getValue(thisRef: DrivetrainConfig, property: KProperty<*>): T { + thisRef.map[property.name].let { + if (it !is T) { + + } + } + } + + override fun setValue(thisRef: DrivetrainConfig, property: KProperty<*>, value: T) { + thisRef.map[property.name] = value + } + +} diff --git a/settings.gradle.kts b/settings.gradle.kts index a1b7719..f035d0a 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -7,3 +7,4 @@ pluginManagement { rootProject.name = "sertain" include("core") +include("drivetrain") From 6f1c495ac935edccb45ed5f8308e18d3fa6bac9e Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Sat, 1 Feb 2020 18:29:35 -0800 Subject: [PATCH 2/9] Not working --- .../org/sert2521/sertain/pathfinding/Paths.kt | 28 +++++++++++++ .../sert2521/sertain/pathfinding/Waypoint.kt | 39 +++++++++++++++++++ .../kotlin/org/sert2521/sertain/utils/Math.kt | 4 ++ 3 files changed, 71 insertions(+) create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt new file mode 100644 index 0000000..b27cfa8 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt @@ -0,0 +1,28 @@ +package org.sert2521.sertain.pathfinding + +import edu.wpi.first.wpilibj.controller.RamseteController +import edu.wpi.first.wpilibj.geometry.Pose2d +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics +import edu.wpi.first.wpilibj.trajectory.Trajectory +import org.sert2521.sertain.utils.timer + +suspend fun runPath( + trajectory: Trajectory, + getPose: () -> Pose2d, + follower: RamseteController, + kinematics: DifferentialDriveKinematics, + outputMetersPerSecond: (left: Double, right: Double) -> Unit +) { + timer(20, 0, (trajectory.totalTimeSeconds * 1000).toLong().also { println("Path time: $it") }) { + val curTime = it.toDouble() / 1000 + + val targetWheelSpeeds = kinematics.toWheelSpeeds( + follower.calculate(getPose(), trajectory.sample(curTime)) + ) + + val leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond + val rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond + + outputMetersPerSecond(leftSpeedSetpoint, rightSpeedSetpoint) + } +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt new file mode 100644 index 0000000..2bc85e1 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt @@ -0,0 +1,39 @@ +package org.sert2521.sertain.pathfinding + +import org.sert2521.sertain.utils.Point +import org.sert2521.sertain.utils.p + +data class Waypoint( + override val x: Double, + override val y: Double, + val ang: Double +) : Point(x, y) + +class PathConfig { + val waypoints = mutableListOf() + + fun wp(x: Double, y: Double, ang: Double) { + waypoints += Waypoint(x, y, ang) + } + + infix fun Point.ang(ang: Double) = wp(x, y, ang) +} + +data class PathSettings( + val maxAcc: Double, + val maxVel: Double, + val trackWidth: Double +) + +data class Path(val waypoints: List) + +fun runPath(settings: PathSettings, configure: PathConfig.() -> Unit) { + val config = PathConfig().configure() + +} + +fun x() { + runPath(0.0, 0.0) { + p(0.0, 0.0) ang 0.0 + } +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt index 958e0b2..705ede0 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt @@ -19,3 +19,7 @@ fun Double.root(n: Int): Double { fun Double.roundTo(decimalPlaces: Int) = round(this * 10.0.pow(decimalPlaces)) / 10.0.pow(decimalPlaces) + +open class Point(open val x: Double, open val y: Double) + +fun p(x: Double, y: Double) = Point(x, y) From da40affc996817e18b3da3aa8dc062ad3b8bd827 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Sat, 1 Feb 2020 21:13:35 -0800 Subject: [PATCH 3/9] Units --- .../main/kotlin/org/sert2521/sertain/Robot.kt | 17 ++++++ .../org/sert2521/sertain/motors/Encoder.kt | 5 +- .../sertain/motors/MotorController.kt | 7 +-- .../sert2521/sertain/units/CompositeUnit.kt | 17 +++--- .../org/sert2521/sertain/units/Conversions.kt | 2 +- .../org/sert2521/sertain/units/MetricUnit.kt | 30 ++++++---- .../org/sert2521/sertain/units/MetricValue.kt | 60 ++++++++++--------- 7 files changed, 83 insertions(+), 55 deletions(-) diff --git a/core/src/main/kotlin/org/sert2521/sertain/Robot.kt b/core/src/main/kotlin/org/sert2521/sertain/Robot.kt index 6d976fc..15391ae 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/Robot.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/Robot.kt @@ -17,6 +17,23 @@ import org.sert2521.sertain.events.fire import org.sert2521.sertain.subsystems.manageTasks import org.sert2521.sertain.subsystems.subsystems import org.sert2521.sertain.subsystems.use +import org.sert2521.sertain.units.MetricValue +import org.sert2521.sertain.units.convertTo +import org.sert2521.sertain.units.degrees +import org.sert2521.sertain.units.div +import org.sert2521.sertain.units.meters +import org.sert2521.sertain.units.millimeters +import org.sert2521.sertain.units.mmps +import org.sert2521.sertain.units.mps +import org.sert2521.sertain.units.mpss +import org.sert2521.sertain.units.rad +import org.sert2521.sertain.units.radians +import org.sert2521.sertain.units.rdps +import org.sert2521.sertain.units.revolutions +import org.sert2521.sertain.units.rpss +import org.sert2521.sertain.units.seconds +import org.sert2521.sertain.units.times +import kotlin.math.PI object Robot { var mode = RobotMode.DISCONNECTED diff --git a/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt b/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt index cb0049c..17aab9f 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt @@ -1,14 +1,15 @@ package org.sert2521.sertain.motors import org.sert2521.sertain.units.Angular +import org.sert2521.sertain.units.AngularVelocity import org.sert2521.sertain.units.MetricUnit -import org.sert2521.sertain.units.Seconds +import org.sert2521.sertain.units.seconds import org.sert2521.sertain.units.div import kotlin.math.PI class Encoder(ticksPerRevolution: Int) { val ticks = EncoderTicks(ticksPerRevolution) - val ticksPerSecond = ticks / Seconds + val ticksPerSecond: MetricUnit = ticks / seconds } class EncoderTicks(ticksPerRevolution: Int) : MetricUnit(Angular, (PI * 2) / ticksPerRevolution, " ticks") diff --git a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt index 11a80d8..a66501c 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt @@ -1,6 +1,7 @@ package org.sert2521.sertain.motors import org.sert2521.sertain.units.Angular +import org.sert2521.sertain.units.AngularVelocity import org.sert2521.sertain.units.Chronic import org.sert2521.sertain.units.CompositeUnit import org.sert2521.sertain.units.CompositeUnitType @@ -189,7 +190,7 @@ class MotorController( ctreMotorController.set(CtreControlMode.Position, position.toDouble()) } - fun > setTargetPosition(position: MetricValue) { + fun setTargetPosition(position: MetricValue) { checkNotNull(encoder) { "You must configure your encoder to use units." } setTargetPosition(position.convertTo(encoder!!.ticks).value.toInt()) } @@ -198,9 +199,7 @@ class MotorController( ctreMotorController.set(CtreControlMode.Velocity, velocity.toDouble()) } - fun setTargetVelocity( - velocity: MetricValue, CompositeUnit> - ) { + fun setTargetVelocity(velocity: MetricValue) { checkNotNull(encoder) { "You must configure your encoder to use units." } setTargetVelocity(velocity.convertTo(encoder!!.ticksPerSecond).value.toInt()) } diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt b/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt index 66f9e09..a2f4fde 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt @@ -11,12 +11,8 @@ open class CompositeUnitType( - val operation: OP, - val unit1: MetricUnit, - val unit2: MetricUnit -) : MetricUnit>( +typealias CompositeUnit = MetricUnit> +fun , U2 : MetricUnit> compositeUnit(operation: OP, unit1: U1, unit2: U2) = CompositeUnit( CompositeUnitType(operation, unit1.type, unit2.type), when (operation) { Per -> unit1.base / unit2.base @@ -39,7 +35,12 @@ open class CompositeUnit MetricUnit.div(other: MetricUnit) = - CompositeUnit(Per, this, other) + compositeUnit(Per, this, other) operator fun MetricUnit.times(other: MetricUnit) = - CompositeUnit(By, this, other) + compositeUnit(By, this, other) + +typealias Velocity = CompositeUnitType +typealias AngularVelocity = CompositeUnitType +typealias Acceleration = CompositeUnitType> +typealias AngularAcceleration = CompositeUnitType> diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt b/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt index 1ddc8fa..68b35a5 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt @@ -4,6 +4,6 @@ fun , U2 : MetricUnit> Double.convert( return this * units.first.base / units.second.base } -fun , U2 : MetricUnit> MetricValue.convertTo(other: U2): MetricValue { +fun MetricValue.convertTo(other: MetricUnit): MetricValue { return MetricValue(other, value.convert(unit to other)) } diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt b/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt index e01d678..64ac3db 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt @@ -2,22 +2,26 @@ package org.sert2521.sertain.units import kotlin.math.PI -abstract class MetricUnit(val type: T, val base: Double, val symbol: String) +open class MetricUnit(val type: T, val base: Double, val symbol: String) -abstract class ChronicUnit(seconds: Double, symbol: String) : MetricUnit(Chronic, seconds, symbol) +// abstract class ChronicUnit(seconds: Double, symbol: String) : MetricUnit(Chronic, seconds, symbol) +typealias ChronicUnit = MetricUnit +fun chronicUnit(seconds: Double, symbol: String): ChronicUnit = MetricUnit(Chronic, seconds, symbol) -object Seconds : ChronicUnit(1.0, " s") -object Minutes : ChronicUnit(60.0, " min") -object Milliseconds : ChronicUnit(0.001, " ms") +val seconds = chronicUnit(1.0, " s") +val minutes = chronicUnit(60.0, " min") +val milliseconds = chronicUnit(0.001, " ms") -abstract class LinearUnit(meters: Double, symbol: String) : MetricUnit(Linear, meters, symbol) +typealias LinearUnit = MetricUnit +fun linearUnit(meters: Double, symbol: String): LinearUnit = MetricUnit(Linear, meters, symbol) -object Meters : LinearUnit(1.0, " m") -object Centimeters : LinearUnit(0.01, " cm") -object Millimeters : LinearUnit(0.001, " mm") +val meters = linearUnit(1.0, " m") +val centimeters = linearUnit(0.01, " cm") +val millimeters = linearUnit(0.001, " mm") -abstract class AngularUnit(radians: Double, symbol: String) : MetricUnit(Angular, radians, symbol) +typealias AngularUnit = MetricUnit +fun angularUnit(radians: Double, symbol: String): AngularUnit = MetricUnit(Angular, radians, symbol) -object Degrees : AngularUnit(PI / 180, "°") -object Radians : AngularUnit(1.0, " rad") -object Revolutions : AngularUnit(PI * 2, " rev") +val degrees = angularUnit(PI / 180, "°") +val radians = angularUnit(1.0, " rad") +val revolutions = angularUnit(PI * 2, " rev") diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt b/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt index 9d97141..d4e2eb7 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt @@ -2,37 +2,37 @@ package org.sert2521.sertain.units import kotlin.math.ceil -data class MetricValue>(val unit: U, val value: Double) { - override fun equals(other: Any?) = if (other is MetricValue<*, *> && unit.type == other.unit.type) { +data class MetricValue(val unit: MetricUnit, val value: Double) { + override fun equals(other: Any?) = if (other is MetricValue<*> && unit.type == other.unit.type) { value * unit.base == other.value * other.unit.base } else { false } - operator fun > plus(other: MetricValue) = + operator fun > plus(other: MetricValue) = MetricValue(unit, value + other.convertTo(unit).value) - operator fun > minus(other: MetricValue) = + operator fun > minus(other: MetricValue) = MetricValue(unit, value - other.convertTo(unit).value) @Suppress("UNCHECKED_CAST") - operator fun > times(other: MetricValue<*, S>) = if (unit.type == other.unit.type) { - MetricValue(CompositeUnit(By, unit, unit), value * (other as MetricValue).convertTo(unit).value) + operator fun times(other: MetricValue) = if (unit.type == other.unit.type) { + MetricValue(compositeUnit(By, unit, unit), value * (other as MetricValue).convertTo(unit).value) } else { - MetricValue(CompositeUnit(By, unit, other.unit), value * other.value) + MetricValue(compositeUnit(By, unit, other.unit), value * other.value) } @Suppress("UNCHECKED_CAST") - operator fun > div(other: MetricValue<*, S>) = if (unit.type == other.unit.type) { - MetricValue(CompositeUnit(Per, unit, unit), value / (other as MetricValue).convertTo(unit).value) + operator fun div(other: MetricValue) = if (unit.type == other.unit.type) { + MetricValue(compositeUnit(Per, unit, unit), value / (other as MetricValue).convertTo(unit).value) } else { - MetricValue(CompositeUnit(Per, unit, other.unit), value / other.value) + MetricValue(compositeUnit(Per, unit, other.unit), value / other.value) } - operator fun > rem(other: MetricValue) = + operator fun > rem(other: MetricValue) = MetricValue(unit, value % other.convertTo(unit).value) - operator fun > compareTo(other: MetricValue) = + operator fun > compareTo(other: MetricValue) = ceil(value * unit.base - other.value * other.unit.base).toInt() override fun hashCode(): Int { @@ -44,18 +44,24 @@ data class MetricValue>(val unit: U, val v override fun toString() = "$value${unit.symbol}" } -val Number.s get() = MetricValue(Seconds, toDouble()) -val Number.min get() = MetricValue(Minutes, toDouble()) -val Number.ms get() = MetricValue(Milliseconds, toDouble()) -val Number.m get() = MetricValue(Meters, toDouble()) -val Number.cm get() = MetricValue(Centimeters, toDouble()) -val Number.mm get() = MetricValue(Millimeters, toDouble()) -val Number.deg get() = MetricValue(Degrees, toDouble()) -val Number.rad get() = MetricValue(Radians, toDouble()) -val Number.rev get() = MetricValue(Revolutions, toDouble()) -val Number.mps get() = MetricValue(Meters / Seconds, toDouble()) -val Number.mmps get() = MetricValue(Millimeters / Seconds, toDouble()) -val Number.dps get() = MetricValue(Degrees / Seconds, toDouble()) -val Number.rdps get() = MetricValue(Radians / Seconds, toDouble()) -val Number.rps get() = MetricValue(Revolutions / Seconds, toDouble()) -val Number.rpm get() = MetricValue(Revolutions / Minutes, toDouble()) +val Number.s get() = MetricValue(seconds, toDouble()) +val Number.min get() = MetricValue(minutes, toDouble()) +val Number.ms get() = MetricValue(milliseconds, toDouble()) +val Number.m get() = MetricValue(meters, toDouble()) +val Number.cm get() = MetricValue(centimeters, toDouble()) +val Number.mm get() = MetricValue(millimeters, toDouble()) +val Number.deg get() = MetricValue(degrees, toDouble()) +val Number.rad get() = MetricValue(radians, toDouble()) +val Number.rev get() = MetricValue(revolutions, toDouble()) +val Number.mps: MetricValue get() = MetricValue(meters / seconds, toDouble()) +val Number.mmps: MetricValue get() = MetricValue(millimeters / seconds, toDouble()) +val Number.dps: MetricValue get() = MetricValue(degrees / seconds, toDouble()) +val Number.rdps: MetricValue get() = MetricValue(radians / seconds, toDouble()) +val Number.rps: MetricValue get() = MetricValue(revolutions / seconds, toDouble()) +val Number.rpm: MetricValue get() = MetricValue(revolutions / minutes, toDouble()) +val Number.mpss: MetricValue get() = MetricValue(meters / (seconds * seconds), toDouble()) +val Number.mmpss: MetricValue get() = MetricValue(millimeters / (seconds * seconds), toDouble()) +val Number.dpss: MetricValue get() = MetricValue(degrees / (seconds * seconds), toDouble()) +val Number.rdpss: MetricValue get() = MetricValue(radians / (seconds * seconds), toDouble()) +val Number.rpss: MetricValue get() = MetricValue(revolutions / (seconds * seconds), toDouble()) +val Number.rpmm: MetricValue get() = MetricValue(revolutions / (minutes * minutes), toDouble()) From 8fd3a7eebf57c42c900f4ef6c65f4267d484fdec Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Mon, 3 Feb 2020 17:55:10 -0800 Subject: [PATCH 4/9] Fix units --- .../main/kotlin/org/sert2521/sertain/Robot.kt | 17 ----- .../org/sert2521/sertain/motors/Encoder.kt | 11 ++- .../sertain/motors/MotorController.kt | 19 +++-- .../sert2521/sertain/units/CompositeUnit.kt | 33 +++++--- .../org/sert2521/sertain/units/Conversions.kt | 10 +-- .../org/sert2521/sertain/units/MetricUnit.kt | 28 +++---- .../org/sert2521/sertain/units/MetricValue.kt | 76 ++++++++++--------- 7 files changed, 93 insertions(+), 101 deletions(-) diff --git a/core/src/main/kotlin/org/sert2521/sertain/Robot.kt b/core/src/main/kotlin/org/sert2521/sertain/Robot.kt index 15391ae..6d976fc 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/Robot.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/Robot.kt @@ -17,23 +17,6 @@ import org.sert2521.sertain.events.fire import org.sert2521.sertain.subsystems.manageTasks import org.sert2521.sertain.subsystems.subsystems import org.sert2521.sertain.subsystems.use -import org.sert2521.sertain.units.MetricValue -import org.sert2521.sertain.units.convertTo -import org.sert2521.sertain.units.degrees -import org.sert2521.sertain.units.div -import org.sert2521.sertain.units.meters -import org.sert2521.sertain.units.millimeters -import org.sert2521.sertain.units.mmps -import org.sert2521.sertain.units.mps -import org.sert2521.sertain.units.mpss -import org.sert2521.sertain.units.rad -import org.sert2521.sertain.units.radians -import org.sert2521.sertain.units.rdps -import org.sert2521.sertain.units.revolutions -import org.sert2521.sertain.units.rpss -import org.sert2521.sertain.units.seconds -import org.sert2521.sertain.units.times -import kotlin.math.PI object Robot { var mode = RobotMode.DISCONNECTED diff --git a/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt b/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt index 17aab9f..641eb08 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/motors/Encoder.kt @@ -1,15 +1,14 @@ package org.sert2521.sertain.motors -import org.sert2521.sertain.units.Angular -import org.sert2521.sertain.units.AngularVelocity -import org.sert2521.sertain.units.MetricUnit -import org.sert2521.sertain.units.seconds +import org.sert2521.sertain.units.AngularUnit +import org.sert2521.sertain.units.AngularVelocityUnit +import org.sert2521.sertain.units.Seconds import org.sert2521.sertain.units.div import kotlin.math.PI class Encoder(ticksPerRevolution: Int) { val ticks = EncoderTicks(ticksPerRevolution) - val ticksPerSecond: MetricUnit = ticks / seconds + val ticksPerSecond: AngularVelocityUnit = ticks / Seconds } -class EncoderTicks(ticksPerRevolution: Int) : MetricUnit(Angular, (PI * 2) / ticksPerRevolution, " ticks") +class EncoderTicks(ticksPerRevolution: Int) : AngularUnit((PI * 2) / ticksPerRevolution, " ticks") diff --git a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt index a66501c..c05dbc7 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt @@ -1,13 +1,12 @@ package org.sert2521.sertain.motors -import org.sert2521.sertain.units.Angular +import org.sert2521.sertain.units.AngularUnit +import org.sert2521.sertain.units.AngularValue import org.sert2521.sertain.units.AngularVelocity -import org.sert2521.sertain.units.Chronic -import org.sert2521.sertain.units.CompositeUnit -import org.sert2521.sertain.units.CompositeUnitType -import org.sert2521.sertain.units.MetricUnit +import org.sert2521.sertain.units.AngularVelocityUnit +import org.sert2521.sertain.units.AngularVelocityValue +import org.sert2521.sertain.units.ChronicUnit import org.sert2521.sertain.units.MetricValue -import org.sert2521.sertain.units.Per import org.sert2521.sertain.units.convertTo import com.ctre.phoenix.motorcontrol.ControlMode as CtreControlMode import com.ctre.phoenix.motorcontrol.NeutralMode as CtreNeutralMode @@ -173,13 +172,13 @@ class MotorController( ctreMotorController.selectedSensorPosition = value } - fun position(unit: MetricUnit) = + fun position(unit: U) = MetricValue(encoder!!.ticks, position.toDouble()).convertTo(unit) val velocity: Int get() = ctreMotorController.getSelectedSensorVelocity(0) - fun velocity(unit: CompositeUnit) = + fun > velocity(unit: U) = MetricValue(encoder!!.ticksPerSecond, velocity.toDouble()).convertTo(unit) fun setPercentOutput(output: Double) { @@ -190,7 +189,7 @@ class MotorController( ctreMotorController.set(CtreControlMode.Position, position.toDouble()) } - fun setTargetPosition(position: MetricValue) { + fun > setTargetPosition(position: V) { checkNotNull(encoder) { "You must configure your encoder to use units." } setTargetPosition(position.convertTo(encoder!!.ticks).value.toInt()) } @@ -199,7 +198,7 @@ class MotorController( ctreMotorController.set(CtreControlMode.Velocity, velocity.toDouble()) } - fun setTargetVelocity(velocity: MetricValue) { + fun > setTargetVelocity(velocity: V) { checkNotNull(encoder) { "You must configure your encoder to use units." } setTargetVelocity(velocity.convertTo(encoder!!.ticksPerSecond).value.toInt()) } diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt b/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt index a2f4fde..d9d0cf9 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/CompositeUnit.kt @@ -6,13 +6,23 @@ object Per : CompositionOperation() object By : CompositionOperation() open class CompositeUnitType( - val operation: OP, - val type1: T1, - val type2: T2 + val operation: OP, + val type1: T1, + val type2: T2 ) : MetricUnitType() -typealias CompositeUnit = MetricUnit> -fun , U2 : MetricUnit> compositeUnit(operation: OP, unit1: U1, unit2: U2) = CompositeUnit( +// By operation is not well tested! +open class CompositeUnit< + OP : CompositionOperation, + T1 : MetricUnitType, + T2 : MetricUnitType, + U1 : MetricUnit, + U2 : MetricUnit +>( + val operation: OP, + val unit1: U1, + val unit2: U2 +) : MetricUnit>( CompositeUnitType(operation, unit1.type, unit2.type), when (operation) { Per -> unit1.base / unit2.base @@ -34,13 +44,18 @@ fun MetricUnit.div(other: MetricUnit) = - compositeUnit(Per, this, other) +operator fun , U2 : MetricUnit> U1.div(other: U2) = + CompositeUnit(Per, this, other) -operator fun MetricUnit.times(other: MetricUnit) = - compositeUnit(By, this, other) +operator fun , U2 : MetricUnit> U1.times(other: U2) = + CompositeUnit(By, this, other) typealias Velocity = CompositeUnitType typealias AngularVelocity = CompositeUnitType typealias Acceleration = CompositeUnitType> typealias AngularAcceleration = CompositeUnitType> + +typealias VelocityUnit = CompositeUnit +typealias AngularVelocityUnit = CompositeUnit +typealias AccelerationUnit = CompositeUnit, U1, CompositeUnit> +typealias AngularAccelerationUnit = CompositeUnit, U1, CompositeUnit> diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt b/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt index 68b35a5..df0633e 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/Conversions.kt @@ -1,9 +1,7 @@ package org.sert2521.sertain.units -fun , U2 : MetricUnit> Double.convert(units: Pair): Double { - return this * units.first.base / units.second.base -} +fun , U2 : MetricUnit> Double.convert(units: Pair) = + this * units.first.base / units.second.base -fun MetricValue.convertTo(other: MetricUnit): MetricValue { - return MetricValue(other, value.convert(unit to other)) -} +fun , U2 : MetricUnit> MetricValue.convertTo(other: U2) = + MetricValue(other, value.convert(unit to other)) diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt b/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt index 64ac3db..8bd531f 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/MetricUnit.kt @@ -4,24 +4,20 @@ import kotlin.math.PI open class MetricUnit(val type: T, val base: Double, val symbol: String) -// abstract class ChronicUnit(seconds: Double, symbol: String) : MetricUnit(Chronic, seconds, symbol) -typealias ChronicUnit = MetricUnit -fun chronicUnit(seconds: Double, symbol: String): ChronicUnit = MetricUnit(Chronic, seconds, symbol) +abstract class ChronicUnit(seconds: Double, symbol: String) : MetricUnit(Chronic, seconds, symbol) -val seconds = chronicUnit(1.0, " s") -val minutes = chronicUnit(60.0, " min") -val milliseconds = chronicUnit(0.001, " ms") +object Seconds : ChronicUnit(1.0, " s") +object Minutes : ChronicUnit(60.0, " min") +object Milliseconds : ChronicUnit(0.001, " ms") -typealias LinearUnit = MetricUnit -fun linearUnit(meters: Double, symbol: String): LinearUnit = MetricUnit(Linear, meters, symbol) +abstract class LinearUnit(meters: Double, symbol: String) : MetricUnit(Linear, meters, symbol) -val meters = linearUnit(1.0, " m") -val centimeters = linearUnit(0.01, " cm") -val millimeters = linearUnit(0.001, " mm") +object Meters : LinearUnit(1.0, " m") +object Centimeters : LinearUnit(0.01, " cm") +object Millimeters : LinearUnit(0.001, " mm") -typealias AngularUnit = MetricUnit -fun angularUnit(radians: Double, symbol: String): AngularUnit = MetricUnit(Angular, radians, symbol) +abstract class AngularUnit(meters: Double, symbol: String) : MetricUnit(Angular, meters, symbol) -val degrees = angularUnit(PI / 180, "°") -val radians = angularUnit(1.0, " rad") -val revolutions = angularUnit(PI * 2, " rev") +object Degrees : AngularUnit(PI / 180, "°") +object Radians : AngularUnit(1.0, " rad") +object Revolutions : AngularUnit(PI * 2, " rev") diff --git a/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt b/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt index d4e2eb7..24fe8ac 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/units/MetricValue.kt @@ -2,37 +2,31 @@ package org.sert2521.sertain.units import kotlin.math.ceil -data class MetricValue(val unit: MetricUnit, val value: Double) { - override fun equals(other: Any?) = if (other is MetricValue<*> && unit.type == other.unit.type) { +data class MetricValue>(val unit: U, val value: Double) { + override fun equals(other: Any?) = if (other is MetricValue<*, *> && unit.type == other.unit.type) { value * unit.base == other.value * other.unit.base } else { false } - operator fun > plus(other: MetricValue) = + operator fun plus(other: MetricValue) = MetricValue(unit, value + other.convertTo(unit).value) - operator fun > minus(other: MetricValue) = + operator fun minus(other: MetricValue) = MetricValue(unit, value - other.convertTo(unit).value) @Suppress("UNCHECKED_CAST") - operator fun times(other: MetricValue) = if (unit.type == other.unit.type) { - MetricValue(compositeUnit(By, unit, unit), value * (other as MetricValue).convertTo(unit).value) - } else { - MetricValue(compositeUnit(By, unit, other.unit), value * other.value) - } + operator fun > times(other: MetricValue) = + MetricValue(CompositeUnit(By, unit, other.unit), value * other.value) @Suppress("UNCHECKED_CAST") - operator fun div(other: MetricValue) = if (unit.type == other.unit.type) { - MetricValue(compositeUnit(Per, unit, unit), value / (other as MetricValue).convertTo(unit).value) - } else { - MetricValue(compositeUnit(Per, unit, other.unit), value / other.value) - } + operator fun > div(other: MetricValue) = + MetricValue(CompositeUnit(Per, unit, other.unit), value / other.value) - operator fun > rem(other: MetricValue) = + operator fun > rem(other: MetricValue) = MetricValue(unit, value % other.convertTo(unit).value) - operator fun > compareTo(other: MetricValue) = + operator fun > compareTo(other: MetricValue) = ceil(value * unit.base - other.value * other.unit.base).toInt() override fun hashCode(): Int { @@ -44,24 +38,32 @@ data class MetricValue(val unit: MetricUnit, val value: D override fun toString() = "$value${unit.symbol}" } -val Number.s get() = MetricValue(seconds, toDouble()) -val Number.min get() = MetricValue(minutes, toDouble()) -val Number.ms get() = MetricValue(milliseconds, toDouble()) -val Number.m get() = MetricValue(meters, toDouble()) -val Number.cm get() = MetricValue(centimeters, toDouble()) -val Number.mm get() = MetricValue(millimeters, toDouble()) -val Number.deg get() = MetricValue(degrees, toDouble()) -val Number.rad get() = MetricValue(radians, toDouble()) -val Number.rev get() = MetricValue(revolutions, toDouble()) -val Number.mps: MetricValue get() = MetricValue(meters / seconds, toDouble()) -val Number.mmps: MetricValue get() = MetricValue(millimeters / seconds, toDouble()) -val Number.dps: MetricValue get() = MetricValue(degrees / seconds, toDouble()) -val Number.rdps: MetricValue get() = MetricValue(radians / seconds, toDouble()) -val Number.rps: MetricValue get() = MetricValue(revolutions / seconds, toDouble()) -val Number.rpm: MetricValue get() = MetricValue(revolutions / minutes, toDouble()) -val Number.mpss: MetricValue get() = MetricValue(meters / (seconds * seconds), toDouble()) -val Number.mmpss: MetricValue get() = MetricValue(millimeters / (seconds * seconds), toDouble()) -val Number.dpss: MetricValue get() = MetricValue(degrees / (seconds * seconds), toDouble()) -val Number.rdpss: MetricValue get() = MetricValue(radians / (seconds * seconds), toDouble()) -val Number.rpss: MetricValue get() = MetricValue(revolutions / (seconds * seconds), toDouble()) -val Number.rpmm: MetricValue get() = MetricValue(revolutions / (minutes * minutes), toDouble()) +typealias ChronicValue = MetricValue +typealias LinearValue = MetricValue +typealias AngularValue = MetricValue +typealias VelocityValue = MetricValue> +typealias AngularVelocityValue = MetricValue> +typealias AccelerationValue = MetricValue> +typealias AngularAccelerationValue = MetricValue> + +val Number.s: ChronicValue get() = MetricValue(Seconds, toDouble()) +val Number.min: ChronicValue get() = MetricValue(Minutes, toDouble()) +val Number.ms: ChronicValue get() = MetricValue(Milliseconds, toDouble()) +val Number.m: LinearValue get() = MetricValue(Meters, toDouble()) +val Number.cm: LinearValue get() = MetricValue(Centimeters, toDouble()) +val Number.mm: LinearValue get() = MetricValue(Millimeters, toDouble()) +val Number.deg: AngularValue get() = MetricValue(Degrees, toDouble()) +val Number.rad: AngularValue get() = MetricValue(Radians, toDouble()) +val Number.rev: AngularValue get() = MetricValue(Revolutions, toDouble()) +val Number.mps: VelocityValue get() = MetricValue(Meters / Seconds, toDouble()) +val Number.mmps: VelocityValue get() = MetricValue(Millimeters / Seconds, toDouble()) +val Number.dps: AngularVelocityValue get() = MetricValue(Degrees / Seconds, toDouble()) +val Number.rdps: AngularVelocityValue get() = MetricValue(Radians / Seconds, toDouble()) +val Number.rps: AngularVelocityValue get() = MetricValue(Revolutions / Seconds, toDouble()) +val Number.rpm: AngularVelocityValue get() = MetricValue(Revolutions / Minutes, toDouble()) +val Number.mpss: AccelerationValue get() = MetricValue(Meters / (Seconds * Seconds), toDouble()) +val Number.mmpss: AccelerationValue get() = MetricValue(Millimeters / (Seconds * Seconds), toDouble()) +val Number.dpss: AngularAccelerationValue get() = MetricValue(Degrees / (Seconds * Seconds), toDouble()) +val Number.rdpss: AngularAccelerationValue get() = MetricValue(Radians / (Seconds * Seconds), toDouble()) +val Number.rpss: AngularAccelerationValue get() = MetricValue(Revolutions / (Seconds * Seconds), toDouble()) +val Number.rpmm: AngularAccelerationValue get() = MetricValue(Revolutions / (Minutes * Minutes), toDouble()) From f77e707b7f8b69c5a18565778716948d8cabae87 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Tue, 4 Feb 2020 15:41:26 -0800 Subject: [PATCH 5/9] Pathful procedures --- .../sertain/pathfinding/PathFollowing.kt | 53 +++++++++++++++++++ .../sertain/pathfinding/WayPointCollection.kt | 33 ++++++++++++ .../sert2521/sertain/pathfinding/Waypoint.kt | 41 ++++---------- .../pathfinding/{Paths.kt => WpilibPaths.kt} | 2 +- .../kotlin/org/sert2521/sertain/utils/Math.kt | 7 ++- 5 files changed, 102 insertions(+), 34 deletions(-) create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt rename core/src/main/kotlin/org/sert2521/sertain/pathfinding/{Paths.kt => WpilibPaths.kt} (97%) diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt new file mode 100644 index 0000000..beafcf9 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt @@ -0,0 +1,53 @@ +package org.sert2521.sertain.pathfinding + +import edu.wpi.first.wpilibj.controller.RamseteController +import edu.wpi.first.wpilibj.geometry.Translation2d +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator + +data class PathSettings( + val maxAcc: Double, + val maxVel: Double, + val trackWidth: Double, + val getWayPoint: () -> WayPoint, + val output: (left: Double, right: Double) -> Unit +) + +suspend fun runPath(settings: PathSettings, configure: WayPointCollection.() -> Unit) { + val wayPoints = WayPointCollection().apply(configure).wayPoints + val kinematics = DifferentialDriveKinematics(settings.trackWidth) + runWpilibPath( + TrajectoryGenerator.generateTrajectory( + wayPoints.map { it.toPose2d() }, + TrajectoryConfig( + settings.maxVel, + settings.maxAcc + ).setKinematics(kinematics) + ), + { settings.getWayPoint().toPose2d() }, + RamseteController(), + kinematics, + settings.output + ) +} + +suspend fun runPath(settings: PathSettings, start: WayPoint, finish: WayPoint, configure: PointCollection.() -> Unit) { + val points = PointCollection().apply(configure).points + val kinematics = DifferentialDriveKinematics(settings.trackWidth) + runWpilibPath( + TrajectoryGenerator.generateTrajectory( + start.toPose2d(), + points.map { Translation2d(it.x, it.y) }, + finish.toPose2d(), + TrajectoryConfig( + settings.maxVel, + settings.maxAcc + ).setKinematics(kinematics) + ), + { settings.getWayPoint().toPose2d() }, + RamseteController(), + kinematics, + settings.output + ) +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt new file mode 100644 index 0000000..ebb4d36 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt @@ -0,0 +1,33 @@ +package org.sert2521.sertain.pathfinding + +import org.sert2521.sertain.units.AngularUnit +import org.sert2521.sertain.units.AngularValue +import org.sert2521.sertain.units.LinearUnit +import org.sert2521.sertain.units.LinearValue +import org.sert2521.sertain.units.Meters +import org.sert2521.sertain.units.Radians +import org.sert2521.sertain.units.convertTo +import org.sert2521.sertain.utils.Point + +class WayPointCollection { + val wayPoints = mutableListOf() + + fun wp(x: LinearValue, y: LinearValue, ang: AngularValue) { + wayPoints += WayPoint(x.convertTo(Meters).value, y.convertTo(Meters).value, ang.convertTo(Radians).value) + } + + fun p(x: LinearValue, y: LinearValue) = + Point(x.convertTo(Meters).value, y.convertTo(Meters).value) + + infix fun Point.ang(angle: AngularValue) { + wayPoints += WayPoint(this, angle.convertTo(Radians).value) + } +} + +class PointCollection { + val points = mutableListOf() + + fun p(x: LinearValue, y: LinearValue) { + points += Point(x.convertTo(Meters).value, y.convertTo(Meters).value) + } +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt index 2bc85e1..e5a038c 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt @@ -1,39 +1,16 @@ package org.sert2521.sertain.pathfinding +import edu.wpi.first.wpilibj.geometry.Pose2d +import edu.wpi.first.wpilibj.geometry.Rotation2d +import edu.wpi.first.wpilibj.geometry.Translation2d +import org.sert2521.sertain.utils.Coordinates import org.sert2521.sertain.utils.Point -import org.sert2521.sertain.utils.p -data class Waypoint( - override val x: Double, - override val y: Double, - val ang: Double -) : Point(x, y) - -class PathConfig { - val waypoints = mutableListOf() - - fun wp(x: Double, y: Double, ang: Double) { - waypoints += Waypoint(x, y, ang) - } - - infix fun Point.ang(ang: Double) = wp(x, y, ang) -} - -data class PathSettings( - val maxAcc: Double, - val maxVel: Double, - val trackWidth: Double -) - -data class Path(val waypoints: List) - -fun runPath(settings: PathSettings, configure: PathConfig.() -> Unit) { - val config = PathConfig().configure() +data class WayPoint(val point: Point, val angle: Double) : Coordinates { + constructor(x: Double, y: Double, angle: Double) : this(Point(x, y), angle) + override val x get() = point.x + override val y get() = point.y } -fun x() { - runPath(0.0, 0.0) { - p(0.0, 0.0) ang 0.0 - } -} +fun WayPoint.toPose2d() = Pose2d(Translation2d(x, y), Rotation2d(angle)) diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt similarity index 97% rename from core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt rename to core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt index b27cfa8..038a692 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Paths.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics import edu.wpi.first.wpilibj.trajectory.Trajectory import org.sert2521.sertain.utils.timer -suspend fun runPath( +suspend fun runWpilibPath( trajectory: Trajectory, getPose: () -> Pose2d, follower: RamseteController, diff --git a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt index 705ede0..8b49be2 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt @@ -20,6 +20,11 @@ fun Double.root(n: Int): Double { fun Double.roundTo(decimalPlaces: Int) = round(this * 10.0.pow(decimalPlaces)) / 10.0.pow(decimalPlaces) -open class Point(open val x: Double, open val y: Double) +interface Coordinates { + val x: Double + val y: Double +} + +data class Point(override val x: Double, override val y: Double) : Coordinates fun p(x: Double, y: Double) = Point(x, y) From 6f13dbc3dc754afb7a1e3a9f5aad90a7d011eb67 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Thu, 6 Feb 2020 14:51:43 -0800 Subject: [PATCH 6/9] It's pathfinding. --- .../sertain/motors/MotorController.kt | 2 +- .../sert2521/sertain/pathfinding/Follower.kt | 75 +++++++++++++++++++ .../org/sert2521/sertain/pathfinding/Path.kt | 55 ++++++++++++++ .../sertain/pathfinding/PathFollowing.kt | 53 ------------- .../sertain/pathfinding/WayPointCollection.kt | 33 -------- .../sert2521/sertain/pathfinding/Waypoint.kt | 16 ---- .../sertain/pathfinding/WaypointPathConfig.kt | 65 ++++++++++++++++ .../kotlin/org/sert2521/sertain/utils/Math.kt | 9 --- drivetrain/build.gradle.kts | 44 ----------- .../subsystems/drivetrain/Drivetrain.kt | 14 ---- .../subsystems/drivetrain/DrivetrainConfig.kt | 33 -------- settings.gradle.kts | 1 - 12 files changed, 196 insertions(+), 204 deletions(-) create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt delete mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt delete mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt delete mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt create mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt delete mode 100644 drivetrain/build.gradle.kts delete mode 100644 drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt delete mode 100644 drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt diff --git a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt index c05dbc7..65032fd 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/motors/MotorController.kt @@ -106,7 +106,7 @@ class MotorController( integralZone ?: pidf[slot]?.integralZone ?: 0, allowedError ?: pidf[slot]?.allowedError ?: 0, maxIntegral ?: pidf[slot]?.maxIntegral ?: 0.0, - maxOutput ?: pidf[slot]?.maxIntegral ?: 0.0, + maxOutput ?: pidf[slot]?.maxOutput ?: 1.0, period ?: pidf[slot]?.period ?: 0 ) } diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt new file mode 100644 index 0000000..a15c933 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt @@ -0,0 +1,75 @@ +package org.sert2521.sertain.pathfinding + +import edu.wpi.first.wpilibj.controller.RamseteController +import edu.wpi.first.wpilibj.geometry.Pose2d +import edu.wpi.first.wpilibj.geometry.Rotation2d +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator + +class PathFollowerConfig { + internal var getPose = { Pose2d(0.0, 0.0, Rotation2d(0.0)) } + internal var outputVelocity = { l: Double, r: Double -> Unit } + + fun pose(get: () -> Pose2d) { + getPose = get + } + + fun outputVelocity(set: (left: Double, right: Double) -> Unit) { + outputVelocity = set + } +} + +data class PathFollower(val getPose: () -> Pose2d, val outputVelocity: (left: Double, right: Double) -> Unit) { + suspend fun follow(path: Path) { + runWpilibPath( + path.toWpilibTrajectory(), + getPose, + RamseteController(), + DifferentialDriveKinematics(path.settings.trackWidth), + outputVelocity + ) + } + + suspend fun followPath(settings: RobotConfig, constraints: PathConstraints, configure: WaypointPathConfig.() -> Unit) { + val waypoints = WaypointPathConfig().apply(configure).waypoints + val kinematics = DifferentialDriveKinematics(settings.trackWidth) + runWpilibPath( + TrajectoryGenerator.generateTrajectory( + waypoints, + TrajectoryConfig( + constraints.maxVel, + constraints.maxAcc + ).setKinematics(kinematics) + ), + getPose, + RamseteController(), + kinematics, + outputVelocity + ) + } + + suspend fun followPath(settings: RobotConfig, constraints: PathConstraints, start: Pose2d, finish: Pose2d, configure: PointPathConfig.() -> Unit) { + val points = PointPathConfig().apply(configure).points + val kinematics = DifferentialDriveKinematics(settings.trackWidth) + runWpilibPath( + TrajectoryGenerator.generateTrajectory( + start, + points, + finish, + TrajectoryConfig( + constraints.maxVel, + constraints.maxAcc + ).setKinematics(kinematics) + ), + getPose, + RamseteController(), + kinematics, + outputVelocity + ) + } +} + +fun pathFollower(configure: PathFollowerConfig.() -> Unit) = with(PathFollowerConfig().apply(configure)) { + PathFollower(getPose, outputVelocity) +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt new file mode 100644 index 0000000..bfe99f3 --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt @@ -0,0 +1,55 @@ +package org.sert2521.sertain.pathfinding + +import edu.wpi.first.wpilibj.geometry.Pose2d +import edu.wpi.first.wpilibj.geometry.Rotation2d +import edu.wpi.first.wpilibj.geometry.Translation2d +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics +import edu.wpi.first.wpilibj.trajectory.Trajectory +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator + +data class RobotConfig( + val trackWidth: Double +) + +data class PathConstraints( + val maxVel: Double, + val maxAcc: Double +) + +class Path private constructor(val settings: RobotConfig, val constraints: PathConstraints, private val wpilibTrajectory: Trajectory) { + constructor(settings: RobotConfig, constraints: PathConstraints, vararg waypoints: Pose2d) : this( + settings, + constraints, + TrajectoryGenerator.generateTrajectory( + waypoints.map { it }, + TrajectoryConfig( + constraints.maxVel, + constraints.maxAcc + ).setKinematics(DifferentialDriveKinematics(settings.trackWidth)) + ) + ) + + constructor(settings: RobotConfig, constraints: PathConstraints, start: Pose2d, finish: Pose2d, vararg points: Translation2d) : this( + settings, + constraints, + TrajectoryGenerator.generateTrajectory( + start, + points.toList(), + finish, + TrajectoryConfig( + constraints.maxVel, + constraints.maxAcc + ).setKinematics(DifferentialDriveKinematics(settings.trackWidth)) + ) + ) + + val startPoint = wpilibTrajectory.initialPose.run { Pose2d(translation.x, translation.y, Rotation2d(rotation.radians)) } + val length get() = wpilibTrajectory.totalTimeSeconds + + operator fun get(t: Double) = wpilibTrajectory.sample(t) + + fun relativeTo(point: Pose2d) = Path(settings, constraints, wpilibTrajectory.relativeTo(point)) + + fun toWpilibTrajectory() = wpilibTrajectory +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt deleted file mode 100644 index beafcf9..0000000 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathFollowing.kt +++ /dev/null @@ -1,53 +0,0 @@ -package org.sert2521.sertain.pathfinding - -import edu.wpi.first.wpilibj.controller.RamseteController -import edu.wpi.first.wpilibj.geometry.Translation2d -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics -import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig -import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator - -data class PathSettings( - val maxAcc: Double, - val maxVel: Double, - val trackWidth: Double, - val getWayPoint: () -> WayPoint, - val output: (left: Double, right: Double) -> Unit -) - -suspend fun runPath(settings: PathSettings, configure: WayPointCollection.() -> Unit) { - val wayPoints = WayPointCollection().apply(configure).wayPoints - val kinematics = DifferentialDriveKinematics(settings.trackWidth) - runWpilibPath( - TrajectoryGenerator.generateTrajectory( - wayPoints.map { it.toPose2d() }, - TrajectoryConfig( - settings.maxVel, - settings.maxAcc - ).setKinematics(kinematics) - ), - { settings.getWayPoint().toPose2d() }, - RamseteController(), - kinematics, - settings.output - ) -} - -suspend fun runPath(settings: PathSettings, start: WayPoint, finish: WayPoint, configure: PointCollection.() -> Unit) { - val points = PointCollection().apply(configure).points - val kinematics = DifferentialDriveKinematics(settings.trackWidth) - runWpilibPath( - TrajectoryGenerator.generateTrajectory( - start.toPose2d(), - points.map { Translation2d(it.x, it.y) }, - finish.toPose2d(), - TrajectoryConfig( - settings.maxVel, - settings.maxAcc - ).setKinematics(kinematics) - ), - { settings.getWayPoint().toPose2d() }, - RamseteController(), - kinematics, - settings.output - ) -} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt deleted file mode 100644 index ebb4d36..0000000 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WayPointCollection.kt +++ /dev/null @@ -1,33 +0,0 @@ -package org.sert2521.sertain.pathfinding - -import org.sert2521.sertain.units.AngularUnit -import org.sert2521.sertain.units.AngularValue -import org.sert2521.sertain.units.LinearUnit -import org.sert2521.sertain.units.LinearValue -import org.sert2521.sertain.units.Meters -import org.sert2521.sertain.units.Radians -import org.sert2521.sertain.units.convertTo -import org.sert2521.sertain.utils.Point - -class WayPointCollection { - val wayPoints = mutableListOf() - - fun wp(x: LinearValue, y: LinearValue, ang: AngularValue) { - wayPoints += WayPoint(x.convertTo(Meters).value, y.convertTo(Meters).value, ang.convertTo(Radians).value) - } - - fun p(x: LinearValue, y: LinearValue) = - Point(x.convertTo(Meters).value, y.convertTo(Meters).value) - - infix fun Point.ang(angle: AngularValue) { - wayPoints += WayPoint(this, angle.convertTo(Radians).value) - } -} - -class PointCollection { - val points = mutableListOf() - - fun p(x: LinearValue, y: LinearValue) { - points += Point(x.convertTo(Meters).value, y.convertTo(Meters).value) - } -} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt deleted file mode 100644 index e5a038c..0000000 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Waypoint.kt +++ /dev/null @@ -1,16 +0,0 @@ -package org.sert2521.sertain.pathfinding - -import edu.wpi.first.wpilibj.geometry.Pose2d -import edu.wpi.first.wpilibj.geometry.Rotation2d -import edu.wpi.first.wpilibj.geometry.Translation2d -import org.sert2521.sertain.utils.Coordinates -import org.sert2521.sertain.utils.Point - -data class WayPoint(val point: Point, val angle: Double) : Coordinates { - constructor(x: Double, y: Double, angle: Double) : this(Point(x, y), angle) - - override val x get() = point.x - override val y get() = point.y -} - -fun WayPoint.toPose2d() = Pose2d(Translation2d(x, y), Rotation2d(angle)) diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt new file mode 100644 index 0000000..5a5a6ac --- /dev/null +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt @@ -0,0 +1,65 @@ +package org.sert2521.sertain.pathfinding + +import edu.wpi.first.wpilibj.geometry.Pose2d +import edu.wpi.first.wpilibj.geometry.Rotation2d +import edu.wpi.first.wpilibj.geometry.Translation2d +import org.sert2521.sertain.units.AngularUnit +import org.sert2521.sertain.units.AngularValue +import org.sert2521.sertain.units.LinearUnit +import org.sert2521.sertain.units.LinearValue +import org.sert2521.sertain.units.Meters +import org.sert2521.sertain.units.Radians +import org.sert2521.sertain.units.convertTo + +class WaypointPathConfig { + var constraints = PathConstraints(0.0, 0.0) + private set + + var maxVel + get() = constraints.maxVel + set(value) { + constraints = constraints.copy(maxVel = value) + } + + var maxAcc + get() = constraints.maxAcc + set(value) { + constraints = constraints.copy(maxAcc = value) + } + + val waypoints = mutableListOf() + + fun wp(x: LinearValue, y: LinearValue, ang: AngularValue) { + waypoints += Pose2d(x.convertTo(Meters).value, y.convertTo(Meters).value, Rotation2d(ang.convertTo(Radians).value)) + } + + fun p(x: LinearValue, y: LinearValue) = + Translation2d(x.convertTo(Meters).value, y.convertTo(Meters).value) + + infix fun Translation2d.ang(angle: AngularValue) { + waypoints += Pose2d(this, Rotation2d(angle.convertTo(Radians).value)) + } +} + +class PointPathConfig { + var constraints = PathConstraints(0.0, 0.0) + private set + + var maxVel + get() = constraints.maxVel + set(value) { + constraints = constraints.copy(maxVel = value) + } + + var maxAcc + get() = constraints.maxAcc + set(value) { + constraints = constraints.copy(maxAcc = value) + } + + internal val points = mutableListOf() + + fun p(x: LinearValue, y: LinearValue) { + points += Translation2d(x.convertTo(Meters).value, y.convertTo(Meters).value) + } +} diff --git a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt index 8b49be2..958e0b2 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/utils/Math.kt @@ -19,12 +19,3 @@ fun Double.root(n: Int): Double { fun Double.roundTo(decimalPlaces: Int) = round(this * 10.0.pow(decimalPlaces)) / 10.0.pow(decimalPlaces) - -interface Coordinates { - val x: Double - val y: Double -} - -data class Point(override val x: Double, override val y: Double) : Coordinates - -fun p(x: Double, y: Double) = Point(x, y) diff --git a/drivetrain/build.gradle.kts b/drivetrain/build.gradle.kts deleted file mode 100644 index 3a816fb..0000000 --- a/drivetrain/build.gradle.kts +++ /dev/null @@ -1,44 +0,0 @@ -import org.jetbrains.kotlin.gradle.tasks.KotlinCompile - -plugins { - kotlin("jvm") - java - `maven-publish` - maven -} - -dependencies { - implementation(kotlin("stdlib-jdk8", "1.3.61")) - implementation(kotlin("reflect", "1.3.61")) - implementation("org.jetbrains.kotlinx:kotlinx-coroutines-core:1.3.3") - implementation("edu.wpi.first.wpilibj:wpilibj-java:2020.1.2") - implementation("edu.wpi.first.hal:hal-java:2020.1.2") - implementation("edu.wpi.first.wpiutil:wpiutil-java:2020.1.2") - implementation("edu.wpi.first.ntcore:ntcore-java:2020.1.2") - implementation("com.ctre.phoenix:api-java:5.14.1") - implementation("com.revrobotics.frc:SparkMax-java:1.4.1") - implementation("com.kauailabs.navx.frc:navx-java:3.1.344") - implementation(project(":core")) -} - -configure { - sourceCompatibility = JavaVersion.VERSION_11 -} - -tasks.withType { - kotlinOptions.jvmTarget = "11" -} - -val compileKotlin: KotlinCompile by tasks - -compileKotlin.kotlinOptions { - freeCompilerArgs = listOf("-XXLanguage:+InlineClasses", "-Xuse-experimental=kotlin.Experimental") -} - -publishing { - publications { - create("maven") { - artifact("$buildDir/libs/${project.name}-${project.version}.jar") - } - } -} diff --git a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt deleted file mode 100644 index ffab976..0000000 --- a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/Drivetrain.kt +++ /dev/null @@ -1,14 +0,0 @@ -package org.sert2521.sertain.subsystems.drivetrain - -import edu.wpi.first.wpilibj.I2C -import org.sert2521.sertain.subsystems.Subsystem -import com.kauailabs.navx.frc.AHRS -import edu.wpi.first.wpilibj.I2C - -class Drivetrain(options: Map) : Subsystem(options["name"] as? String ?: "Drivetrain") { - val navx = (options["navxPort"] as? NavxPort)?.let { - AHRS(SPI.Port.kMXP) - } - - val heading get() = navx -} diff --git a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt b/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt deleted file mode 100644 index 3957baf..0000000 --- a/drivetrain/src/main/kotlin/org/sert2521/sertain/subsystems/drivetrain/DrivetrainConfig.kt +++ /dev/null @@ -1,33 +0,0 @@ -package org.sert2521.sertain.subsystems.drivetrain - -import com.kauailabs.navx.frc.AHRS -import edu.wpi.first.wpilibj.I2C -import edu.wpi.first.wpilibj.SPI -import kotlin.properties.ReadWriteProperty -import kotlin.reflect.KProperty - -sealed class NavxPort - -object Kmpx : NavxPort() -object KOnboard : NavxPort() - -class DrivetrainConfig { - internal val map = mutableMapOf() - - var navx: AHRS? by DrivetrainConfigProperty(null) -} - -class DrivetrainConfigProperty(initialValue: T) : ReadWriteProperty { - override fun getValue(thisRef: DrivetrainConfig, property: KProperty<*>): T { - thisRef.map[property.name].let { - if (it !is T) { - - } - } - } - - override fun setValue(thisRef: DrivetrainConfig, property: KProperty<*>, value: T) { - thisRef.map[property.name] = value - } - -} diff --git a/settings.gradle.kts b/settings.gradle.kts index f035d0a..a1b7719 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -7,4 +7,3 @@ pluginManagement { rootProject.name = "sertain" include("core") -include("drivetrain") From a33791c4393fff513e8fb60ad104b4cf1a89b182 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Thu, 6 Feb 2020 14:52:43 -0800 Subject: [PATCH 7/9] It's pathfinding. --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index d18ef6c..cd5a215 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ .gradle /build/ +/core/build/ # Ignore Gradle GUI config gradle-app.setting From 382a9c381a5458d57250b529fa0feaeab4f3e732 Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Thu, 6 Feb 2020 16:45:08 -0800 Subject: [PATCH 8/9] change it --- .../sert2521/sertain/pathfinding/Follower.kt | 39 ++++++++++++------- .../org/sert2521/sertain/pathfinding/Path.kt | 10 ++--- .../{WaypointPathConfig.kt => PathConfig.kt} | 0 3 files changed, 29 insertions(+), 20 deletions(-) rename core/src/main/kotlin/org/sert2521/sertain/pathfinding/{WaypointPathConfig.kt => PathConfig.kt} (100%) diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt index a15c933..d529a26 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt @@ -11,6 +11,15 @@ class PathFollowerConfig { internal var getPose = { Pose2d(0.0, 0.0, Rotation2d(0.0)) } internal var outputVelocity = { l: Double, r: Double -> Unit } + var constraints = DriveSettings(0.0) + private set + + var trackWidth + get() = constraints.trackWidth + set(value) { + constraints = constraints.copy(trackWidth = value) + } + fun pose(get: () -> Pose2d) { getPose = get } @@ -20,7 +29,11 @@ class PathFollowerConfig { } } -data class PathFollower(val getPose: () -> Pose2d, val outputVelocity: (left: Double, right: Double) -> Unit) { +data class DriveSettings( + val trackWidth: Double +) + +class PathFollower(val settings: DriveSettings, val getPose: () -> Pose2d, val outputVelocity: (left: Double, right: Double) -> Unit) { suspend fun follow(path: Path) { runWpilibPath( path.toWpilibTrajectory(), @@ -31,15 +44,15 @@ data class PathFollower(val getPose: () -> Pose2d, val outputVelocity: (left: Do ) } - suspend fun followPath(settings: RobotConfig, constraints: PathConstraints, configure: WaypointPathConfig.() -> Unit) { - val waypoints = WaypointPathConfig().apply(configure).waypoints + suspend fun followPath(configure: WaypointPathConfig.() -> Unit) { + val config = WaypointPathConfig().apply(configure) val kinematics = DifferentialDriveKinematics(settings.trackWidth) runWpilibPath( TrajectoryGenerator.generateTrajectory( - waypoints, + config.waypoints, TrajectoryConfig( - constraints.maxVel, - constraints.maxAcc + config.maxVel, + config.maxAcc ).setKinematics(kinematics) ), getPose, @@ -49,17 +62,17 @@ data class PathFollower(val getPose: () -> Pose2d, val outputVelocity: (left: Do ) } - suspend fun followPath(settings: RobotConfig, constraints: PathConstraints, start: Pose2d, finish: Pose2d, configure: PointPathConfig.() -> Unit) { - val points = PointPathConfig().apply(configure).points + suspend fun followPath(start: Pose2d, finish: Pose2d, configure: PointPathConfig.() -> Unit) { + val config = PointPathConfig().apply(configure) val kinematics = DifferentialDriveKinematics(settings.trackWidth) runWpilibPath( TrajectoryGenerator.generateTrajectory( start, - points, + config.points, finish, TrajectoryConfig( - constraints.maxVel, - constraints.maxAcc + config.maxVel, + config.maxAcc ).setKinematics(kinematics) ), getPose, @@ -70,6 +83,6 @@ data class PathFollower(val getPose: () -> Pose2d, val outputVelocity: (left: Do } } -fun pathFollower(configure: PathFollowerConfig.() -> Unit) = with(PathFollowerConfig().apply(configure)) { - PathFollower(getPose, outputVelocity) +fun follower(configure: PathFollowerConfig.() -> Unit) = with(PathFollowerConfig().apply(configure)) { + PathFollower(DriveSettings(trackWidth), getPose, outputVelocity) } diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt index bfe99f3..fe47254 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt @@ -8,17 +8,13 @@ import edu.wpi.first.wpilibj.trajectory.Trajectory import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator -data class RobotConfig( - val trackWidth: Double -) - data class PathConstraints( val maxVel: Double, val maxAcc: Double ) -class Path private constructor(val settings: RobotConfig, val constraints: PathConstraints, private val wpilibTrajectory: Trajectory) { - constructor(settings: RobotConfig, constraints: PathConstraints, vararg waypoints: Pose2d) : this( +class Path private constructor(val settings: DriveSettings, val constraints: PathConstraints, private val wpilibTrajectory: Trajectory) { + constructor(settings: DriveSettings, constraints: PathConstraints, vararg waypoints: Pose2d) : this( settings, constraints, TrajectoryGenerator.generateTrajectory( @@ -30,7 +26,7 @@ class Path private constructor(val settings: RobotConfig, val constraints: PathC ) ) - constructor(settings: RobotConfig, constraints: PathConstraints, start: Pose2d, finish: Pose2d, vararg points: Translation2d) : this( + constructor(settings: DriveSettings, constraints: PathConstraints, start: Pose2d, finish: Pose2d, vararg points: Translation2d) : this( settings, constraints, TrajectoryGenerator.generateTrajectory( diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt similarity index 100% rename from core/src/main/kotlin/org/sert2521/sertain/pathfinding/WaypointPathConfig.kt rename to core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt From b998a4654c8900a9ffb21f0a5f0587111995f07b Mon Sep 17 00:00:00 2001 From: Aidan Heller Date: Wed, 19 Feb 2020 11:43:56 -0800 Subject: [PATCH 9/9] Fix stuff hopefully --- .../sert2521/sertain/pathfinding/Follower.kt | 33 ++++++------ .../org/sert2521/sertain/pathfinding/Path.kt | 51 ------------------- .../sertain/pathfinding/PathConfig.kt | 25 ++++----- .../sertain/pathfinding/WpilibPaths.kt | 2 +- 4 files changed, 26 insertions(+), 85 deletions(-) delete mode 100644 core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt index d529a26..c909ffa 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Follower.kt @@ -4,14 +4,20 @@ import edu.wpi.first.wpilibj.controller.RamseteController import edu.wpi.first.wpilibj.geometry.Pose2d import edu.wpi.first.wpilibj.geometry.Rotation2d import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics +import edu.wpi.first.wpilibj.trajectory.Trajectory import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator +data class PathConstraints( + val maxVel: Double, + val maxAcc: Double +) + class PathFollowerConfig { internal var getPose = { Pose2d(0.0, 0.0, Rotation2d(0.0)) } internal var outputVelocity = { l: Double, r: Double -> Unit } - var constraints = DriveSettings(0.0) + var constraints = DriveSettings(1.0) private set var trackWidth @@ -34,12 +40,13 @@ data class DriveSettings( ) class PathFollower(val settings: DriveSettings, val getPose: () -> Pose2d, val outputVelocity: (left: Double, right: Double) -> Unit) { - suspend fun follow(path: Path) { - runWpilibPath( - path.toWpilibTrajectory(), + suspend fun follow(trajectory: Trajectory) { + val kinematics = DifferentialDriveKinematics(settings.trackWidth) + followWpilibPath( + trajectory, getPose, RamseteController(), - DifferentialDriveKinematics(path.settings.trackWidth), + kinematics, outputVelocity ) } @@ -47,25 +54,21 @@ class PathFollower(val settings: DriveSettings, val getPose: () -> Pose2d, val o suspend fun followPath(configure: WaypointPathConfig.() -> Unit) { val config = WaypointPathConfig().apply(configure) val kinematics = DifferentialDriveKinematics(settings.trackWidth) - runWpilibPath( + follow( TrajectoryGenerator.generateTrajectory( config.waypoints, TrajectoryConfig( config.maxVel, config.maxAcc ).setKinematics(kinematics) - ), - getPose, - RamseteController(), - kinematics, - outputVelocity + ) ) } suspend fun followPath(start: Pose2d, finish: Pose2d, configure: PointPathConfig.() -> Unit) { val config = PointPathConfig().apply(configure) val kinematics = DifferentialDriveKinematics(settings.trackWidth) - runWpilibPath( + follow( TrajectoryGenerator.generateTrajectory( start, config.points, @@ -74,11 +77,7 @@ class PathFollower(val settings: DriveSettings, val getPose: () -> Pose2d, val o config.maxVel, config.maxAcc ).setKinematics(kinematics) - ), - getPose, - RamseteController(), - kinematics, - outputVelocity + ) ) } } diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt deleted file mode 100644 index fe47254..0000000 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/Path.kt +++ /dev/null @@ -1,51 +0,0 @@ -package org.sert2521.sertain.pathfinding - -import edu.wpi.first.wpilibj.geometry.Pose2d -import edu.wpi.first.wpilibj.geometry.Rotation2d -import edu.wpi.first.wpilibj.geometry.Translation2d -import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics -import edu.wpi.first.wpilibj.trajectory.Trajectory -import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig -import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator - -data class PathConstraints( - val maxVel: Double, - val maxAcc: Double -) - -class Path private constructor(val settings: DriveSettings, val constraints: PathConstraints, private val wpilibTrajectory: Trajectory) { - constructor(settings: DriveSettings, constraints: PathConstraints, vararg waypoints: Pose2d) : this( - settings, - constraints, - TrajectoryGenerator.generateTrajectory( - waypoints.map { it }, - TrajectoryConfig( - constraints.maxVel, - constraints.maxAcc - ).setKinematics(DifferentialDriveKinematics(settings.trackWidth)) - ) - ) - - constructor(settings: DriveSettings, constraints: PathConstraints, start: Pose2d, finish: Pose2d, vararg points: Translation2d) : this( - settings, - constraints, - TrajectoryGenerator.generateTrajectory( - start, - points.toList(), - finish, - TrajectoryConfig( - constraints.maxVel, - constraints.maxAcc - ).setKinematics(DifferentialDriveKinematics(settings.trackWidth)) - ) - ) - - val startPoint = wpilibTrajectory.initialPose.run { Pose2d(translation.x, translation.y, Rotation2d(rotation.radians)) } - val length get() = wpilibTrajectory.totalTimeSeconds - - operator fun get(t: Double) = wpilibTrajectory.sample(t) - - fun relativeTo(point: Pose2d) = Path(settings, constraints, wpilibTrajectory.relativeTo(point)) - - fun toWpilibTrajectory() = wpilibTrajectory -} diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt index 5a5a6ac..a3874fd 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/PathConfig.kt @@ -3,16 +3,9 @@ package org.sert2521.sertain.pathfinding import edu.wpi.first.wpilibj.geometry.Pose2d import edu.wpi.first.wpilibj.geometry.Rotation2d import edu.wpi.first.wpilibj.geometry.Translation2d -import org.sert2521.sertain.units.AngularUnit -import org.sert2521.sertain.units.AngularValue -import org.sert2521.sertain.units.LinearUnit -import org.sert2521.sertain.units.LinearValue -import org.sert2521.sertain.units.Meters -import org.sert2521.sertain.units.Radians -import org.sert2521.sertain.units.convertTo class WaypointPathConfig { - var constraints = PathConstraints(0.0, 0.0) + var constraints = PathConstraints(1.0, 1.0) private set var maxVel @@ -29,15 +22,15 @@ class WaypointPathConfig { val waypoints = mutableListOf() - fun wp(x: LinearValue, y: LinearValue, ang: AngularValue) { - waypoints += Pose2d(x.convertTo(Meters).value, y.convertTo(Meters).value, Rotation2d(ang.convertTo(Radians).value)) + fun wp(x: Double, y: Double, ang: Double) { + waypoints += Pose2d(x, y, Rotation2d(ang)) } - fun p(x: LinearValue, y: LinearValue) = - Translation2d(x.convertTo(Meters).value, y.convertTo(Meters).value) + fun p(x: Double, y: Double) = + Translation2d(x, y) - infix fun Translation2d.ang(angle: AngularValue) { - waypoints += Pose2d(this, Rotation2d(angle.convertTo(Radians).value)) + infix fun Translation2d.ang(angle: Double) { + waypoints += Pose2d(this, Rotation2d(angle)) } } @@ -59,7 +52,7 @@ class PointPathConfig { internal val points = mutableListOf() - fun p(x: LinearValue, y: LinearValue) { - points += Translation2d(x.convertTo(Meters).value, y.convertTo(Meters).value) + fun p(x: Double, y: Double) { + points += Translation2d(x, y) } } diff --git a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt index 038a692..9e21aa5 100644 --- a/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt +++ b/core/src/main/kotlin/org/sert2521/sertain/pathfinding/WpilibPaths.kt @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics import edu.wpi.first.wpilibj.trajectory.Trajectory import org.sert2521.sertain.utils.timer -suspend fun runWpilibPath( +suspend fun followWpilibPath( trajectory: Trajectory, getPose: () -> Pose2d, follower: RamseteController,