From 26b56c8c21c39c0d5e3a68a26ce23573950b48b1 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 5 Feb 2021 14:27:50 -0700
Subject: [PATCH] Added code to help intake run in between the path

---
 .../java/frc4388/robot/RobotContainer.java    | 19 ++++++++++++++++++-
 .../commands/auto/SixBallAutoMiddle.java      |  7 ++++++-
 2 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a67dc99..ab2aeef 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -52,6 +52,7 @@ import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.TurnDegrees;
+import frc4388.robot.commands.intake.RunIntake;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
 import frc4388.robot.commands.shooter.TrackTarget;
@@ -111,6 +112,10 @@ public class RobotContainer {
 
     SixBallAutoMiddle m_sixBallAutoMiddle;
 
+    SixBallAutoMiddle m_sixBallAuto0;
+
+    SixBallAutoMiddle m_sixBallAuto1;
+
     FigureEight m_figureEight;
 
     EightBallAutoMiddle m_eightBallAutoMiddle;
@@ -281,6 +286,18 @@ public class RobotContainer {
 
         m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
+        String[] sixBallAutoMiddle0 = new String[]{
+            "SixBallMid0"
+        };
+
+        m_sixBallAuto0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0));
+
+        String[] sixBallAutoMiddle1 = new String[]{
+            "SixBallMid1"
+        };
+
+        m_sixBallAuto1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1));
+
         String[] figureEight = new String[]{
             "FigureEight"
         };
@@ -333,7 +350,7 @@ public class RobotContainer {
         try {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
-            return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_sixBallAutoMiddle.andThen(m_sixBallAuto1);
             //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));
diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
index 9900c65..261e4e5 100644
--- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
@@ -15,7 +15,9 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.RobotContainer;
+import frc4388.robot.commands.intake.RunIntake;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Intake;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -27,9 +29,12 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
   public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
+    Intake m_intake = new Intake();
 
     addCommands(
-      paths[0]
+      paths[0],
+      new RunIntake(m_intake)//,
+      //paths[1]
     );
   }
 }