From 364319ecaa03fc4519fafd3d31e0fe2c7f75113e Mon Sep 17 00:00:00 2001
From: cowsed <richie.sommers@outlook.com>
Date: Thu, 22 Feb 2024 20:31:43 -0500
Subject: [PATCH] me when branching works (:))

---
 .vscode/vex_project_settings.json |  2 +-
 src/competition/opcontrol.cpp     | 18 ++++++++++++++++--
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/.vscode/vex_project_settings.json b/.vscode/vex_project_settings.json
index bd6f694..05e90d6 100644
--- a/.vscode/vex_project_settings.json
+++ b/.vscode/vex_project_settings.json
@@ -1,7 +1,7 @@
 {
 	"project": {
 		"name": "BlueAuto",
-		"slot": 2,
+		"slot": 1,
 		"description": "The cool one",
 		"platform": "V5",
 		"creationDate": "Tue, 13 Feb 2024 20:37:40 GMT",
diff --git a/src/competition/opcontrol.cpp b/src/competition/opcontrol.cpp
index 22f84e8..2ef9b57 100644
--- a/src/competition/opcontrol.cpp
+++ b/src/competition/opcontrol.cpp
@@ -22,18 +22,32 @@ auto toggle_brake_mode = []() {
  * Main entrypoint for the driver control period
  */
 void opcontrol() {
+
     vex::timer dont_stop_tmr;
     dont_stop_tmr.reset();
 
     con.ButtonRight.pressed([]() { screen::next_page(); });
     con.ButtonLeft.pressed([]() { screen::prev_page(); });
     //
-    cata_sys.send_command(CataSys::Command::StartDropping);
+    // cata_sys.send_command(CataSys::Command::StartDropping);
 
     while (imu.isCalibrating()) {
         vexDelay(20);
     }
-
+    odom.set_position({0, 0, 0});
+    CommandController cc{
+        new RepeatUntil(
+            InOrder{
+                new DelayCommand(1000),
+                new Branch(new FunctionCondition(
+                               []() { return cata_sys.ball_in_intake(); }),
+                           drive_sys.TurnDegreesCmd(-90),
+                           drive_sys.TurnDegreesCmd(90)),
+                drive_sys.TurnToHeadingCmd(0),
+            },
+            new FunctionCondition([]() { return con.ButtonA.pressing(); })),
+    };
+    cc.run();
     static bool enable_matchload = false;
     static std::atomic<bool> disable_drive(false);