reduce leave community distance and add cube shooting auto

Spartronics4915 · Mar 5, 2023 · 383f1c3 · 383f1c3
1 parent f830ac2
commit 383f1c3
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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);