From 0c2d8b6329332afb4ad2f3225f15622a14d29722 Mon Sep 17 00:00:00 2001
From: team6101 <wtmcrobotics@wccnet.edu>
Date: Thu, 10 Oct 2024 17:48:55 -0400
Subject: [PATCH] Add pseudo code

---
 src/main/java/frc/robot/Robot.java            |  4 ++
 .../java/frc/robot/pseudoCode/CodeRunner.java |  8 +++
 .../robot/pseudoCode/PseudoCodeActions.java   | 51 +++++++++++++++++++
 3 files changed, 63 insertions(+)
 create mode 100644 src/main/java/frc/robot/pseudoCode/CodeRunner.java
 create mode 100644 src/main/java/frc/robot/pseudoCode/PseudoCodeActions.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 23f945e..c790a5d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,6 +22,8 @@
 import frc.robot.auton.SequentialActionRunner;
 import frc.robot.motor.MotorController;
 import frc.robot.motor.MotorControllerFactory;
+import frc.robot.pseudoCode.CodeRunner;
+import frc.robot.pseudoCode.PseudoCodeActions;
 import java.text.DecimalFormat;
 import java.util.ArrayDeque;
 
@@ -261,6 +263,7 @@ public void setupAutonRoutes() {
         );
         autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
         autonRouteChooser.addOption("Test Manual Backward PID movement", AutonRoutes.TEST_PID_BACKWARD_MOVEMENT);
+        autonRouteChooser.addOption("Run Pseudo Code", PseudoCodeActions.pseudoCodeActionQueue);
     }
 
     /*
@@ -316,6 +319,7 @@ public void robotPeriodic() {
      */
     @Override
     public void autonomousInit() {
+        CodeRunner.main(new String[] { "this argument is useless" });
         AutonRoutes.setupCorrectAutonPaths();
         ArrayDeque<AutonAction> route = new ArrayDeque<>(autonRouteChooser.getSelected());
         double delayAmount = SmartDashboard.getNumber("Auton Delay (sec)", 0.0);
diff --git a/src/main/java/frc/robot/pseudoCode/CodeRunner.java b/src/main/java/frc/robot/pseudoCode/CodeRunner.java
new file mode 100644
index 0000000..66149ad
--- /dev/null
+++ b/src/main/java/frc/robot/pseudoCode/CodeRunner.java
@@ -0,0 +1,8 @@
+package frc.robot.pseudoCode;
+
+import static frc.robot.pseudoCode.PseudoCodeActions.*;
+
+public class CodeRunner {
+
+    public static void main(String[] args) {}
+}
diff --git a/src/main/java/frc/robot/pseudoCode/PseudoCodeActions.java b/src/main/java/frc/robot/pseudoCode/PseudoCodeActions.java
new file mode 100644
index 0000000..e6f6c36
--- /dev/null
+++ b/src/main/java/frc/robot/pseudoCode/PseudoCodeActions.java
@@ -0,0 +1,51 @@
+package frc.robot.pseudoCode;
+
+import frc.robot.auton.AutonAction;
+import frc.robot.auton.AutonMoveInches;
+import frc.robot.auton.AutonRotateWithPIDCommand;
+import frc.robot.auton.AutonShoot;
+import frc.robot.auton.AutonShooterFeed;
+import frc.robot.auton.ExtendArms;
+import frc.robot.auton.RetractArms;
+import java.util.ArrayDeque;
+
+public class PseudoCodeActions {
+
+    public static ArrayDeque<AutonAction> pseudoCodeActionQueue = new ArrayDeque<>();
+
+    public static void addAction(AutonAction action) {
+        pseudoCodeActionQueue.add(action);
+    }
+
+    public static void moveForwards(int inches) {
+        addAction(new AutonMoveInches(inches));
+    }
+
+    public static void moveBackwards(int inches) {
+        addAction(new AutonMoveInches(-inches));
+    }
+
+    public static void rotateRight(double degrees) {
+        addAction(new AutonRotateWithPIDCommand(degrees));
+    }
+
+    public static void rotateLeft(double degrees) {
+        addAction(new AutonRotateWithPIDCommand(-degrees));
+    }
+
+    public static void shoot() {
+        addAction(new AutonShoot());
+    }
+
+    public static void feed() {
+        addAction(new AutonShooterFeed());
+    }
+
+    public static void raiseArms() {
+        addAction(new ExtendArms());
+    }
+
+    public static void lowerArms() {
+        addAction(new RetractArms());
+    }
+}