Bang Bang controller

Author: rowan-mcalpin

.kotlin/sessions/kotlin-compiler-4566496875608200174.salive

@@ -19,28 +19,28 @@ class FeedbackElementBuilder {
         apply { feedbackElement = feedback }
 
     fun posPid(coefficients: PIDCoefficients) =
-        custom(PIDElement(PIDType.POSITION, coefficients))
+        custom(PIDElement(FeedbackType.POSITION, coefficients))
 
     @JvmOverloads
     fun posPid(kP: Double, kI: Double = 0.0, kD: Double = 0.0) =
         posPid(PIDCoefficients(kP, kI, kD))
 
     fun velPid(coefficients: PIDCoefficients) =
-        custom(PIDElement(PIDType.VELOCITY, coefficients))
+        custom(PIDElement(FeedbackType.VELOCITY, coefficients))
 
     @JvmOverloads
     fun velPid(kP: Double, kI: Double = 0.0, kD: Double = 0.0) =
         velPid(PIDCoefficients(kP, kI, kD))
 
     fun posSquID(coefficients: PIDCoefficients) =
-        custom(SquIDElement(PIDType.POSITION, coefficients))
+        custom(SquIDElement(FeedbackType.POSITION, coefficients))
 
     @JvmOverloads
     fun posSquID(kP: Double, kI: Double = 0.0, kD: Double = 0.0) =
         posSquID(PIDCoefficients(kP, kI, kD))
 
     fun velSquID(coefficients: PIDCoefficients) =
-        custom(SquIDElement(PIDType.VELOCITY, coefficients))
+        custom(SquIDElement(FeedbackType.VELOCITY, coefficients))
 
     @JvmOverloads
     fun velSquID(kP: Double, kI: Double = 0.0, kD: Double = 0.0) =

nextcontrol/src/main/kotlin/dev/nextftc/nextcontrol/builder/ElementBuilders.kt

@@ -0,0 +1,40 @@
+/*
+ * NextFTC: a user-friendly control library for FIRST Tech Challenge
+ * Copyright (C) 2025 Rowan McAlpin
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package dev.nextftc.nextcontrol.feedback
+
+import dev.nextftc.nextcontrol.KineticState
+import kotlin.math.sign
+
+/**
+ * A [FeedbackElement] that is a Bang Bang controller (when error is positive, output 1, when error is negative, output
+ *  -1, when error is 0, output 0.
+ *
+ * @param feedbackType The type of component this controller operates on
+ *
+ * @author rowan-mcalpin
+ */
+class BangBangElement(
+    private val feedbackType: FeedbackType,
+) : FeedbackElement {
+
+    override fun calculate(error: KineticState): Double = when (feedbackType) {
+        FeedbackType.POSITION -> sign(error.position)
+        FeedbackType.VELOCITY -> sign(error.velocity)
+    }
+}

nextcontrol/src/main/kotlin/dev/nextftc/nextcontrol/feedback/BangBangElement.kt

@@ -42,4 +42,13 @@ fun interface FeedbackElement {
      * Resets this element
      */
     fun reset() { }
-}
+}
+
+/**
+ * Which component a FeedbackElement operates on (position or velocity)
+ *
+ * @author Zach.Waffle, rowan-mcalpin
+ */
+enum class FeedbackType {
+    POSITION, VELOCITY
+}

nextcontrol/src/main/kotlin/dev/nextftc/nextcontrol/feedback/FeedbackElement.kt

@@ -20,7 +20,7 @@ package dev.nextftc.nextcontrol.feedback
 
 import dev.nextftc.nextcontrol.ControlSystem
 import dev.nextftc.nextcontrol.KineticState
