diff --git a/control/src/main/kotlin/dev/nextftc/control/drive/DriveKinematics.kt b/control/src/main/kotlin/dev/nextftc/control/drive/DriveKinematics.kt new file mode 100644 index 0000000..1fe06fc --- /dev/null +++ b/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 { + /** Converts [input] into this drivetrain's output type. */ + fun calculate(input: DriveInput): Output +} diff --git a/control/src/main/kotlin/dev/nextftc/control/drive/MecanumKinematics.kt b/control/src/main/kotlin/dev/nextftc/control/drive/MecanumKinematics.kt new file mode 100644 index 0000000..0d7fce9 --- /dev/null +++ b/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 { + 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, + ) + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/drive/TankKinematics.kt b/control/src/main/kotlin/dev/nextftc/control/drive/TankKinematics.kt new file mode 100644 index 0000000..2c88a5c --- /dev/null +++ b/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 { + 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, + ) + } +} diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt new file mode 100644 index 0000000..70e2b97 --- /dev/null +++ b/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, + 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) +} diff --git a/robot/src/main/kotlin/dev/nextftc/robot/triggers/CommandGamepad.kt b/robot/src/main/kotlin/dev/nextftc/robot/triggers/CommandGamepad.kt index 3e6a289..4fa91ef 100644 --- a/robot/src/main/kotlin/dev/nextftc/robot/triggers/CommandGamepad.kt +++ b/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() }