From 0b8ecf39f8ecc178ad38139af25f5018767babe7 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 5 Feb 2021 15:20:45 -0700
Subject: [PATCH] 6 Ball Auto Path with Intake WORKING

---
 src/main/java/frc4388/robot/RobotContainer.java             | 6 ++++--
 .../java/frc4388/robot/commands/auto/SixBallAutoMiddle.java | 6 +++---
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index ab2aeef..04d83f4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -350,14 +352,14 @@ public class RobotContainer {
         try {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
-            return m_sixBallAutoMiddle.andThen(m_sixBallAuto1);
+            //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_figureEight.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-
+            return new ParallelRaceGroup(m_sixBallAutoMiddle, new RunIntake(m_robotIntake));
         } catch (Exception e) {
             System.err.println("ERROR");
         }
diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
index 261e4e5..17b544c 100644
--- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
@@ -12,9 +12,11 @@ import java.nio.file.Path;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.RobotContainer;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.commands.intake.RunIntake;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
@@ -32,9 +34,7 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
     Intake m_intake = new Intake();
 
     addCommands(
-      paths[0],
-      new RunIntake(m_intake)//,
-      //paths[1]
+      paths[0]
     );
   }
 }