Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Resetting odoTField in auto #39

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Resetting odoTField in auto #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b8800f2
add lastStableOdoTField and change when ResetFieldFrameEstimator is c…
Shilab66 Feb 23, 2024
56d79e6
ran spotless apply
Shilab66 Feb 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,5 +189,9 @@ object RobotContainer {

fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain)

fun drivetrainResetFieldFrameEstimator() {
drivetrain.resetFieldFrameEstimator(drivetrain.fieldTRobot)
}

fun mapTunableCommands() {}
}
2 changes: 2 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package com.team4099.robot2023.auto

import com.team4099.robot2023.RobotContainer
import com.team4099.robot2023.auto.mode.TestAutoPath
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import edu.wpi.first.networktables.GenericEntry
Expand Down Expand Up @@ -57,6 +58,7 @@ object AutonomousSelector {

fun getCommand(drivetrain: Drivetrain): Command {
val mode = AutonomousMode.TEST_AUTO_PATH
RobotContainer.drivetrainResetFieldFrameEstimator()
// val mode = autonomousModeChooser.get()
// println("${waitTime().inSeconds} wait command")
when (mode) {
Expand Down
3 changes: 3 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ private constructor(

private var drivePoseSupplier: () -> Pose2d
private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)

private var errorString = ""

Expand Down Expand Up @@ -214,6 +215,8 @@ private constructor(

override fun initialize() {
odoTField = drivetrain.odomTField
lastStableOdoTField = drivetrain.lastStableOdomTField

pathTransform =
Transform2d(
Translation2d(waypoints.get()[0].translation),
Expand Down
2 changes: 2 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ class FollowPathPlannerPathCommand(

private var drivePoseSupplier: () -> Pose2d
private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
private var lastStableOdoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)

private val atReference: Boolean
get() =
Expand Down Expand Up @@ -174,6 +175,7 @@ class FollowPathPlannerPathCommand(
override fun initialize() {
trajStartTime = Clock.fpgaTime
odoTField = drivetrain.odomTField
lastStableOdoTField = drivetrain.lastStableOdomTField

currentSpeeds = drivetrain.targetedChassisSpeeds
poseRotation = drivePoseSupplier().rotation.inRotation2ds
Expand Down
6 changes: 3 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,15 +181,15 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB

val odomTRobot: Pose2d
get() = Pose2d(swerveDriveOdometry.poseMeters)

// NOTE(parth): This should be expected to be noisy. Avoid using this directly if possible.
// NOTE (parth): This should be expected to be noisy. Avoid using this directly if possible.
val fieldTRobot: Pose2d
get() =
(fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d())
.asPose2d()

val odomTField: Transform2d
get() = fieldFrameEstimator.getLatestOdometryTField()
val lastStableOdomTField: Transform2d
get() = fieldFrameEstimator.getLatestOdometryTField()

private var undriftedPose: Pose2d
get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters)
Expand Down
Loading