From 383f1c35edcb3299cc031961511768c280e80ede Mon Sep 17 00:00:00 2001
From: ss0g <55930267+ss0g@users.noreply.github.com>
Date: Sun, 5 Mar 2023 09:09:15 -0800
Subject: [PATCH] reduce leave community distance and add cube shooting auto

---
 .../spartronics4915/frc2023/Constants.java    |  6 +++++
 .../frc2023/RobotContainer.java               | 23 ++++++++++++++++++-
 .../frc2023/subsystems/ArmSubsystem.java      |  4 +++-
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/src/main/java/com/spartronics4915/frc2023/Constants.java b/src/main/java/com/spartronics4915/frc2023/Constants.java
index 7871288..696071b 100644
--- a/src/main/java/com/spartronics4915/frc2023/Constants.java
+++ b/src/main/java/com/spartronics4915/frc2023/Constants.java
@@ -460,6 +460,12 @@ public ArmMotorConstants(int MotorID, double PositionConversionFactor, boolean I
             Rotation2d.fromDegrees(23),
             Rotation2d.fromDegrees(-36)
         );
+
+        public static final ArmPositionConstants kCubeTopShootConstants = new ArmPositionConstants(
+            0,
+            Rotation2d.fromDegrees(9.996031),
+            Rotation2d.fromDegrees(30.441)
+        );
         
         public static final Rotation2d kTransformAmount = Rotation2d.fromDegrees(0.5);
         public static final double kArmRetractedPriorWaitDuration = 1; // seconds
diff --git a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java
index 3d3b801..5d9e867 100644
--- a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java
+++ b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java
@@ -210,8 +210,29 @@ mAutos.new Strategy(
                 "place, leave community",
                 (Pose2d initialPose) -> new SequentialCommandGroup(
                     mArmCommands.new ReleasePiece(ArmState.FLOOR_POS),
+                    mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE),
+                    new WaitCommand(1),
                     mSwerve.driveCommand(new ChassisSpeeds(-2, 0, 0), false, true),
-                    new WaitCommand(3),
+                    new WaitCommand(2.5),
+                    mSwerve.driveCommand(new ChassisSpeeds(), false, true)
+                )
+            ),
+            mAutos.new Strategy(
+                "cube high (test)",
+                (Pose2d initialPose) -> new SequentialCommandGroup(
+                    mArmCommands.new ReleasePiece(ArmState.SHOOT_HIGH_CUBE)
+                )
+            ),
+            mAutos.new Strategy(
+                "place, move short (test)",
+                (Pose2d initialPose) -> new SequentialCommandGroup(
+                    mArmCommands.new ReleasePiece(ArmState.FLOOR_POS),
+                    mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE),
+                    new WaitCommand(1),
+                    mSwerve.driveCommand(new ChassisSpeeds(-0.5, 0, 0), false, true),
+                    new WaitCommand(2),
+                    mSwerve.driveCommand(new ChassisSpeeds(-1, 0, 0), false, true),
+                    new WaitCommand(1),
                     mSwerve.driveCommand(new ChassisSpeeds(), false, true)
                 )
             )
diff --git a/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java b/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java
index a148560..90c94df 100644
--- a/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java
+++ b/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java
@@ -58,7 +58,9 @@ public enum ArmState {
         CONE_LEVEL_1(kConeLevel1Constants),
         CONE_LEVEL_2(kConeLevel2Constants),
         CUBE_LEVEL_1(kCubeLevel1Constants),
-        CUBE_LEVEL_2(kCubeLevel2Constants);
+        CUBE_LEVEL_2(kCubeLevel2Constants),
+
+        SHOOT_HIGH_CUBE(kCubeTopShootConstants);
 
         // CONE_LEVEL_3(kConeLevel3Constants);