PID (#56)

* feat: pid motor controllers

* feat: ftc dashboard config motor controller

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/linear/Encoders.kt

@@ -7,6 +7,7 @@ import org.firstinspires.ftc.teamcode.api.API
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import org.firstinspires.ftc.teamcode.api.linear.Encoders.driveTo
 import org.firstinspires.ftc.teamcode.api.linear.Encoders.spinTo
+import org.firstinspires.ftc.teamcode.utils.MotorControllerGroup
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
 import kotlin.math.abs
 import kotlin.math.min
@@ -172,6 +173,81 @@ object Encoders : API() {
         }
     }
 
+    fun driveTo2(
+        direction: Direction,
+        inches: Double,
+    ) {
+        TriWheels.stopAndResetMotors()
+
+        val (left, right, _) = this.defineWheels(direction)
+        val ticks = this.inchesToTick(inches)
+
+        val controllers = MotorControllerGroup(ticks, left, right)
+
+        try {
+            right.direction = DcMotorSimple.Direction.REVERSE
+
+            while (!controllers.isDone() && linearOpMode.opModeIsActive()) {
+                controllers.update()
+
+                with(linearOpMode.telemetry) {
+                    addData("Status", "Encoder Driving")
+
+                    addData("Left Power", left.power)
+                    addData("Right Power", right.power)
+
+                    addData("Target", ticks)
+
+                    addData("Left Current", left.currentPosition)
+                    addData("Right Current", right.currentPosition)
+
+                    update()
+                }
+            }
+        } finally {
+            // This reset encoders but also changes the right motor direction back to forward
+            TriWheels.stopAndResetMotors()
+        }
+    }
+
+    fun spinTo2(degrees: Double) {
+        TriWheels.stopAndResetMotors()
+
+        val ticks = -this.degreesToTick(degrees)
+
+        val controllers = MotorControllerGroup(ticks, TriWheels.red, TriWheels.green, TriWheels.blue)
+
+        try {
+            while (
+                !controllers.isDone() && linearOpMode.opModeIsActive()
+            ) {
+                controllers.update()
+
+                with(linearOpMode.telemetry) {
+                    addData("Status", "Encoder Rotating")
+
+                    addData(
+                        "Power",
+                        Triple(TriWheels.red.power, TriWheels.green.power, TriWheels.blue.power),
+                    )
+                    addData(
+                        "Current",
+                        Triple(
+                            TriWheels.red.currentPosition,
+                            TriWheels.green.currentPosition,
+                            TriWheels.blue.currentPosition,
+                        ),
+                    )
+                    addData("Target", ticks)
+
+                    update()
+                }
+            }
+        } finally {
+            TriWheels.stopAndResetMotors()
+        }
+    }
+
     override fun dependencies() = setOf(TriWheels)
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MotorController.kt

@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.utils
+
+import com.qualcomm.robotcore.hardware.DcMotor
+import com.qualcomm.robotcore.util.ElapsedTime
+import kotlin.math.abs
+import kotlin.math.max
+import kotlin.math.min
+
+class MotorControllerGroup(
+    target: Int,
+    vararg motors: DcMotor,
+) {
+    private val controllers = motors.map { MotorController(target, it) }
+
+    fun update() {
+        for (controller in controllers) {
+            controller.update()
+        }
+    }
+
+    fun isDone() = controllers.all { it.isDone() }
+}
+
+class MotorController(
+    private val target: Int,
+    private val motor: DcMotor,
+) {
+    private val runtime = ElapsedTime()
+
+    private var previousTime = 0.0
+    private var previousError = Int.MAX_VALUE
+
+    private var i = 0.0
+
+    fun update() {
+        val currentTime = runtime.milliseconds()
+        val currentError = target - motor.currentPosition
+
+        val p = RobotConfig.MotorController.pid.p * currentError
+
+        i += RobotConfig.MotorController.pid.i * currentError * (currentTime - previousTime)
+
+        // Clamp between [-MAX_I, MAX_I]
+        i = max(min(i, RobotConfig.MotorController.iLimit), -RobotConfig.MotorController.iLimit)
+
+        val d = RobotConfig.MotorController.pid.d * (currentError - previousError) / (currentTime - previousTime)
+
+        motor.power = max(min(p + i + d, RobotConfig.MotorController.powerLimit), -RobotConfig.MotorController.powerLimit)
+
+        previousTime = currentTime
+        previousError = currentError
+    }
+
+    fun isDone() = abs(previousError) < 5
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/RobotConfig.kt

@@ -3,6 +3,7 @@
 package org.firstinspires.ftc.teamcode.utils
 
 import com.acmerobotics.dashboard.config.Config
+import com.qualcomm.robotcore.hardware.PIDCoefficients
 import org.firstinspires.ftc.teamcode.utils.RobotConfig.model
 import org.opencv.core.Rect
 import org.opencv.core.Scalar
@@ -227,6 +228,18 @@ object RobotConfig {
         var DOWN_POSITION = 0.3
     }
 
+    @Config
+    object MotorController {
+        @JvmField
+        var pid = PIDCoefficients(0.01, 0.0, 0.2)
+
+        @JvmField
+        var powerLimit = 0.5
+
+        @JvmField
+        var iLimit = 1.0
+    }
+
     /**
      * Represents what model of robot is running the code.
      *