-import dev.nextftc.nextcontrol.TimeUtil
+import dev.nextftc.nextcontrol.utils.TimeUtil
 import kotlin.math.sign
 
 /**
@@ -36,10 +36,6 @@ data class PIDCoefficients @JvmOverloads constructor(
     @JvmField var kD: Double = 0.0
 )
 
-enum class PIDType {
-    POSITION, VELOCITY
-}
-
 /**
  * Traditional proportional-integral-derivative controller.
  *
@@ -107,22 +103,22 @@ internal class PIDController @JvmOverloads constructor(val coefficients: PIDCoef
  * @author Zach.Waffle, rowan-mcalpin
  */
 class PIDElement @JvmOverloads constructor(
-    private val pidType: PIDType,
+    private val pidType: FeedbackType,
     coefficients: PIDCoefficients,
     resetIntegralOnZeroCrossover: Boolean = true
 ) : FeedbackElement {
 
     @JvmOverloads
-    constructor(pidType: PIDType, kP: Double, kI: Double = 0.0, kD: Double = 0.0) :
+    constructor(pidType: FeedbackType, kP: Double, kI: Double = 0.0, kD: Double = 0.0) :
             this(pidType, PIDCoefficients(kP, kI, kD))
 
     private val controller = PIDController(coefficients, resetIntegralOnZeroCrossover)
 
     val coefficients by controller::coefficients
 
     fun calculate(timestamp: Long, error: KineticState) = when (pidType) {
-        PIDType.POSITION -> controller.calculate(timestamp, error.position, error.velocity)
-        PIDType.VELOCITY -> controller.calculate(timestamp, error.velocity, error.acceleration)
+        FeedbackType.POSITION -> controller.calculate(timestamp, error.position, error.velocity)
+        FeedbackType.VELOCITY -> controller.calculate(timestamp, error.velocity, error.acceleration)
     }
 
     override fun calculate(error: KineticState) = calculate(TimeUtil.nanoTime(), error)

nextcontrol/src/main/kotlin/dev/nextftc/nextcontrol/feedback/PIDElement.kt

@@ -20,7 +20,7 @@ package dev.nextftc.nextcontrol.feedback
 
 import dev.nextftc.nextcontrol.ControlSystem
 import dev.nextftc.nextcontrol.KineticState
-import dev.nextftc.nextcontrol.TimeUtil
+import dev.nextftc.nextcontrol.utils.TimeUtil
 import kotlin.math.abs
 import kotlin.math.sign
 import kotlin.math.sqrt
@@ -87,21 +87,21 @@ internal class SquIDController(val coefficients: PIDCoefficients) {
  * @author Zach.Waffle, rowan-mcalpin
  */
 class SquIDElement(
-    private val pidType: PIDType,
+    private val pidType: FeedbackType,
     coefficients: PIDCoefficients
 ) : FeedbackElement {
 
     @JvmOverloads
-    constructor(pidType: PIDType, kP: Double, kI: Double = 0.0, kD: Double = 0.0) :
+    constructor(pidType: FeedbackType, kP: Double, kI: Double = 0.0, kD: Double = 0.0) :
             this(pidType, PIDCoefficients(kP, kI, kD))
 
     private val controller = SquIDController(coefficients)
 
     val coefficients by controller::coefficients
 
     fun calculate(timestamp: Long, error: KineticState) = when (pidType) {
-        PIDType.POSITION -> controller.calculate(timestamp, error.position, error.velocity)
-        PIDType.VELOCITY -> controller.calculate(timestamp, error.velocity, error.acceleration)
+        FeedbackType.POSITION -> controller.calculate(timestamp, error.position, error.velocity)
+        FeedbackType.VELOCITY -> controller.calculate(timestamp, error.velocity, error.acceleration)
     }
 
     override fun calculate(error: KineticState) = this.calculate(TimeUtil.nanoTime(), error)

nextcontrol/src/main/kotlin/dev/nextftc/nextcontrol/feedback/SquIDElement.kt

@@ -0,0 +1,59 @@
+/*
+ * NextFTC: a user-friendly control library for FIRST Tech Challenge
+ * Copyright (C) 2025 Rowan McAlpin
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package dev.nextftc.nextcontrol.feedback
+
+import dev.nextftc.nextcontrol.KineticState
+import junit.framework.TestCase.assertEquals
+import org.junit.Test
+
+class BangBangElementTest {
+    @Test
+    fun `test bang bang controller on position`() {
+        val element: BangBangElement = BangBangElement(FeedbackType.POSITION)
+
+        // When error is positive, output is positive
+        var error = KineticState(100.0)
+        assertEquals(1.0, element.calculate(error), 0.0)
+
+        // When error is 0, output is 0
+        error = KineticState(0.0)
+        assertEquals(0.0, element.calculate(error), 0.0)
+
+        // When error is negative, output is negative
+        error = KineticState(-100.0)
+        assertEquals(-1.0, element.calculate(error), 0.0)
+    }
+
+    @Test
+    fun `test bang bang controller on velocity`() {
+        val element: BangBangElement = BangBangElement(FeedbackType.VELOCITY)
+
+        // When error is positive, output is positive
+        var error = KineticState(0.0, 100.0)
+        assertEquals(1.0, element.calculate(error), 0.0)
+
+        // When error is 0, output is 0
+        error = KineticState(0.0, 0.0)
+        assertEquals(0.0, element.calculate(error), 0.0)
+
+        // When error is negative, output is negative
+        error = KineticState(0.0, -100.0)
+        assertEquals(-1.0, element.calculate(error), 0.0)
+    }
+}

nextcontrol/src/test/java/dev/nextftc/nextcontrol/feedback/BangBangElementTest.kt

@@ -38,7 +38,7 @@ class PIDElementTest {
         val expected = PIDCoefficients(kP, kI, kD)
 
         // Act
-        val pidElement = PIDElement(PIDType.POSITION, kP, kI, kD)
+        val pidElement = PIDElement(FeedbackType.POSITION, kP, kI, kD)
         val actual = pidElement.coefficients
 
         // Assert
@@ -49,8 +49,8 @@ class PIDElementTest {
     fun `returns zero when all gains are zero`() {
         // Arrange
         val coefficients = PIDCoefficients(0.0, 0.0, 0.0)
-        val positionPID = PIDElement(PIDType.POSITION, coefficients)
-        val velocityPID = PIDElement(PIDType.VELOCITY, coefficients)
+        val positionPID = PIDElement(FeedbackType.POSITION, coefficients)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, coefficients)
         val input = KineticState(10.0, 20.0, 30.0)
         val expected = 0.0
 
@@ -67,8 +67,8 @@ class PIDElementTest {
     fun `returns zero when error has always been zero`() {
         // Arrange
         val coefficients = PIDCoefficients(1.0, 2.0, 3.0)
-        val positionPID = PIDElement(PIDType.POSITION, coefficients)
-        val velocityPID = PIDElement(PIDType.VELOCITY, coefficients)
+        val positionPID = PIDElement(FeedbackType.POSITION, coefficients)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, coefficients)
         val input = KineticState(0.0, 0.0, 0.0)
         val expected = 0.0
 
@@ -88,8 +88,8 @@ class PIDElementTest {
     @Test
     fun `returns error when kP is one and kI and kD are zero`() {
         // Arrange
-        val positionPID = PIDElement(PIDType.POSITION, 1.0, 0.0, 0.0)
-        val velocityPID = PIDElement(PIDType.VELOCITY, 1.0, 0.0, 0.0)
+        val positionPID = PIDElement(FeedbackType.POSITION, 1.0, 0.0, 0.0)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, 1.0, 0.0, 0.0)
         val error = 10.0
         val positionInput = KineticState(error, 20.0, 30.0)
         val velocityInput = KineticState(20.0, error, 30.0)
@@ -107,8 +107,8 @@ class PIDElementTest {
     fun `returns sum of all past errors times deltaT when kI is one and kP and kD are zero`() {
         // Arrange
         val coefficients = PIDCoefficients(0.0, 1.0, 0.0)
-        val positionPID = PIDElement(PIDType.POSITION, coefficients)
-        val velocityPID = PIDElement(PIDType.VELOCITY, coefficients)
+        val positionPID = PIDElement(FeedbackType.POSITION, coefficients)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, coefficients)
 
         mockkObject(TimeUtil)
 
@@ -149,8 +149,8 @@ class PIDElementTest {
     fun `returns error of derivative when kD is one and kP and kI are zero`() {
         // Arrange
         val coefficients = PIDCoefficients(0.0, 0.0, 1.0)
-        val positionPID = PIDElement(PIDType.POSITION, coefficients)
-        val velocityPID = PIDElement(PIDType.VELOCITY, coefficients)
+        val positionPID = PIDElement(FeedbackType.POSITION, coefficients)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, coefficients)
         val derivativeError = 20.0
         val positionPIDError = KineticState(10.0, derivativeError, 30.0)
         val velocityPIDError = KineticState(10.0, 30.0, derivativeError)
@@ -172,8 +172,8 @@ class PIDElementTest {
         val kI = 6.0
         val kD = 7.0
         val coefficients = PIDCoefficients(kP, kI, kD)
-        val positionPID = PIDElement(PIDType.POSITION, coefficients)
-        val velocityPID = PIDElement(PIDType.VELOCITY, coefficients)
+        val positionPID = PIDElement(FeedbackType.POSITION, coefficients)
+        val velocityPID = PIDElement(FeedbackType.VELOCITY, coefficients)
 
         mockkObject(TimeUtil)
 
@@ -208,8 +208,8 @@ class PIDElementTest {
     fun `acceleration error does not affect position pid`() {
         // Arrange
         val coefficients = PIDCoefficients(1.0, 2.0, 3.0)
-        val firstController = PIDElement(PIDType.POSITION, coefficients)
-        val secondController = PIDElement(PIDType.POSITION, coefficients)
+        val firstController = PIDElement(FeedbackType.POSITION, coefficients)
+        val secondController = PIDElement(FeedbackType.POSITION, coefficients)
         val firstError = KineticState(10.0, 20.0, 30.0)
         val secondError = KineticState(10.0, 20.0, 40.0)
         mockkObject(TimeUtil)
@@ -229,8 +229,8 @@ class PIDElementTest {
     fun `position error does not affect velocity pid`() {
         // Arrange
         val coefficients = PIDCoefficients(1.0, 2.0, 3.0)
-        val firstController = PIDElement(PIDType.VELOCITY, coefficients)
-        val secondController = PIDElement(PIDType.VELOCITY, coefficients)
+        val firstController = PIDElement(FeedbackType.VELOCITY, coefficients)
+        val secondController = PIDElement(FeedbackType.VELOCITY, coefficients)
         val firstError = KineticState(10.0, 20.0, 30.0)
         val secondError = KineticState(15.0, 20.0, 30.0)
         mockkObject(TimeUtil)