Sleep deprivation got to me and I wrote the bang bang controller wrong, fixed now (todo: add tests)

Author: rowan-mcalpin

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

@@ -21,20 +21,40 @@ package dev.nextftc.nextcontrol.feedback
 import dev.nextftc.nextcontrol.KineticState
 import kotlin.math.sign
 
+/**
+ * Parameters for a [BangBangElement]
+ *
+ * @param gain the constant that is outputted when the error is positive (negated when error is negative)
+ * @param hysteresis how close to 0 is considered 0
+ */
+data class BangBangParameters @JvmOverloads constructor(
+    @JvmField var gain: Double,
+    @JvmField var hysteresis: Double = 5.0
+)
+
 /**
  * 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
+ * @param gain the constant that the output is multiplied by
  *
  * @author rowan-mcalpin
  */
 class BangBangElement(
     private val feedbackType: FeedbackType,
+    private val parameters: BangBangParameters
 ) : FeedbackElement {
 
-    override fun calculate(error: KineticState): Double = when (feedbackType) {
-        FeedbackType.POSITION -> sign(error.position)
-        FeedbackType.VELOCITY -> sign(error.velocity)
+    override fun calculate(error: KineticState): Double {
+        val feedbackError = when (feedbackType) {
+            FeedbackType.POSITION -> error.position
+            FeedbackType.VELOCITY -> error.velocity
+        }
+        return when {
+            feedbackError >= parameters.hysteresis -> parameters.gain
+            feedbackError <= -parameters.hysteresis -> -parameters.gain
+            else -> 0.0
+        }
     }
 }