From 41aac7d39e38dbb67c71b695205057680c11c2c0 Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Tue, 3 Oct 2023 08:30:57 -0400
Subject: [PATCH] Update autonomous

- Update routine with updated superstructure methods
- Document the autonomous and functions used
- Clean stuff up <3
---
 RELENTLESS/src/autons.cpp | 48 ++++++++++++++++-----------------------
 1 file changed, 20 insertions(+), 28 deletions(-)

diff --git a/RELENTLESS/src/autons.cpp b/RELENTLESS/src/autons.cpp
index 8a023fd..a76f56c 100644
--- a/RELENTLESS/src/autons.cpp
+++ b/RELENTLESS/src/autons.cpp
@@ -7,39 +7,31 @@
 using namespace globals;
 using namespace superstruct;
 
+/*
+  chassis.wait_drive() -> Waits for the drivetrain to complete whatever movement is happening
+  chassis.wait_until(double measurement) -> Waits for the drivetrain to reach a certain measurement before performing the next task.
+*/
+
 void test_auton() {
-  driveChassis(107, true);
-  chassis.wait_drive();
-
-  driveChassis(-38, true);
-  chassis.wait_drive();
-
-  turnChassis(-99);
-  chassis.wait_drive();
-
-  driveChassis(95, true);
-  chassis.wait_drive();
-
-  driveChassis(-10, true);
-  chassis.wait_drive();
-
-  turnChassis(15);
-  chassis.wait_drive();
-
-  driveChassis(40, true);
-  chassis.wait_drive();
+  // Score the triball preload
+  driveSync(107, true); // Go forward
+  
+  // Get the furthest triball
+  driveSync(-38, true); // Move away from the goal
+  turnSync(-99); // Turn towards the triball against the barrier
+  driveSync(95, true); // Drive towards the triball
 
+  // Turn towards the goal, and score the triball.
+  driveSync(-10, true); // Move away from the point
+  turnSync(15); // Turn towards the goal 
+  driveSync(40, true); // Drive towards the goal and score the triball
   leftSwing(60);
   chassis.wait_drive();
 
-  driveChassis(-48, true);
-  chassis.wait_drive();
-
-  turnChassis(-90);
-  chassis.wait_drive();
-  
-  driveChassis(12, true);
-  chassis.wait_drive();
+  //Drive backwards, turn to get the next triball off the line
+  driveSync(-48, true); // Move away from the goal towards the nearest triball
+  turnChassis(-90); // Turn to the triball and intake
+  driveSync(12, true);
 
   // chassis.set_drive_pid(2, DRIVE_SPEED, false, true);
   // chassis.wait_drive();