From 25a95125b117ccf9991f489a97b925f04fe5c43e Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Mon, 19 Feb 2024 00:40:27 -0500 Subject: [PATCH] progress without path planner --- .../pathplanner/paths/Example Path.path | 4 +- .../robot2023/auto/AutonomousSelector.kt | 4 +- .../robot2023/auto/mode/TestAutoPath.kt | 6 +- .../commands/drivetrain/DrivePathCommand.kt | 66 ++++++++++++++++++- .../commands/drivetrain/TeleopDriveCommand.kt | 11 ++-- .../commands/drivetrain/TestDriveCommand.kt | 25 +++++++ .../config/constants/DrivetrainConstants.kt | 4 +- 7 files changed, 106 insertions(+), 14 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index a72b963a..67490b70 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 48e64f40..606e4021 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.auto -import com.team4099.robot2023.auto.mode.ExamplePathAuto +import com.team4099.robot2023.auto.mode.TestAutoPath import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.networktables.GenericEntry import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets @@ -61,7 +61,7 @@ object AutonomousSelector { // println("${waitTime().inSeconds} wait command") when (mode) { AutonomousMode.TEST_AUTO_PATH -> - return WaitCommand(waitTime.inSeconds).andThen(ExamplePathAuto(drivetrain)) + return WaitCommand(waitTime.inSeconds).andThen(TestAutoPath(drivetrain)) else -> println("ERROR: unexpected auto mode: $mode") } return InstantCommand() 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..b220e99a 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -19,17 +19,17 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { { listOf( Waypoint( - Translation2d(0.0.meters, 0.0.meters).translation2d, + Translation2d(5.0.meters, 2.0.meters).translation2d, null, 0.0.degrees.inRotation2ds ), Waypoint( - Translation2d(2.0.meters, 0.0.meters).translation2d, + Translation2d(7.0.meters, 2.0.meters).translation2d, null, 90.0.degrees.inRotation2ds ), Waypoint( - Translation2d(0.0.meters, 1.0.meters).translation2d, + 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..66f6912c 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,17 @@ 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.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 +24,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 @@ -65,6 +72,8 @@ class DrivePathCommand( val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, val endVelocity: Velocity2d = Velocity2d(), + var stateFrame: FrameType = FrameType.ODOMETRY, + var pathFrame: FrameType = FrameType.ODOMETRY, ) : Command() { private val xPID: PIDController> private val yPID: PIDController> @@ -128,6 +137,13 @@ 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 fun generate( waypoints: List, constraints: List = listOf() @@ -173,6 +189,17 @@ 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 + } + } + + drivePoseSupplier = { drivetrain.odomTRobot } + swerveDriveController = CustomHolonomicDriveController( xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController @@ -182,6 +209,13 @@ class DrivePathCommand( } override fun initialize() { + odoTField = drivetrain.odomTField + pathTransform = + Transform2d( + Translation2d(waypoints.get()[0].translation), + waypoints.get()[0].driveRotation?.radians?.radians ?: 0.0.degrees + ) + // trajectory generation! generate(waypoints.get()) @@ -213,6 +247,36 @@ 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) { + // odoTField x fieldTRobot + lastSampledPose = (targetHolonomicPose - pathTransform.asPose2d()).asPose2d() + } else { + when (pathFrame) { + FrameType.ODOMETRY -> + lastSampledPose = odoTField.asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + FrameType.FIELD -> + lastSampledPose = + odoTField.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d()) + } + } + + val pathFrameTRobotPose = (robotPoseInSelectedFrame + pathTransform) + 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 +290,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 = 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/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 8b12ced5..51f6e70c 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 = 1.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 2.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 3.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 3.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