Skip to content

Commit

Permalink
Clear errors in Drive Path command still have errors with .calculate …
Browse files Browse the repository at this point in the history
…(its not abstracted yet) and a few other minor errors to fix
  • Loading branch information
SirBeans committed Jul 17, 2024
1 parent 304cbf7 commit b675fd2
Show file tree
Hide file tree
Showing 4 changed files with 62 additions and 51 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
package com.team4099.lib.trajectory

import com.team4099.robot2023.util.CustomTrajectoryState
import com.team4099.robot2023.util.TrajectoryTypes
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 @@ -116,16 +119,31 @@ class CustomHolonomicDriveController(
*/
fun calculate(
currentPose: Pose2d,
driveState: Trajectory.State,
driveState: DriveStateTypes,
holonomicRotationState: RotationSequence.State
): ChassisSpeeds {
return calculate(
currentPose,
driveState.poseMeters,
driveState.velocityMetersPerSecond,
holonomicRotationState.position,
holonomicRotationState.velocityRadiansPerSec
)

return when (driveState) {
is DriveStateTypes.WPILIBDriveStateType -> {
calculate(
currentPose,
driveState.state.poseMeters,
driveState.state.velocityMetersPerSecond,
holonomicRotationState.position,
holonomicRotationState.velocityRadiansPerSec
)
}
is DriveStateTypes.CustomDriveState -> {
calculate(
currentPose,
driveState.state.pose2d.pose2d,
driveState.state.velocityMetersPerSecond,
holonomicRotationState.position,
holonomicRotationState.velocityRadiansPerSec
)
}
}

}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ 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.Trajectory
import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException
import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint
Expand All @@ -41,24 +42,7 @@ 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.Radian
import org.team4099.lib.units.derived.cos
import org.team4099.lib.units.derived.degrees
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.derived.sin
import org.team4099.lib.units.derived.*
import org.team4099.lib.units.inMetersPerSecond
import org.team4099.lib.units.inMetersPerSecondPerSecond
import org.team4099.lib.units.inRadiansPerSecond
Expand All @@ -71,7 +55,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainReques
class DrivePathCommand<T : Waypoint>
private constructor(
val drivetrain: Drivetrain,
private val trajectory: TrajectoryTypes,
private var trajectory: TrajectoryTypes,
val resetPose: Boolean = false,
val useLowerTolerance: Boolean = false,
val flipForAlliances: Boolean = true,
Expand Down Expand Up @@ -248,6 +232,10 @@ private constructor(
}

override fun execute() {

val trajectory = trajectoryGenerator.driveTrajectory


if (generatedTrajectory.totalStates <= 1) {
return
}
Expand All @@ -258,6 +246,7 @@ private constructor(
}

trajCurTime = Clock.fpgaTime - trajStartTime

var desiredState = generatedTrajectory.sample(trajCurTime)

var desiredRotation =
Expand Down Expand Up @@ -317,35 +306,30 @@ private constructor(
Logger.recordOutput("Pathfollow/targetPoseInStateFrame", lastSampledPose.pose2d)

if (flipForAlliances) {
desiredState = AllianceFlipUtil.apply(desiredState)
desiredState = apply(desiredState)
desiredRotation = AllianceFlipUtil.apply(desiredRotation)
}

val xAccel =
desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
desiredState.curvatureRadPerMeter.radians.cos
var yAccel =
desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
desiredState.curvatureRadPerMeter.radians.sin

var nextDriveState =
swerveDriveController.calculate(
robotPoseInSelectedFrame.pose2d, desiredState, desiredRotation
)

/*
if (leaveOutYAdjustment) {
nextDriveState =
ChassisSpeeds(nextDriveState.vxMetersPerSecond, 0.0, nextDriveState.omegaRadiansPerSecond)
yAccel = 0.0.meters.perSecond.perSecond
}
*/
//TODO figure out wtf this is for and if it was needed reimplement it

drivetrain.targetPose =
Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position))
Pose2d(Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position))

Logger.recordOutput(
"Pathfollow/target",
Pose2dWPILIB.struct,
Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
Pose2dWPILIB(desiredState.pose2d.translation.translation2d, desiredRotation.position)
)

/*
Expand All @@ -358,11 +342,7 @@ private constructor(
*/

drivetrain.currentRequest =
DrivetrainRequest.ClosedLoop(
nextDriveState,
ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
)
drivetrain.currentRequest = desiredState.request

Logger.recordOutput("Pathfollow/thetaPIDPositionErrorRadians", thetaPID.error.inRadians)

Expand All @@ -372,21 +352,15 @@ private constructor(
"Pathfollow/thetaPIDVelocityErrorRadians", thetaPID.errorDerivative.inRadiansPerSecond
)

Logger.recordOutput(
"Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond
)
Logger.recordOutput(
"Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond
)

Logger.recordOutput("Pathfollow/Start Time", trajStartTime.inSeconds)
Logger.recordOutput("Pathfollow/Current Time", trajCurTime.inSeconds)
Logger.recordOutput(
"Pathfollow/Desired Angle in Degrees", desiredState.poseMeters.rotation.degrees
"Pathfollow/Desired Angle in Degrees", desiredState.pose2d.rotation.inDegrees
)

Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference())
Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds)
//totalTimeSEconds

CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true)

Expand Down
10 changes: 10 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/util/CustomTrajectory.kt
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
import com.team4099.robot2023.subsystems.superstructure.Request
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.math.trajectory.Trajectory
import org.littletonrobotics.junction.Logger
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.kinematics.ChassisAccels
import org.team4099.lib.units.base.Time
Expand All @@ -15,6 +16,7 @@ import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.derived.cos
import org.team4099.lib.units.derived.radians
import org.team4099.lib.units.derived.sin
import org.team4099.lib.units.inMetersPerSecondPerSecond
import org.team4099.lib.units.perSecond

class CustomTrajectory(
Expand Down Expand Up @@ -66,6 +68,14 @@ class CustomTrajectory(
val yAccel =
desiredState.accelerationMetersPerSecondSq.meters.perSecond.perSecond *
desiredState.curvatureRadPerMeter.radians.sin

Logger.recordOutput(
"Pathfollow/xAccelMetersPerSecondPerSecond", xAccel.inMetersPerSecondPerSecond
)
Logger.recordOutput(
"Pathfollow/yAccelMetersPerSecondPerSecond", yAccel.inMetersPerSecondPerSecond
)

val chassisAccels = ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB

// Retrieve the target pose
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
package com.team4099.robot2023.util.driver

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
}

0 comments on commit b675fd2

Please sign in to comment.