From 985793b5439c65f7886fad466f6997459f6c43fa Mon Sep 17 00:00:00 2001
From: "Myles A. Knowlin" <knowlim2@1038.org>
Date: Wed, 10 Apr 2024 19:55:19 -0400
Subject: [PATCH 1/2] beginnings of FunctionTesting Command

---
 .../commands/FunctionTestingCommand.java      | 23 +++++++++++++++++++
 .../java/frc/robot/subsystems/Dashboard.java  | 10 ++++++++
 2 files changed, 33 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/FunctionTestingCommand.java

diff --git a/src/main/java/frc/robot/commands/FunctionTestingCommand.java b/src/main/java/frc/robot/commands/FunctionTestingCommand.java
new file mode 100644
index 00000000..3bb78db6
--- /dev/null
+++ b/src/main/java/frc/robot/commands/FunctionTestingCommand.java
@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.constants.ScoringConstants.ScoringLocation;
+import frc.robot.subsystems.ScoringElevator.ElevatorSetpoints;
+
+public class FunctionTestingCommand extends SequentialCommandGroup {
+    public FunctionTestingCommand() {
+        this.addCommands(
+                new ScoringElevatorPositionCommand(ElevatorSetpoints.Trap),
+                new ScoringElevatorPositionCommand(ElevatorSetpoints.Ground),
+                new AcquireCommand(5),
+                new TransitionRunCommand(),
+                new StorageRunCommand(),
+                new ScoringElevatorPositionCommand(ElevatorSetpoints.Amp),
+                new ScoreNoteCommand(ScoringLocation.Amp, 2),
+                new DrawbridgeDownCommand(),
+                new LiftUpCommand(),
+                new DrawbridgeUpCommand(),
+                new LiftDownCommand(),
+                new ShootPasserCommand());
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java
index de9d207b..86ca0e13 100644
--- a/src/main/java/frc/robot/subsystems/Dashboard.java
+++ b/src/main/java/frc/robot/subsystems/Dashboard.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.autons.AutonSelector.AutonChoices;
+import frc.robot.commands.FunctionTestingCommand;
 import frc.robot.constants.ScoringConstants;
 
 public class Dashboard extends SubsystemBase {
@@ -49,6 +50,11 @@ public enum Cameras {
             .withPosition(0, 0)
             .withWidget(BuiltInWidgets.kToggleButton)
             .getEntry();
+    private GenericEntry functionTesting = controlsTab.add("Full Function Testing", false)
+            .withSize(1, 1)
+            .withPosition(5, 2)
+            .withWidget(BuiltInWidgets.kToggleButton)
+            .getEntry();
 
     // Drivers Tab Inputs
     private GenericEntry feedAmpSpeed = driversTab.add("Feed Amp Speed", ScoringConstants.feedAmpSpeed)
@@ -145,6 +151,10 @@ public void periodic() {
             driveTrain.zeroHeading();
             resetGyro.setBoolean(false);
         }
+        if (functionTesting.getBoolean(false)) {
+            new FunctionTestingCommand();
+            functionTesting.setBoolean(false);
+        }
         field.setRobotPose(driveTrain.getPose());
         scoring.setFeedAmpSpeed(feedAmpSpeed.getDouble(ScoringConstants.feedAmpSpeed));
     }

From e082ed4677d22e05672addbbc822c3b3dcefdbb6 Mon Sep 17 00:00:00 2001
From: Rachit Gupta <rachitgupta1901@gmail.com>
Date: Mon, 22 Apr 2024 19:47:58 -0400
Subject: [PATCH 2/2] Made a few minor tweaks and changed the order in the
 periodic function

---
 .../java/frc/robot/commands/FunctionTestingCommand.java    | 2 +-
 src/main/java/frc/robot/subsystems/Dashboard.java          | 7 +++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/FunctionTestingCommand.java b/src/main/java/frc/robot/commands/FunctionTestingCommand.java
index 3bb78db6..b90c88dc 100644
--- a/src/main/java/frc/robot/commands/FunctionTestingCommand.java
+++ b/src/main/java/frc/robot/commands/FunctionTestingCommand.java
@@ -18,6 +18,6 @@ public FunctionTestingCommand() {
                 new LiftUpCommand(),
                 new DrawbridgeUpCommand(),
                 new LiftDownCommand(),
-                new ShootPasserCommand());
+                new ShootPasserCommand(5));
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java
index 86ca0e13..4400a0da 100644
--- a/src/main/java/frc/robot/subsystems/Dashboard.java
+++ b/src/main/java/frc/robot/subsystems/Dashboard.java
@@ -151,12 +151,15 @@ public void periodic() {
             driveTrain.zeroHeading();
             resetGyro.setBoolean(false);
         }
+
+        field.setRobotPose(driveTrain.getPose());
+        scoring.setFeedAmpSpeed(feedAmpSpeed.getDouble(ScoringConstants.feedAmpSpeed));
+
         if (functionTesting.getBoolean(false)) {
             new FunctionTestingCommand();
             functionTesting.setBoolean(false);
         }
-        field.setRobotPose(driveTrain.getPose());
-        scoring.setFeedAmpSpeed(feedAmpSpeed.getDouble(ScoringConstants.feedAmpSpeed));
+
     }
 
     /**