From 55a51307fa95633b9bcdf9e9ec182cacaf7bd473 Mon Sep 17 00:00:00 2001 From: rowan-mcalpin <87948250+rowan-mcalpin@users.noreply.github.com> Date: Sun, 29 Dec 2024 15:54:19 -0800 Subject: [PATCH] v0.5.1-beta1: Controllables! --- .idea/kotlinc.xml | 2 +- core/build.gradle.kts | 6 +-- .../core/command/utility/LambdaCommand.kt | 2 +- .../core/control/controllers/Controller.kt | 15 ++++++ .../control/controllers/PIDFController.kt | 3 +- .../control/controllers/SqrtController.kt | 3 +- .../nextftc/ftc/hardware/BlindPowerMotor.kt | 1 + .../nextftc/ftc/hardware/MotorHoldPosition.kt | 1 + .../nextftc/ftc/hardware/MotorToPosition.kt | 2 + .../nextftc/ftc/hardware/MotorToVelocity.kt | 1 + .../hardware/MultipleMotorsHoldPosition.kt | 1 + .../ftc/hardware/MultipleMotorsToPosition.kt | 1 + .../hardware/controllables/Controllable.kt | 10 ++++ .../ftc/hardware/controllables/MotorEx.kt | 29 +++++++++++ .../ftc/hardware/controllables/MotorGroup.kt | 50 +++++++++++++++++++ .../hardware/controllables/RunToPosition.kt | 27 ++++++++++ .../hardware/controllables/RunToVelocity.kt | 35 +++++++++++++ .../ftc/hardware/controllables/SetPower.kt | 21 ++++++++ .../ftc/hardware/motors/Controllable.kt | 6 --- .../nextftc/ftc/hardware/motors/MotorEx.kt | 17 ------- gradle/libs.versions.toml | 8 +-- maven.rowanmcalpin.com | 2 +- 22 files changed, 208 insertions(+), 35 deletions(-) create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/Controllable.kt create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorEx.kt create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorGroup.kt create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToPosition.kt create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToVelocity.kt create mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/SetPower.kt delete mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/Controllable.kt delete mode 100644 ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/MotorEx.kt diff --git a/.idea/kotlinc.xml b/.idea/kotlinc.xml index 148fdd2..fdf8d99 100644 --- a/.idea/kotlinc.xml +++ b/.idea/kotlinc.xml @@ -1,6 +1,6 @@ - \ No newline at end of file diff --git a/core/build.gradle.kts b/core/build.gradle.kts index 344f542..8158aa7 100644 --- a/core/build.gradle.kts +++ b/core/build.gradle.kts @@ -26,11 +26,11 @@ android { } } compileOptions { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 } kotlinOptions { - jvmTarget = "17" + jvmTarget = "1.8" } } diff --git a/core/src/main/java/com/rowanmcalpin/nextftc/core/command/utility/LambdaCommand.kt b/core/src/main/java/com/rowanmcalpin/nextftc/core/command/utility/LambdaCommand.kt index a881711..8cb2d85 100644 --- a/core/src/main/java/com/rowanmcalpin/nextftc/core/command/utility/LambdaCommand.kt +++ b/core/src/main/java/com/rowanmcalpin/nextftc/core/command/utility/LambdaCommand.kt @@ -13,7 +13,7 @@ import com.rowanmcalpin.nextftc.core.command.Command * @param subsystemCollection a set of subsystems this command implements * @param interruptible whether this command can be stopped due to an overlap of subsystems */ -class LambdaCommand( +class LambdaCommand @JvmOverloads constructor( private val isDoneLambda: () -> Boolean = { true }, private val startLambda: () -> Unit = { }, private val updateLambda: () -> Unit = { }, diff --git a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/Controller.kt b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/Controller.kt index fe0261f..4506232 100644 --- a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/Controller.kt +++ b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/Controller.kt @@ -1,5 +1,7 @@ package com.rowanmcalpin.nextftc.core.control.controllers +import kotlin.math.abs + /** * Interface all controllers must inherit from. */ @@ -9,6 +11,11 @@ interface Controller { */ var target: Double + /** + * The tolerance for being "at the target" + */ + var setPointTolerance: Double + /** * Given a reference, calculates how to best match the target. * @@ -20,4 +27,12 @@ interface Controller { * Resets the control loop */ fun reset() + + /** + * Whether the controller is within a tolerable distance of the target + */ + fun atTarget(reference: Double): Boolean { + if (abs(target - reference) <= setPointTolerance) return true + return false + } } \ No newline at end of file diff --git a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/PIDFController.kt b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/PIDFController.kt index 5dc9592..266315c 100644 --- a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/PIDFController.kt +++ b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/PIDFController.kt @@ -24,7 +24,7 @@ open class PIDFController(val coefficients: PIDFCoefficients): Controller { override var target: Double = 0.0 - var setPointTolerance: Double = 0.0 + override var setPointTolerance: Double = 0.0 private var lastError = 0.0 private var integralSum = 0.0 @@ -45,6 +45,7 @@ open class PIDFController(val coefficients: PIDFCoefficients): Controller { return (error * coefficients.kP) + (integralSum * coefficients.kI) + (derivative * coefficients.kD) + coefficients.kF } + @Deprecated("Removed to shift to Controllable interface") /** * Whether this controller is within a certain distance of the [target]. * diff --git a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/SqrtController.kt b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/SqrtController.kt index fc5cead..77a883b 100644 --- a/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/SqrtController.kt +++ b/core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/SqrtController.kt @@ -12,7 +12,7 @@ import kotlin.math.sqrt open class SqrtController(val kS: Double): Controller { override var target: Double = 0.0 - var setPointTolerance: Double = 0.0 + override var setPointTolerance: Double = 0.0 override fun calculate(reference: Double): Double { val error = target - reference @@ -20,6 +20,7 @@ open class SqrtController(val kS: Double): Controller { return (sqrt(error) * kS) } + @Deprecated("Removed to shift to Controllable interface") /** * Whether this controller is within a certain distance of the [target]. * diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/BlindPowerMotor.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/BlindPowerMotor.kt index 413e327..1471b05 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/BlindPowerMotor.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/BlindPowerMotor.kt @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx import com.rowanmcalpin.nextftc.core.Subsystem import com.rowanmcalpin.nextftc.core.command.Command +@Deprecated("Deprecated in favor of controllables", replaceWith = ReplaceWith("SetPower")) /** * Sets a motor to a specific power without any internal feedback * diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorHoldPosition.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorHoldPosition.kt index ecc7452..0e46699 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorHoldPosition.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorHoldPosition.kt @@ -7,6 +7,7 @@ import com.rowanmcalpin.nextftc.core.command.Command import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController import kotlin.math.abs +@Deprecated("Removed in favor of controllables", replaceWith = ReplaceWith("RunToPosition")) /** * This command holds a motor's position until another command is scheduled that uses the same * subsystem. diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToPosition.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToPosition.kt index 1ebf8e7..49487fa 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToPosition.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToPosition.kt @@ -5,8 +5,10 @@ import com.qualcomm.robotcore.hardware.DcMotorEx import com.rowanmcalpin.nextftc.core.Subsystem import com.rowanmcalpin.nextftc.core.command.Command import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController +import com.rowanmcalpin.nextftc.ftc.hardware.controllables.RunToPosition import kotlin.math.abs +@Deprecated("Removed in favor of controllables", replaceWith = ReplaceWith("RunToPosition")) /** * This implements a PID controller to drive a motor to a specified target position. * diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToVelocity.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToVelocity.kt index 5b4276a..557714e 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToVelocity.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MotorToVelocity.kt @@ -7,6 +7,7 @@ import com.rowanmcalpin.nextftc.core.command.Command import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController import kotlin.math.abs +@Deprecated("Deprecated in favor of controllables", ReplaceWith("RunToVelocity")) /** * This implements a PID controller to drive a motor to a specified target velocity. * diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsHoldPosition.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsHoldPosition.kt index 5bddb3f..372e749 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsHoldPosition.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsHoldPosition.kt @@ -7,6 +7,7 @@ import com.rowanmcalpin.nextftc.core.command.Command import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController import kotlin.math.abs +@Deprecated("Removed in favor of controllables", replaceWith = ReplaceWith("RunToPosition")) /** * This command holds a multiple motor's positions until another command is scheduled that uses the * same subsystem. diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsToPosition.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsToPosition.kt index e9e9a42..7186b6a 100644 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsToPosition.kt +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/MultipleMotorsToPosition.kt @@ -7,6 +7,7 @@ import com.rowanmcalpin.nextftc.core.command.Command import com.rowanmcalpin.nextftc.core.control.controllers.PIDFController import kotlin.math.abs +@Deprecated("Removed in favor of controllables", replaceWith = ReplaceWith("RunToPosition")) /** * This implements a PID controller to drive multiple motors to a specified target position. * diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/Controllable.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/Controllable.kt new file mode 100644 index 0000000..d77ba9d --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/Controllable.kt @@ -0,0 +1,10 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +/** + * An item that has a position and a power. + */ +interface Controllable { + var currentPosition: Double + var velocity: Double + var power: Double +} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorEx.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorEx.kt new file mode 100644 index 0000000..90433d2 --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorEx.kt @@ -0,0 +1,29 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +import com.qualcomm.robotcore.hardware.DcMotor +import com.qualcomm.robotcore.hardware.DcMotorEx +import com.rowanmcalpin.nextftc.ftc.OpModeData + +/** + * Wrapper class for motors that implements controllable (and can therefore be used with RunToPosition + * commands). + */ +class MotorEx(private val motor: DcMotorEx): Controllable { + + constructor(name: String): this(OpModeData.hardwareMap.get(DcMotorEx::class.java, name)) + + override var currentPosition: Double + get() = motor.currentPosition.toDouble() + set(value) { } + + override var velocity: Double + get() = motor.velocity + set(value) { } + + override var power: Double + get() = motor.power + set(value) { + motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER + motor.power = value + } +} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorGroup.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorGroup.kt new file mode 100644 index 0000000..e17b9c8 --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/MotorGroup.kt @@ -0,0 +1,50 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +/** + * A MotorGroup is a collection of [MotorEx]s that are all controlled by a single encoder (connected + * to the leader motor) + * + * @param leader the [MotorEx] with the encoder that will be used + * @param followers any other motors to control + */ +class MotorGroup(val leader: MotorEx, vararg val followers: MotorEx): Controllable { + + constructor(vararg names: String): this( + MotorEx(names[0]), + *createMotors(names) + ) + + override var currentPosition: Double + get() = leader.currentPosition + set(value) { } + + override var velocity: Double + get() = leader.velocity + set(value) { } + + override var power: Double + get() = leader.power + set(value) { + leader.power = value + followers.forEach { + it.power = value + } + } + + companion object { + fun createMotors(names: Array): Array { + val list = mutableListOf() + if (names.isEmpty()) { + throw NotEnoughMotorsException() + } + + for (i in 1..names.size) { + list += MotorEx(names[i]) + } + + return list.toTypedArray() + } + } +} + +class NotEnoughMotorsException(): Exception("You must supply at least one motor to a MotorGroup.") \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToPosition.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToPosition.kt new file mode 100644 index 0000000..ad2864c --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToPosition.kt @@ -0,0 +1,27 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +import com.rowanmcalpin.nextftc.core.Subsystem +import com.rowanmcalpin.nextftc.core.command.Command +import com.rowanmcalpin.nextftc.core.control.controllers.Controller +import kotlin.math.abs + +/** + * This implements a [Controller] to drive a [Controllable] to a specified target position + */ +class RunToPosition @JvmOverloads constructor(val controllable: Controllable, val target: Double, val controller: Controller, + override val subsystems: Set = setOf()): Command() { + override val isDone: Boolean + get() = controller.atTarget(controllable.currentPosition) + + override fun start() { + controller.target = target + controller.reset() + } + + override fun update() { + val calculatedPower = controller.calculate(controllable.currentPosition) + if (abs(controllable.power - calculatedPower) > 0.01) { + controllable.power = calculatedPower + } + } +} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToVelocity.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToVelocity.kt new file mode 100644 index 0000000..392e693 --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/RunToVelocity.kt @@ -0,0 +1,35 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +import com.rowanmcalpin.nextftc.core.Subsystem +import com.rowanmcalpin.nextftc.core.command.Command +import com.rowanmcalpin.nextftc.core.control.controllers.Controller +import kotlin.math.abs + +/** + * This implements a PID controller to drive a motor to a specified target velocity. + * + * @param controllable the [Controllable] to control + * @param targetVelocity the target velocity + * @param controller the [Controller] to implement + * @param subsystems the list of [Subsystem]s this command interacts with (should be whatever + * subsystem holds this command) + * @param outCondition will be evaluated every update, and the command will stop once it returns true + */ +class RunToVelocity @JvmOverloads constructor(val controllable: Controllable, val targetVelocity: Double, + val controller: Controller, override val subsystems: Set = setOf(), + val outCondition: () -> Boolean = { abs(controllable.velocity)-targetVelocity < 10 }): Command() { + override val isDone: Boolean + get() = outCondition.invoke() + + override fun start() { + controller.target = targetVelocity + controller.reset() + } + + override fun update() { + val calculatedPower = controller.calculate(controllable.velocity) + if (abs(controllable.power - calculatedPower) > 0.01) { + controllable.power = calculatedPower + } + } +} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/SetPower.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/SetPower.kt new file mode 100644 index 0000000..8f6a463 --- /dev/null +++ b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/controllables/SetPower.kt @@ -0,0 +1,21 @@ +package com.rowanmcalpin.nextftc.ftc.hardware.controllables + +import com.rowanmcalpin.nextftc.core.Subsystem +import com.rowanmcalpin.nextftc.core.command.Command + +/** + * Sets a controllable to a specific power without any internal feedback + * + * @param controllable the [Controllable] to control + * @param power the power to set the [Controllable] to + * @param subsystems the [Subsystem]s this command interacts with (should be whatever + * subsystem holds this command) + */ +class SetPower @JvmOverloads constructor(val controllable: Controllable, val power: Double, + override val subsystems: Set = setOf()): Command() { + override val isDone = true + + override fun start() { + controllable.power = power + } +} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/Controllable.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/Controllable.kt deleted file mode 100644 index 392722d..0000000 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/Controllable.kt +++ /dev/null @@ -1,6 +0,0 @@ -package com.rowanmcalpin.nextftc.ftc.hardware.motors - -interface Controllable { - var currentPosition: Double - var power: Double -} \ No newline at end of file diff --git a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/MotorEx.kt b/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/MotorEx.kt deleted file mode 100644 index dd4105c..0000000 --- a/ftc/src/main/java/com/rowanmcalpin/nextftc/ftc/hardware/motors/MotorEx.kt +++ /dev/null @@ -1,17 +0,0 @@ -package com.rowanmcalpin.nextftc.ftc.hardware.motors - -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.rowanmcalpin.nextftc.ftc.OpModeData - -class MotorEx(private val motor: DcMotorEx): Controllable { - - constructor(name: String): this(OpModeData.hardwareMap.get(DcMotorEx::class.java, name)) - - override var currentPosition: Double - get() = motor.currentPosition.toDouble() - set(value) { } - - override var power: Double - get() = motor.power - set(value) { motor.power = value } -} \ No newline at end of file diff --git a/gradle/libs.versions.toml b/gradle/libs.versions.toml index 3147f29..9d2e6d6 100644 --- a/gradle/libs.versions.toml +++ b/gradle/libs.versions.toml @@ -1,15 +1,15 @@ [versions] agp = "8.7.3" -kotlin = "1.9.24" +kotlin = "1.9.0" commons-math3 = "3.6.1" pedro = "0.0.1-beta14" ftc = "10.1.1" dokka = "1.9.24" -module-pedro = "0.5.0-beta4" -module-ftc = "0.5.0-beta4" -module-core = "0.5.0-beta4" +module-pedro = "0.5.1-beta1" +module-ftc = "0.5.1-beta1" +module-core = "0.5.1-beta1" [libraries] commons-math3 = { module = "org.apache.commons:commons-math3", version.ref = "commons-math3" } diff --git a/maven.rowanmcalpin.com b/maven.rowanmcalpin.com index ec8e870..18904ef 160000 --- a/maven.rowanmcalpin.com +++ b/maven.rowanmcalpin.com @@ -1 +1 @@ -Subproject commit ec8e870d721fad7657c18d627aa6153b971dba02 +Subproject commit 18904eff04495079df1bfac5b760c17dc4d72f4f