From dc9199d95d92c9187d7664a3ca0b6e3b5e3e25e2 Mon Sep 17 00:00:00 2001
From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com>
Date: Tue, 19 Mar 2024 12:09:49 -0400
Subject: [PATCH] Trying values, testing in teleop

---
 src/main/java/frc/robot/Constants.java        | 19 ++++++++++++----
 src/main/java/frc/robot/RobotContainer.java   |  7 +++++-
 .../frc/robot/commands/ShootSafetyPose.java   | 22 +++++++++++++++++++
 3 files changed, 43 insertions(+), 5 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ShootSafetyPose.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1c72f92..a5e8145 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -35,14 +35,25 @@ public static class ArmConstants {
     public static final double ARM_HOME_POSE = 0;
     public static final double ARM_LOW_POSE = 10;
     public static final double ARM_MID_POSE = 20; // 30 // 20
-    /* @202 inches
-     * 30 was too steep at 
-     * 40 was too flat
-     */
     public static final double ARM_HIGH_POSE = 55;
     public static final double ARM_AMP_POSE = 90;
+    public static final double ARM_SAFETY_POSE = 20;
     public static final int ARM_FWD_LIMIT = 100; // Previous event 91
 
+    /* Distances (inches) from front Bumper Speaker Base to front Bumper and arm in canon units
+    Pose 0 is at the base, Pose 1 is at the leg of the Stage, i.e. "Safety Pose"
+
+    | Pose  | Dist  | Arm |
+    |------ |-----  | --- |
+    | 0     | 0     | 0   |
+    | 1     | 126   | 20  | Safety Pose
+    | 2     | 150   | 00  |
+    | 3     | 180   | 00  |
+    | 4     | 200   | 00  |
+    
+    */
+
+
     public static final int ARM_MAX_ACCEL = 100;
     public static final int ARM_MAX_VEL = 200;
     public static final int ARM_JERK = 0;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 008f84d..af9fd89 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@
 import frc.robot.commands.SetIndex;
 import frc.robot.commands.SetWinch;
 import frc.robot.commands.ShootClose;
+import frc.robot.commands.ShootSafetyPose;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
@@ -88,6 +89,8 @@ public class RobotContainer {
   private final IntakeRevCommandGroup intakeRevGroup = new IntakeRevCommandGroup(index, intake);
   // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest);
 
+  private final ShootSafetyPose shootSafetyPose = new ShootSafetyPose(arm, index, intake, shooter);
+
   /* Autonomous Chooser */
   SendableChooser<Command> autoChooser;
 
@@ -163,7 +166,9 @@ private void configureBindings() {
     m_driverController.a().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_HOME_POSE)));
     m_driverController.b().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
     m_driverController.x().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_AMP_POSE)));
-    m_driverController.y().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
+    
+    // FIXME Using this for Testing
+    m_driverController.y().whileTrue(shootSafetyPose);
 
     m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
     m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));
diff --git a/src/main/java/frc/robot/commands/ShootSafetyPose.java b/src/main/java/frc/robot/commands/ShootSafetyPose.java
new file mode 100644
index 0000000..d56859e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ShootSafetyPose.java
@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.IndexSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystemVelocity;
+
+public class ShootSafetyPose extends SequentialCommandGroup {
+    public ShootSafetyPose(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem intake, ShooterSubsystemVelocity shooter) {
+
+        addCommands(
+            /* Moving the arm is shouldn't be necessary, but just in case */
+            new SetArmPosition(arm, Constants.ArmConstants.ARM_SAFETY_POSE),
+            // new ShooterIndex(shooter, index).withTimeout(1.0)
+            new RevAndShootCommand(index, shooter).withTimeout(1.25).andThen(new InstantCommand(() -> shooter.Disable(), shooter)),
+            new SetIndex(index, 0).withTimeout(.2)
+        );
+    }
+}
\ No newline at end of file