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();