From 7dc00a2ba5c8fb0003c74069c65c23dd3beb8747 Mon Sep 17 00:00:00 2001
From: Matteo Kimura <mateus.sakata@gmail.com>
Date: Fri, 18 Mar 2022 23:02:56 -0500
Subject: [PATCH] Fix two ball timer logic

---
 .../java/frc/robot/commands/TwoBallAuto.java  | 28 +++++++++++++------
 1 file changed, 19 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/commands/TwoBallAuto.java b/src/main/java/frc/robot/commands/TwoBallAuto.java
index d58cb50..8e9c608 100644
--- a/src/main/java/frc/robot/commands/TwoBallAuto.java
+++ b/src/main/java/frc/robot/commands/TwoBallAuto.java
@@ -24,19 +24,24 @@ public TwoBallAuto(Drivetrain drive, Shooter shooter, Indexer indexer, Intake in
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-        new ShootCommand(ShooterConstants.kShooterFenderRPM, 4.0, shooter, indexer),
+        new ShootCommand(ShooterConstants.kShooterFenderRPM, 2.0, shooter, indexer),
         new ParallelCommandGroup(
-            new RunCommand(
-                () -> {
-                  intake.intakeBalls();
-                },
-                intake),
-            new RunCommand(
+                new RunCommand(
+                    () -> {
+                      intake.intakeBalls();
+                    },
+                    intake),
+                new RunCommand(
                     () -> {
                       drive.drive(0.5, 0.0);
                     },
-                    drive)
-                .withTimeout(DrivetrainConstants.kAutoTime)),
+                    drive))
+            .withTimeout(DrivetrainConstants.kAutoTime),
+        new InstantCommand(
+            () -> {
+              drive.drive(0.0, 0.0);
+            },
+            drive),
         new InstantCommand(
             () -> {
               intake.retractIntake();
@@ -48,6 +53,11 @@ public TwoBallAuto(Drivetrain drive, Shooter shooter, Indexer indexer, Intake in
                 },
                 drive)
             .withTimeout(DrivetrainConstants.kAutoTime),
+        new InstantCommand(
+            () -> {
+              drive.drive(0.0, 0.0);
+            },
+            drive),
         new ShootCommand(ShooterConstants.kShooterFenderRPM, 4.0, shooter, indexer));
   }
 }