me when branching works (:))

RIT-VEX-U · Feb 23, 2024 · 364319e · 364319e
1 parent a927d34
commit 364319e
Show file tree

Hide file tree

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