From 8a0440af3f7d960133b2b2ff5047c68c4f771c09 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 20 Feb 2024 12:14:36 -0500 Subject: [PATCH 1/9] fix errors --- simgui.json | 9 ++ .../com/team4099/robot2023/BuildConstants.kt | 15 +++ .../commands/drivetrain/TargetPoseCommand.kt | 123 ++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/BuildConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt diff --git a/simgui.json b/simgui.json index d078f7a2..ed7f36cf 100644 --- a/simgui.json +++ b/simgui.json @@ -23,12 +23,18 @@ "open": false }, "transitory": { +<<<<<<< HEAD "AdvantageKit": { "open": true }, "SmartDashboard": { "TunableNumbers": { "Flywheel": { +======= + "SmartDashboard": { + "TunableNumbers": { + "Drivetrain": { +>>>>>>> 58acdb9 (fixed targeting command) "open": true }, "Pathfollow": { @@ -39,8 +45,11 @@ "open": true } } +<<<<<<< HEAD }, "NetworkTables Info": { "visible": true +======= +>>>>>>> 58acdb9 (fixed targeting command) } } diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt new file mode 100644 index 00000000..7024aaae --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -0,0 +1,15 @@ +package com.team4099.robot2023 + +/** + * Automatically generated file containing build version information. + */ +const val MAVEN_GROUP = "" +const val MAVEN_NAME = "Crescendo-2024" +const val VERSION = "unspecified" +const val GIT_REVISION = -1 +const val GIT_SHA = "7bc23edb5afc91c36f4e019483b12aabb97b6947" +const val GIT_DATE = "2024-01-16T15:40:42Z" +const val GIT_BRANCH = "targeting-command-with-drivetrain" +const val BUILD_DATE = "2024-01-16T17:07:48Z" +const val BUILD_UNIX_TIME = 1705442868297L +const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt new file mode 100644 index 00000000..b370d638 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -0,0 +1,123 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableValue +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.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.derived.Angle +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.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI +import kotlin.math.atan2 + +class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { + private val thetaPID: ProfiledPIDController> + + val thetakP = + LoggedTunableValue( + "Pathfollow/thetakP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetakI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val thetaMaxVel = + LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) + val thetaMaxAccel = + LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) + var desiredAngle: Angle = 0.0.degrees + + init { + addRequirements(drivetrain) + + Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) + + if (RobotBase.isReal()) { + thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + } + + thetaPID = + ProfiledPIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) + ) + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset(drivetrain.odometryPose.rotation) + } + + override fun execute() { + Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) + + val currentPose = drivetrain.odometryPose + val relativeToRobotPose = targetPose.relativeTo(currentPose) + desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians + + val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), + fieldOriented = true + ) + + Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) + Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) + } + + override fun isFinished(): Boolean { + return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < + DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR + } + + override fun end(interrupted: Boolean) { + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) + ) + } +} From dbf164e96a7309185dee1c219188c1311bfb9825 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 20 Feb 2024 12:56:27 -0500 Subject: [PATCH 2/9] make driving allowed when targeting a pose --- simgui.json | 30 ------------------- .../com/team4099/robot2023/BuildConstants.kt | 10 +++---- .../com/team4099/robot2023/RobotContainer.kt | 5 ++++ .../commands/drivetrain/TargetPoseCommand.kt | 12 ++++++-- .../config/constants/FieldConstants.kt | 12 ++++++++ .../subsystems/superstructure/Request.kt | 2 +- 6 files changed, 33 insertions(+), 38 deletions(-) diff --git a/simgui.json b/simgui.json index ed7f36cf..69d2c9e6 100644 --- a/simgui.json +++ b/simgui.json @@ -1,16 +1,4 @@ { - "HALProvider": { - "Other Devices": { - "window": { - "visible": false - } - }, - "Timing": { - "window": { - "visible": false - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -19,24 +7,9 @@ } }, "NetworkTables": { - "Persistent Values": { - "open": false - }, "transitory": { -<<<<<<< HEAD - "AdvantageKit": { - "open": true - }, "SmartDashboard": { "TunableNumbers": { - "Flywheel": { -======= - "SmartDashboard": { - "TunableNumbers": { - "Drivetrain": { ->>>>>>> 58acdb9 (fixed targeting command) - "open": true - }, "Pathfollow": { "open": true }, @@ -45,11 +18,8 @@ "open": true } } -<<<<<<< HEAD }, "NetworkTables Info": { "visible": true -======= ->>>>>>> 58acdb9 (fixed targeting command) } } diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 7024aaae..54571c47 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = -1 -const val GIT_SHA = "7bc23edb5afc91c36f4e019483b12aabb97b6947" -const val GIT_DATE = "2024-01-16T15:40:42Z" +const val GIT_REVISION = 267 +const val GIT_SHA = "8a0440af3f7d960133b2b2ff5047c68c4f771c09" +const val GIT_DATE = "2024-02-20T12:14:36Z" const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-01-16T17:07:48Z" -const val BUILD_UNIX_TIME = 1705442868297L +const val BUILD_DATE = "2024-02-20T12:54:28Z" +const val BUILD_UNIX_TIME = 1708451668558L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c638a280..cabc487b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,9 +2,11 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand +import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim @@ -33,7 +35,9 @@ import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase +import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband +import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -161,6 +165,7 @@ object RobotContainer { ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) + ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FieldConstants.centerSpeakerOpening)) /* TUNING COMMANDS diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index b370d638..c5c1d336 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -2,8 +2,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.FMSData +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -29,8 +32,12 @@ import org.team4099.lib.units.perSecond import kotlin.math.PI import kotlin.math.atan2 -class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { +class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Command() { private val thetaPID: ProfiledPIDController> + private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y } + val targetPose = if (FMSData.allianceColor == DriverStation.Alliance.Red) { + Pose2d(FieldConstants.fieldLength - targetPose.x, targetPose.y, targetPose.rotation) + } else targetPose val thetakP = LoggedTunableValue( @@ -92,6 +99,7 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) val currentPose = drivetrain.odometryPose + val currentFieldVelocity = fieldVelocitySupplier() val relativeToRobotPose = targetPose.relativeTo(currentPose) desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians @@ -100,7 +108,7 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), + currentFieldVelocity, fieldOriented = true ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 9519e3e9..9d99f699 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -1,7 +1,19 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.geometry.Pose2d import org.team4099.lib.units.base.feet +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.degrees object FieldConstants { val fieldLength = 54.feet + + val bottomRightSpeaker = Pose2d(0.0.inches, 238.815.inches, 0.degrees) + val bottomLeftSpeaker = Pose2d(0.0.inches, 197.765.inches, 0.degrees) + val topRightSpeaker = Pose2d(18.055.inches, 238.815.inches, 0.degrees) + val topLeftSpeaker = Pose2d(18.055.inches, 197.765.inches, 0.degrees) + + // Center of the speaker opening for the blue alliance + val centerSpeakerOpening = bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5.seconds) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index cdfd1c29..b66320e6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -33,7 +33,7 @@ sealed interface Request { sealed interface DrivetrainRequest : Request { class OpenLoop( - val angularVelocity: AngularVelocity, + var angularVelocity: AngularVelocity, val driveVector: Pair, val fieldOriented: Boolean = true ) : DrivetrainRequest From 9c15637fa685debeef1ca3d1b352b9cb2bb4f319 Mon Sep 17 00:00:00 2001 From: Parth Oza <3963740+plusparth@users.noreply.github.com> Date: Mon, 19 Feb 2024 16:57:01 -0800 Subject: [PATCH 3/9] Use field frame estimator in drivetrain (#24) forcing reupload Topic: field-frame-drivetrain-2 Relative: refactor-vision-update Reviewers: saraansh, rithvik, pranav --- .../kotlin/com/team4099/lib/math/GeomUtil.kt | 8 + .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../commands/drivetrain/DrivePathCommand.kt | 5 +- .../FollowPathPlannerPathCommand.kt | 10 +- .../commands/drivetrain/ResetPoseCommand.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 200 ++++++++---------- 6 files changed, 103 insertions(+), 126 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt index 7fc92d99..c3de20af 100644 --- a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt @@ -39,3 +39,11 @@ fun Pose2d.purelyTranslateBy(translation2d: Translation2d): Pose2d { fun Pose2d.asTransform2d(): Transform2d { return Transform2d(Pose2d(0.meters, 0.meters, 0.radians), this) } + +/** + * Returns pose of whatever the transform represents, in the original frame of the transform. For + * example, odomTRobot.asPose2d() would give the pose of the robot in the odometry frame. + */ +fun Transform2d.asPose2d(): Pose2d { + return Pose2d(this.translation, this.rotation) +} diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index cabc487b..22dd63d6 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -93,8 +93,8 @@ object RobotContainer { } superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel) - vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) - limelight.poseSupplier = { drivetrain.odometryPose } + vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) + limelight.poseSupplier = { drivetrain.odomTRobot } } fun mapDefaultCommands() { 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 454a69be..ed604241 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.lib.pathfollow.Trajectory import com.team4099.lib.trajectory.CustomHolonomicDriveController import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint @@ -227,9 +226,7 @@ class DrivePathCommand( desiredState.curvatureRadPerMeter.radians.sin var nextDriveState = - swerveDriveController.calculate( - drivetrain.odometryPose.pose2d, desiredState, desiredRotation - ) + swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation) if (leaveOutYAdjustment) { nextDriveState = diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index e9c14ed4..bf58d0a0 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -53,9 +53,9 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla private val atReference: Boolean get() = ( - (lastSampledPose.translation - drivetrain.odometryPose.translation).magnitude.meters <= + (lastSampledPose.translation - drivetrain.odomTRobot.translation).magnitude.meters <= translationToleranceAtEnd && - (lastSampledPose.rotation - drivetrain.odometryPose.rotation).absoluteValue <= + (lastSampledPose.rotation - drivetrain.odomTRobot.rotation).absoluteValue <= thetaToleranceAtEnd ) @@ -176,7 +176,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla } val currentSpeeds = drivetrain.targetedChassisSpeeds - val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds + val poseRotation = drivetrain.odomTRobot.rotation.inRotation2ds val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) // Sampling the trajectory for a state that we're trying to target @@ -194,13 +194,13 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla Logger.recordOutput( "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees ) - Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees) lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) val targetedChassisSpeeds = swerveDriveController.calculateRobotRelativeSpeeds( - drivetrain.odometryPose, stateFromTrajectory + drivetrain.odomTRobot, stateFromTrajectory ) // Set closed loop request diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 305c8f00..96c58b6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -13,7 +13,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() } override fun initialize() { - drivetrain.odometryPose = AllianceFlipUtil.apply(pose) + drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) Logger.recordOutput( "Drivetrain/lastResetPose", Pose2dWPILIB.struct, AllianceFlipUtil.apply(pose).pose2d ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index faed7283..8105f929 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -2,6 +2,8 @@ package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.math.asPose2d +import com.team4099.lib.math.asTransform2d import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants @@ -10,7 +12,7 @@ import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData -import com.team4099.robot2023.util.PoseEstimator +import com.team4099.robot2023.util.FieldFrameEstimator import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.kinematics.SwerveDriveKinematics @@ -34,7 +36,6 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds -import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -49,42 +50,47 @@ import java.util.concurrent.locks.ReentrantLock import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = + private val gyroNotConnectedAlert = Alert( "Gyro is not connected, field relative driving will be significantly worse.", Alert.AlertType.ERROR ) val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - val gyroInputs = GyroIO.GyroIOInputs() + private val gyroInputs = GyroIO.GyroIOInputs() - var gyroYawOffset = 0.0.radians + private var gyroYawOffset = 0.0.radians - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) + // TODO: tune these + private var fieldFrameEstimator = FieldFrameEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) - var angularVelocityTarget = 0.degrees.perSecond + private var angularVelocityTarget = 0.degrees.perSecond - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + private var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - var isFieldOrientated = true // true denotes that when driving, it'll be field oriented. + private var isFieldOriented = true var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private set var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private set var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + private var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + private set - var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + private var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var omegaVelocity = 0.0.radians.perSecond + private var omegaVelocity = 0.0.radians.perSecond var lastGyroYaw = 0.0.radians var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + private set private val testAngle = LoggedTunableValue("Drivetrain/testAngle", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) @@ -95,7 +101,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB val closestAlignmentAngle: Angle get() { for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + if ((odomTRobot.rotation - angle.degrees).absoluteValue <= 45.degrees) { return angle.degrees } } @@ -108,7 +114,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB is DrivetrainRequest.OpenLoop -> { angularVelocityTarget = value.angularVelocity targetedDriveVector = value.driveVector - isFieldOrientated = value.fieldOriented + isFieldOriented = value.fieldOriented } is DrivetrainRequest.ClosedLoop -> { targetedChassisSpeeds = value.chassisSpeeds @@ -144,7 +150,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation ) - val swerveDriveKinematics = + private val swerveDriveKinematics = SwerveDriveKinematics( frontLeftWheelLocation.translation2d, frontRightWheelLocation.translation2d, @@ -152,43 +158,51 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation.translation2d ) - var swerveDriveOdometry = + private var swerveDriveOdometry = SwerveDriveOdometry( swerveDriveKinematics, gyroInputs.gyroYaw.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray() ) - var setPointStates = + private var undriftedSwerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + private var setPointStates = mutableListOf( SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() ) - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - set(value) { - swerveDrivePoseEstimator.resetPose(value) + val odomTRobot: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + // 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() - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) + val odomTField: Transform2d + get() = fieldFrameEstimator.getLatestOdometryTField() + + private var undriftedPose: Pose2d + get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters) set(value) { - swerveDriveOdometry.resetPosition( + undriftedSwerveDriveOdometry.resetPosition( gyroInputs.gyroYaw.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray(), value.pose2d ) } - var rawGyroAngle = odometryPose.rotation + private var rawGyroAngle = odomTRobot.rotation - val lastModulePositions = - mutableListOf( + private val lastModulePositions = + arrayOf( SwerveModulePosition(), SwerveModulePosition(), SwerveModulePosition(), @@ -231,7 +245,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Translation2d( chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + .rotateBy(odomTRobot.rotation) // we don't use this but it's there if you want it ig robotVelocity = Velocity2d(chassisState.vx, chassisState.vy) fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) @@ -246,7 +260,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB // updating odometry every loop cycle updateOdometry() - Logger.recordOutput("Drivetrain/OdometryGyroRotationValue", odometryPose.rotation.inDegrees) + Logger.recordOutput("Drivetrain/OdometryGyroRotationValue", odomTRobot.rotation.inDegrees) Logger.recordOutput( "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond @@ -264,9 +278,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.processInputs("Drivetrain/Gyro", gyroInputs) Logger.recordOutput( VisionConstants.POSE_TOPIC_NAME, - doubleArrayOf( - odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians - ) + doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) @@ -274,21 +286,21 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput( VisionConstants.POSE_TOPIC_NAME, - doubleArrayOf( - odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians - ) + doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) Logger.recordOutput( "Odometry/pose3d", Pose3d( - odometryPose.x, - odometryPose.y, + odomTRobot.x, + odomTRobot.y, 1.0.meters, Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) ) .pose3d ) + Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) + Logger.recordOutput( "Odometry/targetPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) @@ -322,7 +334,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } DrivetrainState.OPEN_LOOP -> { // Outputs - setOpenLoop(angularVelocityTarget, targetedDriveVector, isFieldOrientated) + setOpenLoop(angularVelocityTarget, targetedDriveVector, isFieldOriented) Logger.recordOutput( "Drivetrain/TargetVelocityX", targetedDriveVector.first.inMetersPerSecond ) @@ -348,57 +360,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } private fun updateOdometry() { - - // // find device with min data updates and update odometry with that many deltas - // var deltaCount = - // if (gyroInputs.gyroConnected) gyroInputs.odometryYawPositions.size else - // Integer.MAX_VALUE - // for (i in 0..3) { - // deltaCount = Math.min(deltaCount, swerveModules[i].positionDeltas.size) - // } - // - // // update odometry pose for each delta - // for (deltaIndex in 0..deltaCount - 1) { - // // Read wheel deltas from each module - // val wheelDeltas = arrayOfNulls(4) - // for (moduleIndex in 0..3) { - // wheelDeltas[moduleIndex] = swerveModules[moduleIndex].positionDeltas.removeFirst() - // } - // - // // generate twist from wheel and gyro deltas - // var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas) - // if (gyroInputs.gyroConnected) { - // val currentGyroYaw = gyroInputs.rawGyroYaw - // Logger.recordOutput("Drivetrain/yeeYaw", gyroInputs.rawGyroYaw.inDegrees) - // Logger.recordOutput("Drivetrain/lastYeeYaw", lastGyroYaw.inDegrees) - // driveTwist = - // edu.wpi.first.math.geometry.Twist2d( - // driveTwist.dx, driveTwist.dy, -(currentGyroYaw - lastGyroYaw).inRadians - // ) - // lastGyroYaw = gyroInputs.rawGyroYaw - // Logger.recordOutput("Drivetrain/dxMeters", driveTwist.dx) - // Logger.recordOutput("Drivetrain/dYMeters", driveTwist.dy) - // Logger.recordOutput("Drivetrain/dThetaDegrees", -(currentGyroYaw - - // lastGyroYaw).inRadians) - // } - // - // swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) - // } - - val wheelDeltas = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) for (i in 0 until 4) { - wheelDeltas[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters - - lastModulePositions[i].distanceMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) lastModulePositions[i] = SwerveModulePosition( swerveModules[i].inputs.drivePosition.inMeters, @@ -406,19 +368,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) } - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } - - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + // TODO(parth): The thing I removed here may have removed support for driving field oriented w/o + // a gyro. Check before merging + // TODO(parth): For this to work our gyro rate coefficient thing needs to be correct + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, lastModulePositions) + fieldFrameEstimator.addDriveData(Clock.fpgaTime, odomTRobot) - // reversing the drift to store the ground truth pose + // reversing the drift to store the sim ground truth pose if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { val undriftedModules = arrayOfNulls(4) for (i in 0 until 4) { @@ -432,9 +388,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules[i].modulePosition.angle ) } - swerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) + undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) - drift = undriftedPose.minus(odometryPose) + drift = undriftedPose.minus(odomTRobot) Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) } } @@ -466,7 +422,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB allianceFlippedDriveVector.first, allianceFlippedDriveVector.second, angularVelocity, - odometryPose.rotation + odomTRobot.rotation ) } else { desiredChassisSpeeds = @@ -587,24 +543,40 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB zeroDrive() } + /** Resets the field frame estimator given some current pose of the robot. */ + fun resetFieldFrameEstimator(fieldTRobot: Pose2d) { + fieldFrameEstimator.resetFieldFrameFilter( + odomTRobot.asTransform2d() + fieldTRobot.asTransform2d().inverse() + ) + } + /** * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. * * @param toAngle Zeros the gyro to the value */ fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - + // TODO(parth): This feels incorrect -- I think the first arg should be the gyro angle and the + // undrifted pose should be updated to toAngle if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( + // NOTE(parth): The gyro itself should never need to be reset in-match on a real robot, the + // odometry can be updated directly + gyroIO.zeroGyroYaw(toAngle) + undriftedSwerveDriveOdometry.resetPosition( toAngle.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray(), undriftedPose.pose2d ) } + // TODO(parth): Update the field frame estimator's transform here too, otherwise it will need to + // re-converge + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + lastModulePositions, + Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d + ) + if (!(gyroInputs.gyroConnected)) { gyroYawOffset = toAngle - rawGyroAngle } @@ -637,7 +609,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + fieldFrameEstimator.addVisionData(visionData) } fun lockWheels() { From 838ad3256fd969d2f1039633931ae6171956c441 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar <63820563+sswadkar@users.noreply.github.com> Date: Mon, 19 Feb 2024 21:24:24 -0500 Subject: [PATCH 4/9] Follow paths in either odo frame or field frame (#30) * Use field frame estimator in drivetrain Topic: field-frame-drivetrain-2 Relative: refactor-vision-update Reviewers: saraansh, rithvik, pranav * allow pathfollowing to follow either frame * follow field coordinates in odo frame * set odoTField on command initialize * disallow shifting by odoTField when in field frame * fix var names * working frame irl * frame path following integration * working frame conversions for path following * progress without path planner * working frame following * do proper calculation for applying path transformation * use drivetrain rotation as starting point if one is not provided * assume coordinates are relative to path frame * Enforce check to make sure waypoints match path frames (#33) * enforce check to make sure waypoints match path frames * end command if things don't agree * rename + don't flip odowaypoint * Compile time errors for conflicting waypoints (#35) * enforce check to make sure waypoints match path frames * end command if things don't agree * rename + don't flip odowaypoint * compile time error for conflicting frame path definitions * remove path frame reference * proper naming for these constructors * spotless pls work --------- Co-authored-by: Parth Oza --- simgui-ds.json | 16 +- .../pathplanner/paths/Example Path.path | 2 +- .../team4099/lib/trajectory/FieldWaypoint.kt | 10 ++ .../lib/trajectory/OdometryWaypoint.kt | 10 ++ .../com/team4099/lib/trajectory/Waypoint.kt | 95 ++++++----- .../com/team4099/robot2023/RobotContainer.kt | 9 +- .../robot2023/auto/AutonomousSelector.kt | 3 +- .../com/team4099/robot2023/auto/PathStore.kt | 10 ++ .../robot2023/auto/mode/TestAutoPath.kt | 16 +- .../commands/drivetrain/DrivePathCommand.kt | 135 +++++++++++++++- .../FollowPathPlannerPathCommand.kt | 153 ++++++++++++++---- .../commands/drivetrain/TeleopDriveCommand.kt | 11 +- .../commands/drivetrain/TestDriveCommand.kt | 25 +++ .../team4099/robot2023/config/ControlBoard.kt | 4 +- .../config/constants/DrivetrainConstants.kt | 10 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 10 ++ .../swervemodule/SwerveModuleIOTalon.kt | 5 +- .../robot2023/util/AllianceFlipUtil.kt | 19 ++- .../com/team4099/robot2023/util/FrameType.kt | 6 + .../com/team4099/robot2023/util/GeomUtil.kt | 6 + 20 files changed, 437 insertions(+), 118 deletions(-) create mode 100644 src/main/kotlin/com/team4099/lib/trajectory/FieldWaypoint.kt create mode 100644 src/main/kotlin/com/team4099/lib/trajectory/OdometryWaypoint.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/util/FrameType.kt diff --git a/simgui-ds.json b/simgui-ds.json index 97f4895e..ba7e2e64 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "System Joysticks": { "window": { "visible": false @@ -20,9 +25,14 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 75, + "incKey": 73 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -49,10 +59,6 @@ { "decKey": 74, "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 } ], "axisCount": 2, diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 4da23a2a..67490b70 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -77,5 +77,5 @@ "reversed": false, "folder": null, "previewStartingState": null, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/trajectory/FieldWaypoint.kt b/src/main/kotlin/com/team4099/lib/trajectory/FieldWaypoint.kt new file mode 100644 index 00000000..b288b95c --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/trajectory/FieldWaypoint.kt @@ -0,0 +1,10 @@ +package com.team4099.lib.trajectory + +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.geometry.Translation2d + +class FieldWaypoint( + override val translation: Translation2d, + override val driveRotation: Rotation2d?, + override val holonomicRotation: Rotation2d? +) : Waypoint() diff --git a/src/main/kotlin/com/team4099/lib/trajectory/OdometryWaypoint.kt b/src/main/kotlin/com/team4099/lib/trajectory/OdometryWaypoint.kt new file mode 100644 index 00000000..182209b2 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/trajectory/OdometryWaypoint.kt @@ -0,0 +1,10 @@ +package com.team4099.lib.trajectory + +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.geometry.Translation2d + +class OdometryWaypoint( + override val translation: Translation2d, + override val driveRotation: Rotation2d?, + override val holonomicRotation: Rotation2d? +) : Waypoint() diff --git a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt index 1bb0eab3..98e8caab 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/Waypoint.kt @@ -1,17 +1,16 @@ package com.team4099.lib.trajectory -import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.util.ErrorMessages import java.util.Optional /** A trajectory waypoint, including a translation and optional drive/holonomic rotations. */ -class Waypoint { +abstract class Waypoint { /** Returns the translation component of the waypoint. */ - val translation: Translation2d - val driveRotation: Rotation2d? - val holonomicRotation: Rotation2d? + open val translation: Translation2d + open val driveRotation: Rotation2d? + open val holonomicRotation: Rotation2d? /** * Constructs a Waypoint with a translation, drive rotation, and holonomic rotation. @@ -57,47 +56,47 @@ class Waypoint { return Optional.ofNullable(holonomicRotation) } - companion object { - /** - * Constucts a Waypoint based on a pose. - * - * @param pose Source pose (where the rotation describes the drive rotation) - */ - fun fromDifferentialPose(pose: Pose2d): Waypoint { - ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") - return Waypoint(pose.translation, pose.rotation, null) - } - - /** - * Constucts a Waypoint based on a pose. - * - * @param pose Source pose (where the rotation describes the drive rotation) - * @param holonomicRotation Holonomic rotation - */ - fun fromDifferentialPose(pose: Pose2d, holonomicRotation: Rotation2d?): Waypoint { - ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") - return Waypoint(pose.translation, pose.rotation, holonomicRotation) - } - - /** - * Constucts a Waypoint based on a pose. - * - * @param pose Source pose (where the rotation describes the holonomic rotation) - */ - fun fromHolonomicPose(pose: Pose2d): Waypoint { - ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") - return Waypoint(pose.translation, null, pose.rotation) - } - - /** - * Constucts a Waypoint based on a pose. - * - * @param pose Source pose (where the rotation describes the holonomic rotation) - * @param driveRotation Drive rotation - */ - fun fromHolonomicPose(pose: Pose2d, driveRotation: Rotation2d?): Waypoint { - ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") - return Waypoint(pose.translation, driveRotation, pose.rotation) - } - } + // companion object { + // /** + // * Constucts a Waypoint based on a pose. + // * + // * @param pose Source pose (where the rotation describes the drive rotation) + // */ + // fun fromDifferentialPose(pose: Pose2d): Waypoint { + // ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") + // return Waypoint(pose.translation, pose.rotation, null) + // } + // + // /** + // * Constucts a Waypoint based on a pose. + // * + // * @param pose Source pose (where the rotation describes the drive rotation) + // * @param holonomicRotation Holonomic rotation + // */ + // fun fromDifferentialPose(pose: Pose2d, holonomicRotation: Rotation2d?): Waypoint { + // ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") + // return Waypoint(pose.translation, pose.rotation, holonomicRotation) + // } + // + // /** + // * Constucts a Waypoint based on a pose. + // * + // * @param pose Source pose (where the rotation describes the holonomic rotation) + // */ + // fun fromHolonomicPose(pose: Pose2d): Waypoint { + // ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") + // return Waypoint(pose.translation, null, pose.rotation) + // } + // + // /** + // * Constucts a Waypoint based on a pose. + // * + // * @param pose Source pose (where the rotation describes the holonomic rotation) + // * @param driveRotation Drive rotation + // */ + // fun fromHolonomicPose(pose: Pose2d, driveRotation: Rotation2d?): Waypoint { + // ErrorMessages.requireNonNullParam(pose, "pose", "Waypoint") + // return Waypoint(pose.translation, driveRotation, pose.rotation) + // } + // } } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 22dd63d6..668cd6f4 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -39,7 +39,6 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.degrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { @@ -102,9 +101,9 @@ object RobotContainer { drivetrain.defaultCommand = TeleopDriveCommand( driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { -1 * ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain ) @@ -154,7 +153,7 @@ object RobotContainer { fun mapTeleopControls() { - ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) + ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index bb6f6847..606e4021 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -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 -> diff --git a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt index 44e951f4..c3a3479c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt @@ -1,7 +1,17 @@ package com.team4099.robot2023.auto import com.pathplanner.lib.path.PathPlannerPath +import com.team4099.robot2023.config.constants.DrivetrainConstants +import org.team4099.lib.pplib.PathPlannerHolonomicDriveController object PathStore { val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("Example Path") + + val constraints = + PathPlannerHolonomicDriveController.Companion.PathConstraints( + DrivetrainConstants.MAX_AUTO_VEL, + DrivetrainConstants.MAX_AUTO_ACCEL, + DrivetrainConstants.STEERING_VEL_MAX, + DrivetrainConstants.STEERING_ACCEL_MAX + ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 86b562d8..541c97d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.auto.mode -import com.team4099.lib.trajectory.Waypoint +import com.team4099.lib.trajectory.OdometryWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.wpilibj2.command.SequentialCommandGroup @@ -14,22 +14,22 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { addRequirements(drivetrain) addCommands( - DrivePathCommand( + DrivePathCommand.createPathInOdometryFrame( drivetrain, { listOf( - Waypoint( - Translation2d(0.0.meters, 0.0.meters).translation2d, + OdometryWaypoint( + Translation2d(5.0.meters, 2.0.meters).translation2d, null, 0.0.degrees.inRotation2ds ), - Waypoint( - Translation2d(2.0.meters, 0.0.meters).translation2d, + OdometryWaypoint( + Translation2d(7.0.meters, 2.0.meters).translation2d, null, 90.0.degrees.inRotation2ds ), - Waypoint( - Translation2d(0.0.meters, 1.0.meters).translation2d, + OdometryWaypoint( + Translation2d(7.0.meters, 3.0.meters).translation2d, null, 0.0.degrees.inRotation2ds ), 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 ed604241..22b8e298 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -1,12 +1,19 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.logging.toDoubleArray +import com.team4099.lib.math.asPose2d +import com.team4099.lib.math.asTransform2d import com.team4099.lib.trajectory.CustomHolonomicDriveController import com.team4099.lib.trajectory.CustomTrajectoryGenerator +import com.team4099.lib.trajectory.FieldWaypoint +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.subsystems.superstructure.Request import com.team4099.robot2023.util.AllianceFlipUtil +import com.team4099.robot2023.util.FrameType import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.SwerveDriveKinematics @@ -19,6 +26,8 @@ import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels import org.team4099.lib.units.Velocity @@ -56,16 +65,20 @@ import java.util.function.Supplier import kotlin.math.PI import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest -class DrivePathCommand( +class DrivePathCommand +private constructor( val drivetrain: Drivetrain, - private val waypoints: Supplier>, + private val waypoints: Supplier>, val resetPose: Boolean = false, val keepTrapping: Boolean = false, val flipForAlliances: Boolean = true, val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, val endVelocity: Velocity2d = Velocity2d(), + var stateFrame: FrameType = FrameType.ODOMETRY, + var pathFrame: FrameType = FrameType.FIELD, ) : Command() { + private val xPID: PIDController> private val yPID: PIDController> @@ -128,6 +141,15 @@ class DrivePathCommand( ) ) + private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) + private var lastSampledPose = Pose2d() + private lateinit var pathTransform: Transform2d + + private var drivePoseSupplier: () -> Pose2d + private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees) + + private var errorString = "" + private fun generate( waypoints: List, constraints: List = listOf() @@ -173,6 +195,15 @@ class DrivePathCommand( thetaPID.enableContinuousInput(-PI.radians, PI.radians) + when (stateFrame) { + FrameType.ODOMETRY -> drivePoseSupplier = { drivetrain.odomTRobot } + FrameType.FIELD -> { + drivePoseSupplier = { drivetrain.fieldTRobot } + // if we're already in field frame we do not want to shift by `odoTField` again + pathFrame = FrameType.FIELD + } + } + swerveDriveController = CustomHolonomicDriveController( xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController @@ -182,6 +213,13 @@ class DrivePathCommand( } override fun initialize() { + odoTField = drivetrain.odomTField + pathTransform = + Transform2d( + Translation2d(waypoints.get()[0].translation), + waypoints.get()[0].driveRotation?.radians?.radians ?: drivePoseSupplier().rotation + ) + // trajectory generation! generate(waypoints.get()) @@ -213,6 +251,41 @@ class DrivePathCommand( var desiredRotation = trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds) + val targetHolonomicPose = + Pose2d( + desiredState.poseMeters.x.meters, + desiredState.poseMeters.y.meters, + desiredRotation.position.radians.radians + ) + + val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() + if (pathFrame == stateFrame) { + lastSampledPose = targetHolonomicPose + // pathTransform.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + } else { + when (pathFrame) { + FrameType.ODOMETRY -> + lastSampledPose = + odoTField.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + FrameType.FIELD -> + lastSampledPose = odoTField.asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + } + } + // flip + lastSampledPose = AllianceFlipUtil.apply(lastSampledPose) + + Logger.recordOutput( + "Pathfollow/frameSpecificTargetPose", lastSampledPose.toDoubleArray().toDoubleArray() + ) + val pathFrameTRobotPose = robotPoseInSelectedFrame + Logger.recordOutput( + "Pathfollow/fieldTRobotVisualized", pathFrameTRobotPose.toDoubleArray().toDoubleArray() + ) + Logger.recordOutput( + "Pathfollow/fieldTRobotTargetVisualized", + targetHolonomicPose.toDoubleArray().toDoubleArray() + ) + if (flipForAlliances) { desiredState = AllianceFlipUtil.apply(desiredState) desiredRotation = AllianceFlipUtil.apply(desiredRotation) @@ -226,7 +299,7 @@ class DrivePathCommand( desiredState.curvatureRadPerMeter.radians.sin var nextDriveState = - swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation) + swerveDriveController.calculate(pathFrameTRobotPose.pose2d, desiredState, desiredRotation) if (leaveOutYAdjustment) { nextDriveState = @@ -303,13 +376,14 @@ class DrivePathCommand( override fun isFinished(): Boolean { trajCurTime = Clock.fpgaTime - trajStartTime return endPathOnceAtReference && - (!keepTrapping || swerveDriveController.atReference()) && + (swerveDriveController.atReference()) && trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds } override fun end(interrupted: Boolean) { Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { + DriverStation.reportError(errorString, true) // Stop where we are if interrupted drivetrain.currentRequest = DrivetrainRequest.OpenLoop( @@ -325,4 +399,57 @@ class DrivePathCommand( ) } } + + companion object { + operator fun invoke() { + return + } + fun createPathInOdometryFrame( + drivetrain: Drivetrain, + waypoints: Supplier>, + resetPose: Boolean = false, + keepTrapping: Boolean = false, + flipForAlliances: Boolean = true, + endPathOnceAtReference: Boolean = true, + leaveOutYAdjustment: Boolean = false, + endVelocity: Velocity2d = Velocity2d(), + stateFrame: FrameType = FrameType.ODOMETRY, + ): DrivePathCommand = + DrivePathCommand( + drivetrain, + waypoints, + resetPose, + keepTrapping, + flipForAlliances, + endPathOnceAtReference, + leaveOutYAdjustment, + endVelocity, + stateFrame, + FrameType.ODOMETRY + ) + + fun createPathInFieldFrame( + drivetrain: Drivetrain, + waypoints: Supplier>, + resetPose: Boolean = false, + keepTrapping: Boolean = false, + flipForAlliances: Boolean = true, + endPathOnceAtReference: Boolean = true, + leaveOutYAdjustment: Boolean = false, + endVelocity: Velocity2d = Velocity2d(), + stateFrame: FrameType = FrameType.ODOMETRY, + ): DrivePathCommand = + DrivePathCommand( + drivetrain, + waypoints, + resetPose, + keepTrapping, + flipForAlliances, + endPathOnceAtReference, + leaveOutYAdjustment, + endVelocity, + stateFrame, + FrameType.FIELD + ) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index bf58d0a0..eaca7e80 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -1,14 +1,22 @@ package com.team4099.robot2023.commands.drivetrain import com.pathplanner.lib.path.PathPlannerPath +import com.pathplanner.lib.path.PathPlannerTrajectory import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.logging.toDoubleArray +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 com.team4099.robot2023.util.FrameType +import com.team4099.robot2023.util.inverse +import edu.wpi.first.math.geometry.Rotation2d 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 @@ -38,8 +46,12 @@ import org.team4099.lib.units.derived.perMeterSeconds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond -class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPlannerPath) : - Command() { +class FollowPathPlannerPathCommand( + val drivetrain: Drivetrain, + val path: PathPlannerPath, + var stateFrame: FrameType = FrameType.ODOMETRY, + var pathFrame: FrameType = FrameType.FIELD, +) : Command() { private val translationToleranceAtEnd = 1.inches private val thetaToleranceAtEnd = 2.5.degrees @@ -50,12 +62,16 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) private var lastSampledPose = Pose2d() + + private var drivePoseSupplier: () -> Pose2d + private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees) + private val atReference: Boolean get() = ( - (lastSampledPose.translation - drivetrain.odomTRobot.translation).magnitude.meters <= + (lastSampledPose.translation - drivePoseSupplier().translation).magnitude.meters <= translationToleranceAtEnd && - (lastSampledPose.rotation - drivetrain.odomTRobot.rotation).absoluteValue <= + (lastSampledPose.rotation - drivePoseSupplier().rotation).absoluteValue <= thetaToleranceAtEnd ) @@ -111,6 +127,8 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla var translationPID: PathPlannerTranslationPID var rotationPID: PathPlannerRotationPID + // private val thetaPID: PIDController> + // private val posPID: PIDController> // private val pathConstraints = PathPlannerHolonomicDriveController.Companion.PathConstraints( // maxVelocity = DrivetrainConstants.MAX_AUTO_VEL, // maxAcceleration = DrivetrainConstants.MAX_AUTO_ACCEL, @@ -121,6 +139,15 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla init { addRequirements(drivetrain) + when (stateFrame) { + FrameType.ODOMETRY -> drivePoseSupplier = { drivetrain.odomTRobot } + FrameType.FIELD -> { + drivePoseSupplier = { drivetrain.fieldTRobot } + // if we're already in field frame we do not want to shift by `odoTField` again + pathFrame = FrameType.FIELD + } + } + translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get()) rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get()) @@ -138,29 +165,33 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla ) } + private lateinit var currentSpeeds: ChassisSpeeds + private lateinit var poseRotation: Rotation2d + private lateinit var generatedTrajectory: PathPlannerTrajectory + + private lateinit var pathTransform: Transform2d + override fun initialize() { trajStartTime = Clock.fpgaTime + odoTField = drivetrain.odomTField + + currentSpeeds = drivetrain.targetedChassisSpeeds + poseRotation = drivePoseSupplier().rotation.inRotation2ds + generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) + pathTransform = Pose2d(path.previewStartingHolonomicPose).asTransform2d() } override fun execute() { - if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { + if (thetakP.hasChanged() || + thetakI.hasChanged() || + thetakD.hasChanged() || + poskP.hasChanged() || + poskI.hasChanged() || + poskD.hasChanged() + ) { + translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get()) rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get()) - swerveDriveController = - PathPlannerHolonomicDriveController( - translationPID, - rotationPID, - DrivetrainConstants.MAX_AUTO_VEL, - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, - DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - .magnitude - .meters, - ) - } - if (poskP.hasChanged() || poskI.hasChanged() || poskD.hasChanged()) { - translationPID = PathPlannerTranslationPID(poskP.get(), poskI.get(), poskD.get()) swerveDriveController = PathPlannerHolonomicDriveController( translationPID, @@ -191,20 +222,80 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla lastSampledPose.rotation.inRadians ) ) - Logger.recordOutput( - "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees - ) - Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees) + val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier() + if (pathFrame == stateFrame) { + // odoTField x fieldTRobot + lastSampledPose = + (Pose2d(stateFromTrajectory.targetHolonomicPose) - pathTransform.asPose2d()).asPose2d() + } else { + when (pathFrame) { + FrameType.ODOMETRY -> + lastSampledPose = + odoTField + .asPose2d() + .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d()) + FrameType.FIELD -> + lastSampledPose = + odoTField + .inverse() + .asPose2d() + .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d()) + } + } - lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) + Logger.recordOutput("Pathfollow/thetaSetpointDegrees", lastSampledPose.rotation.inDegrees) + Logger.recordOutput("Pathfollow/currentThetaDegrees", drivePoseSupplier().rotation.inDegrees) - val targetedChassisSpeeds = - swerveDriveController.calculateRobotRelativeSpeeds( - drivetrain.odomTRobot, stateFromTrajectory - ) + // var targetedChassisSpeeds = + // swerveDriveController.calculateRobotRelativeSpeeds( + // (robotPoseInSelectedFrame + pathTransform), stateFromTrajectory + // ) + + val pathFrameTRobotPose = (robotPoseInSelectedFrame + pathTransform) + + val targettedSpeeds = + swerveDriveController.calculateRobotRelativeSpeeds(pathFrameTRobotPose, stateFromTrajectory) + // // Calculate feedforward velocities (field-relative). + // val xFF = (stateFromTrajectory.velocityMps * + // stateFromTrajectory.heading.cos).meters.perSecond + // val yFF = (stateFromTrajectory.velocityMps * + // stateFromTrajectory.heading.sin).meters.perSecond + // + // // Calculate feedback velocities (based on position error). + // val xFeedback = + // posPID.calculate(pathFrameTRobotPose.x, stateFromTrajectory.positionMeters.x.meters) + // val yFeedback = + // posPID.calculate(pathFrameTRobotPose.y, stateFromTrajectory.positionMeters.y.meters) + // val thetaFeedback = + // thetaPID.calculate( + // pathFrameTRobotPose.rotation, + // stateFromTrajectory.targetHolonomicRotation.radians.radians + // ) + // + // // Return next output. + // val targetedChassisSpeeds = + // org.team4099.lib.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( + // xFF + xFeedback, yFF + yFeedback, thetaFeedback, pathFrameTRobotPose.rotation + // ) + // + // Logger.recordOutput("Pathfollow/yFF+F", (yFF + yFeedback).inMetersPerSecond) + // Logger.recordOutput("Pathfollow/xFF+F", (xFF + xFeedback).inMetersPerSecond) + // Logger.recordOutput("Pathfollow/thetaFeedback", (thetaFeedback).inDegreesPerSecond) + // Logger.recordOutput( + // "Pathfollow/thetaFeedbackNorm", targetedChassisSpeeds.omega.inDegreesPerSecond + // ) + + Logger.recordOutput( + "Pathfollow/fieldTRobotVisualized", + (robotPoseInSelectedFrame + pathTransform).toDoubleArray().toDoubleArray() + ) + Logger.recordOutput( + "Pathfollow/fieldTRobotTargetVisualized", + Pose2d(stateFromTrajectory.targetHolonomicPose).toDoubleArray().toDoubleArray() + ) // Set closed loop request - pathFollowRequest.chassisSpeeds = targetedChassisSpeeds.chassisSpeedsWPILIB + pathFollowRequest.chassisSpeeds = targettedSpeeds.chassisSpeedsWPILIB drivetrain.currentRequest = pathFollowRequest // Update trajectory time @@ -218,7 +309,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla // trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds // } override fun isFinished(): Boolean { - return atReference + return atReference && trajCurTime.inSeconds > generatedTrajectory.totalTimeSeconds } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 6a09a062..4ed0d7ce 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.driver.DriverProfile +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -22,10 +23,12 @@ class TeleopDriveCommand( override fun initialize() {} override fun execute() { - val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) - val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) - Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) + if (DriverStation.isTeleop()) { + val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) + val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) + Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) + } } override fun isFinished(): Boolean { return false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt new file mode 100644 index 00000000..06de519e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt @@ -0,0 +1,25 @@ +package com.team4099.robot2023.commands.drivetrain + +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.units.derived.inRotation2ds + +class TestDriveCommand(val drivetrain: Drivetrain) : Command() { + + override fun execute() { + drivetrain.currentRequest = + Request.DrivetrainRequest.ClosedLoop( + ChassisSpeeds.fromFieldRelativeSpeeds( + 4.0, 0.0, 0.0, drivetrain.odomTRobot.rotation.inRotation2ds + ) + ) + Logger.recordOutput("ActiveCommands/TestDriveCommand", true) + } + + override fun isFinished(): Boolean { + return false + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 85d8e866..3a0019ff 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -21,10 +21,10 @@ object ControlBoard { } val strafe: Double - get() = -driver.leftXAxis + get() = driver.leftXAxis val forward: Double - get() = -driver.leftYAxis + get() = driver.leftYAxis val turn: Double get() = driver.rightXAxis diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index daf59558..51f6e70c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -30,7 +30,7 @@ import kotlin.math.sqrt object DrivetrainConstants { const val FOC_ENABLED = false - const val MINIMIZE_SKEW = true + const val MINIMIZE_SKEW = false const val OMOMETRY_UPDATE_FREQUENCY = 250.0 @@ -50,8 +50,8 @@ object DrivetrainConstants { .perSecond // 648 // cruise velocity and accel for steering motor - val STEERING_VEL_MAX = 1393.2.degrees.perSecond - val STEERING_ACCEL_MAX = 13932.degrees.perSecond.perSecond + val STEERING_VEL_MAX = 270.degrees.perSecond + val STEERING_ACCEL_MAX = 600.degrees.perSecond.perSecond const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value @@ -126,7 +126,7 @@ object DrivetrainConstants { val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = 1.degrees.perSecond / 1.degrees + val AUTO_THETA_PID_KP = 15.degrees.perSecond / 1.degrees val AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val AUTO_THETA_PID_KD = (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond @@ -151,7 +151,7 @@ object DrivetrainConstants { val STEERING_KFF = 0.0.volts / 1.0.radians.perSecond // 0.0375 - val DRIVE_KP = 2.6829.volts / 1.meters.perSecond + val DRIVE_KP = 0.0.volts / 1.meters.perSecond val DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) val DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 8105f929..f08c8c25 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.logging.toDoubleArray import com.team4099.lib.math.asPose2d import com.team4099.lib.math.asTransform2d import com.team4099.lib.vision.TimestampedVisionUpdate @@ -14,6 +15,7 @@ import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.FieldFrameEstimator import com.team4099.robot2023.util.Velocity2d +import com.team4099.robot2023.util.inverse import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveDriveOdometry @@ -301,6 +303,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) + Logger.recordOutput( + "FieldFrameEstimator/odomTField", odomTField.toDoubleArray().toDoubleArray() + ) + Logger.recordOutput( "Odometry/targetPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) @@ -577,6 +583,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d ) + fieldFrameEstimator.resetFieldFrameFilter( + Transform2d(odomTField.translation, gyroInputs.gyroYaw) + ) + if (!(gyroInputs.gyroConnected)) { gyroYawOffset = toAngle - rawGyroAngle } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 032c87a2..a588dd40 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.controls.VelocityDutyCycle import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants @@ -123,7 +124,7 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 + driveConfiguration.Slot0.kV = 0.12679 / 15 // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes @@ -137,6 +138,7 @@ class SwerveModuleIOTalon( driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(driveConfiguration) MotorChecker.add( @@ -482,6 +484,7 @@ class SwerveModuleIOTalon( } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } + motorOutputConfig.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(motorOutputConfig) } diff --git a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt index f2d3f810..67323bf0 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/AllianceFlipUtil.kt @@ -1,6 +1,8 @@ package com.team4099.robot2023.util import com.team4099.lib.math.Zone2d +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.lib.trajectory.OdometryWaypoint import com.team4099.lib.trajectory.RotationSequence import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.FieldConstants @@ -70,9 +72,16 @@ object AllianceFlipUtil { } } - fun apply(waypoint: Waypoint): Waypoint { + /* + Don't flip an OdometryWaypoint because it's done relative to robot origin + */ + fun apply(waypoint: OdometryWaypoint): Waypoint { + return waypoint + } + + fun apply(waypoint: FieldWaypoint): Waypoint { return if (shouldFlip()) { - Waypoint(apply(waypoint.translation), waypoint.driveRotation, waypoint.holonomicRotation) + FieldWaypoint(apply(waypoint.translation), waypoint.driveRotation, waypoint.holonomicRotation) } else { waypoint } @@ -120,6 +129,10 @@ object AllianceFlipUtil { } private fun shouldFlip(): Boolean { - return DriverStation.getAlliance().get() == Alliance.Red + if (DriverStation.getAlliance().isPresent) { + return DriverStation.getAlliance().get() == Alliance.Red + } else { + return false + } } } diff --git a/src/main/kotlin/com/team4099/robot2023/util/FrameType.kt b/src/main/kotlin/com/team4099/robot2023/util/FrameType.kt new file mode 100644 index 00000000..dcf4324e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/FrameType.kt @@ -0,0 +1,6 @@ +package com.team4099.robot2023.util + +enum class FrameType { + ODOMETRY, + FIELD +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt b/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt index fcca9ced..9d962728 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/GeomUtil.kt @@ -1,5 +1,7 @@ package com.team4099.robot2023.util +import com.team4099.lib.math.asPose2d +import com.team4099.lib.math.asTransform2d import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Rotation3d @@ -21,6 +23,10 @@ fun multiplyTwist(twist: Twist2d, factor: Double): Twist2d { return Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor) } +fun Pose2d.inverse(): Pose2d { + return this.asTransform2d().inverse().asPose2d() +} + fun Pose2d.toPose3d(): Pose3d { return Pose3d(this.x, this.y, 0.0.meters, Rotation3d(0.0.degrees, 0.0.degrees, this.rotation)) } From 96ed7f18f2fa925e51a2ad4c01a4ceb9e6d52913 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 20 Feb 2024 18:43:35 -0500 Subject: [PATCH 5/9] change to odo/field frames --- simgui-ds.json | 5 --- .../com/team4099/robot2023/BuildConstants.kt | 10 ++--- .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../commands/drivetrain/TargetPoseCommand.kt | 37 +++++++++++++------ .../robot2023/util/FrameCoordinate.kt | 30 +++++++++++++++ 5 files changed, 63 insertions(+), 23 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt diff --git a/simgui-ds.json b/simgui-ds.json index ba7e2e64..5ea3819d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "System Joysticks": { "window": { "visible": false diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 54571c47..cb2665e9 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 267 -const val GIT_SHA = "8a0440af3f7d960133b2b2ff5047c68c4f771c09" -const val GIT_DATE = "2024-02-20T12:14:36Z" +const val GIT_REVISION = 270 +const val GIT_SHA = "441e0228f8376921c8ade0367010a84d394f3994" +const val GIT_DATE = "2024-02-20T15:57:31Z" const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-02-20T12:54:28Z" -const val BUILD_UNIX_TIME = 1708451668558L +const val BUILD_DATE = "2024-02-20T18:35:21Z" +const val BUILD_UNIX_TIME = 1708472121142L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 668cd6f4..eb84ee97 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -33,6 +33,7 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim +import com.team4099.robot2023.util.FrameCoordinate import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.geometry.Pose2d @@ -94,6 +95,7 @@ object RobotContainer { superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel) vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) limelight.poseSupplier = { drivetrain.odomTRobot } + FrameCoordinate.robotInOdometryFrameSupplier = { drivetrain.odomTRobot } } fun mapDefaultCommands() { @@ -164,7 +166,7 @@ object RobotContainer { ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) - ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FieldConstants.centerSpeakerOpening)) + ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FrameCoordinate.FieldCoordinate(FieldConstants.centerSpeakerOpening))) /* TUNING COMMANDS diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index c5c1d336..059c1a12 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -1,11 +1,16 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.math.asPose2d import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.FMSData +import com.team4099.robot2023.util.FrameCoordinate +import com.team4099.robot2023.util.FrameType +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.kinematics.Odometry import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command @@ -32,12 +37,12 @@ import org.team4099.lib.units.perSecond import kotlin.math.PI import kotlin.math.atan2 -class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Command() { +class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoordinate) : Command() { private val thetaPID: ProfiledPIDController> private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y } - val targetPose = if (FMSData.allianceColor == DriverStation.Alliance.Red) { - Pose2d(FieldConstants.fieldLength - targetPose.x, targetPose.y, targetPose.rotation) - } else targetPose + val targetCoordinate = if (FMSData.allianceColor == DriverStation.Alliance.Red && targetCoordinate is FrameCoordinate.FieldCoordinate) { + FrameCoordinate.FieldCoordinate(FieldConstants.fieldLength - targetCoordinate.x, targetCoordinate.y) + } else targetCoordinate val thetakP = LoggedTunableValue( @@ -65,12 +70,11 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman val thetaMaxAccel = LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) var desiredAngle: Angle = 0.0.degrees + lateinit var odometryCoordinateToTarget: FrameCoordinate.OdometryCoordinate init { addRequirements(drivetrain) - Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) - if (RobotBase.isReal()) { thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) @@ -92,15 +96,24 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman } override fun initialize() { - thetaPID.reset(drivetrain.odometryPose.rotation) + thetaPID.reset(drivetrain.odomTRobot.rotation) + + odometryCoordinateToTarget = if (targetCoordinate is FrameCoordinate.FieldCoordinate) { + FrameCoordinate.OdometryCoordinate( + drivetrain.odomTField.asPose2d().transformBy(targetCoordinate.toTransform2d()) + ) + } else { + targetCoordinate as FrameCoordinate.OdometryCoordinate // Always true, kotlin type casting sucks sometimes + } } override fun execute() { Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) - val currentPose = drivetrain.odometryPose + val currentPose = drivetrain.odomTRobot val currentFieldVelocity = fieldVelocitySupplier() - val relativeToRobotPose = targetPose.relativeTo(currentPose) + val relativeToRobotPose = odometryCoordinateToTarget.toPose2d().relativeTo(currentPose) + desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) @@ -112,20 +125,20 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman fieldOriented = true ) - Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odomTRobot.rotation.inDegrees) Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) } override fun isFinished(): Boolean { - return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < + return (drivetrain.odomTRobot.rotation - desiredAngle).absoluteValue < DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR } override fun end(interrupted: Boolean) { drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) + 0.0.radians.perSecond, fieldVelocitySupplier() ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt b/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt new file mode 100644 index 00000000..aa457f8d --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt @@ -0,0 +1,30 @@ +package com.team4099.robot2023.util + +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.derived.degrees + +abstract class FrameCoordinate(val x: Length, val y: Length) { + + fun toTransform2d(): Transform2d { + return Transform2d(Translation2d(x, y), 0.degrees) + } + + fun toPose2d(): Pose2d { + return Pose2d(x, y, 0.degrees) + } + + class OdometryCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) { + constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y) + } + + class FieldCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) { + constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y) + } + + companion object { + lateinit var robotInOdometryFrameSupplier: () -> Pose2d + } +} From 17d8c89f309a6f0cd516ad5b977d44793755d524 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 20 Feb 2024 19:22:23 -0500 Subject: [PATCH 6/9] retrieve latest velocity --- .../kotlin/com/team4099/robot2023/BuildConstants.kt | 10 +++++----- .../commands/drivetrain/TargetPoseCommand.kt | 5 +++-- .../commands/drivetrain/TeleopDriveCommand.kt | 13 +++++++++++-- .../subsystems/drivetrain/drive/Drivetrain.kt | 3 +++ 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index cb2665e9..e6093bef 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 270 -const val GIT_SHA = "441e0228f8376921c8ade0367010a84d394f3994" -const val GIT_DATE = "2024-02-20T15:57:31Z" +const val GIT_REVISION = 271 +const val GIT_SHA = "96ed7f18f2fa925e51a2ad4c01a4ceb9e6d52913" +const val GIT_DATE = "2024-02-20T18:43:52Z" const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-02-20T18:35:21Z" -const val BUILD_UNIX_TIME = 1708472121142L +const val BUILD_DATE = "2024-02-20T19:12:40Z" +const val BUILD_UNIX_TIME = 1708474360868L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index 059c1a12..4fa4974e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -121,7 +121,7 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoord drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - currentFieldVelocity, + drivetrain.speedSupplier(), fieldOriented = true ) @@ -138,7 +138,8 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoord override fun end(interrupted: Boolean) { drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, fieldVelocitySupplier() + drivetrain.rotationSupplier(), drivetrain.speedSupplier(), fieldOriented = true ) + Logger.recordOutput("ActiveCommands/TargetPoseCommand", false) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 4ed0d7ce..2831e0f2 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -5,6 +5,10 @@ import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.perSecond import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class TeleopDriveCommand( @@ -15,17 +19,22 @@ class TeleopDriveCommand( val slowMode: () -> Boolean, val drivetrain: Drivetrain ) : Command() { + private lateinit var speed: Pair + private var rotation: AngularVelocity = 0.degrees.perSecond init { addRequirements(drivetrain) + + drivetrain.speedSupplier = { speed } + drivetrain.rotationSupplier = { rotation } } override fun initialize() {} override fun execute() { if (DriverStation.isTeleop()) { - val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) - val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) + speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) + rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index f08c8c25..a426952d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -89,6 +89,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB private var omegaVelocity = 0.0.radians.perSecond + lateinit var speedSupplier: () -> Pair + lateinit var rotationSupplier: () -> AngularVelocity + var lastGyroYaw = 0.0.radians var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED From 12ac1a0a972b798cbf0d1a77c80fee4ed9e0c32c Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 20 Feb 2024 19:35:28 -0500 Subject: [PATCH 7/9] use controller input --- .../com/team4099/robot2023/BuildConstants.kt | 10 +++++----- .../com/team4099/robot2023/RobotContainer.kt | 12 +++++++++++- .../commands/drivetrain/TargetPoseCommand.kt | 17 ++++++++++++++--- .../commands/drivetrain/TeleopDriveCommand.kt | 9 ++------- .../subsystems/drivetrain/drive/Drivetrain.kt | 3 --- 5 files changed, 32 insertions(+), 19 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index e6093bef..a5773a25 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 271 -const val GIT_SHA = "96ed7f18f2fa925e51a2ad4c01a4ceb9e6d52913" -const val GIT_DATE = "2024-02-20T18:43:52Z" +const val GIT_REVISION = 272 +const val GIT_SHA = "17d8c89f309a6f0cd516ad5b977d44793755d524" +const val GIT_DATE = "2024-02-20T19:22:23Z" const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-02-20T19:12:40Z" -const val BUILD_UNIX_TIME = 1708474360868L +const val BUILD_DATE = "2024-02-20T19:30:24Z" +const val BUILD_UNIX_TIME = 1708475424897L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index eb84ee97..c14c6e0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -166,7 +166,17 @@ object RobotContainer { ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) - ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FrameCoordinate.FieldCoordinate(FieldConstants.centerSpeakerOpening))) + ControlBoard.wristTestDown.whileTrue( + TargetPoseCommand( + Ryan(), + { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + FrameCoordinate.FieldCoordinate(FieldConstants.centerSpeakerOpening) + ) + ) /* TUNING COMMANDS diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index 4fa4974e..08784d85 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -9,6 +9,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.FrameCoordinate import com.team4099.robot2023.util.FrameType +import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.math.Matrix import edu.wpi.first.math.kinematics.Odometry import edu.wpi.first.wpilibj.DriverStation @@ -37,7 +38,15 @@ import org.team4099.lib.units.perSecond import kotlin.math.PI import kotlin.math.atan2 -class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoordinate) : Command() { +class TargetPoseCommand( + val driver: DriverProfile, + val driveX: () -> Double, + val driveY: () -> Double, + val turn: () -> Double, + val slowMode: () -> Boolean, + val drivetrain: Drivetrain, + targetCoordinate: FrameCoordinate +) : Command() { private val thetaPID: ProfiledPIDController> private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y } val targetCoordinate = if (FMSData.allianceColor == DriverStation.Alliance.Red && targetCoordinate is FrameCoordinate.FieldCoordinate) { @@ -121,7 +130,7 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoord drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - drivetrain.speedSupplier(), + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), fieldOriented = true ) @@ -138,7 +147,9 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoord override fun end(interrupted: Boolean) { drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( - drivetrain.rotationSupplier(), drivetrain.speedSupplier(), fieldOriented = true + driver.rotationSpeedClampedSupplier(turn, slowMode), + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true ) Logger.recordOutput("ActiveCommands/TargetPoseCommand", false) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 2831e0f2..68428c28 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -19,22 +19,17 @@ class TeleopDriveCommand( val slowMode: () -> Boolean, val drivetrain: Drivetrain ) : Command() { - private lateinit var speed: Pair - private var rotation: AngularVelocity = 0.degrees.perSecond init { addRequirements(drivetrain) - - drivetrain.speedSupplier = { speed } - drivetrain.rotationSupplier = { rotation } } override fun initialize() {} override fun execute() { if (DriverStation.isTeleop()) { - speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) - rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) + val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) + val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index a426952d..f08c8c25 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -89,9 +89,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB private var omegaVelocity = 0.0.radians.perSecond - lateinit var speedSupplier: () -> Pair - lateinit var rotationSupplier: () -> AngularVelocity - var lastGyroYaw = 0.0.radians var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED From 129541b23fe208031dfecda01e1bea6ff34521f9 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Wed, 21 Feb 2024 09:42:07 -0500 Subject: [PATCH 8/9] fix requested changes + targeting command works whenever now --- .../com/team4099/robot2023/BuildConstants.kt | 10 ++++----- .../com/team4099/robot2023/RobotContainer.kt | 19 +++++++++++------ .../robot2023/auto/mode/TestAutoPath.kt | 11 +++------- .../commands/drivetrain/TargetPoseCommand.kt | 21 +++++++++---------- .../config/constants/DrivetrainConstants.kt | 4 ++-- .../robot2023/util/FrameCoordinate.kt | 17 ++++++++++++++- 6 files changed, 49 insertions(+), 33 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index a5773a25..f32b9ccc 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 272 -const val GIT_SHA = "17d8c89f309a6f0cd516ad5b977d44793755d524" -const val GIT_DATE = "2024-02-20T19:22:23Z" +const val GIT_REVISION = 273 +const val GIT_SHA = "12ac1a0a972b798cbf0d1a77c80fee4ed9e0c32c" +const val GIT_DATE = "2024-02-20T19:35:28Z" const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-02-20T19:30:24Z" -const val BUILD_UNIX_TIME = 1708475424897L +const val BUILD_DATE = "2024-02-21T09:37:27Z" +const val BUILD_UNIX_TIME = 1708526247713L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c14c6e0a..4ecff7c0 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector +import com.team4099.robot2023.auto.mode.TestAutoPath import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand @@ -13,15 +14,19 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator +import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder +import com.team4099.robot2023.subsystems.feeder.FeederIO import com.team4099.robot2023.subsystems.feeder.FeederIONeo import com.team4099.robot2023.subsystems.feeder.FeederIOSim import com.team4099.robot2023.subsystems.flywheel.Flywheel +import com.team4099.robot2023.subsystems.flywheel.FlywheelIO import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon import com.team4099.robot2023.subsystems.intake.Intake +import com.team4099.robot2023.subsystems.intake.IntakeIO import com.team4099.robot2023.subsystems.intake.IntakeIONEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision @@ -36,9 +41,11 @@ import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.util.FrameCoordinate import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.WaitCommand import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband import org.team4099.lib.units.base.feet +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.derived.Angle import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -70,10 +77,10 @@ object RobotContainer { // CameraIONorthstar("backward") ) limelight = LimelightVision(object : LimelightVisionIO {}) - intake = Intake(IntakeIONEO) - feeder = Feeder(FeederIONeo) - elevator = Elevator(ElevatorIONEO) - flywheel = Flywheel(FlywheelIOTalon) + intake = Intake(object: IntakeIO {}) + feeder = Feeder(object : FeederIO {}) + elevator = Elevator(object : ElevatorIO {}) + flywheel = Flywheel(object : FlywheelIO {}) wrist = Wrist(object : WristIO {}) } else { // Simulation implementations @@ -95,7 +102,7 @@ object RobotContainer { superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel) vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) limelight.poseSupplier = { drivetrain.odomTRobot } - FrameCoordinate.robotInOdometryFrameSupplier = { drivetrain.odomTRobot } + FrameCoordinate.odomTField = { drivetrain.odomTField } } fun mapDefaultCommands() { @@ -204,7 +211,7 @@ object RobotContainer { fun mapTestControls() {} - fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain) + fun getAutonomousCommand() = WaitCommand(AutonomousSelector.waitTime.inSeconds).andThen(TestAutoPath(drivetrain)) fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 541c97d1..dbae97f2 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -19,20 +19,15 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { { listOf( OdometryWaypoint( - Translation2d(5.0.meters, 2.0.meters).translation2d, + Translation2d(0.0.meters, 0.0.meters).translation2d, null, 0.0.degrees.inRotation2ds ), OdometryWaypoint( - Translation2d(7.0.meters, 2.0.meters).translation2d, - null, - 90.0.degrees.inRotation2ds - ), - OdometryWaypoint( - Translation2d(7.0.meters, 3.0.meters).translation2d, + Translation2d(-2.0.meters, 0.0.meters).translation2d, null, 0.0.degrees.inRotation2ds - ), + ) ) }, resetPose = true diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index 08784d85..d391bb22 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -45,13 +45,9 @@ class TargetPoseCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain, - targetCoordinate: FrameCoordinate + val targetCoordinate: FrameCoordinate ) : Command() { private val thetaPID: ProfiledPIDController> - private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y } - val targetCoordinate = if (FMSData.allianceColor == DriverStation.Alliance.Red && targetCoordinate is FrameCoordinate.FieldCoordinate) { - FrameCoordinate.FieldCoordinate(FieldConstants.fieldLength - targetCoordinate.x, targetCoordinate.y) - } else targetCoordinate val thetakP = LoggedTunableValue( @@ -108,8 +104,13 @@ class TargetPoseCommand( thetaPID.reset(drivetrain.odomTRobot.rotation) odometryCoordinateToTarget = if (targetCoordinate is FrameCoordinate.FieldCoordinate) { + // Flip alliance first + val coordinateWithAllianceFlip = if (FMSData.allianceColor == DriverStation.Alliance.Red) { + FrameCoordinate.FieldCoordinate(FieldConstants.fieldLength - targetCoordinate.x, targetCoordinate.y) + } else targetCoordinate + FrameCoordinate.OdometryCoordinate( - drivetrain.odomTField.asPose2d().transformBy(targetCoordinate.toTransform2d()) + drivetrain.odomTField.asPose2d().transformBy(coordinateWithAllianceFlip.toTransform2d()) ) } else { targetCoordinate as FrameCoordinate.OdometryCoordinate // Always true, kotlin type casting sucks sometimes @@ -120,8 +121,7 @@ class TargetPoseCommand( Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) val currentPose = drivetrain.odomTRobot - val currentFieldVelocity = fieldVelocitySupplier() - val relativeToRobotPose = odometryCoordinateToTarget.toPose2d().relativeTo(currentPose) + val relativeToRobotPose = odometryCoordinateToTarget.toPose2d().relativeTo(drivetrain.odomTRobot) desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians @@ -134,14 +134,13 @@ class TargetPoseCommand( fieldOriented = true ) - Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odomTRobot.rotation.inDegrees) + Logger.recordOutput("AutoLevel/CurrentYawDegrees", currentPose.rotation.inDegrees) Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) } override fun isFinished(): Boolean { - return (drivetrain.odomTRobot.rotation - desiredAngle).absoluteValue < - DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR + return false } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 51f6e70c..fb32ba45 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -58,8 +58,8 @@ object DrivetrainConstants { val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 3.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 0.5.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 0.5.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 diff --git a/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt b/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt index aa457f8d..1e6aba8f 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/FrameCoordinate.kt @@ -1,9 +1,12 @@ package com.team4099.robot2023.util +import com.team4099.lib.math.asPose2d +import com.team4099.lib.math.asTransform2d import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees abstract class FrameCoordinate(val x: Length, val y: Length) { @@ -17,14 +20,26 @@ abstract class FrameCoordinate(val x: Length, val y: Length) { } class OdometryCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) { + private val pose2d = Pose2d(x, y, 0.degrees) + constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y) + + fun toFieldCoordinate(): FieldCoordinate{ + return FieldCoordinate(pose2d.transformBy(odomTField())) + } } class FieldCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) { + private val pose2d = Pose2d(x, y, 0.degrees) + constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y) + + fun toOdometryCoordinate(): OdometryCoordinate { + return OdometryCoordinate(odomTField().asPose2d().transformBy(pose2d.asTransform2d())) + } } companion object { - lateinit var robotInOdometryFrameSupplier: () -> Pose2d + var odomTField: () -> Transform2d = { Transform2d(Translation2d(0.0.meters, 0.0.meters), 0.0.degrees)} } } From 05d5630b69239abd792db51dbf0f76b4d6e8934d Mon Sep 17 00:00:00 2001 From: Shom770 Date: Wed, 21 Feb 2024 09:43:32 -0500 Subject: [PATCH 9/9] remove buildconstants.kt --- .../com/team4099/robot2023/BuildConstants.kt | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/BuildConstants.kt diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt deleted file mode 100644 index f32b9ccc..00000000 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ /dev/null @@ -1,15 +0,0 @@ -package com.team4099.robot2023 - -/** - * Automatically generated file containing build version information. - */ -const val MAVEN_GROUP = "" -const val MAVEN_NAME = "Crescendo-2024" -const val VERSION = "unspecified" -const val GIT_REVISION = 273 -const val GIT_SHA = "12ac1a0a972b798cbf0d1a77c80fee4ed9e0c32c" -const val GIT_DATE = "2024-02-20T19:35:28Z" -const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-02-21T09:37:27Z" -const val BUILD_UNIX_TIME = 1708526247713L -const val DIRTY = 1