From 7fb6becb01626061ac13eb18f20a45a99913648f Mon Sep 17 00:00:00 2001 From: 28shettr <28shettr@elmbrookstudents.org> Date: Fri, 24 Jul 2026 19:46:41 -0500 Subject: [PATCH 01/11] finished mec kinematic --- .../hardware/sensors/NextDistanceSensor.kt | 2 +- .../dev/nextftc/robot/drive/DriveCommand.kt | 7 +++++ .../nextftc/robot/drive/DriveKinematics.kt | 31 +++++++++++++++++++ 3 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt create mode 100644 robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt index debdcf6..2547400 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt @@ -15,7 +15,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit /** * Lightweight wrapper for a distance sensor that caches the last reading. * Call [update] in periodic to read the hardware. - * + * wait are you able to make new folders and class or nah? * Use [isWithinDistance] to check * if an object is close enough. * diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt new file mode 100644 index 0000000..ba0558b --- /dev/null +++ b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt @@ -0,0 +1,7 @@ +package dev.nextftc.robot.drive; + +interface DriveCommand { + fun drive(forward: Double, strafe: Double, turn: Double) +} +im gonna make a drive train kinematics file and then you can just use that fior comamdns +yes lmao, also make driveUtil to just "drive" k fixed \ No newline at end of file diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt new file mode 100644 index 0000000..78ea735 --- /dev/null +++ b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt @@ -0,0 +1,31 @@ +package dev.nextftc.robot.drive + +import kotlin.math.absoluteValue +import kotlin.math.max + +interface DriveKinematics { + fun calculate(inputPowers: DoubleArray): DoubleArray +} + +object Tankinematics : DriveKinematics{ + // Assume structure of inputPowers is [forwardPower, turnPower] + override fun calculate(inputPowers: DoubleArray) { + val (forward, turn) = inputPowers + val left = forward + turn + val right = forward - turn + + } +} + +object MecanumKinematics : DriveKinematics{ + override fun calculate(inputPowers: DoubleArray): DoubleArray { + val (forward, strafe, turn) = inputPowers + val denominator = max(forward.absoluteValue + strafe.absoluteValue + turn.absoluteValue, 1.0) + val frontLeft = (forward + strafe + turn) / denominator + val frontRight = (forward - strafe - turn) / denominator + val backLeft = (forward - strafe + turn) / denominator + val backRight = (forward + strafe - turn) / denominator + + return doubleArrayOf(frontLeft, frontRight, backLeft, backRight) + } +} \ No newline at end of file From b943050e8ee77feb1d05136fcf690e919b4baaba Mon Sep 17 00:00:00 2001 From: 28shettr <28shettr@elmbrookstudents.org> Date: Mon, 27 Jul 2026 14:27:21 -0500 Subject: [PATCH 02/11] Added drive util for mecanum and tank, swerve is being worked on --- .../nextftc/control/drive/DriveKinematics.kt | 24 ++++ .../control/drive/MecanumKinematics.kt | 39 ++++++ .../nextftc/control/drive/SwerveKinematics.kt | 119 ++++++++++++++++++ .../nextftc/control/drive/TankKinematics.kt | 28 +++++ .../dev/nextftc/robot/drive/DriveCommand.kt | 7 -- .../nextftc/robot/drive/DriveKinematics.kt | 31 ----- .../nextftc/robot/drive/DrivetrainGroup.kt | 95 ++++++++++++++ 7 files changed, 305 insertions(+), 38 deletions(-) create mode 100644 control/src/main/kotlin/dev/nextftc/control/drive/DriveKinematics.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/drive/MecanumKinematics.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/drive/TankKinematics.kt delete mode 100644 robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt delete mode 100644 robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt create mode 100644 robot/src/main/kotlin/dev/nextftc/robot/drive/DrivetrainGroup.kt 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..a6459c6 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/drive/DriveKinematics.kt @@ -0,0 +1,24 @@ +/* + * 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 + +data class DriveInput(val x: Double, val y: Double, val rx: Double) + +/** Computed power for each wheel of a 4-wheel drivetrain. */ +data class WheelPowers( + val frontLeft: Double, + val frontRight: Double, + val backLeft: Double, + val backRight: Double, +) + +/** Converts raw drive input into wheel powers for a specific drivetrain type. */ +interface DriveKinematics { + fun calculate(input: DriveInput): WheelPowers +} 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..2746a43 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/drive/MecanumKinematics.kt @@ -0,0 +1,39 @@ +/* + * 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 + +/** + * 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 : DriveKinematics { + + private val strafeCompensation = 1.1 + + override fun calculate(input: DriveInput): WheelPowers { + val compensatedX = input.x * strafeCompensation + + val denominator = max( + compensatedX.absoluteValue + input.y.absoluteValue + input.rx.absoluteValue, + 1.0 + ) + + return WheelPowers( + frontLeft = (input.y + compensatedX + input.rx) / denominator, + frontRight = (input.y - compensatedX - input.rx) / denominator, + backLeft = (input.y - compensatedX + input.rx) / denominator, + backRight = (input.y + compensatedX - input.rx) / denominator, + ) + } +} \ No newline at end of file diff --git a/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt b/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt new file mode 100644 index 0000000..4a01371 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt @@ -0,0 +1,119 @@ +/* + * Copyright (c) FIRST and other WPILib contributors. + * Open Source Software; you can modify and/or share it under the terms of + * the WPILib BSD license file in the root directory of this project. + * + * Copyright (c) 2026 NextFTC Team + * Portions of this file are original code or adaptations by the 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 dev.nextftc.control.geometry.ChassisVelocities +import dev.nextftc.control.geometry.Rotation2d +import dev.nextftc.control.geometry.Vector2d +import dev.nextftc.units.Meters +import dev.nextftc.linalg.DynamicMatrix +import dev.nextftc.linalg.DynamicVector +import dev.nextftc.units.MetersPerSecond +import dev.nextftc.units.RadiansPerSecond +import dev.nextftc.units.unittypes.DistanceUnit +import kotlin.math.hypot + +/** A single swerve module's commanded velocity and angle. */ +data class SwerveModuleVelocity(val velocity: Double, val angle: Rotation2d) + +/** + * Converts a desired chassis velocity into individual swerve module velocities and angles. + * + * @param moduleLocations The locations of each module relative to the physical center of the + * robot. Order determines the order of returned module velocities. + */ +class SwerveKinematics(vararg moduleLocations: Vector2d) { + + private val numModules = moduleLocations.size + private val moduleXs = DoubleArray(numModules) { moduleLocations[it].x.into(Meters) } + private val moduleYs = DoubleArray(numModules) { moduleLocations[it].y.into(Meters) } + + private var moduleHeadings = Array(numModules) { Rotation2d.zero } + + private var prevCorX = 0.0 + private var prevCorY = 0.0 + private var inverseKinematics = buildInverseKinematics(0.0, 0.0) + + private fun buildInverseKinematics(corX: Double, corY: Double): DynamicMatrix { + val m = DynamicMatrix.zero(rows = numModules * 2, cols = 3) + for (i in 0 until numModules) { + val rx = moduleXs[i] - corX + val ry = moduleYs[i] - corY + m[i * 2, 0] = 1.0; m[i * 2, 1] = 0.0; m[i * 2, 2] = -ry + m[i * 2 + 1, 0] = 0.0; m[i * 2 + 1, 1] = 1.0; m[i * 2 + 1, 2] = rx + } + return m + } + + /** + * Performs inverse kinematics to return module velocities/angles from a desired chassis + * velocity, optionally about a custom center of rotation (e.g. for evasive maneuvers). + */ + @JvmOverloads + fun toSwerveModuleVelocities( + chassisVelocities: ChassisVelocities, + centerOfRotationX: Double = 0.0, + centerOfRotationY: Double = 0.0, + ): Array { + val vx = chassisVelocities.linearVel.x.into(unit = MetersPerSecond) + val vy = chassisVelocities.linearVel.y.into(unit = MetersPerSecond) + val omega = chassisVelocities.angVel.into(unit = RadiansPerSecond) + + if (vx == 0.0 && vy == 0.0 && omega == 0.0) { + return Array(numModules) { SwerveModuleVelocity(0.0, moduleHeadings[it]) } + } + + if (centerOfRotationX != prevCorX || centerOfRotationY != prevCorY) { + inverseKinematics = buildInverseKinematics(centerOfRotationX, centerOfRotationY) + prevCorX = centerOfRotationX + prevCorY = centerOfRotationY + } + + val chassisVector = DynamicVector.of(vx, vy, omega) + val moduleVelocitiesMatrix = inverseKinematics * chassisVector + + return Array(numModules) { i -> + val x = moduleVelocitiesMatrix[i * 2, 0] + val y = moduleVelocitiesMatrix[i * 2 + 1, 0] + val velocity = hypot(x, y) + val angle = if (velocity > 1e-6) Rotation2d(real = x, imag = y) else moduleHeadings[i] + moduleHeadings[i] = angle + SwerveModuleVelocity(velocity, angle) + } + } + + companion object { + /** + * Renormalizes module velocities if any exceed [attainableMaxVelocity], preserving the + * ratio of velocities between modules (and therefore the direction of net motion). + */ + @JvmStatic + fun desaturateWheelVelocities( + moduleVelocities: Array, + attainableMaxVelocity: Double, + ): Array { + val realMaxVelocity = moduleVelocities.maxOf { kotlin.math.abs(it.velocity) } + + return if (realMaxVelocity > attainableMaxVelocity) { + Array(moduleVelocities.size) { i -> + SwerveModuleVelocity( + moduleVelocities[i].velocity / realMaxVelocity * attainableMaxVelocity, + moduleVelocities[i].angle, + ) + } + } else { + moduleVelocities.copyOf() + } + } + } +} \ No newline at end of file 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..20b7d8d --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/drive/TankKinematics.kt @@ -0,0 +1,28 @@ +/* + * 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 + +/** + * Converts raw drive inputs into tank/differential wheel powers. + */ +class TankKinematics: DriveKinematics { + + + override fun calculate(input: DriveInput): WheelPowers { + val denominator = max(input.y.absoluteValue + input.rx.absoluteValue, 1.0) + + val left = (input.y + input.rx) / denominator + val right = (input.y - input.rx) / denominator + + return WheelPowers(frontLeft = left, frontRight = right, backLeft = left, backRight = right) + } +} \ No newline at end of file diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt deleted file mode 100644 index ba0558b..0000000 --- a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommand.kt +++ /dev/null @@ -1,7 +0,0 @@ -package dev.nextftc.robot.drive; - -interface DriveCommand { - fun drive(forward: Double, strafe: Double, turn: Double) -} -im gonna make a drive train kinematics file and then you can just use that fior comamdns -yes lmao, also make driveUtil to just "drive" k fixed \ No newline at end of file diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt deleted file mode 100644 index 78ea735..0000000 --- a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveKinematics.kt +++ /dev/null @@ -1,31 +0,0 @@ -package dev.nextftc.robot.drive - -import kotlin.math.absoluteValue -import kotlin.math.max - -interface DriveKinematics { - fun calculate(inputPowers: DoubleArray): DoubleArray -} - -object Tankinematics : DriveKinematics{ - // Assume structure of inputPowers is [forwardPower, turnPower] - override fun calculate(inputPowers: DoubleArray) { - val (forward, turn) = inputPowers - val left = forward + turn - val right = forward - turn - - } -} - -object MecanumKinematics : DriveKinematics{ - override fun calculate(inputPowers: DoubleArray): DoubleArray { - val (forward, strafe, turn) = inputPowers - val denominator = max(forward.absoluteValue + strafe.absoluteValue + turn.absoluteValue, 1.0) - val frontLeft = (forward + strafe + turn) / denominator - val frontRight = (forward - strafe - turn) / denominator - val backLeft = (forward - strafe + turn) / denominator - val backRight = (forward + strafe - turn) / denominator - - return doubleArrayOf(frontLeft, frontRight, backLeft, backRight) - } -} \ No newline at end of file diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DrivetrainGroup.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DrivetrainGroup.kt new file mode 100644 index 0000000..0813b8c --- /dev/null +++ b/robot/src/main/kotlin/dev/nextftc/robot/drive/DrivetrainGroup.kt @@ -0,0 +1,95 @@ +/* + * 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.qualcomm.robotcore.hardware.Gamepad +import com.pedropathing.ivy.Command +import com.pedropathing.ivy.commands.Commands +import dev.nextftc.control.drive.DriveInput +import dev.nextftc.control.drive.DriveKinematics +import dev.nextftc.hardware.actuators.NextMotor +import java.util.function.Supplier +import kotlin.math.absoluteValue +import kotlin.math.cos +import kotlin.math.sin + +/** + * Creates a 4-wheel drivetrain's group using the motors with a [DriveKinematics] implementation and + * exposes a ready-to-schedule driver-controlled [Command] via [driverControlled]. + * + * Works for any drivetrain type whose kinematics produces [WheelPowers] (mecanum, tank, + * X-drive) — edit [kinematics] to fit your drive train + * + * @param frontLeft The front-left drive motor. + * @param frontRight The front-right drive motor. + * @param backLeft The back-left drive motor. + * @param backRight The back-right drive motor. + * @param kinematics The kinematics implementation used to convert stick input into wheel + * powers (e.g. [MecanumKinematics], [TankKinematics]). + * @param headingSupplier Supplies the robot's current field-relative heading in radians, + * used to rotate stick input for field-centric driving. Pass `null` or nothing at all for + * robot-centric driving. + */ +class DrivetrainGroup( + private val frontLeft: NextMotor, + private val frontRight: NextMotor, + private val backLeft: NextMotor, + private val backRight: NextMotor, + var kinematics: DriveKinematics, +) { + private var headingSupplier: Supplier? = null + + constructor( + frontLeft: NextMotor, + frontRight: NextMotor, + backLeft: NextMotor, + backRight: NextMotor, + kinematics: DriveKinematics, + headingSupplier: Supplier, + ) : this(frontLeft, frontRight, backLeft, backRight, kinematics) { + this.headingSupplier = headingSupplier + } + + /** Multiplier applied to all drive input before kinematics. 1.0 = full speed. */ + var scalar: Double = 1.0 + set (value){ + field = value.coerceIn(0.0, 1.0) + } + + /** Stick values with magnitude below this are treated as zero. Clamped to [0.0, 1.0). */ + var deadZone: Double = 0.05 + set(value) { + field = value.coerceIn(0.0, 0.999) + } + + private fun checkDeadzone(input: Double) : Double = + if (input.absoluteValue < deadZone) 0.0 else input + + /** Returns a command that drives this group from the given gamepad's sticks, indefinitely. */ + fun driverControlled(gamepad: Gamepad): Command = Commands.infinite { + var x = checkDeadzone(gamepad.left_stick_x.toDouble()) + var y = checkDeadzone(-gamepad.left_stick_y.toDouble()) + val rx = checkDeadzone(gamepad.right_stick_x.toDouble()) + + headingSupplier?.let { supplier -> + val heading = supplier.get() + val rotatedX = x * cos(-heading) - y * sin(-heading) + val rotatedY = x * sin(-heading) + y * cos(-heading) + x = rotatedX + y = rotatedY + } + + val powers = kinematics.calculate(DriveInput(x * scalar, y * scalar, rx * scalar)) + frontLeft.setThrottle(powers.frontLeft) + frontRight.setThrottle(powers.frontRight) + backLeft.setThrottle(powers.backLeft) + backRight.setThrottle(powers.backRight) + } +} \ No newline at end of file From 29b5867e0179f2e9fb26515b65ae1861e2dd750c Mon Sep 17 00:00:00 2001 From: 28shettr <28shettr@elmbrookstudents.org> Date: Mon, 27 Jul 2026 18:28:14 -0500 Subject: [PATCH 03/11] no mo swerve --- .../nextftc/control/drive/SwerveKinematics.kt | 119 ------------------ 1 file changed, 119 deletions(-) delete mode 100644 control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt diff --git a/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt b/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt deleted file mode 100644 index 4a01371..0000000 --- a/control/src/main/kotlin/dev/nextftc/control/drive/SwerveKinematics.kt +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Copyright (c) FIRST and other WPILib contributors. - * Open Source Software; you can modify and/or share it under the terms of - * the WPILib BSD license file in the root directory of this project. - * - * Copyright (c) 2026 NextFTC Team - * Portions of this file are original code or adaptations by the 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 dev.nextftc.control.geometry.ChassisVelocities -import dev.nextftc.control.geometry.Rotation2d -import dev.nextftc.control.geometry.Vector2d -import dev.nextftc.units.Meters -import dev.nextftc.linalg.DynamicMatrix -import dev.nextftc.linalg.DynamicVector -import dev.nextftc.units.MetersPerSecond -import dev.nextftc.units.RadiansPerSecond -import dev.nextftc.units.unittypes.DistanceUnit -import kotlin.math.hypot - -/** A single swerve module's commanded velocity and angle. */ -data class SwerveModuleVelocity(val velocity: Double, val angle: Rotation2d) - -/** - * Converts a desired chassis velocity into individual swerve module velocities and angles. - * - * @param moduleLocations The locations of each module relative to the physical center of the - * robot. Order determines the order of returned module velocities. - */ -class SwerveKinematics(vararg moduleLocations: Vector2d) { - - private val numModules = moduleLocations.size - private val moduleXs = DoubleArray(numModules) { moduleLocations[it].x.into(Meters) } - private val moduleYs = DoubleArray(numModules) { moduleLocations[it].y.into(Meters) } - - private var moduleHeadings = Array(numModules) { Rotation2d.zero } - - private var prevCorX = 0.0 - private var prevCorY = 0.0 - private var inverseKinematics = buildInverseKinematics(0.0, 0.0) - - private fun buildInverseKinematics(corX: Double, corY: Double): DynamicMatrix { - val m = DynamicMatrix.zero(rows = numModules * 2, cols = 3) - for (i in 0 until numModules) { - val rx = moduleXs[i] - corX - val ry = moduleYs[i] - corY - m[i * 2, 0] = 1.0; m[i * 2, 1] = 0.0; m[i * 2, 2] = -ry - m[i * 2 + 1, 0] = 0.0; m[i * 2 + 1, 1] = 1.0; m[i * 2 + 1, 2] = rx - } - return m - } - - /** - * Performs inverse kinematics to return module velocities/angles from a desired chassis - * velocity, optionally about a custom center of rotation (e.g. for evasive maneuvers). - */ - @JvmOverloads - fun toSwerveModuleVelocities( - chassisVelocities: ChassisVelocities, - centerOfRotationX: Double = 0.0, - centerOfRotationY: Double = 0.0, - ): Array { - val vx = chassisVelocities.linearVel.x.into(unit = MetersPerSecond) - val vy = chassisVelocities.linearVel.y.into(unit = MetersPerSecond) - val omega = chassisVelocities.angVel.into(unit = RadiansPerSecond) - - if (vx == 0.0 && vy == 0.0 && omega == 0.0) { - return Array(numModules) { SwerveModuleVelocity(0.0, moduleHeadings[it]) } - } - - if (centerOfRotationX != prevCorX || centerOfRotationY != prevCorY) { - inverseKinematics = buildInverseKinematics(centerOfRotationX, centerOfRotationY) - prevCorX = centerOfRotationX - prevCorY = centerOfRotationY - } - - val chassisVector = DynamicVector.of(vx, vy, omega) - val moduleVelocitiesMatrix = inverseKinematics * chassisVector - - return Array(numModules) { i -> - val x = moduleVelocitiesMatrix[i * 2, 0] - val y = moduleVelocitiesMatrix[i * 2 + 1, 0] - val velocity = hypot(x, y) - val angle = if (velocity > 1e-6) Rotation2d(real = x, imag = y) else moduleHeadings[i] - moduleHeadings[i] = angle - SwerveModuleVelocity(velocity, angle) - } - } - - companion object { - /** - * Renormalizes module velocities if any exceed [attainableMaxVelocity], preserving the - * ratio of velocities between modules (and therefore the direction of net motion). - */ - @JvmStatic - fun desaturateWheelVelocities( - moduleVelocities: Array, - attainableMaxVelocity: Double, - ): Array { - val realMaxVelocity = moduleVelocities.maxOf { kotlin.math.abs(it.velocity) } - - return if (realMaxVelocity > attainableMaxVelocity) { - Array(moduleVelocities.size) { i -> - SwerveModuleVelocity( - moduleVelocities[i].velocity / realMaxVelocity * attainableMaxVelocity, - moduleVelocities[i].angle, - ) - } - } else { - moduleVelocities.copyOf() - } - } - } -} \ No newline at end of file From fac99e0b498e0c27b1c7301e760c09a33a52d46d Mon Sep 17 00:00:00 2001 From: 28shettr <28shettr@elmbrookstudents.org> Date: Mon, 27 Jul 2026 18:30:24 -0500 Subject: [PATCH 04/11] no mo swerve --- .idea/caches/deviceStreaming.xml | 169 +++++++++++++++++++++++++++++++ 1 file changed, 169 insertions(+) diff --git a/.idea/caches/deviceStreaming.xml b/.idea/caches/deviceStreaming.xml index dde3dd1..cdd61d8 100644 --- a/.idea/caches/deviceStreaming.xml +++ b/.idea/caches/deviceStreaming.xml @@ -99,6 +99,18 @@