diff --git a/.gradle/8.4/executionHistory/executionHistory.bin b/.gradle/8.4/executionHistory/executionHistory.bin index a40d5091..64e56902 100644 Binary files a/.gradle/8.4/executionHistory/executionHistory.bin and b/.gradle/8.4/executionHistory/executionHistory.bin differ diff --git a/.gradle/8.4/executionHistory/executionHistory.lock b/.gradle/8.4/executionHistory/executionHistory.lock index 8051b929..5b9db1d7 100644 Binary files a/.gradle/8.4/executionHistory/executionHistory.lock and b/.gradle/8.4/executionHistory/executionHistory.lock differ diff --git a/.gradle/8.4/fileHashes/fileHashes.bin b/.gradle/8.4/fileHashes/fileHashes.bin index 12d9cbd6..a1e8845b 100644 Binary files a/.gradle/8.4/fileHashes/fileHashes.bin and b/.gradle/8.4/fileHashes/fileHashes.bin differ diff --git a/.gradle/8.4/fileHashes/fileHashes.lock b/.gradle/8.4/fileHashes/fileHashes.lock index 84ab57db..6e8ea709 100644 Binary files a/.gradle/8.4/fileHashes/fileHashes.lock and b/.gradle/8.4/fileHashes/fileHashes.lock differ diff --git a/.gradle/8.4/fileHashes/resourceHashesCache.bin b/.gradle/8.4/fileHashes/resourceHashesCache.bin index ac844902..466bb369 100644 Binary files a/.gradle/8.4/fileHashes/resourceHashesCache.bin and b/.gradle/8.4/fileHashes/resourceHashesCache.bin differ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 9a8b130a..b90265af 100644 Binary files a/.gradle/buildOutputCleanup/buildOutputCleanup.lock and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 60d7b27a..07835b15 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain +import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.Drivetrain -import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond @@ -14,7 +14,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { override fun execute() { drivetrain.currentRequest = - DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d.ZERO_VELOCITY_VECTOR) + DrivetrainRequest.OpenLoop(0.0.radians.perSecond, DrivetrainConstants.ZERO_VELOCITY_VECTOR) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 4223ccbf..f4c0c6ef 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -73,7 +73,7 @@ private constructor( val flipForAlliances: Boolean = true, val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, - val endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR, + val endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR, var stateFrame: FrameType = FrameType.ODOMETRY, var pathFrame: FrameType = FrameType.FIELD, ) : Command() { @@ -428,7 +428,9 @@ private constructor( // Since we weren't interrupted, we know curTime > endTime execute() drivetrain.currentRequest = - DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d.ZERO_VELOCITY_VECTOR) + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, DrivetrainConstants.ZERO_VELOCITY_VECTOR + ) } } @@ -444,7 +446,7 @@ private constructor( flipForAlliances: Boolean = true, endPathOnceAtReference: Boolean = true, leaveOutYAdjustment: Boolean = false, - endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR, + endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR, stateFrame: FrameType = FrameType.ODOMETRY, ): DrivePathCommand = DrivePathCommand( @@ -468,7 +470,7 @@ private constructor( flipForAlliances: Boolean = true, endPathOnceAtReference: Boolean = true, leaveOutYAdjustment: Boolean = false, - endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR, + endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR, stateFrame: FrameType = FrameType.ODOMETRY, ): DrivePathCommand = DrivePathCommand( diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index f90cd4c6..ed86ffd1 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -1,5 +1,7 @@ package com.team4099.robot2023.config.constants +import com.team4099.robot2023.util.Velocity2d +import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.Velocity @@ -39,6 +41,9 @@ object DrivetrainConstants { val ODOMETRY_UPDATE_FREQUENCY = 250.0.hertz + val ZERO_VELOCITY_VECTOR = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + val ZERO_CHASSIS_SPEED = ChassisSpeeds(0.0, 0.0, 0.0) + const val WHEEL_COUNT = 4 val WHEEL_DIAMETER = (2.083 * 2).inches val DRIVETRAIN_LENGTH = 22.750.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt index 23f3f2cb..64ef4bd4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt @@ -71,26 +71,22 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List