Omnicoders

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

@@ -81,7 +81,7 @@ object TriWheels : API() {
         this.drive(radians, magnitude, rotation)
     }
 
-    private fun wheelRatio(
+    fun wheelRatio(
         radians: Double,
         magnitude: Double,
         rotation: Double = 0.0,

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

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.api.linear
+
+import org.firstinspires.ftc.teamcode.api.API
+import org.firstinspires.ftc.teamcode.api.TriWheels
+import org.firstinspires.ftc.teamcode.utils.MotorController
+import org.firstinspires.ftc.teamcode.utils.RobotConfig
+
+/** An API similar to [Encoders] but it allows traveling in any direction. */
+object Omnicoders : API() {
+    override val isLinear = true
+
+    override fun dependencies() = setOf(TriWheels)
+
+    fun driveTo(
+        radians: Double,
+        inches: Double,
+    ) {
+        TriWheels.stopAndResetMotors()
+
+        // Convert from inches to ticks.
+        val magnitude = inches * RobotConfig.Omnicoders.TICKS_PER_INCH
+
+        // Calculate the distance each wheel needs to travel.
+        val (redTarget, greenTarget, blueTarget) = TriWheels.wheelRatio(radians, magnitude)
+
+        // Create the motor controllers for each wheel.
+        val (redController, greenController, blueController) =
+            Triple(
+                MotorController(redTarget.toInt(), RobotConfig.Omnicoders.PID, TriWheels.red),
+                MotorController(greenTarget.toInt(), RobotConfig.Omnicoders.PID, TriWheels.green),
+                MotorController(blueTarget.toInt(), RobotConfig.Omnicoders.PID, TriWheels.blue),
+            )
+
+        try {
+            // Repeat while `opModeIsActive()` and at least one controller is running.
+            while (!(redController.isDone() && greenController.isDone() && blueController.isDone()) && linearOpMode.opModeIsActive()) {
+                redController.update()
+                greenController.update()
+                blueController.update()
+
+                with(linearOpMode.telemetry) {
+                    addData("Status", "Omnicoder Driving")
+
+                    // TODO: Log position, target, and power of each motor.
+
+                    update()
+                }
+            }
+        } finally {
+            TriWheels.stopAndResetMotors()
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Test.kt

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous
+
+import com.acmerobotics.dashboard.config.Config
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.api.Telemetry
+import org.firstinspires.ftc.teamcode.api.TriWheels
+import org.firstinspires.ftc.teamcode.api.linear.Omnicoders
+import kotlin.math.PI
+
+@Autonomous(name = "Test Omnicoders")
+class Test : LinearOpMode() {
+    // Adjust these fields when testing.
+    @Config
+    object Test {
+        @JvmField
+        var RADIANS = PI / 2.0
+
+        @JvmField
+        var INCHES = 12.0
+    }
+
+    override fun runOpMode() {
+        Telemetry.init(this)
+        TriWheels.init(this)
+        Omnicoders.init(this)
+
+        Telemetry.sayInitialized()
+
+        waitForStart()
+
+        Telemetry.sayStarted()
+
+        Omnicoders.driveTo(Test.RADIANS, Test.INCHES)
+    }
+}

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.debug
 import org.opencv.core.Rect
 import org.opencv.core.Scalar
@@ -203,4 +204,15 @@ object RobotConfig {
         @JvmField
         var I_LIMIT = 0.1
     }
+
+    @Config
+    object Omnicoders {
+        // TODO: Calculate this number.
+        @JvmField
+        var TICKS_PER_INCH = 0
+
+        // Taken from Encoders.driveTo2, may be adjusted in the future.
+        @JvmField
+        var PID = PIDCoefficients(0.0004, 0.0000000125, 0.0)
+    }
 }