From a62ce75294d192f1e47cb79f84aa05d1c22fe21d Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Thu, 14 Dec 2023 17:46:53 -0500 Subject: [PATCH] added all cone auto command --- .../robot2023/auto/AutonomousSelector.kt | 11 +- .../auto/mode/LimelightTestingAuto.kt | 197 ++++++++++++++++++ 2 files changed, 207 insertions(+), 1 deletion(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 2820247d..cf17e399 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -10,6 +10,7 @@ import com.team4099.robot2023.auto.mode.ConeCubeLowOverChargeStationAuto import com.team4099.robot2023.auto.mode.ConeCubeMobilityAuto import com.team4099.robot2023.auto.mode.ConeCubeOverChargeStationAuto import com.team4099.robot2023.auto.mode.ConeMobilityAuto +import com.team4099.robot2023.auto.mode.LimelightTestingAuto import com.team4099.robot2023.auto.mode.PreloadOpenLoopChargeStationBalance import com.team4099.robot2023.auto.mode.ScorePreloadCone import com.team4099.robot2023.auto.mode.TestAutoPath @@ -85,6 +86,10 @@ object AutonomousSelector { autonomousModeChooser.addOption("1 Cone + Mobility", AutonomousMode.CONE_MOBILITY_AUTO) + autonomousModeChooser.addOption( + "LimeLight Testing", AutonomousMode.CONE_LIMELIGHT + ) + // autonomousModeChooser.addOption("1 Cone + Open Loop Charge Station", // AutonomousMode.CONE_MOBILITY_AUTO) @@ -199,6 +204,9 @@ object AutonomousSelector { return WaitCommand(waitTime.inSeconds) .andThen(ScorePreloadCone(drivetrain, superstructure)) .andThen(engageCommand) + AutonomousMode.CONE_LIMELIGHT -> + return WaitCommand(waitTime.inSeconds) + .andThen(LimelightTestingAuto(drivetrain, superstructure)) else -> println("ERROR: unexpected auto mode: $mode") } return InstantCommand() @@ -218,6 +226,7 @@ object AutonomousSelector { CO_CU_LAUNCH_MIDDLE_AUTO, CONE_MOBILITY_AUTO, PRELOAD_SCORE_OPEN_LOOP_CHARGE_STATION_SCORE, - PRELOAD_SCORE_AUTO + PRELOAD_SCORE_AUTO, + CONE_LIMELIGHT } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt new file mode 100644 index 00000000..a7b52005 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt @@ -0,0 +1,197 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.trajectory.Waypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +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.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inRotation2ds + +class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + + init { + addCommands( + ResetPoseCommand( + drivetrain, Pose2d(startingPosX.get(), startingPosY.get(), startingPosTheta.get()) + ), + ParallelCommandGroup( + DrivePathCommand( + drivetrain, + { + listOf( + Waypoint( + Translation2d(1.9.meters, 3.31.meters).translation2d, + null, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d(2.9.meters, 4.8.meters).translation2d, + null, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get(), + FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get() + ) + .translation2d, + 0.0.degrees.inRotation2ds, + 0.0.degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand()) + ), + ParallelCommandGroup( + WaitCommand(1.0).andThen(superstructure.requestIdleCommand()), + DrivePathCommand( + drivetrain, + { + listOf( + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[3]!!.x- intakeOffset.get(), + FieldConstants.StagingLocations.translations[3]!!.y- intakeOffset.get() + ) + .translation2d, + 0.0.degrees.inRotation2ds, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d(3.139.meters, 4.8.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + Waypoint( + Translation2d( + endingPosX.get(), + FieldConstants.Grids.nodeFirstY + + FieldConstants.Grids.nodeSeparationY * 7 + ) + .translation2d, + null, + 180.0.degrees.inRotation2ds + ) + ) + }, + keepTrapping = true + ) + ), + superstructure.prepScoreCommand( + Constants.Universal.GamePiece.CUBE, Constants.Universal.NodeTier.HIGH + ), + superstructure.score(), + WaitCommand(0.5), + ParallelCommandGroup( + DrivePathCommand( + drivetrain, + { + listOf( + Waypoint( + Translation2d(1.9.meters, 3.31.meters).translation2d, + null, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d(2.9.meters, 4.8.meters).translation2d, + null, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get(), + FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get() + ) + .translation2d, + 0.0.degrees.inRotation2ds, + 0.0.degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand()) + ), + ParallelCommandGroup( + WaitCommand(1.0).andThen(superstructure.requestIdleCommand()), + DrivePathCommand( + drivetrain, + { + listOf( + Waypoint( + Translation2d( + FieldConstants.StagingLocations.translations[2]!!.x- intakeOffset.get(), + FieldConstants.StagingLocations.translations[2]!!.y- intakeOffset.get() + ) + .translation2d, + 0.0.degrees.inRotation2ds, + 0.0.degrees.inRotation2ds + ), + Waypoint( + Translation2d(3.139.meters, 4.8.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + Waypoint( + Translation2d( + endingPosX.get(), + FieldConstants.Grids.nodeFirstY + + FieldConstants.Grids.nodeSeparationY * 7 + ) + .translation2d, + null, + 180.0.degrees.inRotation2ds + ) + ) + }, + keepTrapping = true + ) + ) + ) + } + + companion object { + val startingPosX = + LoggedTunableValue( + "Drivetrain/startingPosX1", 1.9.meters, Pair({ it.inMeters }, { it.meters }) + ) + val startingPosY = + LoggedTunableValue( + "Drivetrain/startingPosY1", 3.31.meters, Pair({ it.inMeters }, { it.meters }) + ) + val startingPosTheta = + LoggedTunableValue( + "Drivetrain/startingPosTheta1", 0.0.degrees, Pair({ it.inDegrees }, { it.degrees }) + ) + + val endingPosX = + LoggedTunableValue( + "Drivetrain/endingPosX1", 1.9.meters, Pair({ it.inMeters }, { it.meters }) + ) + val endingPosY = + LoggedTunableValue( + "Drivetrain/endingPosY1", 3.31.meters, Pair({ it.inMeters }, { it.meters }) + ) + val endingPosTheta = + LoggedTunableValue( + "Drivetrain/endingPosTheta1", 180.0.degrees, Pair({ it.inDegrees }, { it.degrees }) + ) + val intakeOffset= + LoggedTunableValue( + "Drivetrain/intakeOffset", 0.0.meters, Pair({ it.inMeters }, { it.meters }) + ) + } +}