From c68ed229bbb2f6a6560f28f63ccf5fca8f9d908e Mon Sep 17 00:00:00 2001 From: SirBeans Date: Thu, 18 Jul 2024 20:11:37 -0400 Subject: [PATCH] SpotlessApply --- .../CustomHolonomicDriveController.kt | 3 +- .../auto/choreoStuff/Choreo1stgen.kt | 11 +- .../commands/drivetrain/DrivePathCommand.kt | 76 ++++++--- .../robot2023/util/AllianceFlipUtil.kt | 20 ++- .../robot2023/util/CustomTrajectory.kt | 158 ++++++++++-------- .../robot2023/util/CustomTrajectoryState.kt | 8 +- .../robot2023/util/TrajectoryTypes.kt | 6 +- .../robot2023/util/driver/DriveStateTypes.kt | 6 +- 8 files changed, 158 insertions(+), 130 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt index b03bf07a..e794cfc5 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt @@ -1,6 +1,5 @@ package com.team4099.lib.trajectory -import com.team4099.robot2023.util.driver.DriveStateTypes import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d @@ -123,7 +122,7 @@ class CustomHolonomicDriveController( return calculate( currentPose, poseMeters, -driveStateVelo, + driveStateVelo, holonomicRotationState.position, holonomicRotationState.velocityRadiansPerSec ) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt index ba494fe9..bcb99395 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt @@ -1,12 +1,3 @@ package com.team4099.robot2023.auto.choreoStuff -import com.choreo.lib.Choreo; -import com.choreo.lib.ChoreoTrajectory; -import edu.wpi.first.math.proto.Trajectory -import org.team4099.lib.geometry.Pose2d -import java.util.function.Supplier - -class Choreo1stgen { - - -} \ No newline at end of file +class Choreo1stgen 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 3f69e003..6648c4b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -10,7 +10,12 @@ import com.team4099.lib.trajectory.OdometryWaypoint import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.* +import com.team4099.robot2023.util.AllianceFlipUtil +import com.team4099.robot2023.util.CustomLogger +import com.team4099.robot2023.util.CustomTrajectory +import com.team4099.robot2023.util.FrameType +import com.team4099.robot2023.util.TrajectoryTypes +import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException @@ -33,7 +38,23 @@ import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inMetersPerSecondPerMeter +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadian +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianPerSecond +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianSeconds +import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.perMeter +import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.perRadian +import org.team4099.lib.units.derived.perRadianPerSecond +import org.team4099.lib.units.derived.perRadianSeconds +import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.inRadiansPerSecond @@ -72,7 +93,7 @@ private constructor( private var changeStartTimeOnExecute = true - private var trajectoryGenerator = CustomTrajectoryGenerator() + private var trajectoryGenerator = CustomTrajectoryGenerator() val thetakP = LoggedTunableValue( @@ -199,12 +220,8 @@ private constructor( } // drop this all into the custom trajectory - generatedTrajectory = CustomTrajectory( - drivePoseSupplier, - trajectory, - trajectoryGenerator, - swerveDriveController - ) + generatedTrajectory = + CustomTrajectory(drivePoseSupplier, trajectory, trajectoryGenerator, swerveDriveController) } override fun initialize() { @@ -227,7 +244,6 @@ private constructor( val trajectory = trajectoryGenerator.driveTrajectory - if (generatedTrajectory.totalStates <= 1) { return } @@ -245,22 +261,22 @@ private constructor( trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds) val targetHolonomicPose = generatedTrajectory.sample(trajCurTime).pose2d - /* - AllianceFlipUtil.apply( - Pose2d( - desiredState.poseMeters.x.meters, - desiredState.poseMeters.y.meters, - desiredRotation.position.radians.radians - ) + /* + AllianceFlipUtil.apply( + Pose2d( + desiredState.poseMeters.x.meters, + desiredState.poseMeters.y.meters, + desiredRotation.position.radians.radians ) - */ + ) + */ var robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() if (pathFrame == stateFrame) { lastSampledPose = targetHolonomicPose when (stateFrame) { FrameType.FIELD -> { - Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized",targetHolonomicPose.pose2d) + Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized", targetHolonomicPose.pose2d) Logger.recordOutput("Pathfollow/fieldTRobot", robotPoseInSelectedFrame.pose2d) } @@ -301,24 +317,30 @@ private constructor( desiredState = AllianceFlipUtil.apply(desiredState) desiredRotation = AllianceFlipUtil.apply(desiredRotation) } - val driveVelocity = sqrt(desiredState.request.chassisSpeeds.vxMetersPerSecond.pow(2) - + desiredState.request.chassisSpeeds.vyMetersPerSecond.pow(2)) + val driveVelocity = + sqrt( + desiredState.request.chassisSpeeds.vxMetersPerSecond.pow(2) + + desiredState.request.chassisSpeeds.vyMetersPerSecond.pow(2) + ) var nextDriveState = swerveDriveController.calculate( - robotPoseInSelectedFrame.pose2d, desiredState.pose2d.pose2d, driveVelocity, desiredRotation + robotPoseInSelectedFrame.pose2d, + desiredState.pose2d.pose2d, + driveVelocity, + desiredRotation ) if (leaveOutYAdjustment) { nextDriveState = ChassisSpeeds(nextDriveState.vxMetersPerSecond, 0.0, nextDriveState.omegaRadiansPerSecond) - desiredState.request.chassisAccels.vyMetersPerSecond = 0.0 + desiredState.request.chassisAccels.vyMetersPerSecond = 0.0 } - //TODO figure out wtf this is for and if it was needed reimplement it - drivetrain.targetPose = - Pose2d(Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position)) + Pose2d( + Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position) + ) Logger.recordOutput( "Pathfollow/target", @@ -354,7 +376,7 @@ private constructor( Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference()) Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds) - //totalTimeSEconds + // totalTimeSEconds CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) diff --git a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt index a4d8b98f..896b58af 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt @@ -111,16 +111,22 @@ object AllianceFlipUtil { } } - fun apply (customTrajectoryState: CustomTrajectoryState): CustomTrajectoryState { + fun apply(customTrajectoryState: CustomTrajectoryState): CustomTrajectoryState { return if (shouldFlip()) { - CustomTrajectoryState(Pose2d(FieldConstants.fieldLength - customTrajectoryState.pose2d.x, - customTrajectoryState.pose2d.y, - Angle(customTrajectoryState.pose2d.rotation.cos, customTrajectoryState.pose2d.rotation.sin)), customTrajectoryState.request) - }else { + CustomTrajectoryState( + Pose2d( + FieldConstants.fieldLength - customTrajectoryState.pose2d.x, + customTrajectoryState.pose2d.y, + Angle( + customTrajectoryState.pose2d.rotation.cos, + customTrajectoryState.pose2d.rotation.sin + ) + ), + customTrajectoryState.request + ) + } else { customTrajectoryState } - - } fun apply(zone: Zone2d, force: Boolean = false): Zone2d { diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt index a114ff17..b895d98e 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt @@ -4,7 +4,6 @@ import com.choreo.lib.ChoreoTrajectory import com.team4099.lib.trajectory.CustomHolonomicDriveController import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.driver.DriveStateTypes import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.trajectory.Trajectory import org.littletonrobotics.junction.Logger @@ -21,92 +20,103 @@ import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.perSecond class CustomTrajectory( - val poseSupplier: () -> Pose2d, - val trajectory: TrajectoryTypes, - val trajectoryGenerator: CustomTrajectoryGenerator, - val swerveDriveController: CustomHolonomicDriveController + val poseSupplier: () -> Pose2d, + val trajectory: TrajectoryTypes, + val trajectoryGenerator: CustomTrajectoryGenerator, + val swerveDriveController: CustomHolonomicDriveController ) { - val totalStates: Int - get() { - return when (trajectory) { - is Trajectory -> trajectory.states.size - is ChoreoTrajectory -> trajectory.samples.size - else -> { - println("Unexpected trajectory type ${trajectory::javaClass}") - return -1337 - } - } + val totalStates: Int + get() { + return when (trajectory) { + is Trajectory -> trajectory.states.size + is ChoreoTrajectory -> trajectory.samples.size + else -> { + println("Unexpected trajectory type ${trajectory::javaClass}") + return -1337 } + } + } - val timeAtFirstState: Time - get() { - return when (trajectory) { - is Trajectory -> trajectory.states[0].timeSeconds.seconds - is ChoreoTrajectory -> trajectory.initialState.timestamp.seconds - else -> { - println("Unexpected trajectory type ${trajectory::javaClass}") - return -1337.seconds - } - } + val timeAtFirstState: Time + get() { + return when (trajectory) { + is Trajectory -> trajectory.states[0].timeSeconds.seconds + is ChoreoTrajectory -> trajectory.initialState.timestamp.seconds + else -> { + println("Unexpected trajectory type ${trajectory::javaClass}") + return -1337.seconds } + } + } - fun sample(time: Time) : CustomTrajectoryState { - return when (trajectory) { - is Trajectory -> { + fun sample(time: Time): CustomTrajectoryState { + return when (trajectory) { + is Trajectory -> { - val desiredState = trajectory.sample(time.inSeconds) - val desiredRotation = trajectoryGenerator.holonomicRotationSequence.sample(time.inSeconds) - val nextDriveState = swerveDriveController.calculate( - poseSupplier().pose2d, - desiredState.poseMeters, - desiredState.velocityMetersPerSecond, - desiredRotation - ) + val desiredState = trajectory.sample(time.inSeconds) + val desiredRotation = trajectoryGenerator.holonomicRotationSequence.sample(time.inSeconds) + val nextDriveState = + swerveDriveController.calculate( + poseSupplier().pose2d, + desiredState.poseMeters, + desiredState.velocityMetersPerSecond, + desiredRotation + ) - val chassisSpeeds = ChassisSpeeds(nextDriveState.vxMetersPerSecond, nextDriveState.vyMetersPerSecond, nextDriveState.omegaRadiansPerSecond) + val chassisSpeeds = + ChassisSpeeds( + nextDriveState.vxMetersPerSecond, + nextDriveState.vyMetersPerSecond, + nextDriveState.omegaRadiansPerSecond + ) - val xAccel = - desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond * - desiredState.curvatureRadPerMeter.radians.cos - val yAccel = - desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond * - desiredState.curvatureRadPerMeter.radians.sin + val xAccel = + desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond * + desiredState.curvatureRadPerMeter.radians.cos + val yAccel = + desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond * + desiredState.curvatureRadPerMeter.radians.sin - Logger.recordOutput( - "Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond - ) - Logger.recordOutput( - "Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond - ) + Logger.recordOutput( + "Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond + ) + Logger.recordOutput( + "Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond + ) - val chassisAccels = ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB + val chassisAccels = + ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB - // Retrieve the target pose - val targetPose = - AllianceFlipUtil.apply( - Pose2d( - desiredState.poseMeters.x.meters, - desiredState.poseMeters.y.meters, - desiredRotation.position.radians.radians - ) - ) + // Retrieve the target pose + val targetPose = + AllianceFlipUtil.apply( + Pose2d( + desiredState.poseMeters.x.meters, + desiredState.poseMeters.y.meters, + desiredRotation.position.radians.radians + ) + ) - val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels) - CustomTrajectoryState(targetPose, drivetrainRequest) - } - is ChoreoTrajectory -> { - val trajectoryState = trajectory.sample(time.inSeconds) + val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(chassisSpeeds, chassisAccels) + CustomTrajectoryState(targetPose, drivetrainRequest) + } + is ChoreoTrajectory -> { + val trajectoryState = trajectory.sample(time.inSeconds) - // Retrieve the target pose - val targetPose = Pose2d(trajectoryState.pose) + // Retrieve the target pose + val targetPose = Pose2d(trajectoryState.pose) - val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds) + val drivetrainRequest = Request.DrivetrainRequest.ClosedLoop(trajectoryState.chassisSpeeds) - CustomTrajectoryState(targetPose, drivetrainRequest) - } else -> { - println("Unexpected trajectory type ${trajectory::javaClass}") - CustomTrajectoryState(Pose2d(0.meters, 0.meters, 0.radians), Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))) - } - } + CustomTrajectoryState(targetPose, drivetrainRequest) + } + else -> { + println("Unexpected trajectory type ${trajectory::javaClass}") + CustomTrajectoryState( + Pose2d(0.meters, 0.meters, 0.radians), + Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) + ) + } } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectoryState.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectoryState.kt index b3cab8e3..f66085c7 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectoryState.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/CustomTrajectoryState.kt @@ -1,9 +1,9 @@ package com.team4099.robot2023.util -import org.team4099.lib.geometry.Pose2d import com.team4099.robot2023.subsystems.superstructure.Request +import org.team4099.lib.geometry.Pose2d data class CustomTrajectoryState( - var pose2d: Pose2d, - val request: Request.DrivetrainRequest.ClosedLoop, -) \ No newline at end of file + var pose2d: Pose2d, + val request: Request.DrivetrainRequest.ClosedLoop, +) diff --git a/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt b/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt index e939284a..e199d6c2 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/TrajectoryTypes.kt @@ -4,6 +4,6 @@ import com.choreo.lib.ChoreoTrajectory import edu.wpi.first.math.trajectory.Trajectory sealed interface TrajectoryTypes { - class WPILIBTrajectoryType : Trajectory() - class ChoreoTrajectoryType : ChoreoTrajectory() -} \ No newline at end of file + class WPILIBTrajectoryType : Trajectory() + class ChoreoTrajectoryType : ChoreoTrajectory() +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/driver/DriveStateTypes.kt b/src/main/kotlin/com/team4099/robot2023/util/driver/DriveStateTypes.kt index e8789146..24504fb1 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/driver/DriveStateTypes.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/driver/DriveStateTypes.kt @@ -4,6 +4,6 @@ import com.team4099.robot2023.util.CustomTrajectoryState import edu.wpi.first.math.trajectory.Trajectory sealed interface DriveStateTypes { - data class WPILIBDriveStateType(val state: Trajectory.State) : DriveStateTypes - data class CustomDriveState(val state: CustomTrajectoryState) : DriveStateTypes -} \ No newline at end of file + data class WPILIBDriveStateType(val state: Trajectory.State) : DriveStateTypes + data class CustomDriveState(val state: CustomTrajectoryState) : DriveStateTypes +}