Skip to content

Commit d85a08e

Browse files
28shettr28shettr
authored
Drive util (#18)
Mecanum and Tank and technically x drive
1 parent 2f7095b commit d85a08e

5 files changed

Lines changed: 211 additions & 0 deletions

File tree

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

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
/*
2+
* Copyright (c) 2026 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
package dev.nextftc.control.drive
10+
11+
/** Raw driver input for a 3-DOF planar drivetrain. */
12+
data class DriveInput(val x: Double, val y: Double, val rx: Double)
13+
14+
/**
15+
* Converts raw drive input into a drivetrain-specific output.
16+
*
17+
* Each drivetrain type (mecanum, tank, etc.) implements this with its own
18+
* [Output] type
19+
*
20+
* @param Output The type produced by this drivetrain's kinematics — e.g.
21+
* `MecanumWheelPowers`, `TankWheelPowers`, or a list of swerve module states.
22+
*/
23+
interface DriveKinematics<Output> {
24+
/** Converts [input] into this drivetrain's output type. */
25+
fun calculate(input: DriveInput): Output
26+
}

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

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
/*
2+
* Copyright (c) 2026 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
package dev.nextftc.control.drive
10+
11+
import kotlin.math.absoluteValue
12+
import kotlin.math.max
13+
14+
data class MecanumWheelPowers(
15+
val frontLeft: Double,
16+
val frontRight: Double,
17+
val backLeft: Double,
18+
val backRight: Double,
19+
)
20+
21+
/**
22+
* Converts raw drive inputs into mecanum wheel powers.
23+
*
24+
* Applies a strafe compensation factor to account for mecanum wheels being
25+
* physically less efficient strafing than driving forward/backward.
26+
*/
27+
class MecanumKinematics @JvmOverloads constructor(private val strafeCompensation: Double = 1.1) :
28+
DriveKinematics<MecanumWheelPowers> {
29+
override fun calculate(input: DriveInput): MecanumWheelPowers {
30+
val compensatedY = input.y * strafeCompensation
31+
32+
val denominator = max(
33+
compensatedY.absoluteValue + input.x.absoluteValue + input.rx.absoluteValue,
34+
1.0,
35+
)
36+
37+
return MecanumWheelPowers(
38+
frontLeft = (input.x + compensatedY + input.rx) / denominator,
39+
frontRight = (input.x - compensatedY - input.rx) / denominator,
40+
backLeft = (input.x - compensatedY + input.rx) / denominator,
41+
backRight = (input.x + compensatedY - input.rx) / denominator,
42+
)
43+
}
44+
}

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

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
/*
2+
* Copyright (c) 2026 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
package dev.nextftc.control.drive
10+
11+
import kotlin.math.absoluteValue
12+
import kotlin.math.max
13+
14+
data class TankWheelPowers(val left: Double, val right: Double)
15+
16+
/**
17+
* Converts raw drive inputs into tank/differential wheel powers.
18+
*
19+
* Tank/differential drivetrains are not holonomic, so they cannot strafe.
20+
* Any [DriveInput.y] value is therefore ignored; only [DriveInput.x]
21+
* (forward/backward) and [DriveInput.rx] (rotation) are used.
22+
*/
23+
24+
class TankKinematics : DriveKinematics<TankWheelPowers> {
25+
override fun calculate(input: DriveInput): TankWheelPowers {
26+
val denominator = max(input.x.absoluteValue + input.rx.absoluteValue, 1.0)
27+
return TankWheelPowers(
28+
left = (input.x + input.rx) / denominator,
29+
right = (input.x - input.rx) / denominator,
30+
)
31+
}
32+
}

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

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
/*
2+
* Copyright (c) 2026 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
package dev.nextftc.robot.drive
10+
11+
import com.pedropathing.ivy.Command
12+
import com.pedropathing.ivy.commands.Commands
13+
import com.qualcomm.robotcore.hardware.Gamepad
14+
import dev.nextftc.control.drive.DriveInput
15+
import dev.nextftc.control.drive.MecanumKinematics
16+
import dev.nextftc.control.drive.TankKinematics
17+
import dev.nextftc.hardware.actuators.NextMotor
18+
import java.util.function.Supplier
19+
import kotlin.math.cos
20+
import kotlin.math.sin
21+
22+
/** Multiplier applied to all drive input; re-read live each loop so it can be adjusted mid-match (e.g. slow-mode). */
23+
var scalar: Double = 1.0
24+
set(value) {
25+
field = value
26+
}
27+
28+
/** Command that drives a mecanum drivetrain from [gamepad]'s sticks. */
29+
fun mecanumDrive(
30+
frontLeft: NextMotor,
31+
frontRight: NextMotor,
32+
backLeft: NextMotor,
33+
backRight: NextMotor,
34+
gamepad: Gamepad,
35+
kinematics: MecanumKinematics = MecanumKinematics(),
36+
): Command = Commands.infinite {
37+
val powers = kinematics.calculate(
38+
DriveInput(
39+
y = gamepad.left_stick_x.toDouble() * scalar,
40+
x = -gamepad.left_stick_y.toDouble() * scalar,
41+
rx = gamepad.right_stick_x.toDouble() * scalar,
42+
),
43+
)
44+
frontLeft.setThrottle(powers.frontLeft)
45+
frontRight.setThrottle(powers.frontRight)
46+
backLeft.setThrottle(powers.backLeft)
47+
backRight.setThrottle(powers.backRight)
48+
}
49+
50+
/**
51+
* Command that drives a mecanum drivetrain from [gamepad]'s sticks, field-centric.
52+
*
53+
* Rotates stick input by [heading] (radians, field-relative) so "forward" on the
54+
* stick always moves the robot away from the driver, regardless of robot orientation.
55+
*
56+
* @param heading Supplies the robot's current field-relative heading in radians —
57+
* e.g. `{ follower.pose.heading }` from Pedro Pathing, or an IMU reading zeroed
58+
* to match the robot's starting orientation on the field.
59+
*/
60+
fun mecanumDriveFieldCentric(
61+
frontLeft: NextMotor,
62+
frontRight: NextMotor,
63+
backLeft: NextMotor,
64+
backRight: NextMotor,
65+
gamepad: Gamepad,
66+
heading: Supplier<Double>,
67+
kinematics: MecanumKinematics = MecanumKinematics(),
68+
): Command = Commands.infinite {
69+
val rawY = gamepad.left_stick_x.toDouble()
70+
val rawX = -gamepad.left_stick_y.toDouble()
71+
val h = heading.get()
72+
val c = cos(-h)
73+
val sinH = sin(-h)
74+
75+
val powers = kinematics.calculate(
76+
DriveInput(
77+
y = (rawX * c - rawY * sinH) * scalar,
78+
x = (rawX * sinH + rawY * c) * scalar,
79+
rx = gamepad.right_stick_x.toDouble() * scalar,
80+
),
81+
)
82+
frontLeft.setThrottle(powers.frontLeft)
83+
frontRight.setThrottle(powers.frontRight)
84+
backLeft.setThrottle(powers.backLeft)
85+
backRight.setThrottle(powers.backRight)
86+
}
87+
88+
/** Command that drives a tank drivetrain from [gamepad]'s sticks. */
89+
fun tankDrive(
90+
frontLeft: NextMotor,
91+
frontRight: NextMotor,
92+
backLeft: NextMotor,
93+
backRight: NextMotor,
94+
gamepad: Gamepad,
95+
kinematics: TankKinematics = TankKinematics(),
96+
): Command = Commands.infinite {
97+
val powers = kinematics.calculate(
98+
DriveInput(
99+
y = 0.0,
100+
x = -gamepad.left_stick_y.toDouble() * scalar,
101+
rx = gamepad.right_stick_x.toDouble() * scalar,
102+
),
103+
)
104+
frontLeft.setThrottle(powers.left)
105+
backLeft.setThrottle(powers.left)
106+
frontRight.setThrottle(powers.right)
107+
backRight.setThrottle(powers.right)
108+
}

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ class CommandGamepad @JvmOverloads constructor(
1616
private val eventLoop: EventLoop = Trigger.defaultEventLoop,
1717
private val gamepad: Gamepad,
1818
) {
19+
1920
/** A range trigger that evaluates to how far the x value of the left stick is moved. */
2021
@get:JvmName("leftStickX")
2122
val leftStickX = RangeTrigger(eventLoop) { gamepad.left_stick_x.toDouble() }

0 commit comments

Comments
 (0)