v0.5.1-beta1: Controllables!

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"
     }
 }
 

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 = { },

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
+    }
 }

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].
      *

core/src/main/java/com/rowanmcalpin/nextftc/core/control/controllers/SqrtController.kt

@@ -12,14 +12,15 @@ 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
 
         return (sqrt(error) * kS)
     }
 
+    @Deprecated("Removed to shift to Controllable interface")
     /**
      * Whether this controller is within a certain distance of the [target].
      *

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
  * 

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.

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.
  * 

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.
  *

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.

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.
  * 

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
+}

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
+        }
+}

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<out String>): Array<out MotorEx> {
+            val list = mutableListOf<MotorEx>()
+            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.")

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<Subsystem> = 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
+        }
+    }
+}

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<Subsystem> = 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
+        }
+    }
+}

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<Subsystem> = setOf()): Command() {
+    override val isDone = true
+
+    override fun start() {
+        controllable.power = power
+    }
+}

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" }