Drive util

control/src/main/kotlin/dev/nextftc/control/drive/DriveKinematics.kt

@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.control.drive
+
+/** Raw driver input for a 3-DOF planar drivetrain. */
+data class DriveInput(val x: Double, val y: Double, val rx: Double)
+
+/**
+ * Converts raw drive input into a drivetrain-specific output.
+ *
+ * Each drivetrain type (mecanum, tank, etc.) implements this with its own
+ * [Output] type
+ *
+ * @param Output The type produced by this drivetrain's kinematics — e.g.
+ *   `MecanumWheelPowers`, `TankWheelPowers`, or a list of swerve module states.
+ */
+interface DriveKinematics<Output> {
+  /** Converts [input] into this drivetrain's output type. */
+  fun calculate(input: DriveInput): Output
+}

control/src/main/kotlin/dev/nextftc/control/drive/MecanumKinematics.kt

@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.control.drive
+
+import kotlin.math.absoluteValue
+import kotlin.math.max
+
+data class MecanumWheelPowers(
+  val frontLeft: Double,
+  val frontRight: Double,
+  val backLeft: Double,
+  val backRight: Double,
+)
+
+/**
+ * Converts raw drive inputs into mecanum wheel powers.
+ *
+ * Applies a strafe compensation factor to account for mecanum wheels being
+ * physically less efficient strafing than driving forward/backward.
+ */
+class MecanumKinematics @JvmOverloads constructor(private val strafeCompensation: Double = 1.1) :
+  DriveKinematics<MecanumWheelPowers> {
+  override fun calculate(input: DriveInput): MecanumWheelPowers {
+    val compensatedY = input.y * strafeCompensation
+
+    val denominator = max(
+      compensatedY.absoluteValue + input.x.absoluteValue + input.rx.absoluteValue,
+      1.0,
+    )
+
+    return MecanumWheelPowers(
+      frontLeft = (input.x + compensatedY + input.rx) / denominator,
+      frontRight = (input.x - compensatedY - input.rx) / denominator,
+      backLeft = (input.x - compensatedY + input.rx) / denominator,
+      backRight = (input.x + compensatedY - input.rx) / denominator,
+    )
+  }
+}

control/src/main/kotlin/dev/nextftc/control/drive/TankKinematics.kt

@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.control.drive
+
+import kotlin.math.absoluteValue
+import kotlin.math.max
+
+data class TankWheelPowers(val left: Double, val right: Double)
+
+/**
+ * Converts raw drive inputs into tank/differential wheel powers.
+ *
+ * Tank/differential drivetrains are not holonomic, so they cannot strafe.
+ * Any [DriveInput.y] value is therefore ignored; only [DriveInput.x]
+ * (forward/backward) and [DriveInput.rx] (rotation) are used.
+ */
+
+class TankKinematics : DriveKinematics<TankWheelPowers> {
+  override fun calculate(input: DriveInput): TankWheelPowers {
+    val denominator = max(input.x.absoluteValue + input.rx.absoluteValue, 1.0)
+    return TankWheelPowers(
+      left = (input.x + input.rx) / denominator,
+      right = (input.x - input.rx) / denominator,
+    )
+  }
+}

robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt

