From 1374b5f975c6a8717c00a332d49e1eb5da6a7021 Mon Sep 17 00:00:00 2001 From: SirBeans Date: Sat, 22 Jun 2024 14:55:13 -0400 Subject: [PATCH] Makes sure that the program builds by fixing up errors whith variables switched from pairs to velocity2d and fixing things messed up by removal of deprocated functions --- .../FeedForwardCharacterizationCommand.kt | 2 +- .../drivetrain/DriveBrakeModeCommand.kt | 7 +- .../commands/drivetrain/DrivePathCommand.kt | 8 +- .../commands/drivetrain/LockDriveCommand.kt | 17 --- .../drivetrain/OpenLoopReverseCommand.kt | 3 +- .../drivetrain/SwerveModuleTuningCommand.kt | 2 +- .../commands/drivetrain/TargetNoteCommand.kt | 9 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 122 +++++++++--------- .../drivetrain/swervemodule/SwerveModule.kt | 2 +- .../robot2023/util/driver/DriverProfile.kt | 8 +- 10 files changed, 80 insertions(+), 100 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt index ebd53be6..f019ca6b 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt @@ -37,7 +37,7 @@ class FeedForwardCharacterizationCommand( } override fun execute() { - drivetrain.swerveModules.forEach { it.zeroSteering() } + drivetrain.swerveModules.forEach { it.zeroSteer() } if (timer.get() < startDelay.inSeconds) { voltageConsumer.accept(0.volts) 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 3eb85b2e..7269ee04 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.wpilibj2.command.Command -import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -13,10 +13,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = - DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d()) 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 34487fba..a5027944 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -420,16 +420,14 @@ private constructor( // Stop where we are if interrupted drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + 0.0.radians.perSecond, + Velocity2d.fromVelocityVectorToVelocity2d(0.0.meters.perSecond, 0.0.radians) ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.currentRequest = - DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d()) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt deleted file mode 100644 index 03a04ff5..00000000 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt +++ /dev/null @@ -1,17 +0,0 @@ -package com.team4099.robot2023.commands.drivetrain - -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.Command - -class LockDriveCommand(val drivetrain: Drivetrain) : Command() { - - override fun execute() { - drivetrain.defaultCommand.end(true) - drivetrain.currentRequest = Request.DrivetrainRequest.LockWheels() - } - - override fun isFinished(): Boolean { - return false - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index 257be010..acafcd26 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees @@ -16,7 +17,7 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.degrees.perSecond, - Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), + Velocity2d(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt index 6f0403e0..819c8175 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt @@ -14,7 +14,7 @@ class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition override fun execute() { for (module in drivetrain.swerveModules) { - module.setPositionClosedLoop( + module.closedLoop( SwerveModuleState(0.0, steeringPosition().inRotation2ds), SwerveModuleState(0.0, steeringPosition().inRotation2ds), false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index 7cf3e7c6..087e7068 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -7,6 +7,7 @@ import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.CustomLogger +import com.team4099.robot2023.util.Velocity2d import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase @@ -125,18 +126,16 @@ class TargetNoteCommand( if (!feeder.hasNote && limelight.inputs.gamePieceTargets.size > 0) { val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) - var autoDriveVector = - hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + var autoDriveVector = hypot(driveVector.x.inMetersPerSecond, driveVector.y.inMetersPerSecond) if (DriverStation.getAlliance().isPresent && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue ) { - autoDriveVector = - -hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + autoDriveVector = -hypot(driveVector.x.inMetersPerSecond, driveVector.y.inMetersPerSecond) } drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - Pair(autoDriveVector.meters.perSecond, 0.0.meters.perSecond), + Velocity2d(autoDriveVector.meters.perSecond, 0.0.meters.perSecond), fieldOriented = false ) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 8cc0240c..9819c6c7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -47,9 +47,11 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perSecond +import java.util.concurrent.locks.Lock +import java.util.concurrent.locks.ReentrantLock import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest -class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { +class Drivetrain(private val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { private val gyroNotConnectedAlert = Alert( "Gyro is not connected, field relative driving will be significantly worse.", @@ -57,7 +59,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - val gyroInputs = GyroIO.GyroIOInputs() + private val gyroInputs = GyroIO.GyroIOInputs() private var gyroYawOffset = 0.0.radians @@ -70,30 +72,24 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB private var isFieldOriented = true - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - private set + private var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - private set + private var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var isInAutonomous = false - private set + private var isInAutonomous = false var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var fieldVelocity = Velocity2d() private set - var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var robotVelocity = Velocity2d() private var omegaVelocity = 0.0.radians.perSecond private var characterizationInput = 0.0.volts - var lastGyroYaw = { gyroInputs.gyroYaw } - - var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED - private set + private var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED private val testAngle = LoggedTunableValue("Drivetrain/testAngle", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) @@ -101,16 +97,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB private val swerveModuleID = LoggedTunableValue("Drivetrain/testModule", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) - val closestAlignmentAngle: Angle - get() { - for (angle in -180..90 step 90) { - if ((odomTRobot.rotation - angle.degrees).absoluteValue <= 45.degrees) { - return angle.degrees - } - } - return 0.0.degrees - } - var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() set(value) { when (value) { @@ -134,6 +120,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB field = value } + // At this point could we js make constants for wheel locations? private val frontLeftWheelLocation = Translation2d( DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 @@ -231,22 +218,22 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB gyroIO.updateInputs(gyroInputs) // Read new gyro and wheel data from sensors - // odometryLock.lock() // Prevents odometry updates while reading data swerveModules.forEach { it.updateInputs() } - // odometryLock.unlock() CustomLogger.processInputs("Gyro", gyroInputs) - gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) // send gyro alert if gyro disconnects swerveModules.forEach { it.periodic() } // Update field velocity + // create a list to hold updated velos val measuredStates = mutableListOf( SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() ) + // collect updated velos for (i in 0..3) { measuredStates[i] = SwerveModuleState( @@ -254,6 +241,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules[i].inputs.steerPosition.inRotation2ds ) } + // convert them into wpi lib chassis speeds val chassisState: ChassisSpeeds = ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates.toTypedArray())) val fieldVelCalculated = @@ -280,19 +268,21 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fieldFrameEstimator.getLatestOdometryTSpeaker().transform2d ) - CustomLogger.recordOutput("Drivetrain/OdometryGyroRotationValue", odomTRobot.rotation.inDegrees) + CustomLogger.recordOutput( + "Drivetrain/OdometryGyroRotationValueInDegrees", odomTRobot.rotation.inDegrees + ) CustomLogger.recordOutput( - "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond + "Drivetrain/xRobotVelocityMetersPerSecondInMPS", robotVelocity.x.inMetersPerSecond ) CustomLogger.recordOutput( - "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond + "Drivetrain/xRobotVelocityMetersPerSecondInMPS", robotVelocity.x.inMetersPerSecond ) CustomLogger.recordOutput( - "Drivetrain/xFieldVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond + "Drivetrain/xFieldVelocityMetersPerSecondInMPS", fieldVelocity.x.inMetersPerSecond ) CustomLogger.recordOutput( - "Drivetrain/yFieldVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond + "Drivetrain/yFieldVelocityMetersPerSecondInMPS", fieldVelocity.y.inMetersPerSecond ) CustomLogger.processInputs("Drivetrain/Gyro", gyroInputs) @@ -405,7 +395,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) { CustomLogger.recordOutput("Drivetrain/isFieldOriented", fieldOriented) - // flip the direction base don alliance color + // flip the direction based on alliance color val driveVectorRespectiveToAlliance = if (FMSData.allianceColor == DriverStation.Alliance.Blue) driveVector else driveVector.unaryMinus() @@ -422,7 +412,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB val swerveModuleStates: Array var desiredChassisSpeeds: ChassisSpeeds - // calculated chasis speeds, apply field oriented transformation + // calculated chassis speeds, apply field oriented transformation if (fieldOriented) { desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( @@ -494,7 +484,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) ) { var chassisSpeeds = chassisSpeeds - + // If theres skew consider slowing down if (DrivetrainConstants.MINIMIZE_SKEW) { chassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds.discretize( @@ -625,33 +615,45 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fieldFrameEstimator.addSpeakerVisionData(visionData) } - // Drivetrain states for state machine. - enum class DrivetrainState { - UNINITIALIZED, - IDLE, - ZEROING_SENSORS, - OPEN_LOOP, - CLOSED_LOOP, - CHARACTERIZE; - - inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { - return ( - (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || - (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || - (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || - (request is DrivetrainRequest.Idle && this == IDLE) || - (request is DrivetrainRequest.Characterize && this == CHARACTERIZE) - ) + companion object { + // Drivetrain multithreading + var odometryLock: Lock = ReentrantLock() + fun setOdometryLock(Locked: Boolean) { + if (Locked) { + odometryLock.lock() + } else { + odometryLock.unlock() + } } - } - inline fun fromRequestToState(request: Request.DrivetrainRequest): DrivetrainState { - return when (request) { - is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP - is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP - is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS - is DrivetrainRequest.Idle -> DrivetrainState.IDLE - is DrivetrainRequest.Characterize -> DrivetrainState.CHARACTERIZE + // Drivetrain states for state machine. + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP, + CHARACTERIZE; + + inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) || + (request is DrivetrainRequest.Characterize && this == CHARACTERIZE) + ) + } + } + + inline fun fromRequestToState(request: Request.DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + is DrivetrainRequest.Characterize -> DrivetrainState.CHARACTERIZE + } } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index 2ac75213..67bd563a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -243,7 +243,7 @@ class SwerveModule(val io: SwerveModuleIO) { fun resetModuleZero() { io.resetModuleZero() } - fun zeroSteer(isInAuto: Boolean) { + fun zeroSteer(isInAuto: Boolean = false) { io.zeroSteering() } fun setDriveBrakeMode(brake: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/util/driver/DriverProfile.kt b/src/main/kotlin/com/team4099/robot2023/util/driver/DriverProfile.kt index 37cce083..582f6d60 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/driver/DriverProfile.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/driver/DriverProfile.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.util.driver import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.util.Velocity2d import org.team4099.lib.units.AngularVelocity -import org.team4099.lib.units.LinearVelocity import java.util.function.BooleanSupplier import java.util.function.DoubleSupplier import kotlin.math.pow @@ -22,7 +22,7 @@ abstract class DriverProfile( driveX: DoubleSupplier, driveY: DoubleSupplier, slowMode: BooleanSupplier - ): Pair { + ): Velocity2d { var xSpeedCurve = driveX.asDouble.pow(sensitivityDrivePowerConstant) * invertDriveMultiplier var ySpeedCurve = driveY.asDouble.pow(sensitivityDrivePowerConstant) * invertDriveMultiplier @@ -32,12 +32,12 @@ abstract class DriverProfile( } if (slowMode.asBoolean) { - return Pair( + return Velocity2d( DrivetrainConstants.DRIVE_SETPOINT_MAX * xSpeedCurve * slowModeClamp, DrivetrainConstants.DRIVE_SETPOINT_MAX * ySpeedCurve * slowModeClamp ) } else { - return Pair( + return Velocity2d( DrivetrainConstants.DRIVE_SETPOINT_MAX * xSpeedCurve, DrivetrainConstants.DRIVE_SETPOINT_MAX * ySpeedCurve )