Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
follow field coordinates in odo frame
Browse files Browse the repository at this point in the history
sswadkar committed Feb 18, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 084fbb1 commit 0a238d6
Showing 2 changed files with 30 additions and 16 deletions.
Original file line number Diff line number Diff line change
@@ -56,7 +56,8 @@ object AutonomousSelector {
get() = secondaryWaitInAuto.getDouble(0.0).seconds

fun getCommand(drivetrain: Drivetrain): Command {
val mode = autonomousModeChooser.get()
val mode = AutonomousMode.TEST_AUTO_PATH
// val mode = autonomousModeChooser.get()
// println("${waitTime().inSeconds} wait command")
when (mode) {
AutonomousMode.TEST_AUTO_PATH ->
Original file line number Diff line number Diff line change
@@ -2,13 +2,16 @@ package com.team4099.robot2023.commands.drivetrain

import com.pathplanner.lib.path.PathPlannerPath
import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.lib.math.asPose2d
import com.team4099.lib.math.asTransform2d
import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Request
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.wpilibj2.command.Command
import org.littletonrobotics.junction.Logger
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Transform2d
import org.team4099.lib.geometry.Translation2d
import org.team4099.lib.hal.Clock
import org.team4099.lib.pplib.PathPlannerHolonomicDriveController
@@ -41,7 +44,8 @@ import org.team4099.lib.units.perSecond
class FollowPathPlannerPathCommand(
val drivetrain: Drivetrain,
val path: PathPlannerPath,
useOdoFrame: Boolean = true
useOdoFrame: Boolean = true,
followingFieldCoordinates: Boolean = true,
) : Command() {
private val translationToleranceAtEnd = 1.inches
private val thetaToleranceAtEnd = 2.5.degrees
@@ -54,13 +58,15 @@ class FollowPathPlannerPathCommand(
private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
private var lastSampledPose = Pose2d()

private var drivetrainPoseSupplier: () -> Pose2d
private var odoTRobotSupplier: () -> Pose2d
private var odoTFieldSupplier: () -> Transform2d

private val atReference: Boolean
get() =
(
(lastSampledPose.translation - drivetrainPoseSupplier().translation).magnitude.meters <=
(lastSampledPose.translation - odoTRobotSupplier().translation).magnitude.meters <=
translationToleranceAtEnd &&
(lastSampledPose.rotation - drivetrainPoseSupplier().rotation).absoluteValue <=
(lastSampledPose.rotation - odoTRobotSupplier().rotation).absoluteValue <=
thetaToleranceAtEnd
)

@@ -127,9 +133,15 @@ class FollowPathPlannerPathCommand(
addRequirements(drivetrain)

if (useOdoFrame) {
drivetrainPoseSupplier = { drivetrain.odomTRobot }
odoTRobotSupplier = { drivetrain.odomTRobot }
} else {
odoTRobotSupplier = { drivetrain.fieldTRobot }
}

if (followingFieldCoordinates) {
odoTFieldSupplier = { drivetrain.odomTField }
} else {
drivetrainPoseSupplier = { drivetrain.fieldTRobot }
odoTFieldSupplier = { Transform2d(Translation2d(), 0.0.degrees) }
}

translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get())
@@ -187,7 +199,7 @@ class FollowPathPlannerPathCommand(
}

val currentSpeeds = drivetrain.targetedChassisSpeeds
val poseRotation = drivetrainPoseSupplier().rotation.inRotation2ds
val poseRotation = odoTRobotSupplier().rotation.inRotation2ds
val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)

// Sampling the trajectory for a state that we're trying to target
@@ -202,17 +214,18 @@ class FollowPathPlannerPathCommand(
lastSampledPose.rotation.inRadians
)
)
Logger.recordOutput(
"Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees
)
Logger.recordOutput("Pathfollow/currentTheta", drivetrainPoseSupplier().rotation.inDegrees)
lastSampledPose =
odoTFieldSupplier()
.asPose2d()
.transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d().inverse())

lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
Logger.recordOutput("Pathfollow/thetaSetpointDegrees", lastSampledPose.rotation.inDegrees)
Logger.recordOutput("Pathfollow/currentThetaDegrees", odoTRobotSupplier().rotation.inDegrees)

val fieldFrameRobotPose =
odoTFieldSupplier().inverse().asPose2d().transformBy(odoTRobotSupplier().asTransform2d())
val targetedChassisSpeeds =
swerveDriveController.calculateRobotRelativeSpeeds(
drivetrainPoseSupplier(), stateFromTrajectory
)
swerveDriveController.calculateRobotRelativeSpeeds(fieldFrameRobotPose, stateFromTrajectory)

// Set closed loop request
pathFollowRequest.chassisSpeeds = targetedChassisSpeeds.chassisSpeedsWPILIB

0 comments on commit 0a238d6

Please sign in to comment.