Skip to content

Commit

Permalink
SpotlessApply
Browse files Browse the repository at this point in the history
  • Loading branch information
@SirBeans
SirBeans committed Jul 19, 2024
1 parent 0d8502d commit c68ed22
Show file tree
Hide file tree
Showing 8 changed files with 158 additions and 130 deletions.
3 changes: 1 addition & 2 deletions src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -123,7 +122,7 @@ class CustomHolonomicDriveController(
return calculate(
currentPose,
poseMeters,
driveStateVelo,
driveStateVelo,
holonomicRotationState.position,
holonomicRotationState.velocityRadiansPerSec
)
Expand Down
11 changes: 1 addition & 10 deletions src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt
View file
Original file line number Diff line number Diff line change
@@ -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 {


}
class Choreo1stgen
76 changes: 49 additions & 27 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -72,7 +93,7 @@ private constructor(

private var changeStartTimeOnExecute = true

private var trajectoryGenerator = CustomTrajectoryGenerator()
private var trajectoryGenerator = CustomTrajectoryGenerator()

val thetakP =
LoggedTunableValue(
Expand Down Expand Up @@ -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() {
Expand All @@ -227,7 +244,6 @@ private constructor(

val trajectory = trajectoryGenerator.driveTrajectory


if (generatedTrajectory.totalStates <= 1) {
return
}
Expand All @@ -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)
}
Expand Down Expand Up @@ -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",
Expand Down Expand Up @@ -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)

Expand Down
20 changes: 13 additions & 7 deletions src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
158 changes: 84 additions & 74 deletions src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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))
)
}
}
}
}
}
Loading

0 comments on commit c68ed22

Please sign in to comment.