From faacca681993794bcf71e68b8112415c458bed58 Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Wed, 13 Dec 2023 07:38:44 -0500
Subject: [PATCH] new autons - 6tb and async testing

fake 6 triball, first half implemented
async testing, running intake in parallel

paths are to be optimized, lots of time is lost due to turning speed and timeouts alone
---
 CLOUDS/include/autons.hpp |  2 ++
 CLOUDS/src/autons.cpp     | 39 +++++++++++++++++++++++++++++++++++++--
 2 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/CLOUDS/include/autons.hpp b/CLOUDS/include/autons.hpp
index cd0edd9..81eecd0 100644
--- a/CLOUDS/include/autons.hpp
+++ b/CLOUDS/include/autons.hpp
@@ -4,4 +4,6 @@
 
 void near_side();
 void far_side();
+void six_triball();
+void testingAsyncRunning();
 void skills();
\ No newline at end of file
diff --git a/CLOUDS/src/autons.cpp b/CLOUDS/src/autons.cpp
index 7159534..e72bada 100644
--- a/CLOUDS/src/autons.cpp
+++ b/CLOUDS/src/autons.cpp
@@ -11,8 +11,8 @@ using namespace superstruct;
 */
 void near_side() {
   // Score the triball preload
-  togglePto(true);
-  runIntake(-12000);
+  //togglePto(true);
+  setIntakeSpeed(-12000);
   driveSync(90, true);
   turnSync(90);
 
@@ -78,6 +78,34 @@ void far_side() {
   turnSync(120);
 }
 
+void six_triball() {
+  /*  */
+  togglePto(true);
+  driveSync(35, true);
+  turnSync(-37);
+  setIntakeSpeed(-12000);
+  driveSync(80, true);
+  turnSync(90);
+  wings.open();
+  setIntakeSpeed(12000);
+  driveSync(55, true);
+  wings.close();
+  driveSync(-10, true);
+  turnSync(-130);
+  setIntakeSpeed(-12000);
+  driveSync(60, true);
+  driveSync(-10, true);
+  turnSync(67);
+  setIntakeSpeed(12000);
+  wings.open();
+  driveSync(80, true);
+  driveSync(-10, true);
+  wings.close();
+  turnSync(180);
+  driveSync(55, true);
+  
+}
+
 void skills() {
   //toggleIntake(true);
   driveSync(30, true);
@@ -106,4 +134,11 @@ void skills() {
   turnSync(45);
   driveSync(125, true);
   //turnSync();
+}
+
+void testingAsyncRunning() {
+  togglePto(true);
+  setIntakeSpeed(-12000);
+  pros::delay(5000);
+  driveSync(30, true);
 }
\ No newline at end of file