@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.robot.drive
+
+import com.pedropathing.ivy.Command
+import com.pedropathing.ivy.commands.Commands
+import com.qualcomm.robotcore.hardware.Gamepad
+import dev.nextftc.control.drive.DriveInput
+import dev.nextftc.control.drive.MecanumKinematics
+import dev.nextftc.control.drive.TankKinematics
+import dev.nextftc.hardware.actuators.NextMotor
+import java.util.function.Supplier
+import kotlin.math.cos
+import kotlin.math.sin
+
+/** Multiplier applied to all drive input; re-read live each loop so it can be adjusted mid-match (e.g. slow-mode). */
+var scalar: Double = 1.0
+  set(value) {
+    field = value
+  }
+
+/** Command that drives a mecanum drivetrain from [gamepad]'s sticks. */
+fun mecanumDrive(
+  frontLeft: NextMotor,
+  frontRight: NextMotor,
+  backLeft: NextMotor,
+  backRight: NextMotor,
+  gamepad: Gamepad,
+  kinematics: MecanumKinematics = MecanumKinematics(),
+): Command = Commands.infinite {
+  val powers = kinematics.calculate(
+    DriveInput(
+      y = gamepad.left_stick_x.toDouble() * scalar,
+      x = -gamepad.left_stick_y.toDouble() * scalar,
+      rx = gamepad.right_stick_x.toDouble() * scalar,
+    ),
+  )
+  frontLeft.setThrottle(powers.frontLeft)
+  frontRight.setThrottle(powers.frontRight)
+  backLeft.setThrottle(powers.backLeft)
+  backRight.setThrottle(powers.backRight)
+}
+
+/**
+ * Command that drives a mecanum drivetrain from [gamepad]'s sticks, field-centric.
+ *
+ * Rotates stick input by [heading] (radians, field-relative) so "forward" on the
+ * stick always moves the robot away from the driver, regardless of robot orientation.
+ *
+ * @param heading Supplies the robot's current field-relative heading in radians —
+ *   e.g. `{ follower.pose.heading }` from Pedro Pathing, or an IMU reading zeroed
+ *   to match the robot's starting orientation on the field.
+ */
+fun mecanumDriveFieldCentric(
+  frontLeft: NextMotor,
+  frontRight: NextMotor,
+  backLeft: NextMotor,
+  backRight: NextMotor,
+  gamepad: Gamepad,
+  heading: Supplier<Double>,
+  kinematics: MecanumKinematics = MecanumKinematics(),
+): Command = Commands.infinite {
+  val rawY = gamepad.left_stick_x.toDouble()
+  val rawX = -gamepad.left_stick_y.toDouble()
+  val h = heading.get()
+  val c = cos(-h)
+  val sinH = sin(-h)
+
+  val powers = kinematics.calculate(
+    DriveInput(
+      y = (rawX * c - rawY * sinH) * scalar,
+      x = (rawX * sinH + rawY * c) * scalar,
+      rx = gamepad.right_stick_x.toDouble() * scalar,
+    ),
+  )
+  frontLeft.setThrottle(powers.frontLeft)
+  frontRight.setThrottle(powers.frontRight)
+  backLeft.setThrottle(powers.backLeft)
+  backRight.setThrottle(powers.backRight)
+}
+
+/** Command that drives a tank drivetrain from [gamepad]'s sticks. */
+fun tankDrive(
+  frontLeft: NextMotor,
+  frontRight: NextMotor,
+  backLeft: NextMotor,
+  backRight: NextMotor,
+  gamepad: Gamepad,
+  kinematics: TankKinematics = TankKinematics(),
+): Command = Commands.infinite {
+  val powers = kinematics.calculate(
+    DriveInput(
+      y = 0.0,
+      x = -gamepad.left_stick_y.toDouble() * scalar,
+      rx = gamepad.right_stick_x.toDouble() * scalar,
+    ),
+  )
+  frontLeft.setThrottle(powers.left)
+  backLeft.setThrottle(powers.left)
+  frontRight.setThrottle(powers.right)
+  backRight.setThrottle(powers.right)
+}

robot/src/main/kotlin/dev/nextftc/robot/triggers/CommandGamepad.kt

@@ -16,6 +16,7 @@ class CommandGamepad @JvmOverloads constructor(
   private val eventLoop: EventLoop = Trigger.defaultEventLoop,
   private val gamepad: Gamepad,
 ) {
+
   /** A range trigger that evaluates to how far the x value of the left stick is moved. */
   @get:JvmName("leftStickX")
   val leftStickX = RangeTrigger(eventLoop) { gamepad.left_stick_x.toDouble() }